第 5 章 TF2 与机器人建模

TF2 维护一棵坐标系树，让任意两个 frame 之间的空间关系在任何时刻可查询；URDF 用 XML 描述机器人的连杆与关节结构，经 robot_state_publisher 转换为 TF 发布。两者配合构成 ROS 2 中"机器人在哪里、长什么样"的统一表达。本章从 TF2 的树模型与查询接口讲起，到 URDF 骨干元素与 xacro 参数化，给出从零搭一棵完整 TF 树并让 RViz2 正确渲染的操作路径。

1. TF2 树模型与 frame 规范

TF2（Transform Framework 2）是一棵有根有向树：每个节点是一个坐标系（frame），边是一条带时间戳的刚体变换（平移 + 旋转）。约束：

• 每个 frame 至多有一个父 frame（树结构，不允许成环）；
• 一个 frame 可以有任意多个子 frame；
• 整棵树允许多个根（多机器人场景），但同一时刻同一 frame 只允许一个 broadcaster 发布。

REP 105 定义了标准 frame 命名约定：

frame 名	含义	原点	轴向
base_link	机器人本体几何参考	本体上任意固定点	x 前、y 左、z 上
base_footprint	base_link 在地面的投影	地面	同 base_link 绕 z
odom	里程计世界坐标系	启动时 base 位置	平坦地面投影
map	全局地图坐标系	地图原点	与 odom 的关系由 SLAM/AMCL 纠正
imu_link	IMU 传感器坐标系	IMU 物理中心	REP 103（x 前、y 左、z 上）
laser_link	激光雷达坐标系	雷达旋转中心	x 前方，z 向上

典型差速小车的 TF 树：

flowchart LR
    map((map)) -->|"AMCL / SLAM"| odom((odom))
    odom -->|"里程计"| base((base_link))
    base --> laser((laser_link))
    base --> cam((camera_link))
    base --> imu((imu_link))

map → odom 由定位算法（AMCL / SLAM）发布，odom → base_link 由里程计发布，base_link → 传感器 link 由 static_transform_publisher 或 URDF 模型自动发布。

2. TF 发布、查询与坐标变换

TF 数据的发布与查询通过 tf2_ros 接口完成。

发布动态变换（odom → base_link，里程计的典型用法）：

import tf2_ros
import geometry_msgs.msg

tf_broadcaster = tf2_ros.TransformBroadcaster(node)

t = geometry_msgs.msg.TransformStamped()
t.header.stamp = node.get_clock().now().to_msg()   # 时间戳必须用当前时钟
t.header.frame_id = 'odom'                          # 父 frame
t.child_frame_id = 'base_link'                      # 子 frame
t.transform.translation.x = odom_x                 # 平移（米）
t.transform.rotation = odom_quat                   # 旋转（四元数）
tf_broadcaster.sendTransform(t)

查询变换（把 laser_link 下的点转到 odom 坐标系）：

import tf2_ros
from std_msgs.msg import Header

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

## 等待变换可用（超时 1 秒）
try:
    t = tf_buffer.lookup_transform('odom', 'laser_link',
                                    rclpy.time.Time(),               # 最新时刻
                                    timeout=rclpy.duration.Duration(seconds=1.0))
except tf2_ros.LookupException as e:
    node.get_logger().warn(f'TF lookup failed: {e}')

## 将点从 laser_link 变换到 odom
p = geometry_msgs.msg.PointStamped(
    header=Header(frame_id='laser_link', stamp=node.get_clock().now().to_msg()),
    point=...,
)
p_out = tf2_geometry_msgs.do_transform_point(p, t)

查询时的 frame_id 顺序是 lookup_transform(target_frame, source_frame, time)——将 source 下的坐标转换到 target 系。颠倒参数方向是常见错误。

tf2_geometry_msgs 对标准消息提供变换函数，点与位姿共用同一套 frame 约定：

