TF2 静态广播器（C++）

目标： 学习如何向 tf2 广播静态坐标系。

教程级别： 中级

预计时间： 15 分钟

背景

发布静态变换可用于定义机器人基座与其传感器或非移动部件之间的关系。 例如，在以激光扫描仪中心为原点的坐标系中分析激光扫描数据最为方便。

本教程涵盖静态变换的基础知识，是一个独立教程，分为两部分： 第一部分编写代码向 tf2 发布静态变换； 第二部分介绍如何使用 tf2_ros 提供的 static_transform_publisher 命令行工具。

在接下来的两个教程中，我们将编写代码来重现 tf2 简介教程中的演示。 后续教程将在此基础上，介绍如何使用更高级的 tf2 功能来扩展该演示。

前提条件

在之前的教程中，你已经学习了如何创建工作空间和功能包。

任务

1 创建功能包

首先，我们将创建一个用于本教程及后续教程的功能包。 learning_tf2_cpp 功能包依赖于 geometry_msgs、rclcpp、tf2、tf2_ros 和 turtlesim。 本教程的代码可以在这里找到。

打开一个新终端，source 你的 ROS 2 安装，以便 ros2 命令可用。 然后切换到工作空间的 src 目录，创建一个新功能包：

ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim -- learning_tf2_cpp

终端将返回一条消息，确认功能包 learning_tf2_cpp 及其所有必要的文件和文件夹已创建成功。

2 编写静态广播器节点

首先创建源文件。 在 src/learning_tf2_cpp/src 目录下，运行以下命令下载示例静态广播器代码：

Linux/macOS：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp

Windows：

在 Windows 命令行提示符中：

curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp -o static_turtle_tf2_broadcaster.cpp

或在 PowerShell 中：

curl https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp -o static_turtle_tf2_broadcaster.cpp

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

#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_ros/static_transform_broadcaster.h"

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher(char * transformation[])
  : Node("static_turtle_tf2_broadcaster")
  {
    tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms(transformation);
  }

private:
  void make_transforms(char * transformation[])
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = transformation[1];

    t.transform.translation.x = atof(transformation[2]);
    t.transform.translation.y = atof(transformation[3]);
    t.transform.translation.z = atof(transformation[4]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[5]),
      atof(transformation[6]),
      atof(transformation[7]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    tf_static_broadcaster_->sendTransform(t);
  }

  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Obtain parameters from command line arguments
  if (argc != 8) {
    RCLCPP_INFO(
      logger, "Invalid number of parameters\nusage: "
      "$ ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
      "child_frame_name x y z roll pitch yaw");
    return 1;
  }

  // As the parent frame of the transform is `world`, it is
  // necessary to check that the frame name passed is different
  if (strcmp(argv[1], "world") == 0) {
    RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
    return 1;
  }

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
  rclcpp::shutdown();
  return 0;
}

2.1 代码解析

现在来看与向 tf2 发布静态乌龟位姿相关的代码。 首先引入必要的头文件。 先包含 geometry_msgs/msg/transform_stamped.hpp，以便使用 TransformStamped 消息类型，后续要将该消息发布到变换树中。

#include "geometry_msgs/msg/transform_stamped.hpp"

接下来包含 rclcpp，以便使用 rclcpp::Node 类。

#include "rclcpp/rclcpp.hpp"

tf2::Quaternion 是一个四元数类，提供了在欧拉角与四元数之间相互转换的便捷函数。 此外，我们还包含 tf2_ros/static_transform_broadcaster.h，用 StaticTransformBroadcaster 来简化静态变换的发布。

#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_ros/static_transform_broadcaster.h"

StaticFramePublisher 类的构造函数以名称 static_turtle_tf2_broadcaster 初始化节点， 然后创建 StaticTransformBroadcaster，在启动时发送一个静态变换。

tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

this->make_transforms(transformation);

接下来创建一个 TransformStamped 对象，它就是即将发送的消息，填充数据后将其发出。 在设置实际的变换值之前，需要先填充必要的元数据。

1. 首先，为要发布的变换添加时间戳，这里直接使用当前时间 this->get_clock()->now()
2. 然后，设置所创建连接的父坐标系名称，本例中为 world
3. 最后，设置所创建连接的子坐标系名称

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = transformation[1];

这里填充乌龟的 6D 位姿（平移和旋转）。

t.transform.translation.x = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
  atof(transformation[5]),
  atof(transformation[6]),
  atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

