TF2 广播器（C++）

目标： 学习如何将机器人的状态广播到 tf2。

教程级别： 中级

预计时间： 15 分钟

背景

在接下来的两个教程中，我们将编写代码来重现 tf2 简介教程中的演示。后续教程将在此基础上引入更高级的 tf2 功能来扩展该演示，包括变换查询中的超时处理和时间旅行。

前提条件

本教程假设你已经具备 ROS 2 基础知识，并且完成了 tf2 简介教程和 tf2 静态广播器教程（C++）。我们将复用上一个教程中的 learning_tf2_cpp 功能包。

在之前的教程中，你已经学习了如何创建工作空间和功能包。

任务

1 编写广播器节点

首先创建源文件。进入上一个教程中创建的 learning_tf2_cpp 功能包，在 src 目录中输入以下命令，下载示例广播器代码：

Linux/macOS：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp

Windows：

在 Windows 命令行提示符中：

curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp -o turtle_tf2_broadcaster.cpp

或者在 PowerShell 中：

curl https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp -o turtle_tf2_broadcaster.cpp

使用你喜欢的文本编辑器打开该文件。

#include <functional>
#include <memory>
#include <sstream>
#include <string>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim_msgs/msg/pose.hpp"

class FramePublisher : public rclcpp::Node
{
public:
  FramePublisher()
  : Node("turtle_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    auto handle_turtle_pose = [this](const std::shared_ptr<const turtlesim_msgs::msg::Pose> msg){
        geometry_msgs::msg::TransformStamped t;

        // Read message content and assign it to
        // corresponding tf variables
        t.header.stamp = this->get_clock()->now();
        t.header.frame_id = "world";
        t.child_frame_id = turtlename_.c_str();

        // Turtle only exists in 2D, thus we get x and y translation
        // coordinates from the message and set the z coordinate to 0
        t.transform.translation.x = msg->x;
        t.transform.translation.y = msg->y;
        t.transform.translation.z = 0.0;

        // For the same reason, turtle can only rotate around one axis
        // and this why we set rotation in x and y to 0 and obtain
        // rotation in z axis from the message
        tf2::Quaternion q;
        q.setRPY(0, 0, msg->theta);
        t.transform.rotation.x = q.x();
        t.transform.rotation.y = q.y();
        t.transform.rotation.z = q.z();
        t.transform.rotation.w = q.w();

        // Send the transformation
        tf_broadcaster_->sendTransform(t);
    };
    subscription_ = this->create_subscription<turtlesim_msgs::msg::Pose>(
      topic_name, 10,
      handle_turtle_pose);
  }

private:
  rclcpp::Subscription<turtlesim_msgs::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtlename_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FramePublisher>());
  rclcpp::shutdown();
  return 0;
}

1.1 代码解析

下面来看看与将乌龟位姿发布到 tf2 相关的代码。

首先，定义并获取参数 turtlename，用于指定乌龟名称，例如 turtle1 或 turtle2。

turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

然后，节点订阅话题 turtleX/pose，每收到一条消息就调用 handle_turtle_pose 回调函数。

subscription_ = this->create_subscription<turtlesim_msgs::msg::Pose>(
  topic_name, 10,
  handle_turtle_pose);

接下来创建一个 TransformStamped 对象，并设置相应的元数据。

1. 首先为要发布的变换设置时间戳，通过调用 this->get_clock()->now() 获取当前时间，返回的是该节点使用的当前时间。

2. 然后设置所创建变换的父坐标系名称，这里是 world。

3. 最后设置子坐标系名称，即乌龟本身的名称。

乌龟位姿消息的回调函数负责广播该乌龟的平移和旋转，将其作为从 world 坐标系到 turtleX 坐标系的变换发布出去。

geometry_msgs::msg::TransformStamped t;

// Read message content and assign it to
// corresponding tf variables
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = turtlename_.c_str();

这里将乌龟的位姿信息填入 3D 变换结构中。

// Turtle only exists in 2D, thus we get x and y translation
// coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg->x;
t.transform.translation.y = msg->y;
t.transform.translation.z = 0.0;

// For the same reason, turtle can only rotate around one axis
// and this why we set rotation in x and y to 0 and obtain
// rotation in z axis from the message
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

最后，将构造好的变换传递给 TransformBroadcaster 的 sendTransform 方法，由它负责广播。

// Send the transformation
tf_broadcaster_->sendTransform(t);

1.2 CMakeLists.txt

返回上一级到 learning_tf2_cpp 目录，该目录下存放着 CMakeLists.txt 和 package.xml 文件。

打开 CMakeLists.txt，添加可执行文件并命名为 turtle_tf2_broadcaster，稍后你可以通过 ros2 run 来运行它。

find_package(turtlesim_msgs REQUIRED)
add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
target_link_libraries(
    turtle_tf2_broadcaster PUBLIC
    geometry_msgs::geometry_msgs
    rclcpp::rclcpp
    tf2::tf2
    tf2_ros::tf2_ros
    turtlesim_msgs::turtlesim_msgs
)

最后，添加 install(TARGETS…) 部分，以便 ros2 run 能找到你的可执行文件：

