简单的tf广播者和监听者
在机器人系统中，我们经常会遇到坐标变换相关的问题。比如说，在一个导航系统中，我们通过激光雷达、双目摄像头等传感器对周围环境进行感知，但是这些传感器直接采集的数据是相对于传感器而言的， 而传感器在机器人身上的安装位置又各不相同，我们关心的则是外部环境与机器人本体之间的关系，所以还需要把相对于传感器的数据变换成为相对于机器人的数据。这个过程就是所谓的坐标变换。
如果涉及到多个机器人之间的协同工作，则需要维护两两之间的相对位置关系，在机器人很多的情况下，或者说是自由度很多的情况下，这种坐标系之间的变换关系就相对比较复杂。 ROS系统提供了一个叫做tf的坐标变换库，这个库使得我们能够在各种坐标系之间自由地切换。
本文以官方的例子介绍tf的功能和使用方法。
1. 两只乌龟慢慢爬
在这个例子中，我们将看到在一个窗口中有两只乌龟。如果我们按动键盘的方向键，可以控制其中的一直乌龟在窗口中摩擦。同时，我们将看到另一只乌龟总是以最近的路线跟踪这只受控的乌龟。
我们通过运行如下的launch文件开启例程：
$ roslaunch turtle_tf turtle_tf_demo.launch
我们就可以看到如下左图所示的两只乌龟了(乌龟的样子和颜色是随机的，如果看到不一样的乌龟也没有关系)，其中白色的乌龟将是我们要控制的对象，红色的乌龟则不断的靠近白色乌龟。 我们在运行launch的终端中按动方向键，就可以看到白色的乌龟在地上摩擦，留下带有棱角的步伐，而红色的乌龟则总是以最短的距离靠近它，留下平滑的步伐，如下右图所示。
 |
 |
红龟要想以最短的距离靠近白龟，就必须知道白龟相对自己的位置。为实现这一目标，可以建立三个坐标系：世界坐标系、白龟坐标系、红龟坐标系。我们在控制白龟的时候， 不断的更新白龟坐标系相对于世界坐标系的位置，同时告知红龟它已经移动了，那么红龟则根据自身的坐标系与世界坐标系之间的关系，把白龟相对于世界坐标系的位置转换为， 白龟相对于红龟的位置。
这一过程就是通过tf来实现的。一个tf系统由tf广播者(tf broadcaster)和tf监听者(tf listener)两个部分构成的。这里的白龟和红龟就是两个tf广播者，它们不断的向外发送自己相对于世界坐标系的位置。 另外有一个监听者负责维护两个乌龟的坐标系，通过tf的API系统计算白龟相对红龟的位置，并控制红龟跟踪白龟。下面是turtle_tf_demo.launch的文件内容：
<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<node name="turtle1_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
<param name="turtle" type="string" value="turtle1" />
</node>
<node name="turtle2_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
<param name="turtle" type="string" value="turtle2" />
</node>
<node name="turtle_pointer" pkg="turtle_tf" type="turtle_tf_listener.py" respawn="false" output="screen" ></node>
</launch>
在这个launtch文件中打开了五个node，其中第3,4行就是我们在ROS的Graph和Node中见过的用于显示乌龟的窗口和按键控制。
第9~14行，则是这里的两个乌龟的tf广播者。第15行是tf监听者。
我们可以通过工具rqt_tf_tree来查看系统中的坐标关系：
$ rosrun rqt_tf_tree rqt_tf_tree
运行该指令之后，就可以看到如下图所示的树形结构。从中我们可以看到系统中存在三个坐标系'/world','/turtle1'和'/turtle2'。
其中'/turtle1'和'/turtle2'是'/world'的两个子节点，分别对应着两个广播器'/turtle1_tf_broadcaster' 和 '/turtle2_tf_broadcaster'。
 |
