本文主要内容参考自ROS wiki:Writing a tf broadcaster (C++)，在加入了自己的一些理解的同时，我也对原文进行了适当的修改。原文使用Creative Commons Attribution 3.0，本文使用知识共享署名-非商业性使用-相同方式共享 4.0 国际许可协议协议，若使用本文，请自觉遵守相关协议。

上一篇文章我们演示了一个有趣的例子，而且对tf中的转换信息做了简要的说明。之后的几篇文章中，我们将使用tf库一步一步地实现这个小例子。

首先，我们需要先大概了解一些这个小例子的基本原理。其实原理很简单，我们只需要根据两只乌龟的转换关系，来对另一只乌龟发送相应的速度指令，即可使他跟踪第一只乌龟了。其中比较关键的就是如何获取它们之间的转换关系。而这个问题对于ROS世界就很容易了，TF会为我们处理好一切。

我们所需要做的就是发布两只乌龟相对于世界(说白了就是原点坐标系，坐标为(0,0,0))的转换关系，这项工作其实很容易。下面我们就介绍如何发布相对于世界的转换关系。

创建ROS功能包

在开始之前，我们需要创建一个ROS功能包作为沙盒来运行我们的例子。

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/src
 $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim

上面命令中你需要将%YOUR_CATKIN_WORKSPACE_HOME%替换为你的工作空间名字。该功能包所依赖的库有tf、roscpp、rospy、turtlesim。如果不小心漏掉了几个库，后面的过程将会遇到问题。不过也没关系，因为我们可以手动添加功能包所依赖的库，详细信息可以参考ROS wiki：手动创建ROS package。

为了后续可以使用rosbash(如roscd)命令访问我们新建的功能包，我们需要对创建好的功能包进行编译：

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/
 $ catkin_make
 $ source ./devel/setup.bash

如果你对ROS中创建功能包的过程还不熟悉，可以参考官方wiki教程：创建ROS程序包和编译ROS程序包。

下面我们开始编写tf广播器节点，进入learning_tf的功能包目录下的src文件夹，然后创建文件turtle_tf_broadcaster.cpp。 下面我们来一步步地编写这个节点。

使用到的库

首先是必要的头文件：

#include 
#include 
#include 

要编写ros程序必须要使用ros/ros.h头文件，而我们这里的例子是基于tf库的，所以tf库文件也少不了——tf.h，这里创建的是广播器，所以用到的就是transform_broadcaster.h。除此之外，这个demo中使用了ROS中的turtlesim功能包(如果你曾经看过ROS的入门教程的话，你应该对它比较熟悉)，由于我们需要乌龟的位姿信息来计算它与世界坐标的转换关系，因此需要Pose消息格式的头文件。

获取位姿信息

如果你之前接触过实体机器人的话，应该会明白，这里的位姿信息相当于里程计信息。由于我们使用的是动画模拟的乌龟，没有里程计来为我们提供这些信息，好在乌龟模拟器可以发布这些消息，我们可以订阅这些消息以获取我们所需要的信息。因此，我们下面的代码其实是在写一个订阅器。

首先是声明turtle_name变量，它用来保存命令行参数(名字空间)，由于我们需要在多个函数中使用它，因此我们需要在前面声明一个全局变量。

std::string turtle_name;

然后，我们需要编写ROS节点的基本代码：

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初级教程:Writing a Simple Publisher and Subscriber。

你需要知道我们编写这个代码的目的是向tf广播小乌龟的tf转换(相对于世界左边系的转换)，因此首先我们需要获取它的位姿信息。当我们运行turtlesim中的乌龟节点(rosrun turtlesim turtlesim_node)后，它会向“turtle1/pose”主题发送位姿信息，所以我们可以从该主题获取位姿信息。

ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;

其中，subscrible函数的第一个参数是主题名字，由于我们想获取位姿信息，因此我们需要使用相应的位姿主题；第二个参数是队列大小；最后一个参数是回调函数，即接收到消息之后，便会立即调用该函数，下面我们会介绍。

发布tf转换

当我们接收到位姿信息之后，我们就可以发布转换了，这部分工作我们在回调函数中完成。首先，创建转换广播器br和转换transform:

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

然后根据获取到的位姿信息设置transform:

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

考虑到一些人对turtlesim功能包不熟悉，这里做一些额外的解释。这里我们从turtle_name/pose订阅到的是turtlesim功能包中的Pose消息。Pose消息的格式你可以用下面的命令查看：

rosmsg show turtlesim/Pose

结果如下：

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

你会看到位置信息只有x方向和y方向的，这很容易解释，因为乌龟只能活动在二维平面中：

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

方向信息只有theta，即绕着z轴的旋转角度，对应欧拉角rpy中的yaw:

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

最后，设置好transform后，我们就可以发布它了。

编译

打开learning_tf功能包下的CMakeList.txt文件，然后在其中添加一下代码：

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

然后，进入你的工作空间编译即可：

cd %YOUR_CATKIN_WORKSPACE_HOME%/
catkin_make

运行

由于我们这个例子中用到不止一个节点，所以启动这些节点最方便的方式是使用启动文件(.launch)。在你的learning_tf功能包下创建文件夹launch，并在其中添加文件start_demo.launch，内容如下：

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

    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <node pkg="learning_tf" type="turtle_tf_broadcaster"
 args="/turtle1" name="turtle1_tf_broadcaster" />

launch>

启动文件应该很容易看懂。首先启动一个乌龟节点，并命名为sim；然后启动乌龟的控制节点；接着添加两个系统参数scale_linear和scale_angular；最后启动tf广播器节点，并将该节点命名为turtle1_tf_broadcaster，这样的话我们会看到两个tf转换(除了世界坐标系)。

下面运行启动文件：

roslaunch learning_tf start_demo.launch

查看结果

前一篇文章中，我们已经介绍了查看tf转换的工具，这里我们会再次用到它们。首先是tf中的tf_echo工具：

$ rosrun tf tf_echo /world /turtle1

这时，你会看到一堆描述各个时刻转换的数据，我们在上一篇文章已经对它们进行了简单的解释，如果你希望详细了解它们，可以参考我的这篇文章关于TF转换信息(Transforms)的理解。

如果你想更直观地观察，你可以打开rviz：

rosrun rviz rviz

同样，按照上一篇文章的描述设置rviz，你就可以看到这样的效果：

你可以尝试将turtle1移动到world的位置，然后再查看tf_echo的结果。多尝试一些其他的操作，你就会慢慢理解tf转换信息。

这篇文章就写到这里，下一篇文章我们会继续完善这个例子。

参考资料

1. ROS wiki:Writing a tf broadcaster (C++)
2. ROS wiki:手动创建ROS package
3. ROS wiki:创建ROS程序包
4. ROS wiki:编译ROS程序包

本作品采用知识共享署名-非商业性使用-相同方式共享 4.0 国际许可协议进行许可。