ros tf

研究ros tf的使用

这两篇篇文章说的蛮好的这篇是概念上的
这篇是数学上的解释
安装参看tf introduction

view frames会生成frames.pdf，打开pdf可以查看坐标系关系

rosrun tf view_frames

也可以直接使用rqt_tf_tree查看

rosrun rqt_tf_tree rqt_tf_tree

两个坐标系之间的关系可以用tf_echo查看

rosrun tf tf_echo [reference_frame] [target_frame]

Ｔ的第一个坐标系是reference，第二个是target

$$\mathbf{T}_{turtle1\_turtle2} =\mathbf{T}_{turtle1\_world} *\mathbf{T}_{world\_turtle2}$$

标定后发布机器人和相机的关系可以用static publisher
最后一个是周期，周期越小频率越高

rosrun tf static_transform_publisher x y z qx qy qz qw frame_id child_frame_id  period_in_ms

如果要放到launch中

<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1 100" />
</launch>

创建一个包

catkin_create_pkg learning_tf tf roscpp rospy turtlesim

创建发布

static tf::TransformBroadcaster br;

发布的变换

  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);

发送变换

 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

四个参数的解释
第一个就是transform
时间戳是当前时间
父坐标系
子坐标系
其实就是world到turtle的坐标系变换，也是turtle在world中的描述
Note: sendTransform and StampedTransform have opposite ordering of parent and child.

接收transform

 tf::TransformListener listener;

tf::StampedTransform transform;
    try{
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

lookup第一个参数是target frame, 第二个参数是source frame，这里不是很懂，从代码和实际情况来看，查询的是turtle2到turtle1的变换，ros::Time(0)是最新的坐标系，存储到transform中。