接下来，我们用C++分别实现坐标变换的广播器和监听器，从而复现这个官方例程。首先我们创建一个叫做'learning_tf'的包，并将之导入ROS的运行环境中:
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
$ cd ..
$ catkin_make
$ source ./devel/setup.bash
2. tf广播者
这里是一个tf广播器C++实现，它订阅了一只乌龟的pose主题， 在接收到该主题下的消息后，通过回调函数poseCallback()来广播乌龟在世界坐标系下的变换。
首先我们需要导入一些头文件。其中"ros/ros.h"中有我们访问master的接口， "turtlesim/Pose.h"描述了将要订阅Pose主题的消息类型(参考简单的Topic发布者和订阅者)。 我们要广播坐标变换信息就必须导入"tf/transform_broadcaster.h"，它提供了tf广播器的各种API。
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
下面是广播器的main函数，在运行该程序的时候，我们必须通过名字指定一只乌龟。在第7行我们具现了一个订阅者，它订阅指定乌龟的pose主题，并注册回调函数posCallback()。 最后通过ros::spin()保持进程一直运行。
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
turtle_name = argv[1];
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
}
在回调函数中，我们具现了一个tf的广播器对象br。回调函数的第3~12行根据接收到的消息内容描述坐标变换，将之保存到tf::Transform对象transform中。 在函数的最后通过br的成员函数sendTransform广播坐标变换，它的第一个参数是坐标变换内容，第二个参数是一个时间戳， "world"和turtle_name则表示本次坐标变换是turtle_name对应的坐标系相对于世界坐标系的变换。
void poseCallback(const turtlesim::PoseConstPtr& msg){
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));
}
至于如何编译运行该程序，参考简单的Topic发布者和订阅者和 简单的Service服务器和客户端两篇文章。
3. tf监听者
这里是一个tf监听器的C++实现。代码量比广播者要多了很多，但其中与tf监听相关的内容只有三个部分。
首先我们包含头文件"tf/transform_listener.h"，它提供了tf监听器的各种API。
#include <tf/transform_broadcaster.h>
然后，在程序中具现化监听器对象。只要创建了这个对象，它就开始监听系统中的各个坐标变换，并将之缓存10秒。
tf::TransformListener listener;
最后，我们就可以通过监听器对象的成员函数lookupTransform()来查询关注的两个坐标系之间的变换关系，我们这里在try-catch语句块中查询关系。try-catch是C++语言的基本规则， 我们可以在try块中完成需要的功能，如果出现任何异常就通过catch块报告异常，进行错误处理。
try{
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
} catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
这里我们只关注"/turtle1"相对于"/turtle2"的坐标关系，对应的就是lookupTransform()中的前两个参数。第三个参数'ros::Time(0)'则指获取当前时刻的坐标关系。
最后一个参数tranform则记录了具体的变换内容，它是一个tf::StampedTransform对象。
在监听到坐标变换后，我们就可以根据变换内容做一些运算和控制。比如说，在我们的监听器例程的最后，我们根据两个坐标系之间的相对位置，重新计算了"/turtle2"的控制指令并通过主题的形式发布出去了：
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
4. 新建tf广播器和监听器的运行
为了运行我们新写的tf广播器和监听器，我们把官方例程中的launch文件抄过来，简单的修改一下，保存到文件turtle_tf_demo.launch中。
<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1"/>
<node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2"/>
<node name="turtle_tf_listener" pkg="learning_tf" type="turtle_tf_listener"/>
</launch>
然后，我们回到工作空间的根目录下，编译运行，就可以看到和官方例程类似的效果。
$ cd ~/catkin
$ catkin_make
$ source ./devel/setup.bash
$ roslaunch learning_tf turtle_tf_demo.launch
5. 总结
在本文中，我们实现了简单的tf广播器和监听器，用C++复现了官方例程turtle_tf的效果。
我们通过包含头文件"tf/transform_broadcaster.h"和"tf/transform_listener.h"来分别访问广播器和监听器的API。广播坐标变换时我们需要定义一个tf::TransformBroadcaster的对象， 调用其成员函数sendTransform()进行广播。监听坐标变换时，需要定义tf::TransformListener的对象，调用成员函数lookupTransform()来获取指定的两个坐标系之间的相对关系。