install(TARGETS
    turtle_tf2_broadcaster
    DESTINATION lib/${PROJECT_NAME})

2 编写 launch 文件

接下来为这个演示创建一个 launch 文件。在 src/learning_tf2_cpp 目录下创建一个 launch 文件夹。用文本编辑器在其中创建一个名为 turtle_tf2_demo_launch 的新文件，扩展名为 .py、.xml 或 .yaml，添加以下内容：

XML：

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <node pkg="turtlesim" exec="turtlesim_node" name="sim" />
  <node pkg="learning_tf2_cpp" exec="turtle_tf2_broadcaster" name="broadcaster1">
    <param name="turtlename" value="turtle1" />
  </node>
</launch>

YAML：

%YAML 1.2
---
launch:
  - node:
      pkg: "turtlesim"
      exec: "turtlesim_node"
      name: "sim"
  - node:
      pkg: "learning_tf2_cpp"
      exec: "turtle_tf2_broadcaster"
      name: "broadcaster1"
      param:
      - name: "turtlename"
        value: "turtle1"

Python：

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_cpp',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

2.1 代码解析

下面来分析 launch 文件的结构。每种格式都有各自的写法：

XML：

XML launch 文件以 XML 声明和根元素 <launch> 开头。

<?xml version="1.0" encoding="UTF-8"?>
<launch>

YAML：

YAML launch 文件以 YAML 版本声明和 launch: 键开头。

%YAML 1.2
---
launch:

Python：

在 Python launch 文件中，首先从 launch 和 launch_ros 包导入所需模块。需要注意的是，launch 是一个通用的启动框架（并非 ROS 2 专用），而 launch_ros 包含了 ROS 2 特有的功能，比如这里导入的节点。

from launch import LaunchDescription
from launch_ros.actions import Node

接下来启动 turtlesim 仿真节点，并使用 turtle_tf2_broadcaster 节点将 turtle1 的状态广播到 tf2。

XML：

  <node pkg="turtlesim" exec="turtlesim_node" name="sim" />
  <node pkg="learning_tf2_cpp" exec="turtle_tf2_broadcaster" name="broadcaster1">
    <param name="turtlename" value="turtle1" />
  </node>

YAML：

  - node:
      pkg: "turtlesim"
      exec: "turtlesim_node"
      name: "sim"
  - node:
      pkg: "learning_tf2_cpp"
      exec: "turtle_tf2_broadcaster"
      name: "broadcaster1"
      param:
      - name: "turtlename"
        value: "turtle1"

Python：

    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_cpp',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

2.2 添加依赖

返回上一级到 learning_tf2_cpp 目录，该目录下存放着 CMakeLists.txt 和 package.xml 文件。

用文本编辑器打开 package.xml，添加与 launch 文件导入语句对应的以下依赖：

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

这样声明了运行时所需的 launch 和 launch_ros 依赖。

请确保保存文件。

2.3 CMakeLists.txt

重新打开 CMakeLists.txt，添加以下内容以便安装 launch/ 文件夹中的 launch 文件：

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME})

你可以在创建 launch 文件教程中了解更多相关信息。

3 构建

在工作空间根目录运行 rosdep 检查缺失的依赖。

Linux：

rosdep install -i --from-path src --rosdistro {DISTRO} -y

macOS：

rosdep 仅在 Linux 上可用，你需要自行安装 geometry_msgs 和 turtlesim 依赖。

Windows：

rosdep 仅在 Linux 上可用，你需要自行安装 geometry_msgs 和 turtlesim 依赖。

仍在工作空间根目录下构建功能包：

Linux/macOS：

colcon build --packages-select learning_tf2_cpp

Windows：

colcon build --merge-install --packages-select learning_tf2_cpp

打开一个新终端，导航到工作空间根目录，并 source 环境配置文件：

Linux/macOS：

. install/setup.bash

Windows：

在 Windows 命令行提示符中：

call install\setup.bat

或者在 PowerShell 中：

.\install\setup.ps1

4 运行

现在运行 launch 文件，它会启动 turtlesim 仿真节点和 turtle_tf2_broadcaster 节点：

XML：

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.xml

YAML：

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.yaml

Python：

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py

在第二个终端窗口中输入以下命令：

ros2 run turtlesim turtle_teleop_key

你会看到 turtlesim 仿真已启动，其中有一只可以用键盘控制的乌龟。

现在，用 tf2_echo 工具检查乌龟位姿是否确实被广播到了 tf2：

ros2 run tf2_ros tf2_echo world turtle1

这会显示第一只乌龟的位姿。用方向键驱动乌龟移动（确保当前活动窗口是 turtle_teleop_key 所在的终端，而不是仿真器窗口）。控制台输出类似如下：

At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]

如果你对 world 和 turtle2 之间的变换运行 tf2_echo，此时看不到任何输出，因为第二只乌龟尚不存在。不过当我们在下一个教程中添加第二只乌龟后，turtle2 的位姿就会被广播到 tf2。

总结

在本教程中，你学习了如何将机器人的位姿（即乌龟的位置和朝向）广播到 tf2，以及如何使用 tf2_echo 工具进行验证。要实际使用广播到 tf2 的变换，请继续学习下一篇关于创建 tf2 监听器的教程。