上次咱们学习了 TF 的基本概念和如何公布动态的 TF 坐标：

  • ROS 机器人技术 - TF 坐标零碎基本概念
  • ROS 机器人技术 - 动态 TF 坐标帧

这次来总结下如何公布一个自定义的 TF 坐标转换，并监听这个变换。

一、编写 TF 广播者

进入上次创立的 learning_tf2 包中：

roscd learning_tf2

在 src 下新建一个 turtle_tf2_broadcaster.cpp 文件，代码如下：

#include <ros/ros.h>// 存储要公布的坐标变换#include <geometry_msgs/TransformStamped.h>// 四元数#include <tf2/LinearMath/Quaternion.h>// 变换广播者#include <tf2_ros/transform_broadcaster.h>// 乌龟的位姿定义#include <turtlesim/Pose.h>std::string turtle_name;void poseCallback(const turtlesim::PoseConstPtr& msg) {    // 创立 tf 播送对象    static tf2_ros::TransformBroadcaster br;    // 存储要公布的坐标变换音讯    geometry_msgs::TransformStamped transformStamped;    // 变换的工夫戳    transformStamped.header.stamp = ros::Time::now();        // 父坐标系名称    transformStamped.header.frame_id = "world";        // 以后要公布的坐标系名称 - 乌龟的名字    transformStamped.child_frame_id = turtle_name;    // 乌龟在二维立体静止，所以 z 坐标高度为 0    transformStamped.transform.translation.x = msg->x;    transformStamped.transform.translation.y = msg->y;    transformStamped.transform.translation.z = 0.0;    // 用四元数存储乌龟的旋转角    tf2::Quaternion q;    // 因为乌龟在二维立体静止，只能绕 z 轴旋转，所以 x，y 轴的旋转量为 0    q.setRPY(0, 0, msg->theta);    // 把四元数拷贝到要公布的坐标变换中    transformStamped.transform.rotation.x = q.x();    transformStamped.transform.rotation.y = q.y();    transformStamped.transform.rotation.z = q.z();    transformStamped.transform.rotation.w = q.w();        // 用 tf 广播者把订阅的乌龟位姿公布到 tf 中    br.sendTransform(transformStamped);}int main(int argc, char** argv){    // 以后节点的名称    ros::init(argc, argv, "my_tf2_broadcaster");    ros::NodeHandle private_node("~");    // 判断以后要播送的乌龟节点名字    if (!private_node.hasParam("turtle")) {        // launch 文件和命令行都没有传递乌龟名称，就间接退出        if (argc != 2) {            ROS_ERROR("need turtle name as argument");            return -1;        };        // launch 文件中如果没有定义乌龟名称，就在命令行中加上        turtle_name = argv[1];    } else {        // 从 launch 文件获取乌龟名称参数        private_node.getParam("turtle", turtle_name);    }    ros::NodeHandle node;    // 订阅一个节点的 pose msg，在回调函数中播送订阅的位姿音讯到 tf2 坐标零碎中    // turtle_name 为 turtle1 时播送 turtle1 的位姿到 tf 中    // turtle_name 为 turtle2 时播送 turtle2 的位姿到 tf 中    ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback);    ros::spin();    return 0;};

这个程序的意思是订阅输出乌龟的 pose 话题，而后在 poseCallback 回调函数中公布 world 到乌龟的 TF 变换，留神这个程序能够接管不同乌龟的 pose 音讯，只有运行时指定乌龟的名称 turtle_name 即可，代码正文很具体，其余的就不说了，而后增加编译规定：

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

间接编译：

catkin_make

基本上不会出问题，为了不便启动咱们在 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"/>    <!-- 第一个乌龟的 tf 广播者节点，参数为乌龟 1 的名字 /tutle1 -->    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" />        <!-- 第二个乌龟的 tf 广播者节点，还是用雷同的节点，只不过扭转了传递的参数为乌龟 2 的名字 /turtle2 -->     <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" />  </launch>

而后就能够间接启动了：

roslaunch learning_tf2 start_demo.launch

为了确定是否胜利播送了变换，应用上面的命令查看一个变换的输入：

rosrun tf tf_echo /world /turtle1

如果在控制台输入相似上面的音讯，则阐明变换公布胜利：

上面咱们来编写一个 TF 接收者来应用咱们下面公布的变换。

二、编写 TF 接收者

同样在 src 目录下创立 turtle_tf2_listener.cpp，代码如下：

