机器人操作系统ROS从入门到放弃(九)：使用tf追踪不同坐标系

一个机械臂，每一个关节都需要一个坐标系，一个无人小车不同的传感器一般都需要自己的坐标系。必不可少的，他们都需要世界坐标系。 有些坐标系是移动的，比如在一个车子上的传感器的自己的坐标系，随着车子的移动而移动；有的坐标系是静止的，比如世界坐标系。那些固定在机器人身上的传感器，他们之间的位置是固定的，有了一者的位置，我们应该就能知道另一者的位置。 追踪不同的坐标系，看起来是个很简单的活，但是实现起来挺复杂的，各个坐标系之间需要良好的及时的通讯，而ROS最牛叉的就是用来通讯嘛。 在这一讲，我们会使用ROS的tf包，去解决不同坐标系追踪的问题。我们首先使用官网的第一个例子入门，然后再用我自己编写的例子更深入的了解。我自己的例子要实现的功能是：假设有一个在移动的机器人，它上面有两个能定位的传感器(比如GPS和相机，相机定位通过SLAM实现)。相机和GPS都有自己的坐标系，这两坐标系要一直附着在我们的机器人上移动即位置相对机器人固定，但是相对世界坐标系移动。结果会在rviz上显示出来。你们将会看到如下内容:

moving_robot.gif

  从动图中我们可以看到在机器人身上有两个坐标系，一个叫camera一个叫gps，他们的位置相对固定，然后整个机器人相对世界坐标系移动。(说好的机器人怎么是个绿方块啊喂!!恩…你就假装它是个机器人好了…)。下面我们就来看一下怎么实现上面描述的功能。 之前我们讲坐标系变换是通过tf这个package实现的，不过我才发现tf的更新版tf2都出来不少时间了，他们之间大同小异。我们就使用最新的tf2吧。由于上一讲我们通过  

catkin_create_pkg learn_rviz_tf roscpp rospy std_msgs geometry_msgs visualization_msgs tf

  创建了learn_rviz_tf这个package，但是现在我们想添加新的依赖项tf2进去，需要修改CMakeLists.txt和package.xml，具体如果和修改我们在机器人操作系统ROS:从入门到放弃(三)　发布接收不同消息2 里讲过，这里不赘述了。你如果从我的github上直接下载这个package当然就不用修改什么了，我已经改好了。

tf2基础例子

ROS官网例子链接如下 http://wiki.ros.org/tf2/Tutorials 我们使用第二个 Writing a tf2 broadcaster (C++)来讲解。 进入链接后我们直接从2.1，the code部分开始，在我们的learn_rviz_tf/src中创建turtle_tf2_broadcaster.cpp并把代码复制进去。这儿为了方便讲解我也复制过来了  

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf2_ros::TransformBroadcaster br;
  geometry_msgs::TransformStamped transformStamped;
  
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  transformStamped.child_frame_id = turtle_name;
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  br.sendTransform(transformStamped);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");

  ros::NodeHandle private_node("~");
  if (! private_node.hasParam("turtle"))
  {
    if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
    turtle_name = argv[1];
  }
  else
  {
    private_node.getParam("turtle", turtle_name);
  }
    
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};

  官网的代码解读不甚详细，我赘述一遍并加点东西。 首先坐标系之间转换的发布不再是定义一个publisher然后用publisher.publish()函数来发布了，而是在tf2_ros::TransformBroadcaster这个类下定义一个专门的发布坐标转换的对象，使用对象.sendTransform(msg_type)来发布坐标之间的关系。发布的msg_type是特定的消息类型geometry_msgs/TransformStamped.h。基于这一点我们来看代码。 1: 头文件部分包含了#include <tf2/LinearMath/Quaternion.h>，代码有一行定义了tf::quaternion的对象tf::Quaternion q需要通过包含这个头文件来实现。 回想我们在使用geometry_msgs/PoseStamped时曾需要定义消息的pose，而pose包含position和orientation，其中orientation的消息类型是geometry/Quaternion，也就是说我们也可以通过geometry::Quaternion q定义一个四元数对象q，这也是四元数。这两种四元数的定义的对象是有很大的区别的。后者只能通过q.w, q.x, q.y, q.z调用四元数包含的数据。而前者拥有更广泛的作用。具体可以参考 tf: tf::Quaternion Class Reference 点进去, public member function显示了tf::Quaternion对象可以调用的一切函数。其中就有  

  我们上面的代码中使用了这个函数，q.setRPY(0, 0, msg->theta)，这个函数完成了从roll pitch yaw到四元数的转换功能，通过设定roll pitch yaw来设定四元数的具体数据。tf::Quaternion定义的对象q获取四元数的具体方法是q.x(),q.y()…有别于geometry::Quaternion定义的q通过q.x,q.y…的获取方式。x(),y()..这几个函数我们并没有在上面的链接看到，原因是tf::Quaternion继承了QuadWord这个类的，在后者的定义中我们能看到能通过x()，y()这类型的函数来获取quaternion的具体数据。这需要读者去仔细寻找资料了，在多使用ROS的过程中你会获取到更多的经验。 另外很重要的是，在链接中我们可以看到下面的内容  

