编写tf2广播器 | Bistol Lab

静态变换发布器（可解释什么是静态）

现在我们尝试使用 tf2_ros 提供的 static_transform_publisher 工具发布一个非常简单的变换。我们将发布一个从坐标系 base_link 到坐标系 base_camera 的变换，平移量为（x: 0.1m, y: 0.0m, z: 0.2m）。

打开终端并执行以下命令：

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

下面的命令使用四元数表示旋转，但发布的是同样的静态变换：

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

通过这个命令成功地在 tf2 中发布了 base_link 到 base_camera 的变换。我们通过使用tf2_echo 检查它是否正常工作。打开一个单独的终端并执行以下命令：

ros2 run tf2_ros tf2_echo base_link base_camera

可以观察到类似于下面的信息不断的输出。

- Translation: [0.000, 0.000, 1.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
  1.000  0.000  0.000  0.000
  0.000  1.000  0.000  0.000
  0.000  0.000  1.000  1.000
  0.000  0.000  0.000  1.000

现在成功地使用 tf2 发布了从 base_link 到 base_camera 的坐标变换。注意，在实际的机器人项目中不推荐使用上述方法发布变换，这里只是理解 tf2 的一个简单示例。对于一个真正的机器人系统，我们会创建一个URDF文件，其中嵌入了这些信息以及更多关于机器人的信息，使用robot_state_publisher 而不是 static_transform_publisher，该方法会在后面的内容中具体介绍。

ROS社区的标准

ROS 社区规定了一些标准，以确保不同功能包之间的正确运行坐标变换。其中一个为 REP 105 - 移动平台的坐标系，另一个标准为REP 103，它规定了标准单位和坐标约定。

简单总结REP 105 的内容，此文档规定了 ROS 中使用的不同的坐标系的命名约定和语义含义。本教程涉及到了 base_link、odom 和 map 坐标系，base_link 是一个固定在机器人上的坐标系，通常位于其主底盘和旋转中心。odom 坐标系是一个相对于机器人起始位置的固定坐标系，主要用于局部一致的距离表示。最后，map 坐标系是一个固定的世界坐标系，用于全局一致的距离表示。

REP 103 则讨论了一些标准单位和其他相关约定，以尽量减少不同 ROS 功能包之间的集成问题。基本概述是，坐标系使用右手规则来定义，Z轴向上，X轴向前，单位应为标准的SI单位。

如果有多个传感器基坐标（例如 base_ladia, base_motor 等），那么每一个坐标系都需要和base_link 发生关联。

编写一个 tf2 广播器 (Python)

本节将介绍如何使用 Python 编写一个 tf2 广播器。坐标变换是机器人系统中的一个核心概念，它用于描述不同坐标系之间的关系。ROS 2 提供了 tf2 库，用于实现这种变换的广播和监听。

图 1: tf2工作流程图

图 1 展示了 ROS2 中 tf2 坐标变换系统的工作机制。在系统中，TransformBroadcaster 和 StaticTransformBroadcaster 分别用于动态和静态地发布坐标变换，它们通过 sendTransform() 方法将数据发布到 /tf 或 /tf_static 话题。TransformListener 作为订阅者监听这些话题，并将收到的变换信息存入 Buffer。Buffer 是 tf2 的核心组件，负责缓存所有的坐标变换数据，并提供lookup_transform() 和 transform() 等接口，供其他节点查询坐标系之间的关系或进行坐标转换。通过这种机制，ROS 2 系统中的不同模块可以清晰地获取和使用各个坐标系之间的相对位置关系，实现机器人在不同参考系之间的空间理解和导航能力。

tf2 广播器代码

首先，创建一个新的 ROS 2 功能包。打开终端进入到 ros2_ws/src 并输入以下命令：

ros2 pkg create --build-type ament_python tf2_broadcaster_py

这将创建一个名为 tf2_broadcaster_py 的功能包，并使用 Python 构建系统。进入到功能包的 tf2_broadcaster_py 目录，并创建一个名为 tf2_broadcaster.py 的文件。在该文件中，编写一个广播器节点，具体代码如下：

import math
from geometry_msgs.msg import TransformStamped
import numpy as np
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from turtlesim.msg import Pose


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 FramePublisher(Node):
    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')
        # 声明 `turtlename` 参数
        self.turtlename = self.declare_parameter(
          'turtlename', 'turtle').get_parameter_value().string_value

        # 初始化广播变换
        self.tf_broadcaster = TransformBroadcaster(self)

        # 订阅 turtle{1}{2}/pose 话题并在每次接收到信息时调用 handle_turtle_pose回调函数
        self.subscription = self.create_subscription(
            Pose,f'/{self.turtlename}/pose', self.handle_turtle_pose,1)
        self.subscription  # 防止未使用的参数警告

    def handle_turtle_pose(self, msg):
        t = TransformStamped()
        # 为广播器参数赋值
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename
        # 坐标轴的位移量，也就是海龟的位置
        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        # 坐标轴的旋转角度，也就是海龟的旋转角度
        q = quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # 发送坐标变换
        self.tf_broadcaster.sendTransform(t)

def main():
    rclpy.init()
    node = FramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

代码解释

接下来详细解释广播器的代码。首先，这段代码定义了一个名为turtlename的参数，该参数指定了一个海龟名称，例如turtle1或turtle2。

self.turtlename = self.declare_parameter(
  'turtlename', 'turtle').get_parameter_value().string_value

之后，节点订阅了 {self.turtlename}/pose 话题，并在每个接收的消息上运行handle_turtle_pose 函数。

self.subscription = self.create_subscription(
    Pose,
    f'/{self.turtlename}/pose',
    self.handle_turtle_pose,
    1)

接下来创建一个TransformStamped对象，并给它适当的数据：

首先用当前时间作为时间戳，通过调用self.get_clock().now()函数实现。
然后需要设置正在创建的父坐标的名称，在这里是world。
最后，设置创建的子坐标系的名称，这里为self.turtlename。

handle_turtle_pose 函数接收到海龟的姿态数据后，以从坐标系 word 到坐标系 turtelx 的变换进行广播.

t = TransformStamped()

# 读取消息内容并分配给对应的tf变量
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = self.turtlename

这段代码将海龟3D姿态中的信息复制到3D变换中。

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

# 同样的原因，海龟只能绕z轴旋转，这就是为什么我们将x和y的旋转设置为0
q = quaternion_from_euler(0, 0, msg.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]

最后，我们将构建的变换传递给 TransformBroadcaster 的 sendTransform 方法进行广播。

# 发送变换
self.tf_broadcaster.sendTransform(t)

功能包配置与构建

为了让 ros2 run 命令运行该节点，在 setup.py 文件中的 console_scripts 括号之间添加以下代码：

'turtle_tf2_broadcaster = tf2_broadcaster_py.tf2_broadcaster:main',

编写启动文件

现在为该工程创建一个启动文件，在 src/tf2_broadcaster_py 目录中创建一个 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='tf2_broadcaster_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

添加依赖项

在 learning_tf2_py 目录，用文本编辑器打开 package.xml 并添加相对应的依赖项：

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

保存文件。

更新setup.py

重新打开 setup.py 并在文件顶部添加以下代码，以便从 launch/ 文件夹安装启动文件。在data_files 字段添加如下代码：

data_files=[
    ...
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
],

还需要在文件顶部导入适当的模块：

import os
from glob import glob

构建并运行功能包

在工作空间的根目录，构建功能包：

colcon build --packages-select tf2_broadcaster_py

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

source install/setup.bash

现在运行启动文件，它将启动 turtlesim 仿真节点和 turtle_tf2_broadcaster 节点：

ros2 launch tf2_broadcaster_py turtle_tf2_demo_launch.py

在第二个终端窗口中输入以下命令：

ros2 run turtlesim turtle_teleop_key

执行上述操作打开键盘控制器并控制海龟运动。

接下来，使用 tf2_echo 工具检查海龟姿态是否被广播到tf2：

ros2 run tf2_ros tf2_echo world turtle1

这应该显示第一只海龟的姿态。使用箭头键控制海龟移动，在你的控制台输出中应该看到类似如下的内容：

图 2: tf2广播器运行结果

静态变换发布器 （可解释什么是静态）​

ROS社区的标准​

编写一个 tf2 广播器 (Python)​

tf2 广播器代码​

代码解释​

功能包配置与构建​