TF2 坐标系变换：让机器人知道自己"在哪、朝哪"

01引例：这束激光该画在哪

02先看现象：启动案例，打印 TF 树，对照仿真画面

三步、两分钟：启动 EXP-15 的 rviz 仿真案例 → 终端里打印出 TF 树 → 对照仿真画面——本节只复制粘贴；树怎么建、五种玩法在 04 节。

本课先用实验环境里现成的 car1_description 仿真案例看现象：一条 ros2 launch car1_description display.launch.py 同时拉起三个节点——joint_state_publisher_gui（关节滑块窗）、robot_state_publisher（读 URDF，把整车坐标系发布成 TF 树——现象的"发布者"就是它）、rviz2（仿真画面）。下面每条命令都可直接复制到终端执行，命令之后紧跟"应该看到的结果"与排错提示；三步跑通后，再对照图 19-1 把现象还原成"谁发、走哪条话题、谁读"的系统逻辑：

ros2 launch car1_description display.launch.py
# [INFO] [joint_state_publisher_gui]: ...        ← 关节滑块窗弹出
# [INFO] [robot_state_publisher]: ...            ← TF 发布者已上线
# [INFO] [rviz2]: ...                            ← rviz2 主窗弹出（Grid + 小车 + TF 轴）

source /opt/ros/humble/setup.bash && source ~/ros2_ws/install/setup.bash
ros2 run tf2_tools view_frames

3对照 RViz 仿真画面看屏幕

终端里的树"字长什么样"已经见过，再看它"画长什么样"——这就是启动后桌面上该有的样子（EXP-15 图 15-1 实拍）：

✔ 三样东西逐一对上：坐标轴原点都钉在模型的关节/部件位置（树=模型结构）、帧名能和 frames.pdf 里的名字互相点名、拖任意关节滑块 → 部件转、对应坐标轴跟着动（这背后是哪条链路、静态/动态两类边怎么分工——04 节第 2、3 块拆开讲）。

三步都跑通之后，回过头看这个系统的逻辑关系——启动、发树、读树、看画，各环节在链路里怎么咬合：

03原理讲解

TF2 树：单父、无环、两条话题

建立在 EXP-06 话题与 QoS 之上——坐标系之间的"关系"也是靠两条话题发出来的。

TF2 把机器人身上所有坐标系组织成一棵树：每个坐标系最多一个父坐标系，不能成环。典型结构：

map                              ← 世界原点（SLAM 维护）
 └── odom                         ← 里程计原点（会漂移）
      └── base_link               ← 车体中心
           ├── laser_frame        ← 雷达（安装位置 = 静态变换）
           ├── camera_link        ← 相机
           └── arm_base
                └── gripper_link  ← 夹爪

意思很直白：laser_frame 的父是 base_link，base_link 的父是 odom，odom 的父是 map。任何两个坐标系之间只有一条路——所以查变换永远有唯一解。

两类角色，分工和话题的 pub/sub 一模一样——一个广播，一个监听：

一条变换里在算什么：先旋转、后平移

承接上一主场：每条边存的是"平移 + 旋转"两个量——先在二维世界里拖动它们，看子坐标系的读数怎么变成父坐标系的坐标：

一次 TF 查询的全过程

接上一主场的"监听者"——要问"雷达系里的点 (0.3, 0)，在 map 里是哪"，tf2 只干两件事：先向上爬，再向下拼，全程不用你手写矩阵：

1. 从 laser_frame 沿父指针一路向上爬，收集沿途每一段变换；
2. 从 map 也向上爬，找到两条路第一次相遇的共同祖先（汇合点）；
3. 从汇合点按下回程，该取逆的取逆、该连乘的连乘，拼出 A→B 的总变换。

旋转为什么要用四元数

承接上一主场：查询链里每一段的"旋转"在 TF 中都以四元数存储与连乘——先把三个姿态角拖出肌肉记忆，再看欧拉角为什么在 ±90° 会出事：

三种表示法对比——看 ROS2 为什么只认四元数：

半角公式（本实验只需 yaw）：机器人在地面打转，本质是绕 Z 轴转 yaw 角，四元数退化为两个非零分量：

# 绕 Z 轴旋转 yaw：四元数只有 z、w 两个分量非零
z = sin(yaw / 2)
w = cos(yaw / 2)   # x = y = 0