import tf2_geometry_msgs
from geometry_msgs.msg import PointStamped, PoseStamped
from std_msgs.msg import Header

## 变换一个点：frame_id 属于 header，不在 PointStamped 顶层
pt = PointStamped(header=Header(frame_id='camera_link'), point=...)
pt_odom = tf2_geometry_msgs.do_transform_point(pt, transform_stamped)

## 变换一个位姿（同时旋转平移）
pose = PoseStamped(header=Header(frame_id='camera_link'), pose=...)
pose_map = tf2_geometry_msgs.do_transform_pose(pose, transform_stamped)

do_transform_point 要求消息的 header.frame_id 与 transform 的 source frame 一致——若 pt.header.frame_id = 'camera_link'，transform 就应是 target ← camera_link 方向。

四元数与欧拉角互转不属于 tf2 核心，但 tf_transformations（随 ros-humble-tf-transformations 安装）提供 quaternion_from_euler 与 euler_from_quaternion，与 tf2 的四元数约定一致。transforms3d.quaternions.quat2axangle 得到的是轴角，不是欧拉角，不要拿来做欧拉转换。

3. static_transform_publisher

不随时间变化的固定变换（如 base_link → laser_link）用 static 发布器，只需发布一次，TF 系统缓存于 /tf_static 话题（QoS: Transient Local + Reliable）：

先 ros2 run tf2_tools view_frames 确认当前树结构，再任选一种发布方式：

命令行代码内发布

## Humble 的旗标式接口：父子帧用 --frame-id / --child-frame-id（非位置参数）
ros2 run tf2_ros static_transform_publisher \
  --x 0.1 --y 0.0 --z 0.2 \
  --qx 0 --qy 0 --qz 0 --qw 1 \
  --frame-id base_link --child-frame-id laser_link

static_pub = tf2_ros.StaticTransformBroadcaster(node)

static transform 的 QoS 是 Transient Local——新加入的节点订阅 /tf_static 时可立即收到历史数据，无需等待下一次发布（因为 static 只发一次）。

判据：静态变换用 static publisher

固定安装关系（base_link → laser_link）交给 static_transform_publisher 或 URDF 一次发布即可；每帧重发静态变换只会挤占带宽，不会更准。

4. 时间戳与 lookupTransform 缓冲

TF buffer 是一个时间序列数据——每个变换附带时间戳，buffer 保留最近一段历史的采样值。查询时机与 buffer 内数据的对齐是关键：

• rclpy.time.Time() 或 Time(seconds=0) 表示"最新可用变换"——buffer 内最新一条。
• Time.from_msg(msg.header.stamp) 按消息时间戳查询——如果该时刻不在 buffer 范围内（太早或太晚），返回外推或报错。

时间不齐是 TF 报错的首要成因：传感器消息的 header.stamp 与 TF 数据的时间戳差超过 buffer 允许范围，lookupTransform 抛 ExtrapolationException。解决方式：

1. 确保发布传感器消息时 header.stamp 使用硬件时间戳或采集时刻的系统时间；
2. 查询时给一个容许的 timeout（timeout=Duration(seconds=0.1)）让 TF 等待数据到达；
3. 检查各节点是否使用同一时钟源（use_sim_time 不一致导致时间轴错乱）。

tf_buffer.can_transform(target, source, time)（即 tf2_ros.Buffer 实例的 can_transform）可在查询前做非阻塞检查。

坑：lookupTransform 失败先查两处

目标时刻超出缓冲（默认 10 s）或时间戳来自不同时钟（一个系统时间、一个仿真时间）时查询直接失败；先对齐 use_sim_time，再放大缓冲。

5. URDF 骨干元素

URDF（Unified Robot Description Format）是 XML 描述的机器人运动学模型。核心三元素：

