使用tf库：编写tf监听器

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

上一个教程中，我们创建了一个tf广播器来发布乌龟的位姿信息，这篇文章我们来学习下如何创建tf监听器。该节点的主要任务是监听tf中turtle2到turtle1转换，然后将转换信息转化为速度信息发送给turtle2。

使用到的库

ros基础库(ros/ros.h)就不用提了，必须要添加进来；其次就是tf中的transform_listener.h，这里也不多做说明。由于我们需要将监听到的消息转换为速度消息，因此我们需要geometry_msgs/Twist消息的头文件；另外，我们需要在同一窗口中再生成一个乌龟turtle2，而turtlesim功能包中spawn服务提供了此项功能，因此我们需要spawn服务的头文件。

note:和消息类似，关于服务的创建过程也可以在ROS基础教程中找到：Creating a ROS msg and srv。另外，在ROS初级教程中，我们也曾使用过spawn服务，只不过是使用相应的命令和参数，具体可以参考：Understanding ROS Services and Parameters当然，如果你希望了解spawn(再生)服务的详细信息的话，你可以参考这里：Spawn.h。

调用spawn服务

ros中的基础工作（如初始化、创建节点句柄等），我们就不介绍了。下面介绍下如何调用spawn服务，建议如果有不懂的地方可以查看ros的基础教程：Writing a Simple Service and Client (C++)。

首先等待spawn服务，直到它启动起来；然后创建spawn服务的客户端，以及spawn服务。

ros::service::waitForService("spawn");ros::ServiceClient add_turtle =    node.serviceClient<turtlesim::Spawn>("spawn");turtlesim::Spawn srv;

然后使用客户端调用服务即可：

add_turtle.call(srv);

创建消息发布器

使用下面的命令即可创建消息发布器，这里我们设置消息的类型为geometry_msgs::Twist，发布主题为"turtle2/cmd_vel"，缓冲区的大小设置为10；

ros::Publisher turtle_vel =    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

监听tf

首先，我们需要创建一个监听器用于监听是否存在需要的tf：

tf::TransformListener listener;

然后，我们使用一个循环，在循环中，我们主要完成以下工作：

• 监听器查找特定的tf
• 根据查找的tf信息计算相应的速度信息
• 将速度信息发布给turtle2

// 发布频率为10Hzros::Rate rate(10.0);  while (node.ok()){    tf::StampedTransform transform;    try{      // 查找特定的tf      listener.lookupTransform("/turtle2", "/turtle1",                               ros::Time(0), transform);    }    catch (tf::TransformException &ex) {      ROS_ERROR("%s",ex.what());      ros::Duration(1.0).sleep();      continue;    }    // 根据tf计算速度信息    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));    // 发布速度信息给turtle2                                 turtle_vel.publish(vel_msg);    rate.sleep();  }

其中，lookupTransform函数，我们需要详细说明一下。前两个参数是指定从arg1到arg2的tf的，我们这里查找的是从turtle2到turtle1的转换；第三个参数是指定tf的时间的，我们这里使用的是ros::Time(0)，这样的话，我们可以获取到最近存在的tf；最后一个参数用来保存查找到的tf。

编译

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

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

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

cd %YOUR_CATKIN_WORKSPACE_HOME%/catkin_make

运行

我们仅仅需要在上一篇文章中的launch文件中添加两行代码即可：

  <launch>    ...    <node pkg="learning_tf" type="turtle_tf_broadcaster"          args="/turtle2" name="turtle2_tf_broadcaster" />    <node pkg="learning_tf" type="turtle_tf_listener"           name="listener" />  </launch>

这里我们利用了上一篇博客中的发布转换信息的节点——turtle_tf_broadcaster，用它我们可以将turtle2的转换也发布出来。只有这样，本文中的节点才能顺利找到turtle1到turtle2的转换。

然后启动launch文件即可：

roslaunch learning_tf start_demo.launch

查看结果

使用方向键控制第一个乌龟，你会发现后面的乌龟始终跟着前一个乌龟。

还是和上一篇文章类似，我们使用同样的工具来查看tf转换，首先是tf_echo：

rosrun tf tf_echo /turtle2 /turtle1

当然，为了力求直观，我们可以使用rviz来查看tf转换的信息，具体步骤请查看前面的文章。

到这里，我们已经基本实现了前面介绍的Demo，不过这个Demo还不完善，后面我们会继续完善这个Demo。

参考资料

1. ROS wiki:Writing a tf listener (C++)
2. ROS机器人程序设计，机械工业出版社

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