这意味着，利用tf定义的四元数是重载了计算符号的，即你如果有如下定义

tf::Quaternion q_AB //A坐标到B坐标的的旋转变换
tf::Quaternion q_BC //B坐标到C坐标的旋转变换

q_AB * q_BC

ros::Publisher pub_abc = .... //定义publisher
std_msgs::Float64 abc; //定义要发布的消息类型
abc.data = .....; //给要发布的消息赋值 
pub_abc.publish(abc);//发布消息

tf2_ros::TransformBroadcaster br; //定义坐标转换的publisher
geometry_msgs::TransformStamped transformStamped; //定义坐标转换要发布的消息类型(只能是此类型)
...//赋值，赋值部分是最上面代码中位于poseCallback()函数中transformedStamed.header.stamp = ...以及后面的是来行的内容，稍后再讲
br.sendTransform(transformStamped);//发布坐标转换

  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
...
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);

rviz很清晰地显示了两个坐标系之间的联系。在你跑roslaunch的terminal中移动小乌龟，你会看到turtle1这个坐标系也在移动。 同样类似于你可以用rqt_graph命令来观察publisher和subscriber的关系，你可以在terminal中用下面的命令观察坐标系之间的关系

rosrun rqt_tf_tree rqt_tf_tree

  输入该命令后会出现类似于下面的图像

这表示world是母坐标系,turtle1是子坐标系。

tf2多个坐标系追踪

讲完了基础例子，我们就可以来实现我第一张动图moving_robot.gif。在那个动图中，我们有三个坐标系，camera和gps相对静止，和我们的机器人(方块marker)一块儿相对于世界坐标系world移动。官网的例子很不错，不过turtlesim之类的东西内部是咋样的我们毕竟不是那么了解，还有局部nodeHandle，几个例子里还使用了service之类，虽然都不是复杂东西，但和在一起总会有人不清楚其中的部分。我还是倾向于大家能懂tutorial的例子的每一行代码，代码的内容我们在之前都有所接触。 我们在learn_rviz_tf/src中创建一个叫moving_coordinate_system.cpp的文件。写入下面内容

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <tf2/LinearMath/Quaternion.h>

class MovingObject{
public:
    MovingObject(ros::NodeHandle& nh);
    void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void set_marker_fixed_property();
private:
    ros::Publisher pub_object_;
    visualization_msgs::Marker mk_;
    tf2_ros::TransformBroadcaster br_;
};

int main(int argc, char** argv){
    ros::init(argc, argv, "tf2_broadcaster");

    ros::NodeHandle nh;

    MovingObject mo(nh);

    ros::Subscriber sub_gps = nh.subscribe("/chatter", 10, &MovingObject::PoseCallback, &mo);
  
    ros::spin();
}

MovingObject::MovingObject(ros::NodeHandle& nh){
    pub_object_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
    set_marker_fixed_property();
}