<?xml version="1.0"?>
<robot name="my_car">

  <!-- material：定义颜色/纹理，被 link 的 visual 引用 -->
  <material name="blue">
    <color rgba="0.2 0.4 0.8 1.0"/>
  </material>

  <!-- link：刚体，包含视觉(visual)、碰撞(collision)、惯性(inertial) -->
  <link name="base_link">
    <visual>
      <geometry><box size="0.4 0.3 0.1"/></geometry>   <!-- 视觉形状 -->
      <material name="blue"/>
    </visual>
    <collision>
      <geometry><box size="0.4 0.3 0.1"/></geometry>   <!-- 碰撞形状（可简化） -->
    </collision>
    <inertial>
      <mass value="2.0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.02" iyz="0" izz="0.03"/>
    </inertial>
  </link>

  <link name="laser_link"/>  <!-- 传感器 link 只需名字，无几何 -->

  <!-- joint：连接父子 link，定义运动学关系 -->
  <joint name="laser_joint" type="fixed">   <!-- fixed/prismatic/revolute/continuous -->
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin xyz="0.1 0.0 0.2" rpy="0 0 0"/>  <!-- 子相对父的位姿 -->
  </joint>

</robot>

joint 类型：fixed（刚性连接，URDF 中用于传感器安装位置）、revolute（有限角度旋转关节）、continuous（无限旋转，如轮子）、prismatic（直线滑动）。

与 MuJoCo MJCF 的对照：URDF 的 <link> 对应 MJCF 的 <body>（但 MJCF 的 geom 是 body 子元素）；URDF 的 <joint> 对应 MJCF 的 <joint>（MJCF 中 joint 只声明自由度，不携带 <origin>——位置由 <body pos> 决定）。

6. robot_state_publisher 与 joint_state_publisher

robot_state_publisher 读取 URDF（/robot_description 参数），订阅 /joint_states，计算每个 link 的世界坐标位姿并持续发布 TF。

<!-- launch 文件中加载 URDF 到 robot_state_publisher -->
<!-- 把文件内容读进字符串参数是 robot_state_publisher 示例 launch 文件的写法 -->
<node pkg="robot_state_publisher" exec="robot_state_publisher" name="rsp">
  <param name="robot_description"
         value="$(command 'cat $(find-pkg-share my_robot_desc)/urdf/my_car.urdf')"/>
</node>

joint_state_publisher 从 source_list 参数列出的话题收集各关节角度（未列出时发布全零位姿），汇总为一条 /joint_states 消息。滑杆界面是独立可执行文件 joint_state_publisher_gui，不是 joint_state_publisher 的参数。

## 带 GUI 滑杆（调试用，正式系统由驱动发布 joint_states）
ros2 run joint_state_publisher_gui joint_state_publisher_gui

数据流：

flowchart LR
    urdf["URDF 文件"] --> rsp["robot_state_publisher"]
    js["joint_state_publisher 或硬件驱动"] -->|"/joint_states"| rsp
    rsp -->|"/tf + /tf_static"| rviz["RViz2 RobotModel"]
    rsp -->|"/robot_description"| rviz

RViz2 中 RobotModel Display 若显示为默认灰色或不动，检查：/robot_description 参数是否存在且内容合法、/joint_states 是否在发、joint name 是否与 URDF 一致。

7. xacro 宏与参数化

URDF 原生不支持循环、条件与宏。xacro（XML Macros）在 URDF 之上增加参数化、宏定义与数学表达式：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_car">

  <!-- 属性定义——全局参数 -->
  <xacro:property name="wheel_radius" value="0.05"/>
  <xacro:property name="wheel_width"  value="0.03"/>

  <!-- 宏定义：可复用的 link+joint 组 -->
  <xacro:macro name="wheel" params="prefix y_reflect">
    <link name="${prefix}_wheel">
      <visual>
        <geometry><cylinder radius="${wheel_radius}" length="${wheel_width}"/></geometry>
      </visual>
      <collision>
        <geometry><cylinder radius="${wheel_radius}" length="${wheel_width}"/></geometry>
      </collision>
      <inertial>
        <mass value="0.1"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial>
    </link>
    <joint name="${prefix}_wheel_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_wheel"/>
      <origin xyz="0.0 ${y_reflect * 0.18} ${-0.05}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>    <!-- 旋转轴 -->
    </joint>
  </xacro:macro>

  <!-- 调用宏——左右各一个轮 -->
  <xacro:wheel prefix="left"  y_reflect="1"/>
  <xacro:wheel prefix="right" y_reflect="-1"/>

