TF2 静态广播器（Python）

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

教程级别： 中级

预计时间： 15 分钟

背景

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

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

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

前提条件

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

任务

1 创建功能包

首先，我们将创建一个用于本教程及后续教程的功能包。 learning_tf2_py 功能包依赖于 geometry_msgs、python3-numpy、rclpy、tf2_ros_py 和 turtlesim_msgs。本教程的代码可以在这里找到。

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

ros2 pkg create --build-type ament_python --license Apache-2.0 -- learning_tf2_py

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

2 编写静态广播器节点

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

Linux/macOS：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py

Windows：

在 Windows 命令行提示符中：

curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py -o static_turtle_tf2_broadcaster.py

或在 PowerShell 中：

curl https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py -o static_turtle_tf2_broadcaster.py

用你喜欢的文本编辑器打开 static_turtle_tf2_broadcaster.py 文件。

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster


def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


class StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame.
    The transforms are only published once at startup, and are constant for all
    time.
    """

    def __init__(self, transformation):
        super().__init__('static_turtle_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms(transformation)

    def make_transforms(self, transformation):
        t = TransformStamped()

        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = transformation[1]

        t.transform.translation.x = float(transformation[2])
        t.transform.translation.y = float(transformation[3])
        t.transform.translation.z = float(transformation[4])
        quat = quaternion_from_euler(
            float(transformation[5]), float(transformation[6]), float(transformation[7]))
        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]

        self.tf_static_broadcaster.sendTransform(t)


def main():
    try:
        logger = rclpy.logging.get_logger('logger')

        # obtain parameters from command line arguments
        if len(sys.argv) != 8:
            logger.info('Invalid number of parameters. Usage: \n'
                        '$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster'
                        'child_frame_name x y z roll pitch yaw')
            sys.exit(1)

        if sys.argv[1] == 'world':
            logger.info('Your static turtle name cannot be "world"')
            sys.exit(2)

        # pass parameters and initialize node
        with rclpy.init():
            node = StaticFramePublisher(sys.argv)
            rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass

2.1 代码解析

现在来看与向 tf2 发布静态乌龟位姿相关的代码。首先是导入所需的包。从 geometry_msgs 导入 TransformStamped，它提供了将要发布到变换树中的消息模板。

from geometry_msgs.msg import TransformStamped

接下来导入 rclpy，以便使用 Node 类。

import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node

tf2_ros 包提供了 StaticTransformBroadcaster 来简化静态变换的发布。要使用 StaticTransformBroadcaster，我们需要从 tf2_ros 模块中导入它。

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

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

self.tf_static_broadcaster = StaticTransformBroadcaster(self)
self.make_transforms(transformation)

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

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

t = TransformStamped()

t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = transformation[1]

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

t.transform.translation.x = float(transformation[2])
t.transform.translation.y = float(transformation[3])
t.transform.translation.z = float(transformation[4])
quat = quaternion_from_euler(
    float(transformation[5]), float(transformation[6]), float(transformation[7]))
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]

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

self.tf_static_broadcaster.sendTransform(t)

2.2 更新 package.xml

返回上一级到 src/learning_tf2_py 目录，这里已经为你创建了 setup.py、setup.cfg 和 package.xml 文件。

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

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

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

在上述行之后，添加与你的节点导入语句对应的以下依赖：

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf2_ros_py</exec_depend>
<exec_depend>turtlesim_msgs</exec_depend>

这声明了代码执行时所需的 geometry_msgs、python3-numpy、rclpy、tf2_ros_py 和 turtlesim_msgs 依赖。确保保存文件。

2.3 添加入口点

要让 ros2 run 命令能运行你的节点，必须在 setup.py（位于 src/learning_tf2_py 目录下）中添加入口点。

在 'console_scripts': 的方括号中添加以下行：

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

3 构建

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

Linux：

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

macOS/Windows：

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

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

Linux/macOS：

colcon build --packages-select learning_tf2_py

Windows：

colcon build --merge-install --packages-select learning_tf2_py

打开一个新终端，切换到工作空间根目录，并 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_py 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 文件来发布所需的静态变换。