8. tf2 坐标变换管理

运动学函数算出的末端位置只是一个数字，没有人知道它属于哪个坐标系。这一讲用 tf2 统一管理所有坐标系变换，让整个系统共享同一棵坐标系树。

---

前言：坐标系还散落在代码里

上一讲我们的系统已经能一键启动了。但有一个问题：

运动学函数算出的末端位置 (x, y) 只是一个数字，没有人知道它是在哪个坐标系下的。相机节点、规划节点、可视化节点各自维护自己的坐标系，互相不认识。

tf2 就是解决这个问题的：它是 ROS2 里统一管理所有坐标系变换关系的基础设施。

没有 tf2	有 tf2
每个节点自己算坐标变换	所有变换统一发布到 tf2
坐标系命名各自为政	全局唯一的坐标系树
时间同步靠自己处理	tf2 内置时间戳管理
无法可视化坐标系	RViz2 直接显示 tf2 树

---

8.1 tf2 的核心概念

8.1.1 坐标系树

tf2 管理的是一棵有向树，每条边表示"子坐标系相对父坐标系的变换"：

world
  └── base_link
        └── link1        ← 由 θ1 决定
              └── link2  ← 由 θ2 决定
                    └── end_effector

任意两个坐标系之间的变换，tf2 都能自动沿树路径计算出来。

8.1.2 静态变换 vs 动态变换

类型	适用场景	广播器
静态变换	固定不变，如相机安装位置	StaticTransformBroadcaster
动态变换	随时间变化，如关节转动	TransformBroadcaster

两连杆机械臂里：

• world → base_link：静态（机械臂固定在桌上）
• base_link → link1：动态（随 θ1 变化）
• link1 → link2：动态（随 θ2 变化）
• link2 → end_effector：静态（末端固定偏移）

8.1.3 变换的数学表示

tf2 内部用四元数 + 平移向量表示变换，等价于第 2 讲的齐次变换矩阵：

TransformStamped
├── header.stamp        # 时间戳
├── header.frame_id     # 父坐标系名称
├── child_frame_id      # 子坐标系名称
└── transform
    ├── translation     # 平移 (x, y, z)
    └── rotation        # 旋转（四元数 x, y, z, w）

2D 平面旋转 θ 对应的四元数：(x=0, y=0, z=sin(θ/2), w=cos(θ/2))

---

8.2 广播静态变换

8.2.1 用命令行广播（最快验证方式）

# 广播 world → base_link 的静态变换（无旋转，无平移）
ros2 run tf2_ros static_transform_publisher \
  --x 0 --y 0 --z 0 \
  --roll 0 --pitch 0 --yaw 0 \
  --frame-id world \
  --child-frame-id base_link

8.2.2 在节点中广播静态变换

vim $ROS_WS/src/my_robot_basics/my_robot_basics/tf_broadcaster_node.py

# my_robot_basics/tf_broadcaster_node.py
# 运行方式：ros2 run my_robot_basics tf_broadcaster_node
#
# ============================================================
# 节点目标：订阅 /joint_states，把关节角转换为坐标系变换并广播
#           到 tf2，让整个系统共享机械臂各连杆的坐标系关系
# 执行逻辑：
#   1. __init__ 创建动态/静态广播器，广播固定变换（world→base_link）
#   2. 订阅 /joint_states，收到关节角后触发 joint_state_callback
#   3. callback 里根据 θ1、θ2 构造 TransformStamped 消息
#   4. sendTransform() 把变换发布到 /tf Topic
#   5. RViz2、tf_listener 等节点从 /tf 读取并使用这些变换
# 注意：本节点是第 9 讲 robot_state_publisher 的手动实现版本
#       理解这里的逻辑，才能明白 RSP 背后在做什么
# ============================================================

import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
# TransformBroadcaster：发布动态变换到 /tf Topic（随时间变化的坐标系关系）
# StaticTransformBroadcaster：发布静态变换到 /tf_static Topic（固定不变的关系）
# 两者的区别：静态变换只需广播一次，会被持久缓存；动态变换需要持续广播
from tf2_ros import TransformBroadcaster, StaticTransformBroadcaster
from sensor_msgs.msg import JointState