</robot>

xacro 展开为纯 URDF：

xacro my_car.urdf.xacro -o my_car.urdf
check_urdf my_car.urdf          # 检查展开后的 URDF 结构

launch 文件中直接加载 xacro（自动展开）：

from launch.substitutions import Command

robot_desc = Command(['xacro ', find_package_share('my_robot_desc') + '/urdf/my_car.urdf.xacro'])
## 传给 robot_state_publisher 的 robot_description 参数

8. mesh 文件与 package:// 路径

复杂几何体（3D 打印件、商业传感器外壳）用 mesh 文件（.dae / .stl）描述，URDF 中通过 package:// 协议引用：

<visual>
  <geometry>
    <mesh filename="package://my_robot_desc/meshes/camera.dae" scale="1 1 1"/>
  </geometry>
</visual>

package://<包名>/<相对路径> 由 RViz2 与 Gazebo 在运行时解析为文件系统的绝对路径。mesh 目录须安装在 <pkg>/share/<pkg>/meshes/ 下，否则 package:// 解析失败，RViz 显示为空白。

碰撞体简化原则：视觉用精细 mesh，碰撞用基元（box/cylinder/sphere）或低面数 mesh——物理引擎对高精度三角网格的碰撞检测性能极差。

9. check_urdf 校验与典型坑

sudo apt install -y ros-humble-urdfdom
check_urdf my_car.urdf       # 打印 link/joint 树结构；语法错误直接报行号

常见坑：

• frame 名不一致：URDF 中 <link name="laser_link">，但传感器驱动发布的消息 header.frame_id = "laser"。RViz 报 "Frame [laser] does not exist"。需让驱动与 URDF 使用完全相同的字符串。
• 树成环：两个 URDF 文件被合并时，同一 link 被两个不同 parent 声明。TF 检测到环后停止处理该子树。
• 时间不齐：/joint_states 的 header.stamp 全零（某些关节驱动未打时间戳），robot_state_publisher 无法正确插值。确认 joint_states 消息的 stamp 非零且单调递增。
• fixed joint 不发布 TF：若 URDF 中某 link 仅通过 fixed joint 连接，robot_state_publisher 在模型加载时将其一次性写入 /tf_static——后续不再更新。若需动态变换，应使用 revolute/prismatic joint 并配合 /joint_states。

10. 要点

• TF2 是一棵带时间戳的有根有向树，每个 frame 只有一个父 frame；REP 105 定义标准命名与轴向约定。
• 动态变换用 TransformBroadcaster，静态变换用 StaticTransformBroadcaster（QoS: Transient Local）；查询时参数顺序是 (target, source, time)。
• TF 报错首查时间戳对齐——use_sim_time 不一致、header.stamp 全零或超出 buffer 范围是三大成因。
• URDF 的 fixed joint 用于描述传感器安装关系，robot_state_publisher 将其自动转为 static TF；continuous/revolute 关节的 TF 随 /joint_states 更新。
• xacro 提供参数化与宏复用，launch 中用 Command substitution 直接展开——避免手工维护多份近似 URDF。
• package:// 路径由运行时解析，mesh 目录须在包的 share/ 下正确安装。

参考文献与延伸阅读

• ROS 2. About TF2. 文档
• ROS 2. URDF 教程入口. 文档
• ROS Enhancement Proposals. REP 103: Standard Units of Measure and Coordinate Conventions. 规范