您正在阅读的是旧版本但仍受支持的 ROS 2 文档。 Jazzy.

编写广播程序（C++）

目标 了解如何向 tf2 广播机器人的状态。

辅导水平： 中级

时间 15 分钟

背景介绍

在接下来的两个教程中，我们将编写代码来重现来自 tf2 简介 教程。之后，下面的教程将重点使用更高级的 tf2 功能来扩展演示，包括在转换查找和时间旅行中使用超时。

先决条件

本教程假定您已经掌握了 ROS 2 的工作知识，并且已经完成了 tf2 教程简介 和 tf2 静态广播员教程（C++）.我们将重复使用 learning_tf2_cpp 软件包。

在前面的教程中，您学习了如何 创建工作区 和 创建软件包.

任务

1 写入广播员节点

首先创建源文件。转到 learning_tf2_cpp 软件包。在 来源 输入以下命令，下载广播员代码示例：

利纳克斯MacOS视窗

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp

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

#include 功能强大；
#include <内存>；
#include 流>；
#include <字符串>；

#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"；

类 框架发布者 : 公 rclcpp::节点
{
公:
  框架发布者()
  : 节点("turtle_tf2_frame_publisher"；)
  {
    // 声明并获取 `turtlename` 参数
    turtlename_ = 此->；declare_parameter<；标准::字符串>；("turtlename"；, 乌龟；);

    // 初始化转换广播器
    tf_broadcaster_ =
      标准::make_unique<；tf2_ros::改造播音员>；(*此);

    // 订阅海龟{1}{2}/姿势主题并调用 handle_turtle_pose
    // 每条信息的回调函数
    标准::ostringstream 溪流;
    溪流 <<； "/"； <<； turtlename_.c_str() <<； "/pose"；;
    标准::字符串 topic_name = 溪流.字符串();

    订阅_ = 此->；创建订阅<；海龟::信息::姿势>；(
      topic_name, 10,
      标准::约束(及样品；框架发布者::乌龟姿势, 此, 标准::占位符::_1));
  }

私人:
  空白 乌龟姿势(缢 标准::共享_ptr<；海龟::信息::姿势>； 信息)
  {
    几何参数::信息::TransformStamped t;

    // 读取信息内容并将其分配给
    // 相应的 tf 变量
    t.页眉.盖章 = 此->；获取时钟()->；现在();
    t.页眉.frame_id = "世界"；;
    t.子帧标识 = turtlename_.c_str();

    // 海龟只存在于二维空间，因此我们可以得到 x 和 y 的平移值
    // 从信息中获取坐标，并将 Z 坐标设为 0
    t.变.译文.x = 信息->；x;
    t.变.译文.y = 信息->；y;
    t.变.译文.z = 0.0;

    // 出于同样的原因，乌龟只能绕一条轴旋转
    // 因此，我们将 x 轴和 y 轴的旋转角度设为 0，从而得到
    // 从报文的 Z 轴开始旋转
    tf2::四元数 q;
    q.设置 RPY(0, 0, 信息->；θ);
    t.变.自转.x = q.x();
    t.变.自转.y = q.y();
    t.变.自转.z = q.z();
    t.变.自转.w = q.w();

    // 发送转换
    tf_broadcaster_->；发送变换(t);
  }

  rclcpp::订阅<；海龟::信息::姿势>:：SharedPtr 订阅_;
  标准::唯一参数<；tf2_ros::改造播音员>； tf_broadcaster_;
  标准::字符串 turtlename_;
};

int 主要(int 参数, 烧焦 * 参数[])
{
  rclcpp::启动(参数, 参数);
  rclcpp::后旋(标准::共享<；框架发布者>；());
  rclcpp::关闭();
  返回 0;
}

1.1 检查代码

现在，让我们看看将乌龟姿势发布到 tf2 的相关代码。首先，我们定义并获取一个参数 海龟名，其中指定了一个乌龟名称，例如 乌龟1 或 乌龟2.

turtlename_ = 此->；declare_parameter<；标准::字符串>；("turtlename"；, 乌龟；);

之后，节点会订阅主题 乌龟 X/姿势 并运行函数 乌龟姿势 对每一条收到的信息进行处理。

订阅_ = 此->；创建订阅<；海龟::信息::姿势>；(
  topic_name, 10,
  标准::约束(及样品；框架发布者::乌龟姿势, 此, _1));

现在，我们创建一个 TransformStamped 对象，并赋予其相应的元数据。

1. 我们需要给正在发布的变换加上一个时间戳，然后通过调用 this->get_clock()->now().这将返回 节点.

2. 然后，我们需要设置所创建链接的父框架名称，本例中为 世界.

3. 最后，我们需要设置所创建链接的子节点名称，在本例中就是乌龟本身的名称。

乌龟姿态信息的处理函数会广播乌龟的平移和旋转，并将其作为从帧 世界 框 乌龟X.

几何参数::信息::TransformStamped t;

// 读取信息内容并将其分配给
// 相应的 tf 变量
t.页眉.盖章 = 此->；获取时钟()->；现在();
t.页眉.frame_id = "世界"；;
t.子帧标识 = turtlename_.c_str();

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

// 海龟只存在于二维空间，因此我们可以得到 x 和 y 的平移值
// 从信息中获取坐标，并将 Z 坐标设为 0
t.变.译文.x = 信息->；x;
t.变.译文.y = 信息->；y;
t.变.译文.z = 0.0;

