ros2 从零开始27 编写广播C++

前言

背景

在接下来的两个教程中，我们将编写代码来复现"tf2入门教程"中的演示。之后，后续教程将专注于使用更高级的tf2功能扩展这个演示，包括在变换查找中使用超时和时间旅行功能。依赖于《编写静态广播C++》，我们现在编写一个监听某个小乌龟的位置信息，并且转换为world坐标系，广播出来。

实践

1.创建一个包

本次不另外创建包，我们使用《编写静态广播C++》。

2.编写广播C++

在learning_tf2_cpp/src目录下，创建源文件turtle_tf2_broadcaster.cpp , 其内容如下：

c++ 复制代码

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

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/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();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1));
  }

private:
  void handle_turtle_pose(const std::shared_ptr<const turtlesim::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);
  }

  rclcpp::Subscription<turtlesim::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;
}

2.1 代码解析

现在，让我们看一下与将海龟位姿发布到 tf2 相关的代码。首先，我们定义并获取一个参数 turtlename，它指定了海龟的名称，例如 turtle1 或 turtle2。

c++ 复制代码

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

随后，该节点会订阅主题 turtleX/pose，并对每条接收到的消息运行函数 handle_turtle_pose。

c++ 复制代码

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1));

现在，我们创建一个geometry_msgs::msg::TransformStamped t对象并为其设置适当的值。

我们需要为正在发布的变换添加时间戳，这里我们只需通过调用 this->get_clock()->now() 来使用当前时间进行标记
我们需要设置所创建链接的父坐标系名称，在本例中为 world
我们需要设置所创建链接的子节点名称，在本例中即为海龟本身的名称。
海龟位姿消息的处理函数会广播该海龟的平移和旋转信息，并将其作为从 world 坐标系到 turtleX 坐标系的变换进行发布。

c++ 复制代码

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();

在这里，我们将三维海龟位姿的信息复制到三维变换中。

c++ 复制代码

// Turtle 是2D的,因而只需要x和y的转换，z直接设置为0  
t.transform.translation.x = msg->x;
t.transform.translation.y = msg->y;
t.transform.translation.z = 0.0;

// 同样的，乌龟只能设置 z 偏航角速度（yaw）,所以x 滚转角速度（roll）和 y 俯仰角速度（pitch） 均设置为0 
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();

最后，我们用函数sendTransform()广播变换。

c++ 复制代码

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

2.3 更新CMakeLists.txt

在CMakeLists.txt添加配置可执行程序turtle_tf2_broadcaster, 如下：

cmake 复制代码

add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
    turtle_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2
    tf2_ros
    turtlesim
)

最后，添加安装配置install(TARGETS...)

cmake 复制代码

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

3 启动文件

现在为此演示创建一个启动文件。在src/learning_tf2_cpp 目录下创建一个 launch 文件夹。使用文本编辑器，在 launch 文件夹中创建一个名为 turtle_tf2_demo_launch 的新文件，扩展名为 .py、.xml 或 .yaml，随便选一个。并添加以下内容：

3.1.1 XML

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>

3.1.2 YAML

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"

3.1.3 PYTHON

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个node，一个是小乌龟turtlesim_node，另一个是广播小乌龟位置的turtle_tf2_broadcaster。第2个需要参数turtlename。

3.2 修改package.xml

回到包learning_tf2_cpp目录，打开package.xml,添加launch依赖

xml 复制代码

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

3.3 修改CMakeLists.txt

CMakeLists.txt增加目录launch/的安装

cmake 复制代码

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

4 编译和运行

进入工作区，用colcon build ，等待编译完成。

bash 复制代码

root@bc2bf85b2e4a:/# cd ~/ros2_ws
root@bc2bf85b2e4a:~/ros2_ws# colcon build --packages-select learning_tf2_cpp
Starting >>> learning_tf2_cpp
Finished <<< learning_tf2_cpp [5.52s]                     

Summary: 1 package finished [5.87s]

编译完成后，我们需要安装后才能运行，使用

bash 复制代码

source install/setup.sh

打开一个终端，输入如下命令运行(这里取turtle_tf2_demo_launch.xml,如果你用yaml或python，需要自己替换)

bash 复制代码

root@bc2bf85b2e4a:~/ros2_ws# source install/setup.sh 
root@bc2bf85b2e4a:~/ros2_ws# ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.xml
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-05-29-07-51-27-981371-bc2bf85b2e4a-3013
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [turtlesim_node-1]: process started with pid [3024]
[INFO] [turtle_tf2_broadcaster-2]: process started with pid [3026]
[turtlesim_node-1] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
[turtlesim_node-1] [INFO] [1780041088.307532624] [sim]: Starting turtlesim with node name /sim
[turtlesim_node-1] [INFO] [1780041088.312853930] [sim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]

这将会运行小乌龟节点和广播小乌龟位置的节点。

另外打开一个终端，运行小乌龟控制节点，控制小乌龟的移动。

bash 复制代码

root@bc2bf85b2e4a:~/ros2_ws# ros2 run turtlesim turtle_teleop_key

最后，我们用tf2_echo tool来检查一下，小乌龟位置是不是真的被广播到tf2中，命令如下：

bash 复制代码

ros2 run tf2_ros tf2_echo world turtle1

可以看到，小乌龟实时位置，在tf2中，也会收到其转换后的实时位置，如果你移动小乌龟（用执行turtle_teleop_key终端），tf2中实时位置值也会随着移动而实时变化

复制代码

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]

如果你运行 tf2_echo 来查看 world 和 turtle2 之间的坐标变换，你应该看不到任何变换，因为第二只乌龟还不存在。不过，一旦我们在下一个教程中添加了第二只乌龟，turtle2 的位姿就会被广播到 tf2 中。

总结

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