ROS 机器人技术 - 添加一个 TF 帧

版权声明：本文为 DLonng 原创文章，可以随意转载，但必须在明确位置注明出处！

在实际的机器人中往往有很多个传感器，比如我们组目前用的小车上就有相机，雷达，IMU 等，为了能够在 TF 系统中找到传感器之间的转换，就需要把每个传感器的坐标系加到系统的 TF 树中，方法很简单，下面一起来学习下。

一、TF 树的注意事项

在实际使用和调试 TF 的时候一定要时刻记住：TF 树中的一个节点可以有多个子节点，但是只能有一个父节点，并且 TF 树中不能出现回环！

一个典型的 TF 树如下：

这个 TF 树中有 3 个坐标系：

world：世界坐标系
turtle1：乌龟 1 的坐标系，父节点是 world
turtle2：乌龟 2 的坐标系，父节点时 world

如果你再发布一个「xxx -> turtle1」的变换，那 turtle1 就有 2 个父节点，这样是不可行的，违反了 TF 树的构建规则，实际使用是一定要注意了，如果你想查看系统当前的 TF 树，使用下面的命令：

rosrun rqt_tf_tree rqt_tf_tree

下面来学习如何为一个节点添加子坐标系。

二、添加子坐标系

同样进入 learning_tf2 包中：

roscd learning_tf2

然后在 src 下新建 frame_tf2_broadcaster.cpp 文件，代码如下：

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_tf2_broadcaster");
    ros::NodeHandle node;

    tf2_ros::TransformBroadcaster tfb;
    geometry_msgs::TransformStamped transformStamped;
    // 指定 carrot1 的父节点时 turtle1 
    // 即添加一个新的 carrot1 子坐标系到 turtle1 
    transformStamped.header.frame_id = "turtle1";
    transformStamped.child_frame_id = "carrot1";

    // carrot1 相对于 tutle1 做了 y 轴的偏移
    transformStamped.transform.translation.x = 0.0;
    transformStamped.transform.translation.y = 2.0;
    transformStamped.transform.translation.z = 0.0;

    tf2::Quaternion q;
    q.setRPY(0, 0, 0);

    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

    ros::Rate rate(10.0);
    while (node.ok()) {
        transformStamped.header.stamp = ros::Time::now();

        // 这两行表示让该 carrot1 参考系随着时间移动，打开注释小乌龟 2 的位置会随着时间改变
        //transformStamped.transform.translation.x = 2.0 * sin(ros::Time::now().toSec());
        //transformStamped.transform.translation.y = 2.0 * cos(ros::Time::now().toSec());

        // 将 carrot1 相对于 tutle1 的坐标变换广播到 TF 系统中
        tfb.sendTransform(transformStamped);
        rate.sleep();
        printf("sending\n");
    }
};

代码中最关键的就是要正确指定发布转换的 ID：

frame_id：父坐标系名称
child_frame_id：子坐标系名称

然后添加编译规则：

add_executable(frame_tf2_broadcaster src/frame_tf2_broadcaster.cpp)
target_link_libraries(frame_tf2_broadcaster ${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" />

    <!-- 启动新添加的 carrot1 坐标系广播者节点 -->
    <node pkg="learning_tf2" type="frame_tf2_broadcaster" name="broadcaster_frame" />
</launch>

开始启动节点：

roslaunch learning_tf2 start_demo.launch

不过你应该发现小乌龟的跟随运动跟上一个实验一模一样，我们自己添加的坐标系没有产生作用，这是为何呢？这是因为虽然我们发布了新的变换，但是我们并没有使用它，来修改下 listener 的代码：

transformStamped = listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0));

把 /turtle1 坐标系改为 /carrot1，因为我们要使用新添加的坐标系，所以寻找变换的坐标系参数就要填写新添加的坐标系名称，这样系统才能正确找到新添加的变换，再次编译重新运行：

catkin_make

roslaunch learning_tf2 start_demo.launch

你应该会发现现在的小乌龟产生的跟随运动与之前不一定了，两者之间的 y 方向有一定的距离，这个距离就是我们发布变换时指定的坐标系的相对位置：

OK！TF 系统常用的基础就学完了，目前因为项目有用到 TF，所以写了几篇基础的文章，之前没有看过的可以再回过头看下：

本文原创首发于微信公号「登龙」，分享机器学习、算法编程、Python、机器人技术等原创文章，扫码即可关注！