ROS2

On this page

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 中配置

步骤 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 是排查 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 的入场券

Decision making

Motion control

Perception

Engineering foundations

AMD projects

Simulation projects

Real robot projects

View as Markdown

8. tf2 坐标变换管理

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

8.1 tf2 的核心概念

8.1.1 坐标系树

8.1.2 静态变换 vs 动态变换

8.1.3 变换的数学表示

8.2 广播静态变换

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

8.2.2 在节点中广播静态变换

8.2.3 注册入口点

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

8.3.1 监听节点

8.3.2 注册入口点

8.4 构建并在 RViz2 中验证

8.4.1 构建

8.4.2 启动系统

8.4.3 在 RViz2 中配置

8.5 用 CLI 工具排查 TF

8.6 tf2 与运动学的关系

8.7 常见错误

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

8.7.2 错误二：`lookup_transform` 报 `ExtrapolationException`

8.7.3 错误三：坐标系树断链

8.8 练习题

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

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

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

本讲核心总结

参考代码

下一讲预告

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

8.1 tf2 的核心概念​

8.1.1 坐标系树​

8.1.2 静态变换 vs 动态变换​

8.1.3 变换的数学表示​

8.2 广播静态变换​

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

8.2.2 在节点中广播静态变换​

8.2.3 注册入口点​

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

8.3.1 监听节点​

8.3.2 注册入口点​

8.4 构建并在 RViz2 中验证​

8.4.1 构建​

8.4.2 启动系统​

8.4.3 在 RViz2 中配置​

8.5 用 CLI 工具排查 TF​

8.6 tf2 与运动学的关系​

8.7 常见错误​

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

8.7.2 错误二：lookup_transform 报 ExtrapolationException​

8.7.3 错误三：坐标系树断链​

8.8 练习题​

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

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

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

本讲核心总结​

参考代码​

下一讲预告​

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

8.1 tf2 的核心概念

8.1.1 坐标系树

8.1.2 静态变换 vs 动态变换

8.1.3 变换的数学表示

8.2 广播静态变换

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

8.2.2 在节点中广播静态变换

8.2.3 注册入口点

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

8.3.1 监听节点

8.3.2 注册入口点

8.4 构建并在 RViz2 中验证

8.4.1 构建

8.4.2 启动系统

8.4.3 在 RViz2 中配置

8.5 用 CLI 工具排查 TF

8.6 tf2 与运动学的关系

8.7 常见错误

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

8.7.2 错误二：`lookup_transform` 报 `ExtrapolationException`

8.7.3 错误三：坐标系树断链

8.8 练习题

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

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

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

本讲核心总结

参考代码

下一讲预告