编写广播器（C++）

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

教程级别： 中级

时间： 15分钟

背景

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

先决条件

本教程假设您已经具备ROS 2的工作知识，并且已经完成了 Introduction to tf2 教程 和 tf2静态广播器教程 (C++)。在之前的教程中，您学习了如何 创建工作空间 和 创建软件包。您还创建了“learning_tf2_cpp” 软件包，我们将从这里继续工作。

任务

1. 编写广播器节点

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

LinuxmacOSWindows

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/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.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<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;
}

1.1 检查代码

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

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

接下来，节点订阅了 turtleX/pose 主题，并在每个传入的消息上运行函数 handle_turtle_pose。

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

现在，我们创建一个 TransformStamped 对象，并为其添加适当的元数据。

1. 我们需要为发布的变换设置一个时间戳，通过调用 this->get_clock()->now() 将其标记为当前时间。这将返回 Node 使用的当前时间。

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乌龟姿态的信息复制到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``中使用它。

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...)``部分，以便``ros2 run``可以找到你的可执行文件：

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

2 编写启动文件

现在为此演示创建一个启动文件。使用文本编辑器，在``launch``文件夹中创建一个名为``turtle_tf2_demo.launch.py``的新文件，并添加以下内容：

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 和 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。

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。根据你的启动文件的导入语句，添加以下依赖项：

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

当执行其代码时，这将声明额外所需的 launch 和 launch_ros 依赖项。

确保保存文件.

2.3 CMakeLists.txt

重新打开 CMakeLists.txt 并添加该行，以便安装 launch/ 文件夹中的启动文件。

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

你可以在 这个教程 中了解更多关于创建启动文件的信息。

3 构建

在你的工作空间的根目录中运行 rosdep，以检查缺少的依赖项。

LinuxmacOSWindows

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

从你的工作空间的根目录中构建更新的软件包：

LinuxmacOSWindows

colcon build --packages-select learning_tf2_cpp

打开一个新终端，进入你的工作空间的根目录，并源化设置文件：

LinuxmacOSWindows

. install/setup.bash

4 运行

现在运行启动文件，启动turtlesim仿真节点和``turtle_tf2_broadcaster``节点:

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 listener。