void MovingObject::PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
    ROS_INFO("get gps position, %f, %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "world";
    transformStamped.child_frame_id = "gps";
    transformStamped.transform.translation.x = msg->pose.position.x - 0.1;
    transformStamped.transform.translation.y = msg->pose.position.y;
    transformStamped.transform.translation.z = msg->pose.position.z;
    transformStamped.transform.rotation.w = msg->pose.orientation.w;
    transformStamped.transform.rotation.x = msg->pose.orientation.x;
    transformStamped.transform.rotation.y = msg->pose.orientation.y;
    transformStamped.transform.rotation.z = msg->pose.orientation.z;
    br_.sendTransform(transformStamped);

    mk_.header.stamp = ros::Time();
    mk_.pose = msg->pose;
    pub_object_.publish(mk_);
}

void MovingObject::set_marker_fixed_property(){
    mk_.ns = "my_namespace";
    mk_.id = 0;
    mk_.header.frame_id = "world";
    mk_.type = visualization_msgs::Marker::CUBE;

    mk_.scale.x = 0.5;
    mk_.scale.y = 0.5;
    mk_.scale.z = 0.5;

    mk_.color.a = 0.3; 
    mk_.color.r = 0.0;
    mk_.color.g = 1.0;
    mk_.color.b = 0.0;

    mk_.action = visualization_msgs::Marker::ADD;
}

  头文件除了第一个基础例子的以外，增加了

#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>

  我们依次讲解程序内容 1:我们在上一章可视化marker的时候写了一个subscriber文件，用来接收rosbag发出的机器人pose的信息，并转换为marker的pose使之能在rviz中显示出来。而我们现在的例子同样显示marker，marker的pose后面可以看到同样来自于rosbag发出的消息。所以包含了这两个上一章节使用过的头文件，一个用来使用PoseStamped消息，一个用来使用Marker消息。并不意外。 2:定义了MovingObject类。 成员函数： 类的构造函数传入了nodehandle，用来初始化一些性质并定义publisher的内容。 PoseCallback函数用来接收来自于rosbag的机器人位置的消息并转化为marker类型的消息发布出去。 set_marker_fixed_property()函数用来设置marker一些我们不想改变的性质，和上一章类似。 成员变量： 数据成员pub_object_用来发布marker的pose使rviz接收 mk_就是maker了 br_即用来发布坐标系之间的关系的。 3：主函数中sub_gps就是用来接收来自rosbag发布的poseStamped类型消息。消息在PoseCallback函数中处理。 4：主函数之后，是MovingObject类的构造函数，在构造函数中，我们首先给pub_object赋值，负责发布marker类型的消息，pub_object发布的内容会用来产生我们动图1中的小方块。构造函数调用了set_marker_fixed_property()函数，在上一讲我们用这个函数来设置一些我们不想要改变的marker性质。除了颜色和大小不想改变外，我们并不想像上次那样，每接收一个pose就产生一个新的marker，我们希望当前的marker接收到一个新的pose后就移动到那个位置，所以marker只能有一个。上一讲我们说过marker的id和namespace两个量共同定义一个marker，所以在set_marker_fixed_property中把这两个量设置为常亮就可以了。即

    mk_.ns = "my_namespace";
    mk_.id = 0;

  5：最后是PoseCallback函数。 我们要发布坐标系之间的tf，和第一个例子一样，需要定义是哪两个坐标系，即frame_id和child_frame_id，分别是world和gps。需要定义transform的平移和旋转。pose和transform本就是一个东西，都需要定义平移和旋转，他们在transformStamped和PoseStamped的中的名字不一样。所以我们挨个把pose的position赋值给transform的translation，把pose的orinetation赋值给transform的rotation就可以了。 代码中有一行