class TFBroadcasterNode(Node):
    def __init__(self):
        super().__init__('tf_broadcaster_node')

        self.declare_parameter('l1', 1.0)
        self.declare_parameter('l2', 0.8)
        self.L1 = self.get_parameter('l1').value
        self.L2 = self.get_parameter('l2').value

        # 动态变换广播器：用于随关节角变化的坐标系（link1、link2）
        self.tf_broadcaster = TransformBroadcaster(self)

        # 静态变换广播器：用于固定不变的坐标系（world→base_link）
        # 静态变换只需广播一次，tf2 会永久缓存它
        self.static_broadcaster = StaticTransformBroadcaster(self)

        # 立即广播静态变换（节点启动时执行一次即可）
        self._broadcast_static_transforms()

        # 订阅关节角，每次收到新关节角就更新动态变换
        # [注意] 订阅 '/kinematics/joint_states' 而不是 '/joint_states'，与 kinematics_publisher 保持一致
        self.subscription = self.create_subscription(
            JointState,
            '/kinematics/joint_states',
            self.joint_state_callback,
            10,
        )

        self.get_logger().info('TF Broadcaster 启动，等待关节角...')

    def _broadcast_static_transforms(self):
        """广播固定不变的坐标系关系：world → base_link"""
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'       # 父坐标系
        t.child_frame_id = 'base_link'    # 子坐标系
        t.transform.translation.x = 0.0
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.0
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0  # 单位四元数（x=0,y=0,z=0,w=1）表示无旋转
        self.static_broadcaster.sendTransform(t)

    def joint_state_callback(self, msg):
        if len(msg.position) < 2:
            return

        theta1 = msg.position[0]  # 弧度，joint1 的角度
        theta2 = msg.position[1]  # 弧度，joint2 的角度
        now = self.get_clock().now().to_msg()

        # 广播 base_link → link1（由 θ1 决定旋转）
        # 平面绕 z 轴旋转 θ 的四元数：(x=0, y=0, z=sin(θ/2), w=cos(θ/2))
        # 这是从欧拉角到四元数的标准转换公式（绕单轴旋转的简化形式）
        t1 = TransformStamped()
        t1.header.stamp = now
        t1.header.frame_id = 'base_link'
        t1.child_frame_id = 'link1'
        t1.transform.translation.x = 0.0  # link1 原点与 base_link 原点重合
        t1.transform.translation.y = 0.0
        t1.transform.translation.z = 0.0
        t1.transform.rotation.x = 0.0
        t1.transform.rotation.y = 0.0
        t1.transform.rotation.z = math.sin(theta1 / 2)
        t1.transform.rotation.w = math.cos(theta1 / 2)

        # 广播 link1 → link2（由 θ2 决定旋转，原点在 link1 末端）
        t2 = TransformStamped()
        t2.header.stamp = now
        t2.header.frame_id = 'link1'
        t2.child_frame_id = 'link2'
        # link2 的原点在 link1 坐标系下的 x=L1 处（即 link1 的末端）
        t2.transform.translation.x = self.L1
        t2.transform.translation.y = 0.0
        t2.transform.translation.z = 0.0
        t2.transform.rotation.x = 0.0
        t2.transform.rotation.y = 0.0
        t2.transform.rotation.z = math.sin(theta2 / 2)
        t2.transform.rotation.w = math.cos(theta2 / 2)

        # 广播 link2 → end_effector（固定偏移，link2 末端，无旋转）
        t3 = TransformStamped()
        t3.header.stamp = now
        t3.header.frame_id = 'link2'
        t3.child_frame_id = 'end_effector'
        t3.transform.translation.x = self.L2  # end_effector 在 link2 末端
        t3.transform.translation.y = 0.0
        t3.transform.translation.z = 0.0
        t3.transform.rotation.x = 0.0
        t3.transform.rotation.y = 0.0
        t3.transform.rotation.z = 0.0
        t3.transform.rotation.w = 1.0  # 无旋转

        # 一次调用发送多个变换，比分三次调用更高效，时间戳也保持一致
        self.tf_broadcaster.sendTransform([t1, t2, t3])


def main(args=None):
    rclpy.init(args=args)
    node = TFBroadcasterNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

8.2.3 注册入口点

在 setup.py 的 console_scripts 里追加（当前完整内容）：