最后，使用 sendTransform() 函数广播静态变换。

tf_static_broadcaster_->sendTransform(t);

2.2 更新 package.xml

返回上一级进入 src/learning_tf2_cpp 目录，这里已经为你创建了 CMakeLists.txt 和 package.xml 文件。

用文本编辑器打开 package.xml。

如创建功能包教程中所述，确保填写 <description>、<maintainer> 和 <license> 标签：

<description>Learning tf2 with rclcpp</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache-2.0</license>

记得保存文件。

2.3 CMakeLists.txt

在 CMakeLists.txt 中添加可执行文件，命名为 static_turtle_tf2_broadcaster，稍后你将通过 ros2 run 来运行它。

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
target_link_libraries(
   static_turtle_tf2_broadcaster PUBLIC
   geometry_msgs::geometry_msgs
   rclcpp::rclcpp
   tf2::tf2
   tf2_ros::tf2_ros
)

最后，添加 install(TARGETS…) 部分，以便 ros2 run 能找到你的可执行文件：

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

3 构建

在构建之前，建议先在工作空间根目录运行 rosdep 检查缺失的依赖：

Linux：

rosdep install -i --from-path src --rosdistro {DISTRO} -y

macOS/Windows：

rosdep 仅在 Linux 上可用，可直接跳到下一步。

继续在工作空间根目录下构建新功能包：

Linux/macOS：

colcon build --packages-select learning_tf2_cpp

Windows：

colcon build --merge-install --packages-select learning_tf2_cpp

打开一个新终端，切换到工作空间根目录，并 source 环境配置文件：

Linux/macOS：

. install/setup.bash

Windows：

在 Windows 命令行提示符中：

call install\setup.bat

或在 PowerShell 中：

.\install\setup.ps1

4 运行

现在运行 static_turtle_tf2_broadcaster 节点：

ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

这会广播 mystaticturtle 的位姿，使其悬浮在离地面 1 米的高度。

现在，可以通过回显 tf_static 话题来验证静态变换是否已发布。 如果一切正常，应该能看到一个静态变换：

$ ros2 topic echo /tf_static
transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   frame_id: world
child_frame_id: mystaticturtle
transform:
   translation:
      x: 0.0
      y: 0.0
      z: 1.0
   rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

发布静态变换的正确方式

本教程旨在展示如何使用 StaticTransformBroadcaster 发布静态变换。 在实际开发中，通常不需要自己编写这些代码，而应使用 tf2_ros 提供的专用工具来完成。 tf2_ros 提供了一个名为 static_transform_publisher 的可执行文件，既可以用作命令行工具，也可以作为节点添加到 launch 文件中。

以下命令向 tf2 发布一个静态坐标变换，使 world 和 mystaticturtle 坐标系之间在 z 轴上有 1 米的偏移且无旋转。 在 ROS 2 中，roll/pitch/yaw 分别表示绕 x/y/z 轴的旋转。

ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 1 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id mystaticturtle

以下命令发布与上述相同的静态坐标变换，但使用四元数来表示旋转。

ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 1 --qx 0 --qy 0 --qz 0 --qw 1 --frame-id world --child-frame-id mystaticturtle

static_transform_publisher 既可以作为命令行工具手动使用，也可以在 launch 文件中用于设置静态变换。 例如：

XML：

<launch>
  <node
    pkg="tf2_ros" exec="static_transform_publisher"
    args="--x 0 --y 0 --z 1 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id mystaticturtle"
  />
</launch>

YAML：

launch:
  - node:
      pkg: "tf2_ros"
      exec: "static_transform_publisher"
      args: "--x 0 --y 0 --z 1 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id mystaticturtle"

Python：

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=[
                '--x', '0', '--y', '0', '--z', '1',
                '--yaw', '0', '--pitch', '0', '--roll',
                '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
        ),
    ])

注意，除了 --frame-id 和 --child-frame-id 之外，所有参数都是可选的；如果未指定某个选项，将默认使用单位变换（identity）。

总结

在本教程中，你学习了如何使用静态变换来定义坐标系之间的静态关系，例如 mystaticturtle 与 world 坐标系之间的关系。 此外，你还了解到静态变换对于理解传感器数据（例如激光扫描仪数据）非常有用——它可以将数据关联到一个公共坐标系。 最后，你编写了自己的节点来向 tf2 发布静态变换，并学习了如何使用 static_transform_publisher 可执行文件和 launch 文件来发布所需的静态变换。