使用tf库：编写tf广播器(C++)

来源：互联网发布：java的历史编辑：程序博客网时间：2024/05/29 03:42

本文主要内容参考自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 <ros/ros.h>#include <tf/transform_broadcaster.h>#include <turtlesim/Pose.h>

要编写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>    <!-- 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 pkg="learning_tf" type="turtle_tf_broadcaster"          args="/turtle1" name="turtle1_tf_broadcaster" /></launch>