'console_scripts': [
    'hello_node              = my_robot_basics.hello_node:main',
    'kinematics_node         = my_robot_basics.kinematics_node:main',
    'kinematics_publisher    = my_robot_basics.kinematics_publisher:main',
    'kinematics_subscriber   = my_robot_basics.kinematics_subscriber:main',
    'ik_service_node         = my_robot_basics.ik_service_node:main',
    'ik_client_node          = my_robot_basics.ik_client_node:main',
    'move_to_action_server   = my_robot_basics.move_to_action_server:main',
    'move_to_action_client   = my_robot_basics.move_to_action_client:main',
    'tf_broadcaster_node     = my_robot_basics.tf_broadcaster_node:main',  # 新增
],

---

8.3 监听变换——查询末端位置

8.3.1 监听节点

vim $ROS_WS/src/my_robot_basics/my_robot_basics/tf_listener_node.py

# my_robot_basics/tf_listener_node.py
# 运行方式：ros2 run my_robot_basics tf_listener_node
#
# ============================================================
# 节点目标：每秒查询一次末端执行器在 world 坐标系下的位置，
#           验证 tf2 广播的坐标系变换与 FK 计算结果一致
# 执行逻辑：
#   1. __init__ 创建 Buffer 和 TransformListener（开始缓存 tf2 数据）
#   2. 创建 1 秒定时器，每秒触发一次查询
#   3. timer_callback 调用 lookup_transform() 查询变换
#   4. 从变换结果中提取末端位置并打印
# ============================================================

import rclpy
from rclpy.node import Node
# Buffer：在内存中缓存一段时间内的所有 tf2 变换数据
# TransformListener：订阅 /tf 和 /tf_static，把收到的变换存入 Buffer
# 两者必须配合使用：Listener 负责收数据，Buffer 负责存和查
from tf2_ros import TransformListener, Buffer
from rclpy.duration import Duration


class TFListenerNode(Node):
    def __init__(self):
        super().__init__('tf_listener_node')

        # Buffer 默认缓存最近 10 秒的变换数据
        # 查询历史时刻的变换时，数据必须在缓存窗口内
        self.tf_buffer = Buffer()

        # TransformListener(buffer, node)：
        # 内部会创建对 /tf 和 /tf_static 的订阅
        # 收到变换消息后自动存入 tf_buffer，不需要手动处理
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # 每秒查询一次末端位置
        self.timer = self.create_timer(1.0, self.timer_callback)
        self.get_logger().info('TF Listener 启动，每秒查询末端位置...')

    def timer_callback(self):
        try:
            # lookup_transform(目标坐标系, 源坐标系, 时间点)
            # 含义：返回"把源坐标系的点变换到目标坐标系"所需的变换矩阵
            # 即：end_effector 原点在 world 坐标系下的位置和姿态
            # rclpy.time.Time()：查询最新时刻的变换（时间戳为 0 表示"最新"）
            # 如果 tf2 还没收到数据，会抛出异常（见错误处理章节）
            t = self.tf_buffer.lookup_transform(
                'world',           # 目标坐标系（想要结果表示在哪个坐标系下）
                'end_effector',    # 源坐标系（想要查询哪个坐标系的位置）
                rclpy.time.Time(), # 查询最新时刻
            )
            # t.transform.translation 就是 end_effector 原点在 world 坐标系下的坐标
            x = t.transform.translation.x
            y = t.transform.translation.y
            self.get_logger().info(f'末端位置 (world 坐标系): x={x:.4f}, y={y:.4f}')

        except Exception as e:
            # 常见异常：
            # - LookupException：坐标系不存在（广播节点未启动）
            # - ExtrapolationException：查询时间超出缓存范围
            self.get_logger().warn(f'查询失败: {e}')


def main(args=None):
    rclpy.init(args=args)
    node = TFListenerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

8.3.2 注册入口点

在 setup.py 的 console_scripts 里追加（当前完整内容）：

'console_scripts': [
    'hello_node              = my_robot_basics.hello_node:main',
    'kinematics_node         = my_robot_basics.kinematics_node:main',
    'kinematics_publisher    = my_robot_basics.kinematics_publisher:main',
    'kinematics_subscriber   = my_robot_basics.kinematics_subscriber:main',
    'ik_service_node         = my_robot_basics.ik_service_node:main',
    'ik_client_node          = my_robot_basics.ik_client_node:main',
    'move_to_action_server   = my_robot_basics.move_to_action_server:main',
    'move_to_action_client   = my_robot_basics.move_to_action_client:main',
    'tf_broadcaster_node     = my_robot_basics.tf_broadcaster_node:main',
    'tf_listener_node        = my_robot_basics.tf_listener_node:main',     # 新增
],

