4. TF2坐标变换进阶：静态与动态广播、坐标监听与变换、时间戳管理、多机器人坐标系统

坐标变换，说白了就是让机器人知道「我在哪，东西在哪」。

TF2 是 ROS2 里处理这个问题的核心工具。我刚开始接触时也觉得不就是个坐标转换嘛，后来才发现，时间戳、缓存、多机器人这些坑，一个比一个深。这一章，咱们就把这些进阶玩法彻底捋清楚。

4.1 静态广播与动态广播

TF2 的广播分两种：静态和动态。区别很简单——静态的发布一次就不变了，动态的得一直发。

4.1.1 静态广播

静态广播适合那些永远不会动的坐标关系。比如机器人底座到激光雷达的安装位置，或者相机到 IMU 的固定偏移。

我个人习惯用命令行直接发布静态变换，省事：

ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll parent_frame child_frame

举个例子，把激光雷达装在机器人前方 0.3 米处：

ros2 run tf2_ros static_transform_publisher 0.3 0 0 0 0 0 base_link laser_frame

在代码里，静态广播通常用 StaticTransformBroadcaster：

from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped

broadcaster = StaticTransformBroadcaster(node)
t = TransformStamped()
t.header.frame_id = 'base_link'
t.child_frame_id = 'laser_frame'
t.transform.translation.x = 0.3
t.transform.rotation.w = 1.0
broadcaster.sendTransform(t)

嗯，这里要注意：静态变换只发一次就行，TF2 会把它存到缓存里，后面谁用都能查到。

4.1.2 动态广播

动态广播适合那些一直在变的坐标关系。比如机械臂的每个关节，或者移动机器人的里程计位置。

我在项目中遇到过一个问题：动态广播如果发布频率太低，下游节点查变换时会报错「Lookup would require extrapolation」。说白了就是缓存里没有你要的那个时间点的数据。

动态广播的标准写法：

from tf2_ros import TransformBroadcaster

broadcaster = TransformBroadcaster(node)
t = TransformStamped()

while rclpy.ok():
    t.header.stamp = node.get_clock().now().to_msg()
    t.header.frame_id = 'odom'
    t.child_frame_id = 'base_link'
    t.transform.translation.x = current_x
    t.transform.translation.y = current_y
    t.transform.rotation.z = sin(yaw/2)
    t.transform.rotation.w = cos(yaw/2)
    broadcaster.sendTransform(t)
    rclpy.spin_once(node, timeout_sec=0.01)

你想想看，这里最关键的是时间戳。每次发布都要带上当前时间，否则下游节点没法正确插值。

4.2 坐标监听与变换

广播是发出去，监听是收回来。TF2 的监听器会自动缓存所有收到的变换，并提供查询接口。

4.2.1 基本监听

用 TransformListener 就能拿到缓存：

from tf2_ros import TransformListener, Buffer

tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, node)

# 在某个回调里查询变换
try:
    transform = tf_buffer.lookup_transform(
        target_frame='base_link',
        source_frame='laser_frame',
        time=rclpy.time.Time()
    )
    # 拿到 transform 后，就能知道激光雷达相对于机器人底座的位置
except Exception as e:
    node.get_logger().warn(f'变换查询失败: {e}')

这里有个坑：lookup_transform 默认查的是当前时间。如果广播和监听之间有延迟，就会报错。我曾经因为网络延迟，查变换时一直报 extrapolation 错误，折腾了半天才发现是时间戳没对齐。

4.2.2 变换的数学运算

拿到变换后，你可以把点、向量、姿态从一个坐标系转到另一个坐标系：

from tf2_geometry_msgs import do_transform_point
from geometry_msgs.msg import PointStamped

point_in_laser = PointStamped()
point_in_laser.header.frame_id = 'laser_frame'
point_in_laser.point.x = 1.0

point_in_base = do_transform_point(point_in_laser, transform)
# 现在 point_in_base 就是在 base_link 坐标系下的坐标了

说白了，TF2 帮你做了矩阵乘法，你只需要告诉它「从哪到哪」就行。

4.3 时间戳管理

时间戳是 TF2 的灵魂。没有正确的时间戳，坐标变换就是一团乱麻。

4.3.1 时间戳的作用

TF2 缓存里存的是不同时间点的变换关系。当你查询时，它会根据时间戳做插值或外推。

• 插值：如果你要的时间点在两个已知变换之间，TF2 会线性插值。
• 外推：如果你要的时间点比最新缓存还晚，TF2 会尝试外推，但超过阈值就报错。

4.3.2 常见时间戳问题

问题	原因	解决方法
Lookup would require extrapolation	查询时间超出缓存范围	增加广播频率，或使用 lookup_transform 时指定更早的时间
Frame id does not exist	坐标系名称拼错或未广播	检查 frame_id 拼写，确认广播节点在运行
Cannot transform from ... to ...	两个坐标系之间没有连通路径	检查 TF 树是否完整，中间是否有断链

我记得有一次调试，激光雷达的数据总是对不上位置。查了半天，发现是激光雷达驱动里时间戳用的是传感器时间，而里程计用的是系统时间，两个时间不同步。最后统一用系统时间才解决。

4.4 多机器人坐标系统

多机器人系统里，每个机器人都有自己的 TF 树。怎么把它们统一到一个全局坐标系下？这是个大问题。

4.4.1 独立 TF 树的问题

每个机器人单独跑 TF2，它们的坐标系是隔离的。比如机器人 A 的 base_link 和机器人 B 的 base_link 是两个不同的 frame，TF2 不知道它们之间的关系。

4.4.2 解决方案：全局坐标系

常见的做法是引入一个全局坐标系，比如 map 或 world。每个机器人把自己的 base_link 发布到 map 下：

• 机器人 A：map → robot1/base_link
• 机器人 B：map → robot2/base_link

这样，任何节点都能通过 map 查到两个机器人之间的相对位置。

4.4.3 命名空间与 frame_id

为了避免 frame_id 冲突，我建议给每个机器人的坐标系加上前缀。比如 robot1/base_link、robot2/laser_frame。

在代码里可以这样处理：

robot_ns = 'robot1'
child_frame = f'{robot_ns}/base_link'
t.header.frame_id = 'map'
t.child_frame_id = child_frame

你想想看，如果两个机器人都叫 base_link，那 TF2 就懵了——到底哪个是哪个？

4.4.4 多机器人 TF 树示例

下面这张图展示了一个典型的多机器人 TF 树结构：

4.5 避坑指南与最佳实践

最后，分享几个我踩过的坑，希望能帮你少走弯路。

说白了，TF2 的核心就三件事：广播、监听、时间戳。广播要准时，监听要容错，时间戳要对齐。把这三点做好，坐标变换这块基本就稳了。