【ROS2】中级:tf2-编写广播器 (C++)

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

教程级别：中级

 时间：15 分钟

 目录

•  背景

•  先决条件

•  任务

  • 1. 编写广播器节点

  • 2. 编写启动文件

  •  3. 构建

  •  4. 运行

•  摘要

 背景

在接下来的两个教程中，我们将编写代码来复现“tf2 入门”教程中的演示。之后，后续教程将专注于使用更高级的 tf2 功能来扩展演示，包括在变换查找和时间旅行中使用超时。

 先决条件

本教程假设您具有 ROS 2 的工作知识，并且您已经完成了 tf2 教程 https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Introduction-To-Tf2.html 和 tf2 静态广播器教程（C++）https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Static-Broadcaster-Cpp.html 的介绍。我们将重用上一个教程中的 learning_tf2_cpp 包。

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

 任务

1. 编写广播器节点

让我们先创建源文件。转到我们在上一教程中创建的 learning_tf2_cpp 包。在 src 目录中，通过输入以下命令下载示例广播代码：

cxy@ubuntu2404-cxy:~/ros2_ws/src/learning_tf2_cpp/src$ wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp

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

// 版权 2021 开源机器人基金会
//
// 根据 Apache 许可证 2.0 版（“许可证”）授权；
// 除非符合许可证，否则您不得使用此文件。
// 您可以在以下网址获取许可证副本：
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// 除非适用法律要求或书面同意，否则根据许可证分发的软件
// 是按“原样”分发的，不附带任何明示或暗示的担保或条件。
// 请参阅许可证以了解管理权限和限制的特定语言。


#include <functional>  // 导入功能性库
#include <memory>  // 导入内存管理库
#include <sstream>  // 导入字符串流库
#include <string>  // 导入字符串库


#include "geometry_msgs/msg/transform_stamped.hpp"  // 导入TransformStamped消息类型
#include "rclcpp/rclcpp.hpp"  // 导入rclcpp库
#include "tf2/LinearMath/Quaternion.h"  // 导入tf2四元数库
#include "tf2_ros/transform_broadcaster.h"  // 导入tf2变换广播器库
#include "turtlesim/msg/pose.hpp"  // 导入Pose消息类型


class FramePublisher : public rclcpp::Node  // 定义FramePublisher类，继承自rclcpp::Node
{
public:
  FramePublisher()  // 构造函数
  : Node("turtle_tf2_frame_publisher")  // 调用父类构造函数，初始化节点名称为"turtle_tf2_frame_publisher"
  {
    // 声明并获取`turtlename`参数
    turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");


    // 初始化变换广播器
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);


    // 订阅turtle{1}{2}/pose主题，并在每个消息上调用handle_turtle_pose回调函数
    std::ostringstream stream;  // 创建字符串流对象
    stream << "/" << turtlename_.c_str() << "/pose";  // 构建主题名称
    std::string topic_name = stream.str();  // 获取主题名称字符串


    auto handle_turtle_pose =[this](const std::shared_ptr<turtlesim::msg::Pose> msg) {  // 定义回调函数
        geometry_msgs::msg::TransformStamped t;  // 创建TransformStamped对象


        // 读取消息内容并将其分配给相应的tf变量
        t.header.stamp = th