【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 = this->get_clock()->now();  // 设置时间戳
        t.header.frame_id = "world";  // 设置父坐标系ID
        t.child_frame_id = turtlename_.c_str();  // 设置子坐标系ID


        // 乌龟只存在于2D，因此我们从消息中获取x和y平移坐标，并将z坐标设置为0
        t.transform.translation.x = msg->x;  // 设置x平移坐标
        t.transform.translation.y = msg->y;  // 设置y平移坐标
        t.transform.translation.z = 0.0;  // 设置z平移坐标为0


        // 出于同样的原因，乌龟只能绕一个轴旋转，这就是为什么我们将x和y的旋转设置为0，并从消息中获取z轴的旋转
        tf2::Quaternion q;  // 创建四元数对象
        q.setRPY(0, 0, msg->theta);  // 设置四元数的RPY值
        t.transform.rotation.x = q.x();  // 设置四元数的x分量
        t.transform.rotation.y = q.y();  // 设置四元数的y分量
        t.transform.rotation.z = q.z();  // 设置四元数的z分量
        t.transform.rotation.w = q.w();  // 设置四元数的w分量


        // 发送变换
        tf_broadcaster_->sendTransform(t);
      };
    subscription_ = this->create_subscription<turtlesim::msg::Pose>(  // 创建订阅
      topic_name, 10,  // 订阅主题名称和队列大小
      handle_turtle_pose);  // 设置回调函数
  }


private:
  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
  rclcpp::spin(std::make_shared<FramePublisher>());  // 创建并运行FramePublisher节点
  rclcpp::shutdown();  // 关闭rclcpp
  return 0;  // 返回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,
  handle_turtle_pose);

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

1. 我们需要为正在发布的转换加上一个时间戳，我们将通过调用 this->get_clock()->now() 来用当前时间来标记它。这将返回 Node 使用的当前时间。

2. 然后我们需要设置我们正在创建的链接的父框架的名称，在这种情况下是 world 。

3. 最后，我们需要设置我们正在创建的链接的子节点的名称，在这种情况下，这就是乌龟本身的名称。

处理函数为 turtle 姿态消息广播此 turtle 的平移和旋转，并将其作为从框架 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})

// 设置CMake的最低版本要求为3.8
cmake_minimum_required(VERSION 3.8) // 设置CMake的最低版本要求为3.8
// 定义一个名为learning_tf2_cpp的项目
project(learning_tf2_cpp) // 定义一个名为learning_tf2_cpp的项目


// 如果编译器是GNU或Clang，添加编译选项
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic) // 添加编译选项
endif()


// 寻找依赖项
find_package(ament_cmake REQUIRED) // 寻找ament_cmake包
find_package(geometry_msgs REQUIRED) // 寻找geometry_msgs包
find_package(rclcpp REQUIRED) // 寻找rclcpp包
find_package(tf2 REQUIRED) // 寻找tf2包
find_package(tf2_ros REQUIRED) // 寻找tf2_ros包
find_package(turtlesim REQUIRED) // 寻找turtlesim包


// 添加可执行文件
add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp) // 添加名为static_turtle_tf2_broadcaster的可执行文件
// 添加目标依赖项
ament_target_dependencies(
   static_turtle_tf2_broadcaster // 目标文件
   geometry_msgs // 依赖项
   rclcpp // 依赖项
   tf2 // 依赖项
   tf2_ros // 依赖项
)


// 安装目标文件
install(TARGETS
   static_turtle_tf2_broadcaster // 目标文件
   DESTINATION lib/${PROJECT_NAME}) // 安装位置
   
// 添加可执行文件
add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp) // 添加名为turtle_tf2_broadcaster的可执行文件
// 添加目标依赖项
ament_target_dependencies(
    turtle_tf2_broadcaster // 目标文件
    geometry_msgs // 依赖项
    rclcpp // 依赖项
    tf2 // 依赖项
    tf2_ros // 依赖项
    turtlesim // 依赖项
)


// 安装目标文件
install(TARGETS
    turtle_tf2_broadcaster // 目标文件
    DESTINATION lib/${PROJECT_NAME}) // 安装位置


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


// 如果构建测试
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED) // 寻找ament_lint_auto包
  // 跳过检查版权的linter
  set(ament_cmake_copyright_FOUND TRUE) // 设置ament_cmake_copyright_FOUND为真
  // 跳过cpplint（只在git仓库中工作）
  set(ament_cmake_cpplint_FOUND TRUE) // 设置ament_cmake_cpplint_FOUND为真
  ament_lint_auto_find_test_dependencies() // 自动寻找测试依赖项
endif()


// 打包
ament_package() // 打包

2. 编写启动文件

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

# 导入LaunchDescription模块
from launch import LaunchDescription
# 导入Node模块
from launch_ros.actions import Node


# 定义一个名为generate_launch_description的函数
def generate_launch_description(): 
    # 返回一个LaunchDescription对象
    return LaunchDescription([ 
        # 创建一个Node对象
        Node( 
            # 设置包名为'turtlesim'
            package='turtlesim',
            # 设置可执行文件名为'turtlesim_node'
            executable='turtlesim_node', 
            # 设置节点名为'sim'
            name='sim' 
        ),
        # 创建另一个Node对象
        Node( 
            # 设置包名为'learning_tf2_cpp'
            package='learning_tf2_cpp', 
            # 设置可执行文件名为'turtle_tf2_broadcaster'
            executable='turtle_tf2_broadcaster',
            # 设置节点名为'broadcaster1'
            name='broadcaster1', 
            # 设置参数
            parameters=[ // 设置参数
                # 设置'turtlename'参数为'turtle1'
                {'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 以检查缺失的依赖项。

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

仍在工作区的根目录下，构建您的包：

colcon build --packages-select learning_tf2_cpp

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

. 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 终端窗口是活跃的，不是你的模拟器窗口）。在你的控制台输出中，你会看到类似这样的东西：

cxy@ubuntu2404-cxy:~/ros2_ws$ ros2 run tf2_ros tf2_echo world turtle1
[INFO] [1720692090.983150908] [tf2_echo]: Waiting for transform world ->  turtle1: Invalid frame ID "world" passed to canTransform argument target_frame - frame does not exist
At time 1720692091.946408378
- Translation: [3.773, 3.598, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.992, 0.125]
- Rotation: in RPY (radian) [0.000, -0.000, 2.891]
- Rotation: in RPY (degree) [0.000, -0.000, 165.653]
- Matrix:
 -0.969 -0.248  0.000  3.773
  0.248 -0.969  0.000  3.598
  0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

如果您运行 tf2_echo 以实现 world 和 turtle2 之间的转换，您不应该看到任何转换，因为第二只乌龟还没有出现。然而，一旦我们在下一个教程中添加了第二只乌龟， turtle2 的姿态将会广播到 tf2。

 摘要

在本教程中，您学习了如何将机器人的姿态（乌龟的位置和方向）广播到 tf2，以及如何使用 tf2_echo 工具。为了实际使用广播到 tf2 的变换，您应该继续学习下一教程，关于创建 tf2 监听器 https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.html 。