ROS中的tf(transform)的理解 ，你追我小乌龟的深入剖析

对于ros中的tf其实一直理解不是很深，最近工作上一直在用，就很懵逼，出来混果然是要还的～于是这两天把ros官方提供的小乌龟版的你追我，如果你追到我，我就让你xxx，一个一个命令跑了一下，现在有了一个较深的理解，如果哪里说的不对，还请指正。

首先看launch文件内容

 <launch>
    <!-- 海龟仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!-- 两只海龟的tf广播 -->
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />

    <!-- 监听tf广播，并且控制turtle2移动 -->
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />

 </launch>

分析一下，首先启动了turtlesim的节点，然后是键盘控制，这里有一点需要注意一下，至少我是一直没搞得很清楚

<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

转化为命令行即为：

rosrun turtlesim turtlesim_node __name:=sim

最后那个 __name:=sim 即给节点起名为sim，如果不改名的话，就是你代码里起的名字了，但是需要注意的是因为在例程中因为要启动 /turtle1 和 /turtle2，如果不起名的话，第二只就把第一只干掉了，所以起名还是很重要的～

然后就是启动两只乌龟的learning_tf_broadcaster，注意这里启动完之后，就会有第一只和第二只和世界坐标系之间的关系，当然我们也可以定义第三只第四只乌龟。启动三只乌龟后的tf图如下所示：

这时候，其实，三只乌龟以及世界坐标系的关系就都可以相互转换了，比如第二只要去追第一只，得到第二只相对第一只的transform就可以求啦～同理，第三只追第二只，第三只追第一只，以及第n只追第k只都可以定义啦～

            listener.waitForTransform("/turtle3", "/turtle2", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/turtle3", "/turtle2", ros::Time(0), transform3);
	   		listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform2);

然后分别发布到 “turtle2/cmd_vel” “turtle3/cmd_vel” 再加上键盘发布 “turtle1/cmd_vel” 的消息，于是三只乌龟就开始了愉快的，你追我的小游戏。乌龟的世界太疯狂，而且追上了还在上面扭来扭去的，咳咳咳…

对了，记得也要调用服务生成乌龟3，原来代码里只生成了乌龟2。

补充一些关于广播tf和接收tf的内容

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // tf广播器
    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));
}

对于广播tf的话，回调函数传入的是turtle1/pose，会发现只调用了一次这个回调函数，也即为了初始化。

    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

首先setOrigin为turtle1当前的x,y坐标，也即把自己当前的坐标认为自己在全局坐标系下的坐标，以此来确定父坐标系的原点。同理，设定父坐标系的方向，自己当前的方向认为是自己相对于父坐标系的方向。然后发布出去。

对于接收者，只需要

listener.waitForTransform("/turtle3", "/turtle2", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle3", "/turtle2", ros::Time(0), transform3);

就可以得到转换以后的关系啦～

突然奇想，如果两个乌龟定义的世界坐标系不一样会怎么样呢，于是改了改试了一下～

transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

所有的乌龟setOrigin都是这么设置的，那么如果第二个乌龟的世界坐标系改一下呢，于是改成了

transform.setOrigin( tf::Vector3(msg->x + 1, msg->y + 1, 0.0) );

即第二个乌龟的世界原点是（1,1），不是（0,0），也即第二个乌龟应该都会滞后第一个乌龟（1,1），第一个乌龟在（5,5）的话，在第二个乌龟看来，就是在（4,4），跑一下看看效果～

果然是这样子的～

然后官网上有一个添加一个carrot框架到现有的坐标系下的例子，代码是酱紫的～

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

跟broadcaster很像是不是，这里的话，尤其要注意一下

    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

相当于设置turtle1这个动坐标系认为是父坐标系，然后以父坐标系向右偏移两个单位为carrot坐标系的原点，然后carrot坐标系就要以这个动坐标系为基准。所以listen里面把

  listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);

此时小乌龟实际上就会以一个2的偏移开始运动～

还有个小疑问，关于ros::Time::now() 和 ros::Time(0)，晚点把时间这块琢磨一下，一起补充。