#include <ros/ros.h>// 承受 tf 变换#include <tf2_ros/transform_listener.h>// 转换音讯 #include <geometry_msgs/TransformStamped.h>// 公布到乌龟 2 的静止音讯：角速度和线速度#include <geometry_msgs/Twist.h>// 再生服务#include <turtlesim/Spawn.h>// 实现乌龟 2 追随乌龟 1 静止int main(int argc, char** argv){    // 以后节点的名字    ros::init(argc, argv, "my_tf2_listener");    ros::NodeHandle node;    ros::service::waitForService("spawn");    ros::ServiceClient spawner = node.serviceClient<turtlesim::Spawn>("spawn");        turtlesim::Spawn turtle;        turtle.request.x = 4;    turtle.request.y = 2;    turtle.request.theta = 0;    turtle.request.name = "turtle2";    spawner.call(turtle);    // 角速度和线速度音讯发布者，用来公布计算后的新的速度音讯    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);    // tf 变换缓存区，最多缓存 10 秒    tf2_ros::Buffer tfBuffer;    // 创立监听 tf 变换对象，创立结束即开始监听，通常定义为成员变量    tf2_ros::TransformListener tfListener(tfBuffer);        ros::Rate rate(10.0);    while (node.ok()) {        // 用来保留寻找的坐标变换        geometry_msgs::TransformStamped transformStamped;        try{              // 寻找坐标变换                transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));        }        catch (tf2::TransformException &ex) {            ROS_WARN("%s",ex.what());            ros::Duration(1.0).sleep();            continue;        }        // 用来保留角速度和线速度        geometry_msgs::Twist vel_msg;        // 新的角速度为寻找到的变换角速度的 4 倍 - 使得第二个乌龟的静止轨迹转弯更快，且轨迹是弧线        vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);                // 新的线速度是寻找到的变换线速度的 0.5 倍 - 使得第二个乌龟的静止速度为第一个乌龟的一半        vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));                // 公布新的速度音讯，乌龟 2 节点的外部订阅了这个音讯，所以乌龟 2 会收到新的角速度和线速度，以此产生跟随运动        turtle_vel.publish(vel_msg);              rate.sleep();    }    return 0;};

这里要害的代码如下：

// 保留寻找的变换geometry_msgs::TransformStamped transformStamped;// 寻找 turtle1 到 turtle2 的坐标变换// target_frame: turtle2 // source_frame: turtle1// ros::Time(0): 获取变换的工夫，transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

同样增加编译规定：

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

而后编译：

catkin_make

在下面广播者的 launch 文件中加上接收者的启动：

<!--   这个例子一共创立了 5 个节点：    1. 乌龟节点，蕴含 2 个小乌龟    2. 管制乌龟静止的键盘节点    3. 第一个乌龟的 tf 广播者节点    4. 第二个乌龟的 tf 广播者节点    5. tf 坐标零碎的监听节点，用来监听 2 个乌龟之间的坐标变换--><launch>     <!-- 乌龟节点，这个节点的外部应该是创立了 2 个乌龟...... -->    <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"/>    <!-- 第一个乌龟的 tf 广播者节点，参数为乌龟 1 的名字 /tutle1 -->    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" />        <!-- 第二个乌龟的 tf 广播者节点，还是用雷同的节点，只不过扭转了传递的参数为乌龟 2 的名字 /turtle2 -->     <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" />    <!-- 启动 tf 坐标系同的监听节点 -->    <node pkg="learning_tf2" type="turtle_tf2_listener" name="listener" />  </launch>

而后启动：

roslaunch learning_tf2 start_demo.launch

运行时会呈现 2 个小乌龟，把窗口焦点放到终端，按上下左右键会发现第二个乌龟追随第一个乌龟静止：

然而刚启动时终端会报个谬误：

[ERROR] [1418082761.220546623]: "turtle2" passed to lookupTransform argument target_frame does not exist.[ERROR] [1418082761.320422000]: "turtle2" passed to lookupTransform argument target_frame does not exist.

这是因为咱们在 turtle2 还没有产生之前就寻找变换，导致没有找到它，为了解决这个问题能够在寻找变换前期待变换可用：

// 第四个参数是阻塞期待的超时工夫listener.waitForTransform("/turtle2", "/turtle1", ros::Time::now(), ros::Duration(3.0));transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

加上这句运行时就不会报错了，明天就写到这里，下次见：）