---

8.4 构建并在 RViz2 中验证

8.4.1 构建

cd $ROS_WS
colcon build --packages-select my_robot_basics
source install/setup.bash

8.4.2 启动系统

终端 1：发布关节角

ros2 run my_robot_basics kinematics_publisher

终端 2：广播 TF

ros2 run my_robot_basics tf_broadcaster_node

终端 3：监听末端位置

ros2 run my_robot_basics tf_listener_node

终端 4：打开 RViz2

rviz2

8.4.3 在 RViz2 中配置

RViz2 TF 可视化效果

步骤 1：设置参考坐标系

RViz2 启动后，左侧面板顶部有 Global Options，展开它，找到 Fixed Frame 字段，默认值是 map。点击它，手动输入 world 并回车。

如果输入后变红，说明 tf_broadcaster_node 还没启动，或者 world 坐标系还没发布。先确认终端 2 的广播节点在运行。

步骤 2：添加 TF 显示插件

点击左侧面板最底部的 Add 按钮，弹出对话框：

• 选择 By display type 标签页
• 在列表里找到 TF，双击或点 OK

左侧面板会出现一个新的 TF 条目。

步骤 3：调整 TF 显示参数

展开左侧的 TF 条目，找到以下参数并调整：

参数	建议值	说明
Show Names	勾选	在坐标系旁显示名称
Show Axes	勾选	显示 RGB 三轴箭头（红=X，绿=Y，蓝=Z）
Show Arrows	可关闭	父子连线箭头，关掉更清晰
Marker Scale	0.3	默认 1 太大，坐标轴会互相遮挡，改小

步骤 4：调整视角

机械臂总长约 1.8 米（L1=1.0 + L2=0.8），默认视角可能太近。

• 滚轮向后滚，缩小视图，直到能看到整条臂
• 按住鼠标左键拖动旋转视角
• 按住鼠标中键拖动平移

预期效果

调整完后，3D 视口里应该能看到：

world / base_link（重叠在原点，因为两者无偏移）
    ↓ 沿 X 轴约 1.0 米处
  link2（随 θ1 旋转）
    ↓ 再沿 X 轴约 0.8 米处
  end_effector

关节角在持续变化（kinematics_publisher 在发布正弦波），坐标系应该能看到缓慢摆动。

如果坐标系还是扎堆

# 确认末端位置确实在变化
ros2 run tf2_ros tf2_echo world end_effector

看 Translation 的 x 值是否在 1.6～1.8 之间变化。如果是，说明数据正常，只需继续调小 Marker Scale。

---

8.5 用 CLI 工具排查 TF

# 查看当前所有坐标系
ros2 run tf2_tools view_frames

# 生成坐标系树的 PDF（在当前目录生成 frames.pdf）
# 运行后用 evince frames.pdf 查看

# 实时查询两个坐标系之间的变换
ros2 run tf2_ros tf2_echo world end_effector

# 查看 /tf topic 的原始数据
ros2 topic echo /tf

命令行执行, 可以查看具体的frames的结果

xdg-open frames_2026-04-06_18.25.53.pdf

view_frames 输出示例

view_frames 是排查 TF 树断链问题的最快手段，能直观看到哪些坐标系连通、哪些断开。

---

8.6 tf2 与运动学的关系

这是一个重要的理解点：

运动学函数（第 3 讲）：
  输入：θ1, θ2, L1, L2
  输出：末端坐标 (x, y)
  作用：数学计算

tf2（本讲）：
  输入：TransformStamped（带时间戳的变换）
  输出：全系统共享的坐标系树
  作用：系统级坐标管理

tf2 不替代运动学，而是让运动学结果变成系统共享资源。

后面的 robot_state_publisher 会根据 URDF 自动做这件事，不需要手写每个变换。但手写一遍，能让你真正理解 URDF + RSP 背后在做什么。

---

8.7 常见错误

8.7.1 错误一：RViz2 看不到坐标系

排查步骤：

# 1. 确认 TF 在发布
ros2 topic hz /tf