mk_.pose = msg->pose;

  我们直接把接收到的消息的pose直接赋值给了marker的pose。如果gps到world的transform也用接收到的消息的pose，那么marker的中心就会和gps坐标系的中心重合。所以代码中我把pose.position.x减了0.1，只是人为地把为了gps的坐标系原点和marker中心做点区别，不是非得加。 定义好了tranformStamped之后就可以用br_.sendTranform把它发布出去，定义好了marker的pose之后，由于它的其他性质我们已经在set_marker_fixed_property中定义，那么就可以用pub_object_把marker发布出去了。这两个东西同时发布出去，用的同一个pose(除了x方向有0.1的固定差别)，如果我们像第一个例子那样编译并跑程序，设置好rviz，跑rosbag文件发布pose，理论上就可以在rviz当中看到gps这个坐标系随着我们的机器人(marker一起移动了)。 等一下！还有一个坐标系呢？？动图里还有个camera坐标系，可是仔细看我们的代码里，没有任何关于camera的东西。原来当两个坐标系相对静止的时候我们，我们最好直接把他们之间的tf直接设置到launch文件里。 首先我们把源文件写到CMakelists里进行编译。

add_executable(moving_coordinate_system src/moving_coordinate_system.cpp)
target_link_libraries(moving_coordinate_system ${catkin_LIBRARIES})

  然后在launch文件夹里创建一个叫run_mcs.launch的launch文件。并把下面的内容写到launch文件中

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="0.3 0 0 0 0 0 1 gps camera" />
    <node type="moving_coordinate_system" pkg="learn_rviz_tf" name="moving_coordinate_system" output="screen">
    </node>
</launch>

  可以看到<node pkg = "tf2_ros"...>那一行，我们使用了名叫tf2_ros这个pakcage里名叫static_transform_publisher这个可执行文件，节点名我们命名为link1_broadcaster，args即我们要传入的transform了，前三个数字是transform的平移，后四个是四元数x,y,z,w(如果只有三个数字默认roll pitch yaw)。gps camera表示前面那组平移旋转就是这两个坐标系的关系。接着如果我们运行我们刚刚编译好的moving_coordinate_system，就可以看到一个新的坐标系出现在rviz里了。 我们先打开rviz

rosrun rviz rviz

  如果你保存了之前rviz的信息，现在你rviz可能直接能接收marker和tf的信息，但我们假设是空白的。现在由于我们的程序既要发布marker的消息，又要发布tf的消息，自然我们需要加两个接收器。如上一章一样，点击rviz中ADD按钮，选择Marker点击OK。再次点击ADD按钮，选择TF，点击OK。把Global Options下面的Fixed_Frame设置为world。这时候你的rviz界面应该是这样的(注意左边栏包含了TF和Marker)。  

下面我们运行roslaunch

roslaunch learn_tf_rviz run_mcs.launch

  这时候我们注意到rviz中有些变化，如下图  

可以看到，rviz左边的Global Statue有刚刚的黄色警告变成了红色错误,TF也有个地方变成黄色警告。点击拥有红色错误标识的Fixed Frame，它旁边写着Fixed Frame [world] does not exist。是world frame还没有定义。 这是因为我们的程序中world是定义在PoseCallback函数里的，也就是需要接收到消息后，world才有定义，如果没有接收到消息，那么world 就一直没有定义。所以我们只要让程序开始接收消息，错误就会消失。下面跑我们的rosbag。这里的rosbag本质上和我们在rosbag那一章创造的rosbag没有不同，使用的同样是第三讲那个程序，不过我把每两个pose之间的时间缩短了，这样我们的方块儿移动起来看起来就更流畅。新的rosbag的名字叫robot_pose.bag，你用我们之前的bag文件跑也是可以的，只是看起来没那么流畅。

rosbag play robot_pose.bag

  这行命令一执行，rviz里的错误会消失，你也就可以看到类似于我们本讲第一个动图里的东西了。

总结

关于静态坐标系之间的关系，当然也可以在程序中设置，不过直接写到launch文件中是优先推荐的。关于tf还有很多其他内容，在官网中有写。 tf2/Tutorials – ROS Wiki 我们没有讲到的，既然能写tf的发布程序，那么我们也能写tf的接收程序，这和publisher,subscriber一个道理。即官网里writing a tf listener那一讲。另外我们在设置transformStamped的时候总是设置了每一个tf对应的时间的，这意味着我们可以随时查看之前的tf，即learn about tf and time那一讲。我再讲一次就太冗余了，能看到这儿相信那些剩下的内容你也自己能看懂了。关于rviz和tf我们就说这么多，至少有个简单的认识，当你实际遇到更多的问题时，google一下。