# yaw = 90°  ⇒  z = sin45° = 0.7071,  w = cos45° = 0.7071
# yaw = 180° ⇒  z = sin90° = 1.0,     w = cos90° = 0.0
# 反解：yaw = 2·atan2(z, w)   ← 看代码时反过来算角度就用它

在 ROS2 消息里长什么样：geometry_msgs/msg/Transform = translation(x,y,z) + rotation(x,y,z,w)；Quaternion 四个字段顺序固定为 x, y, z, w（不是 w,x,y,z！阅读时留心）。

04实验步骤

围绕 02 节那条 ros2 launch car1_description display.launch.py 分五步：① 理解 TF2 变换树概念 → ② 发布静态 TF 变换 → ③ 发布动态 TF 变换 → ④ 可视化验证 TF 树 → ⑤ 查询任意坐标系相对位姿。每步按内容选呈现方式——是命令就逐项解释怎么用，是代码就截关键段，要可视化就把画面放出来，是查询就给命令并逐行解读输出。

第 1 步 · 理解 TF2 变换树概念

树不是凭空出现的：案例里 URDF 的每个 <link> = 一个坐标系，每条 <joint> = 一条父子边；launch 把整份 URDF 塞给 robot_state_publisher，它照着 URDF 把树发进两条话题。

关键代码 · URDF 里的“树根与第一条边”（截自 car1_description/urdf/car1.urdf 开头）：

<link name="base_footprint" />                    <!-- 根坐标系：车在地面的投影点 -->
<joint name="base_joint" type="fixed">            <!-- 第一条边，静态 -->
  <origin xyz="0 0 0.029" rpy="0 0 0" />          <!-- 边上存的量：z 抬高 2.9cm -->
  <parent link="base_footprint" />                <!-- 父 -->
  <child link="base_link" />                      <!-- 子：child 只出现一次 = 单父 -->
</joint>
<joint name="wheel_lf_joint" type="continuous">   <!-- 活动边：随时在变 -->
  <parent link="base_link" /><child link="wheel_lf_link" />
  <axis xyz="0 1 0" />                            <!-- 绕 y 轴滚 -->
</joint>

关键代码 · launch 怎么给树“上电”（截自 display.launch.py 的骨架）：

with open(urdf_file, 'r') as f:
    robot_desc = f.read()                                # ① 整份 URDF 读成字符串

LaunchDescription([
    Node(package='joint_state_publisher_gui', ...        # ② 滑块窗：活动关节角的输入源
         parameters=[{'robot_description': robot_desc}]),
    Node(package='robot_state_publisher', ...            # ③ 树的发布者（本课主角）
         parameters=[{'robot_description': robot_desc}], output='screen'),
    Node(package='rviz2', arguments=['-d', rviz_config]) # ④ 消费者：读两条话题画轴
])

ros2 node list
# /joint_state_publisher_gui   ← 滑块窗（活动关节角来源）
# /robot_state_publisher       ← 树的发布者（本课主角）
# /rviz2                       ← 消费者：读 /tf、/tf_static 画坐标轴

整棵树的轮廓——02 节步骤 ② 导出的 frames.pdf 就是它的实拍版，先在这里把名字认全：

base_footprint                          # 根
└── base_link                           # base_joint · fixed（静态边）
    ├── wheel_lf/rf/lb/rb_link …        # 4×continuous（动态边）
    ├── laser_link                      # laser_joint · fixed（静态边）
    ├── imu_link                        # imu_joint · fixed
    └── arm_base_link → arm_link1…5     # revolute 链（动态边）
        └── camera… / tcp_link          # 相机链挂在 arm_link4 下

第 2 步 · 发布静态 TF 变换

案例里所有 type="fixed" 的关节就是静态边：URDF 写好、launch 一启动，robot_state_publisher 一次性发到 /tf_static。下面三条路给你——命令读出它已发布的边、命令再发一条新边、代码怎么写。

关键代码 · URDF 里两条有代表性的 fixed 边（一纯平移、一带旋转）：

<!-- 雷达安装位：纯平移，车体前方 14.6cm、高 8.7cm -->
<joint name="laser_joint" type="fixed">
  <origin xyz="0.146213088049965 0 0.0874000000000005" rpy="0 0 0" />
  <parent link="base_link" /><child link="laser_link" />
</joint>

