编写广播器（C++）

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

教程级别： 中级

时间： 15 分钟

背景

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

先决条件

本教程假设您具有 ROS 2 的工作知识，并且已完成 tf2 简介教程 和 tf2 静态广播器教程 (C++)。 我们将重用上一个教程中的 learning_tf2_cpp 包。

在之前的教程中，您学习了如何：doc:创建工作区 和 创建包。

任务

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 编写启动文件

现在为该演示创建一个启动文件。 在“src/learning_tf2_cpp”目录中创建一个“launch”文件夹。 使用文本编辑器，在“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})

您可以在：doc:本教程 中了解有关创建启动文件的更多信息。

3 构建

在工作区根目录中运行“rosdep”以检查是否缺少依赖项。

LinuxmacOSWindows

rosdep install -i --from-path src --rosdistro rolling -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]

如果您运行“tf2_echo”来在“world”和“turtle2”之间进行变换，则应该看不到变换，因为第二只海龟还没有出现。 但是，在下一个教程中添加第二只海龟后，“turtle2”的姿势将立即广播到 tf2。

摘要

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