上次我们学习了 TF 的基本概念和如何发布静态的 TF 坐标：
这次来总结下如何发布一个自定义的 TF 坐标转换，并监听这个变换。
一、编写 TF 广播者
进入上次创建的 learning_tf2 包中：
roscd learning_tf2
在 src 下新建一个 turtle_tf2_broadcaster.cpp 文件，代码如下：
1#include <ros/ros.h> 2 3// 存储要发布的坐标变换 4#include <geometry_msgs/TransformStamped.h> 5 6// 四元数 7#include <tf2/LinearMath/Quaternion.h> 8 9// 变换广播者 10#include <tf2_ros/transform_broadcaster.h> 11 12// 乌龟的位姿定义 13#include <turtlesim/Pose.h> 14 15std::string turtle_name; 16 17void poseCallback(const turtlesim::PoseConstPtr& msg) 18{ 19 // 创建 tf 广播对象 20 static tf2_ros::TransformBroadcaster br; 21 22 // 存储要发布的坐标变换消息 23 geometry_msgs::TransformStamped transformStamped; 24 25 // 变换的时间戳 26 transformStamped.header.stamp = ros::Time::now(); 27 28 // 父坐标系名称 29 transformStamped.header.frame_id = "world"; 30 31 // 当前要发布的坐标系名称 - 乌龟的名字 32 transformStamped.child_frame_id = turtle_name; 33 34 // 乌龟在二维平面运动，所以 z 坐标高度为 0 35 transformStamped.transform.translation.x = msg->x; 36 transformStamped.transform.translation.y = msg->y; 37 transformStamped.transform.translation.z = 0.0; 38 39 // 用四元数存储乌龟的旋转角 40 tf2::Quaternion q; 41 42 // 因为乌龟在二维平面运动，只能绕 z 轴旋转，所以 x，y 轴的旋转量为 0 43 q.setRPY(0, 0, msg->theta); 44 45 // 把四元数拷贝到要发布的坐标变换中 46 transformStamped.transform.rotation.x = q.x(); 47 transformStamped.transform.rotation.y = q.y(); 48 transformStamped.transform.rotation.z = q.z(); 49 transformStamped.transform.rotation.w = q.w(); 50 51 // 用 tf 广播者把订阅的乌龟位姿发布到 tf 中 52 br.sendTransform(transformStamped); 53} 54 55int main(int argc, char** argv) 56{ 57 // 当前节点的名称 58 ros::init(argc, argv, "my_tf2_broadcaster"); 59 ros::NodeHandle private_node("~"); 60 61 // 判断当前要广播的乌龟节点名字 62 if (!private_node.hasParam("turtle")) { 63 // launch 文件和命令行都没有传递乌龟名称，就直接退出 64 if (argc != 2) { 65 ROS_ERROR("need turtle name as argument"); 66 return -1; 67 }; 68 69 // launch 文件中如果没有定义乌龟名称，就在命令行中加上 70 turtle_name = argv[1]; 71 } else { 72 // 从 launch 文件获取乌龟名称参数 73 private_node.getParam("turtle", turtle_name); 74 } 75 76 ros::NodeHandle node; 77 78 // 订阅一个节点的 pose msg，在回调函数中广播订阅的位姿消息到 tf2 坐标系统中 79 // turtle_name 为 turtle1 时广播 turtle1 的位姿到 tf 中 80 // turtle_name 为 turtle2 时广播 turtle2 的位姿到 tf 中 81 ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback); 82 83 ros::spin(); 84 return 0; 85};
这个程序的意思是订阅输入乌龟的 pose 话题，然后在 poseCallback 回调函数中发布 world 到乌龟的 TF 变换，注意这个程序可以接收不同乌龟的 pose 消息，只要运行时指定乌龟的名称 turtle_name 即可，代码注释很详细，其他的就不说了，然后添加编译规则：
1add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp) 2target_link_libraries(turtle_tf2_broadcaster ${catkin_LIBRARIES})
直接编译：
catkin_make
基本上不会出问题，为了方便启动我们在 launch 文件中启动广播者：
1<launch> 2 <!-- 乌龟节点 --> 3 <node pkg="turtlesim" type="turtlesim_node" name="sim"/> 4 5 <!-- 控制乌龟运动的键盘节点 --> 6 <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> 7 8 <!-- 线速度和角速度的定义，但是在这个例子中并没有用到哎... --> 9 <param name="scale_linear" value="2" type="double"/> 10 <param name="scale_angular" value="2" type="double"/> 11 12 <!-- 第一个乌龟的 tf 广播者节点，参数为乌龟 1 的名字 /tutle1 --> 13 <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" /> 14 15 <!-- 第二个乌龟的 tf 广播者节点，还是用相同的节点，只不过改变了传递的参数为乌龟 2 的名字 /turtle2 --> 16 <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" /> 17 18 </launch>
然后就可以直接启动了：
roslaunch learning_tf2 start_demo.launch
为了确定是否成功广播了变换，使用下面的命令查看一个变换的输出：
rosrun tf tf_echo /world /turtle1
如果在控制台输出类似下面的消息，则说明变换发布成功：
下面我们来编写一个 TF 接收者来使用我们上面发布的变换。
二、编写 TF 接收者
同样在 src 目录下创建 turtle_tf2_listener.cpp，代码如下：
1#include <ros/ros.h> 2 3// 接受 tf 变换 4#include <tf2_ros/transform_listener.h> 5 6// 转换消息 7#include <geometry_msgs/TransformStamped.h> 8 9// 发布到乌龟 2 的运动消息：角速度和线速度 10#include <geometry_msgs/Twist.h> 11 12// 再生服务 13#include <turtlesim/Spawn.h> 14 15// 实现乌龟 2 跟随乌龟 1 运动 16int main(int argc, char** argv) 17{ 18 // 当前节点的名字 19 ros::init(argc, argv, "my_tf2_listener"); 20 21 ros::NodeHandle node; 22 ros::service::waitForService("spawn"); 23 ros::ServiceClient spawner = node.serviceClient<turtlesim::Spawn>("spawn"); 24 25 turtlesim::Spawn turtle; 26 27 turtle.request.x = 4; 28 turtle.request.y = 2; 29 turtle.request.theta = 0; 30 turtle.request.name = "turtle2"; 31 spawner.call(turtle); 32 33 // 角速度和线速度消息发布者，用来发布计算后的新的速度消息 34 ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10); 35 36 // tf 变换缓存区，最多缓存 10 秒 37 tf2_ros::Buffer tfBuffer; 38 39 // 创建监听 tf 变换对象，创建完毕即开始监听，通常定义为成员变量 40 tf2_ros::TransformListener tfListener(tfBuffer); 41 42 ros::Rate rate(10.0); 43 while (node.ok()) { 44 // 用来保存寻找的坐标变换 45 geometry_msgs::TransformStamped transformStamped; 46 try{ 47 // 寻找坐标变换 48 transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0)); 49 } 50 catch (tf2::TransformException &ex) { 51 ROS_WARN("%s",ex.what()); 52 ros::Duration(1.0).sleep(); 53 continue; 54 } 55 56 // 用来保存角速度和线速度 57 geometry_msgs::Twist vel_msg; 58 59 // 新的角速度为寻找到的变换角速度的 4 倍 - 使得第二个乌龟的运动轨迹转弯更快，且轨迹是弧线 60 vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x); 61 62 // 新的线速度是寻找到的变换线速度的 0.5 倍 - 使得第二个乌龟的运动速度为第一个乌龟的一半 63 vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2)); 64 65 // 发布新的速度消息，乌龟 2 节点的内部订阅了这个消息，所以乌龟 2 会收到新的角速度和线速度，以此产生跟随运动 66 turtle_vel.publish(vel_msg); 67 68 rate.sleep(); 69 } 70 71 return 0; 72};
这里关键的代码如下：
1// 保存寻找的变换 2geometry_msgs::TransformStamped transformStamped; 3 4// 寻找 turtle1 到 turtle2 的坐标变换 5// target_frame: turtle2 6// source_frame: turtle1 7// ros::Time(0): 获取变换的时间， 8transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
同样添加编译规则：
1add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp) 2target_link_libraries(turtle_tf2_listener ${catkin_LIBRARIES})
然后编译：
catkin_make
在上面广播者的 launch 文件中加上接收者的启动：
1<!-- 2 这个例子一共创建了 5 个节点： 3 1. 乌龟节点，包含 2 个小乌龟 4 2. 控制乌龟运动的键盘节点 5 3. 第一个乌龟的 tf 广播者节点 6 4. 第二个乌龟的 tf 广播者节点 7 5. tf 坐标系统的监听节点，用来监听 2 个乌龟之间的坐标变换 8--> 9<launch> 10 <!-- 乌龟节点，这个节点的内部应该是创建了 2 个乌龟...... --> 11 <node pkg="turtlesim" type="turtlesim_node" name="sim"/> 12 13 <!-- 控制乌龟运动的键盘节点 --> 14 <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> 15 16 <!-- 线速度和角速度的定义，但是在这个例子中并没有用到哎... --> 17 <param name="scale_linear" value="2" type="double"/> 18 <param name="scale_angular" value="2" type="double"/> 19 20 <!-- 第一个乌龟的 tf 广播者节点，参数为乌龟 1 的名字 /tutle1 --> 21 <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" /> 22 23 <!-- 第二个乌龟的 tf 广播者节点，还是用相同的节点，只不过改变了传递的参数为乌龟 2 的名字 /turtle2 --> 24 <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" /> 25 26 <!-- 启动 tf 坐标系同的监听节点 --> 27 <node pkg="learning_tf2" type="turtle_tf2_listener" name="listener" /> 28 29 </launch>
然后启动：
roslaunch learning_tf2 start_demo.launch
运行时会出现 2 个小乌龟，把窗口焦点放到终端，按上下左右键会发现第二个乌龟跟随第一个乌龟运动：
但是刚启动时终端会报个错误：
1[ERROR] [1418082761.220546623]: "turtle2" passed to lookupTransform argument target_frame does not exist. 2[ERROR] [1418082761.320422000]: "turtle2" passed to lookupTransform argument target_frame does not exist.
这是因为我们在 turtle2 还没有产生之前就寻找变换，导致没有找到它，为了解决这个问题可以在寻找变换前等待变换可用：
1// 第四个参数是阻塞等待的超时时间 2listener.waitForTransform("/turtle2", "/turtle1", ros::Time::now(), ros::Duration(3.0)); 3 4transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
加上这句运行时就不会报错了，今天就写到这里，下次见：）