<!-- 深度相机光学系：rpy 把“z 朝前”的车体系扭成光学约定 -->
<joint name="camera_depth_optical_joint" type="fixed">
  <origin xyz="0 0.01655 0" rpy="-1.57079633 0 -1.57079633" />
  <parent link="camera_depth_link" /><child link="camera_depth_optical_frame" />
</joint>

ros2 topic echo /tf_static --qos-durability transient_local
# transforms:                            ← 数组，每条 fixed 边一项
# - header: {frame_id: base_footprint}   ← 父
#   child_frame_id: base_link             ← 子
#   transform: {translation: {z: 0.029},
#               rotation: {w: 1.0}}      ← 数值 = URDF 里的原值
# - {frame_id: base_link, child_frame_id: laser_link, ...}

# 假想雷达：车体前方 10cm、高 20cm、姿态不转
ros2 run tf2_ros static_transform_publisher \
  --x 0.1 --y 0 --z 0.2 \
  --yaw 0 --pitch 0 --roll 0 \
  --frame-id base_link --child-frame-id laser_frame
# --x/--y/--z           平移，单位米
# --yaw/--pitch/--roll  欧拉角，单位弧度（90° = 1.5708）
# --frame-id            父坐标系；--child-frame-id 子坐标系（别写反！）
# 旧式裸位置参数已弃用，与 flag 混用直接报错退出

代码 A · 静态发布节点 StaticTfPub（代码方式）

同一条边的第三种发法（代码版）——与命令二殊途同归，逐字段含义见注释：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import StaticTransformBroadcaster

class StaticTfPub(Node):
    def __init__(self):
        super().__init__('static_tf_pub')
        # ① StaticTransformBroadcaster —— 内部自动用 /tf_static + TransientLocal
        self.pub = StaticTransformBroadcaster(self)
        self.send()

    def send(self):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id  = 'base_link'   # 父
        t.child_frame_id   = 'laser_frame'  # 子（只能有一个父！）
        t.transform.translation.x = 0.10    # 雷达在车体前方 10cm
        t.transform.translation.z = 0.20    # 高度 20cm（TF2 支持 3D）
        # 无旋转 ⇒ 单位四元数 (x=0, y=0, z=0, w=1)
        t.transform.rotation.w = 1.0
        self.pub.sendTransform(t)   # 只发这一次，后加入的订阅者也能收到

第 3 步 · 发布动态 TF 变换

案例里 continuous / revolute 关节就是动态边。链路从滑块开始：拖滑块 → /joint_states → robot_state_publisher 计算 → /tf 持续刷新，rviz 与 tf2_echo 都从这里读“活”值。

关键代码 · URDF 里两种活动关节（轮子无上限、机械臂有限位）：

<joint name="wheel_lf_joint" type="continuous">     <!-- 轮子：可无限转 -->
  <origin xyz="0.0973 0.1058 0.0195" rpy="0 0 0" />
  <parent link="base_link" /><child link="wheel_lf_link" />
  <axis xyz="0 1 0" />                              <!-- 绕 y 滚动 -->
</joint>
<joint name="arm_joint1" type="revolute">            <!-- 机械臂第一轴：正负限位 -->
  <parent link="arm_base_link" /><child link="arm_link1" />
  <axis xyz="0 0 1" />                              <!-- 绕 z 转 -->
  <limit lower="-3.14" upper="3.14" effort="100" velocity="1" />
</joint>

ros2 topic hz /tf
# average rate: 33.3xx          ← 每秒约 33 条（随滑块活动波动）
# min: …s  max: …s  window: …s  ← 窗口内最快/最慢间隔

ros2 topic echo /tf
# - header: {stamp: {sec: ...}, frame_id: base_link}  ← 时间戳持续前进
#   child_frame_id: wheel_lf_link                      ← 活动关节的子系
#   transform: {translation: {...},
#               rotation: {x, y, z, w}}               ← 数值随滑块实时变

ros2 topic echo /joint_states
# header: {stamp: {sec: ...}}              ← 拖滑块时时间戳前进
# name: [arm_joint1, wheel_lb_joint, ...]   ← 关节名列表（对上 URDF 的 joint）
# position: [0.52, -1.20, ...]              ← 各关节当前角（弧度）——拖哪个，哪个变

三个名词的对应关系——joint_state_publisher、/joint_states、TF 各扮什么角色，一次说清：

第 4 步 · 可视化验证 TF 树