# 2. 确认坐标系存在
ros2 run tf2_tools view_frames

# 3. 确认 Fixed Frame 设置正确（必须是树的根节点）

---

8.7.2 错误二：lookup_transform 报 ExtrapolationException

ExtrapolationException: Lookup would require extrapolation into the future

原因：查询时间比 TF 数据的时间戳更新，缓存里还没有数据。

解决：查询最新时刻用 rclpy.time.Time()，或者加超时等待：

t = self.tf_buffer.lookup_transform(
    'world',
    'end_effector',
    rclpy.time.Time(),
    timeout=Duration(seconds=1.0),  # 等待最多 1 秒
)

---

8.7.3 错误三：坐标系树断链

view_frames 输出里看到两棵独立的树，说明某个变换没有发布。

常见原因：

• 父坐标系名字拼写错误（base_link vs base-link）
• 广播节点没有启动
• 静态变换只广播了一次但节点已退出

---

8.8 练习题

8.8.1 练习 1：验证 tf2 末端位置与 FK 一致

同时运行 kinematics_publisher、tf_broadcaster_node、tf_listener_node，对比：

• kinematics_publisher 日志里打印的末端坐标
• tf_listener_node 查询到的末端坐标

两者应该完全一致。

---

8.8.2 练习 2：用 tf2_echo 实时观察变换

ros2 run tf2_ros tf2_echo base_link end_effector

观察输出中的 translation 字段，和 FK 公式的结果对比。

---

8.8.3 练习 3：添加一个传感器坐标系

在 tf_broadcaster_node.py 里增加一个静态变换：end_effector → camera，表示末端安装了一个相机，相机相对末端偏移 (0.05, 0, 0)，无旋转。

用 view_frames 验证坐标系树包含 camera 节点。

参考答案：

t_cam = TransformStamped()
t_cam.header.stamp = self.get_clock().now().to_msg()
t_cam.header.frame_id = 'end_effector'
t_cam.child_frame_id = 'camera'
t_cam.transform.translation.x = 0.05
t_cam.transform.translation.y = 0.0
t_cam.transform.translation.z = 0.0
t_cam.transform.rotation.w = 1.0
self.static_broadcaster.sendTransform(t_cam)

---

本讲核心总结

概念	一句话理解
tf2	全系统统一的坐标系树管理器，所有模块共享
静态变换	固定不变的坐标系关系，用 StaticTransformBroadcaster 广播一次
动态变换	随时间变化的关系，用 TransformBroadcaster 持续广播
lookup_transform	查询任意两坐标系之间的变换，tf2 自动沿树路径计算
四元数	tf2 用四元数表示旋转，2D 绕 z 轴旋转 θ 对应 (0, 0, sin(θ/2), cos(θ/2))
view_frames	生成坐标系树 PDF，排查断链问题的第一手段

---

参考代码

本讲对应的完整参考代码位于：

ros_ws/reference/src/my_robot_basics/my_robot_basics/
├── tf_broadcaster_node.py      ← 8.2 广播静态/动态变换
└── tf_listener_node.py         ← 8.3 查询末端位置

使用方式：

cp $ROS_WS/reference/src/my_robot_basics/my_robot_basics/tf_broadcaster_node.py \
   $ROS_WS/src/my_robot_basics/my_robot_basics/
cp $ROS_WS/reference/src/my_robot_basics/my_robot_basics/tf_listener_node.py \
   $ROS_WS/src/my_robot_basics/my_robot_basics/

代码注释中详细解释了四元数与旋转角的转换关系（z=sin(θ/2), w=cos(θ/2)）、静态变换与动态变换的区别，以及 Buffer + TransformListener 的配合原理。

---

下一讲预告

9. URDF 机器人建模与 robot_state_publisher

本讲我们手写了每个坐标系变换，代码量不小。真实工程里，机器人结构用 URDF 文件描述，robot_state_publisher 会自动根据 URDF 和关节角发布整棵 TF 树——不需要手写任何 TransformStamped。

下一讲我们会：

• 用 XML 写出两连杆机械臂的 URDF 模型
• 启动 robot_state_publisher，让它替代本讲的 tf_broadcaster_node
• 用 joint_state_publisher_gui 拖动滑块，在 RViz2 里看到机械臂动起来
• 理解 URDF 是 MoveIt 2 的入场券