// 出于同样的原因，乌龟只能绕一条轴旋转
// 因此，我们将 x 轴和 y 轴的旋转角度设为 0，从而得到
// 从报文的 Z 轴开始旋转
tf2::四元数 q;
q.设置 RPY(0, 0, 信息->；θ);
t.变.自转.x = q.x();
t.变.自转.y = q.y();
t.变.自转.z = q.z();
t.变.自转.w = q.w();

最后，我们将构建的变换传递给 发送变换 的方法 改造播音员 它将负责广播。

// 发送转换
tf_broadcaster_->；发送变换(t);

1.2 CMakeLists.txt

导航一级回到 learning_tf2_cpp 目录中的 CMakeLists.txt 和 package.xml 文件的位置。

现在打开 CMakeLists.txt 添加可执行文件并命名 乌龟_TF2_播音员，稍后将与 玫瑰2 运行.

add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
    乌龟_TF2_播音员
    几何参数
    rclcpp
    tf2
    tf2_ros
    海龟
)

最后，添加 install(TARGETS...) 因此 玫瑰2 运行 能找到你的可执行文件：

install(TARGETS
    乌龟_TF2_播音员
    DESTINATION lib/${PROJECT_NAME})

2 编写启动文件

现在为该演示创建一个启动文件。创建一个 启动 文件夹中的 src/learning_tf2_cpp 目录。用文本编辑器新建一个名为 turtle_tf2_demo.launch.py 在 启动 文件夹，并添加以下几行：

从 启动 舶来品 启动说明
从 launch_ros.actions 舶来品 节点


捍卫 生成发射描述():
    返回 启动说明([
        节点(
            包装='turtlesim'；,
            可执行='turtlesim_node',
            名字='sim'；
        ),
        节点(
            包装='learning_tf2_cpp',
            可执行='turtle_tf2_broadcaster'；,
            名字='broadcaster1',
            参数=[
                {'turtlename'；: 'turtle1'；}
            ]
        ),
    ])

2.1 检查代码

首先，我们从 启动 和 发射 包。需要注意的是 启动 是一个通用的启动框架（非 ROS 2 专用），而 发射 具有 ROS 2 的特定功能，比如我们在这里导入的节点。

从 启动 舶来品 启动说明
从 launch_ros.actions 舶来品 节点

现在，我们运行节点，启动海龟模拟并广播 乌龟1 使用我们的 乌龟_TF2_播音员 节点。

节点(
    包装='turtlesim'；,
    可执行='turtlesim_node',
    名字='sim'；
),
节点(
    包装='learning_tf2_cpp',
    可执行='turtle_tf2_broadcaster'；,
    名字='broadcaster1',
    参数=[
        {'turtlename'；: 'turtle1'；}
    ]
),

2.2 添加依赖项

导航一级回到 learning_tf2_cpp 目录中的 CMakeLists.txt 和 package.xml 文件的位置。

开放 package.xml 文本编辑器。在启动文件的导入语句中添加以下依赖项：

执行依赖关系；启动</exec_depend>；
执行依赖关系；发射</exec_depend>；

这声明了额外需要的 启动 和 发射 执行其代码时的依赖关系。

确保保存文件。

2.3 CMakeLists.txt

重新开放 CMakeLists.txt 中的启动文件。 发射/ 文件夹将被安装。

安装（DIRECTORY 启动
  DESTINATION share/${PROJECT_NAME})

有关创建启动文件的更多信息，请参阅 本教程.

3 建

运行 rosdep 在工作区的根目录下，检查是否存在缺失的依赖项。

利纳克斯MacOS视窗

rosdep install -i --from-path src --rosdistro humble -y

还是在工作区的根目录下，构建你的软件包：

利纳克斯MacOS视窗

colcon build --packages-select learning_tf2_cpp

打开一个新终端，导航到工作区的根目录，然后获取设置文件：

利纳克斯MacOS视窗

install/setup.bash

4 运行

现在运行启动文件，该文件将启动 turtlesim 仿真节点并 乌龟_TF2_播音员 节点：

ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py

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

ros2 run turtlesim turtle_teleop_key

现在您将看到海龟模拟已经开始，您可以控制一只海龟。

现在，使用 tf2_echo 工具来检查乌龟姿势是否真的被广播到了 tf2：

ros2 run tf2_ros tf2_echo world turtle1

这会显示第一只乌龟的姿势。使用方向键绕着乌龟行驶（确保你的 乌龟遥控钥匙 终端窗口处于活动状态，而不是模拟器窗口）。在控制台输出中，您将看到类似下面的内容：

时间 1625137663.912474878
- 翻译：[5.276, 7.930, 0.000]
- 旋转：用四元数表示 [0.000, 0.000, 0.934, -0.357]
时间 1625137664.950813527
- 翻译：[3.750, 6.563, 0.000]
- 旋转：用四元数表示 [0.000, 0.000, 0.934, -0.357]
时间 1625137665.906280726
- 翻译：[2.320, 5.282, 0.000]
- 旋转：用四元数表示 [0.000, 0.000, 0.934, -0.357]
时间 1625137666.850775673
- 翻译： [2.153, 5.133, 0.000][2.153, 5.133, 0.000]
- 旋转：以四元数 [0.000, 0.000, -0.365, 0.931] 表示

如果运行 tf2_echo 之间的变换 世界 和 乌龟2，你应该看不到变换，因为第二只乌龟还没有出现。不过，一旦我们在下一个教程中添加了第二只乌龟，它的姿势就会发生变化。 乌龟2 将向 tf2 播放。

摘要

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