同一棵树四种看法：终端导出快照（02 步骤 ② 已跑过，这里给检查点）→ RViz 直播：拖滑块看坐标轴实时转动 → rqt_tf_tree 常驻树图（⟳ 点刷新，要先 apt 装包） → 面板和树图互相对照。命令的现场你都有了，本步给的是“看什么、怎么算对”。

📸 关键操作与现象截图（2 图）

sudo apt install ros-humble-rqt-tf-tree      # ① 只需一次；不装则 ② 直接 command not found
ros2 run rqt_tf_tree rqt_tf_tree             # ② 打开常驻树窗口
# 树画出来后：拖 arm_joint1 滑块 → 点 ⟳ 刷新
#   · 树形不变 = 拓扑由 URDF 定死，拖滑块不增删边（正常！）
#   · 变的是边标签： Most recent transform 时间戳往前走、Average rate 显示在频发
# 数值平移/转角在第 5 步 tf2_echo，轴的转动在刚才的 RViz——三窗同看才完整

第 5 步 · 查询任意坐标系相对位姿

查询是 TF2 的最终出口——任意两个 frame 之间，一条命令出数值。命令行用 tf2_echo，代码里用 Buffer + lookup_transform；两种形态一个语义：参数顺序永远是（目标系·父，源系·子）。

ros2 run tf2_ros tf2_echo base_footprint base_link
# At time 1792054921.106               ← 查询时刻（默认取最新；TF 全程带时间戳）
# Translation: [0.000, 0.000, 0.029]    ← 平移，单位米 = URDF base_joint 的 z
# Rotation: in RPY (radian): [0.000, -0.000, 0.000]   ← 姿态，弧度；全 0 = 无旋转
# （还会持续打印 Quaternion x y z w 与 4×4 Matrix：左上 3×3 旋转、第四列平移）

# 查雷达安装位（静态边）
ros2 run tf2_ros tf2_echo base_link laser_link
#   Translation ≈ [0.146, 0.000, 0.087]     ← URDF laser_joint 的 xyz 原值

# 两参数对调 = 求逆：数字取反
ros2 run tf2_ros tf2_echo base_link base_footprint
#   Translation ≈ [0.000, 0.000, -0.029]    ← 第一条查询的逆

# 查活动边：拖 arm_joint1 滑块，输出实时变
ros2 run tf2_ros tf2_echo base_link arm_link1

代码 B · 查询节点 Buffer + Listener（代码方式）

from tf2_ros import Buffer, TransformListener

self.buffer = Buffer()
self.listener = TransformListener(self.buffer, self)   # 监听 /tf + /tf_static 填充 buffer

# 查询 laser_frame 的数据 → map 系；超时 1.0s（树还没齐时阻塞等待）
t = self.buffer.lookup_transform(
    'map', 'laser_frame',
    rclpy.time.Time(),      # Time()=最新时刻；传具体时间则做历史插值
    timeout=rclpy.duration.Duration(seconds=1.0))
tx = t.transform.translation.x
qz, qw = t.transform.rotation.z, t.transform.rotation.w
yaw = 2.0 * math.atan2(qz, qw)   # 03 节半角公式反解

沙盘探究：四元数换算 + TF 链查询

回到 03 节"查询全过程"与"四元数"两块主场，这里用两块沙盘动手验证：①拖动偏航角，观察四元数分量与朝向实时变化；②自选两端坐标系，看 TF2 如何拼出查询路径。

05注意事项与验收

注意事项（七条红线）

验收标准（六条全绿才算过）

• ros2 launch car1_description display.launch.py 一条命令拉起三节点，滑块窗与 rviz2 同时弹出，车模 + TF 轴可见
• tf2_echo base_footprint base_link 输出平移 [0.000, 0.000, 0.029]（与 URDF base_joint 逐位一致）、旋转零姿态
• view_frames 生成 frames.pdf，肉眼确认以 base_footprint 为根、单父、无环
• 独立终端 ros2 topic echo /tf_static --qos-durability transient_local 能读到 base_footprint→base_link 等 fixed 边
• 拖动任一关节滑块，四段链路依次对上账：ros2 topic echo /joint_states 的 position 变 → ros2 topic echo /tf 对应边数值变 → rviz 对应轴随动，RViz TF display 各 frame 标签正确且与 frames.pdf 互点得上
• 页面交互台：拖 yaw 到 180° 能说出 w=0 的原因；链查询能报出跨 3 层的拼接路径

06本课总结与验证题