ROS1/ROS2中TF坐标变换---个人学习篇

爱凤的小光2026-04-15 10:06

文章目录

  • [1.TF 变换系统概述](#1.TF 变换系统概述)
    • [1.1 为什么需要TF？](#1.1 为什么需要TF？)
    • [1.2 TF 的发展历史](#1.2 TF 的发展历史)
    • [1.3 TF 系统的核心功能](#1.3 TF 系统的核心功能)
  • 2.坐标系与变换的数学基础
    • [2.1 坐标系的定义](#2.1 坐标系的定义)
    • [2.2 平移变换](#2.2 平移变换)
    • [2.3 旋转变换](#2.3 旋转变换)
      • [2.3.1 旋转矩阵](#2.3.1 旋转矩阵)
      • [2.3.2 欧拉角](#2.3.2 欧拉角)
      • [2.3.3 四元数(Quaternion）](#2.3.3 四元数(Quaternion）)
      • [2.3.4 欧拉角与四元数相互转换](#2.3.4 欧拉角与四元数相互转换)
        • [2.3.4.1 欧拉角与四元数相互转换(ROS C++）](#2.3.4.1 欧拉角与四元数相互转换(ROS C++）)
    • [2.4 齐次变换矩阵](#2.4 齐次变换矩阵)
    • [2.5 ROS中的变换消息格式](#2.5 ROS中的变换消息格式)
  • [3.ROS TF 体系架构](#3.ROS TF 体系架构)
    • [3.1 TF树(Transform Tree)](#3.1 TF树(Transform Tree))
    • [3.2 TF 话题(Topics)](#3.2 TF 话题(Topics))
    • [3.3 TF 时间缓冲(Buffer)](#3.3 TF 时间缓冲(Buffer))
    • [3.4 广播者与监听者](#3.4 广播者与监听者)
  • 4.标准机器人坐标系约定
    • [4.1 REP-105：移动平台坐标系标准](#4.1 REP-105：移动平台坐标系标准)
      • [4.1.1 `map` --- 全局地图坐标系](#4.1.1 map — 全局地图坐标系)
      • [4.1.2 `odom` --- 里程计坐标系](#4.1.2 odom — 里程计坐标系)
      • [4.1.3 `base_link` --- 机器人底盘坐标系](#4.1.3 base_link — 机器人底盘坐标系)
      • [4.1.4 典型完整变换链：](#4.1.4 典型完整变换链：)
    • [4.2 REP-103：坐标系轴向约定](#4.2 REP-103：坐标系轴向约定)
  • [5.ROS1 TF 实践：命令与编程](#5.ROS1 TF 实践：命令与编程)
    • [5.1 命令行工具](#5.1 命令行工具)
      • [5.1.1 查看TF树(view_frames）](#5.1.1 查看TF树(view_frames）)
      • [5.1.2 实时监听变换(tf_echo)](#5.1.2 实时监听变换(tf_echo))
      • [5.1.3 静态变换发布(static_transform_publisher)](#5.1.3 静态变换发布(static_transform_publisher))
      • [5.1.4 TF监控(tf_monitor)](#5.1.4 TF监控(tf_monitor))
    • [5.2 在 Launch文件中发布静态TF](#5.2 在 Launch文件中发布静态TF)
    • [5.3 C++ 编程：发布TF变换](#5.3 C++ 编程：发布TF变换)
      • [5.3.1 广播静态变换](#5.3.1 广播静态变换)
      • [5.3.2 广播动态变换](#5.3.2 广播动态变换)
    • [5.4 C++ 编程：监听TF变换](#5.4 C++ 编程：监听TF变换)
    • [5.5 Python 编程(ROS1）](#5.5 Python 编程(ROS1）)
      • [5.5.1 Python发布静态变换](#5.5.1 Python发布静态变换)
      • [5.5.2 Python监听变换](#5.5.2 Python监听变换)
    • [5.6 欧拉角与四元数转换(ROS1)](#5.6 欧拉角与四元数转换(ROS1))
  • [6.ROS2 TF2实践：命令与编程](#6.ROS2 TF2实践：命令与编程)
    • [6.1 命令行工具](#6.1 命令行工具)
      • [6.1.1 查看TF树](#6.1.1 查看TF树)
      • [6.1.2 实时监听变换](#6.1.2 实时监听变换)
      • [6.1.3 命令行发布静态变换](#6.1.3 命令行发布静态变换)
      • [6.1.4 查看 TF 话题](#6.1.4 查看 TF 话题)
    • [6.2 C++ 编程(ROS2)](#6.2 C++ 编程(ROS2))
      • [6.2.1 发布静态变换](#6.2.1 发布静态变换)
      • [6.2.2 发布动态变换(从里程计)](#6.2.2 发布动态变换(从里程计))
      • [6.2.3 监听变换(ROS2 C++)](#6.2.3 监听变换(ROS2 C++))
    • [6.3 Python 编程(ROS2）](#6.3 Python 编程(ROS2）)
      • [6.3.1 Python发布静态变换](#6.3.1 Python发布静态变换)
      • [6.3.2 Python发布动态变换(定时器驱动）](#6.3.2 Python发布动态变换(定时器驱动）)
      • [6.3.3 Python监听变换(ROS2)](#6.3.3 Python监听变换(ROS2))
    • [6.4 ROS2 Launch文件中发布静态TF](#6.4 ROS2 Launch文件中发布静态TF)
  • 7.静态变换vs动态变换
    • [7.1 核心区别](#7.1 核心区别)
    • [7.2 静态变换的特殊性](#7.2 静态变换的特殊性)
    • [7.3 何时使用静态变换](#7.3 何时使用静态变换)
  • [8.URDF与robot_state_publisher 自动发布TF](#8.URDF与robot_state_publisher 自动发布TF)
    • [8.1 URDF中的TF关系](#8.1 URDF中的TF关系)
      • [8.1.1 典型的简单机器人URDF示例：](#8.1.1 典型的简单机器人URDF示例：)
    • [8.2 robot_state_publisher启动配置](#8.2 robot_state_publisher启动配置)
    • 8.3TF发布流程
  • [9.相机 TF 发布详解](#9.相机 TF 发布详解)
    • [9.1 两种相机坐标系](#9.1 两种相机坐标系)
      • [9.1.1 ROS 相机坐标系(camera_link）](#9.1.1 ROS 相机坐标系(camera_link）)
      • [9.1.2 光学相机坐标系(camera_optical_frame)](#9.1.2 光学相机坐标系(camera_optical_frame))
      • [9.1.3 两种坐标系之间的旋转关系](#9.1.3 两种坐标系之间的旋转关系)
    • [9.2 常见相机坐标系命名规范](#9.2 常见相机坐标系命名规范)
    • [9.3 Intel RealSense相机TF结构](#9.3 Intel RealSense相机TF结构)
    • [9.4 手动为相机发布TF(完整节点）](#9.4 手动为相机发布TF(完整节点）)
    • [9.5 URDF中定义相机TF](#9.5 URDF中定义相机TF)
    • [9.6 深度相机点云坐标系变换](#9.6 深度相机点云坐标系变换)
  • 10.调试与可视化工具
    • [10.1 view_frames:TF树可视化](#10.1 view_frames:TF树可视化)
    • [10.2 tf2_echo:实时变换监控](#10.2 tf2_echo:实时变换监控)
    • [10.3 rqt_tf_tree:图形化TF树](#10.3 rqt_tf_tree:图形化TF树)
    • [10.4 Rviz/Rviz2:3D可视化](#10.4 Rviz/Rviz2:3D可视化)
    • [10.5 命令行快速排查](#10.5 命令行快速排查)
  • [11.ROS1→ ROS2 TF迁移指南](#11.ROS1→ ROS2 TF迁移指南)
    • [11.1 API对比](#11.1 API对比)
    • [11.2 命令行工具对比](#11.2 命令行工具对比)
    • [11.3 静态变换参数顺序变化](#11.3 静态变换参数顺序变化)
    • [11.4 Python API迁移](#11.4 Python API迁移)
  • 12.常见问题与解决方案
  • 13.综合实战:多传感器TF构建
    • [13.1 场景描述](#13.1 场景描述)
    • [13.2 完整的URDF描述](#13.2 完整的URDF描述)
    • [13.3 完整的Launch文件](#13.3 完整的Launch文件)
    • [13.4 最终TF树结构](#13.4 最终TF树结构)
    • [13.5 验证TF树正确性](#13.5 验证TF树正确性)
  • [14.常用 TF 命令速查表](#14.常用 TF 命令速查表)
    • [14.1 ROS1命令](#14.1 ROS1命令)
    • [14.2 ROS2命令](#14.2 ROS2命令)
    • [14.3 常用坐标系名称参考](#14.3 常用坐标系名称参考)
    • [14.4 包依赖速查(CMakeLists.txt)](#14.4 包依赖速查(CMakeLists.txt))

1.TF 变换系统概述

1.1 为什么需要TF？

在机器人系统中，机器人身上往往安装了多种传感器：激光雷达、RGB-D 相机、IMU、超声波、GPS 等等。每种传感器都有自己的"参考坐标系"------激光雷达测到的障碍物坐标是相对于激光雷达本身的，相机拍到的目标也是相对于相机镜头中心的。

然而，我们真正关心的往往是：这个障碍物或目标相对于机器人底盘 在哪里？或者相对于世界地图在哪里？

这就是 TF(Transform Framework）要解决的核心问题：在机器人系统中，统一管理所有坐标系之间的空间变换关系，使任意一个点的坐标能够在不同坐标系之间自由转换。

复制代码 
典型场景示例：
─────────────────────────────────────────────────────
传感器输入：激光雷达检测到障碍物位于 (1.5m, 0.3m, 0m)
           → 这是相对于 laser_link 坐标系的坐标

机器人需要：障碍物在 base_link（底盘）坐标系的位置？
           障碍物在 map（地图）坐标系的绝对位置？

TF 变换链：laser_link → base_link → odom → map
─────────────────────────────────────────────────────

1.2 TF 的发展历史

版本 时期 特点
tf ROS Groovy 以前 第一代实现，功能基础，有性能瓶颈
tf2 ROS Groovy 起 重构版，性能更高，同时支持 ROS1/ROS2，推荐使用
tf2 in ROS2 ROS2 所有版本 原生支持，完全兼容 tf2 API

重要提示：tf(第一代)在 ROS Kinetic 之后已被官方标记为 deprecated（弃用），在 ROS1中也应使用tf2，在 ROS2中只有tf2。

1.3 TF 系统的核心功能

  1. 维护坐标系树：以树形结构存储所有坐标系之间的变换关系
  2. 时间序列缓存 ：按时间戳缓存最近一段时间的变换数据(默认 10 秒）
  3. 任意两坐标系查询：给定变换树，自动计算任意两个坐标系之间的变换
  4. 时间插值：在有时间戳的变换之间进行插值，处理传感器时间同步
  5. 点/姿态变换：将具体的点坐标或位姿从一个坐标系变换到另一个坐标系

2.坐标系与变换的数学基础

理解TF，需要先掌握坐标变换的数学基础。

2.1 坐标系的定义

一个三维坐标系由原点位置 和三个正交轴方向 确定。在 ROS 中，所有坐标系遵循右手定则 ：

cpp 复制代码 
右手坐标系：
  - 伸出右手，食指从X轴弯向Y轴
  - 大拇指所指方向即为Z轴正方向

ROS 标准轴向约定（ENU - East/North/Up）：
  - X 轴：向前（Forward）
  - Y 轴：向左（Left）
  - Z 轴：向上（Up）

2.2 平移变换

从坐标系 A 到坐标系 B，如果 B 的原点在 A 中的位置为 (tx, ty, tz) ，则 A 中的点 (x_A, y_A, z_A) 在 B 中的坐标为：

复制代码 
[x_B]   [x_A - tx]
[y_B] = [y_A - ty]
[z_B]   [z_A - tz]

2.3 旋转变换

旋转用旋转矩阵 R 或四元数 q 表示。

2.3.1 旋转矩阵

3×3正交矩阵，行列式为 1：

cpp 复制代码 
        [r11 r12 r13]
  R  =  [r21 r22 r23]
        [r31 r32 r33]

绕Z轴旋转θ角的旋转矩阵(偏航角/Yaw）：

cpp 复制代码 
       [cos(θ) -sin(θ)  0]
Rz  =  [sin(θ)  cos(θ)  0]
       [  0       0     1]

2.3.2 欧拉角

用三个角度描述旋转：

  • Roll（横滚角）：绕 X 轴旋转
  • Pitch（俯仰角）：绕 Y 轴旋转
  • Yaw（偏航角）：绕 Z 轴旋转

⚠️ 欧拉角的缺陷：存在"万向节死锁"问题(Gimbal Lock)，当 Pitch = ±90° 时，Roll和Yaw会重合，丢失一个自由度。因此在ROS中，姿态通常用四元数传输，只在用户接口处转换为欧拉角。

2.3.3 四元数(Quaternion）

四元数是ROS中表示旋转最常用的方式：

cpp 复制代码 
q = w + xi + yj + zk
  = (x, y, z, w)

其中：
  w = cos(θ/2)       --- 实部（标量）
  x = ax·sin(θ/2)    --- 虚部 i 分量
  y = ay·sin(θ/2)    --- 虚部 j 分量
  z = az·sin(θ/2)    --- 虚部 k 分量

(ax, ay, az) 是旋转轴的单位向量，θ 是旋转角度

单位四元数满足：x² + y² + z² + w² = 1

▲ 图：四元数旋转的几何意义------旋转轴与旋转角度θ决定了四元数的四个分量。

无旋转(单位旋转)时 ：q = (0, 0, 0, 1)，即 w=1，x=y=z=0。

2.3.4 欧拉角与四元数相互转换

cpp 复制代码 
欧拉角 → 四元数（ZYX 顺序）：

  cy = cos(yaw/2),   sy = sin(yaw/2)
  cp = cos(pitch/2), sp = sin(pitch/2)
  cr = cos(roll/2),  sr = sin(roll/2)

  q.w = cr*cp*cy + sr*sp*sy
  q.x = sr*cp*cy - cr*sp*sy
  q.y = cr*sp*cy + sr*cp*sy
  q.z = cr*cp*sy - sr*sp*cy
2.3.4.1 欧拉角与四元数相互转换(ROS C++）
cpp 复制代码 
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

// ===== 欧拉角 → 四元数 =====
double roll = 0.0, pitch = 0.0, yaw = M_PI_2; // 90°
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
// 结果：q = (0, 0, 0.707, 0.707)

// ===== 四元数 → 欧拉角 =====
tf2::Matrix3x3 rot_matrix(q);
double r, p, y_out;
rot_matrix.getRPY(r, p, y_out);
// 单位：弧度（需 × 180/π 转换为角度）

2.4 齐次变换矩阵

将旋转和平移合并到一个 4×4 矩阵中：

cpp 复制代码 
     [R  | t]   [r11 r12 r13 | tx]
T =  [---|--] = [r21 r22 r23 | ty]
     [0  | 1]   [r31 r32 r33 | tz]
                [  0   0   0 |  1]

变换链：如果A→B 的变换是 T_AB，B→C 的变换是T_BC，则 A→C 的变换：

cpp 复制代码 
T_AC = T_AB · T_BC

这就是TF系统内部计算变换链时的核心数学操作。

2.5 ROS中的变换消息格式

cpp 复制代码 
geometry_msgs/TransformStamped:
  std_msgs/Header header
  uint32 seq              # ROS1 专有
  builtin_interfaces/Time stamp
  string frame_id         # 父坐标系 ID
  string child_frame_id    # 子坐标系 ID
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

3.ROS TF 体系架构

3.1 TF树(Transform Tree)

TF的核心数据结构是一棵有向树（Directed Tree），也称为"坐标系树"。

▲ 图：典型 ROS 机器人 TF 树结构。从根节点 world 到 map、odom、base_link，再到各传感器坐标系。

cpp 复制代码 
                        world
                          │
                         map
                          │
                         odom
                          │
                       base_link
                    /     |      \
           laser_link  camera_link  imu_link
                            |
                    camera_optical_frame

关键规则：

  • 每个坐标系节点有且只有一个父节点(根节点除外）
  • 树不能形成循环(否则变换会矛盾）
  • 任意两个节点之间的路径是唯一的

3.2 TF 话题(Topics)

TF数据通过两个ROS话题传播：

话题 消息类型 用途
/tf tf2_msgs/TFMessage 动态变换(实时发布）
/tf_static tf2_msgs/TFMessage 静态变换(QoS=TRANSIENT_LOCAL，持久化)

查看话题内容：

bash 复制代码 
# ROS1
rostopic echo /tf

# ROS2
ros2 topic echo /tf
ros2 topic echo /tf_static

3.3 TF 时间缓冲(Buffer)

TF Listener 维护一个时间序列缓冲区，默认保存最近 10 秒的变换历史。这使得查询历史时刻的变换成为可能。

cpp 复制代码 
时间轴：
t=0  t=0.1  t=0.2  ... t=9.8  t=9.9  t=10.0
 │     │      │            │      │      │
[T0]  [T1]  [T2]   ...  [T98] [T99] [T100]   ← 缓冲区内
                                             ↑ 超出缓冲范围后被删除

当查询时刻t的变换时，如果 t在两个已知变换时间之间，TF会进行线性插值 （平移）和球面线性插值 SLERP（旋转四元数）。

3.4 广播者与监听者

⚠️TF Buffer 默认保存最近10秒的变换历史，支持时间插值(平移线性插值 + 旋转 SLERP)。

4.标准机器人坐标系约定

4.1 REP-105：移动平台坐标系标准

ROS官方通过 REP-105(ROS Enhancement Proposals #105)定义了移动机器人的标准坐标系：

▲ 图：ROS 移动机器人三大核心坐标系。蓝色 map 为全局地图坐标系，绿色 odom 为里程计坐标系，红色 base_link 为机器人底盘坐标系。

坐标系 发布者 特点
map --- 全局地图 SLAM (GMapping, Cartographer 等) 长期精确，可能突变（重定位时）
odom --- 里程计 驱动/里程计节点 连续平滑，但会漂移
base_link --- 底盘 里程计/驱动节点 固连在机器人底盘上

4.1.1 map --- 全局地图坐标系

  • 含义：固定在世界上的绝对坐标系，代表地图原点
  • 发布者：SLAM 系统（如 GMapping、Cartographer、SLAM Toolbox）
  • 变换 ：map → odom
  • 特点 ：
    • 是整个 TF 树的根节点
    • 坐标系可能发生突变(当地图被修正/重定位时）
    • 长期精确，但非连续

4.1.2 odom --- 里程计坐标系

  • 含义：机器人出发点为原点，通过积分里程计数据估算位置
  • 发布者：里程计节点(驱动层、轮式编码器、视觉里程计等）
  • 变换 ：odom → base_link
  • 特点 ：
    • 连续平滑，无突变
    • 会随时间累积误差（drift）
    • 短期精确，但长期漂移

4.1.3 base_link --- 机器人底盘坐标系

  • 含义：固连在机器人底盘上的坐标系，随机器人运动
  • 发布者：里程计/驱动节点
  • 特点 ：
    • 通常原点在机器人底盘中心
    • 所有传感器坐标系都以此为参考挂载

4.1.4 典型完整变换链：

cpp 复制代码 
map → odom → base_link → base_footprint → sensor_link
 ↑          ↑               ↑
SLAM     里程计         机器人模型(URDF)

4.2 REP-103：坐标系轴向约定

坐标系类型 X 轴 Y 轴 Z 轴
机器人体坐标系 向前 向左 向上
相机(ROS 风格) 向前 向左 向上
相机(光学风格) 向右 向下 向前（视线方向）
地理/NED 向北 向东 向下
地理/ENU 向东 向北 向上

注意：相机有两种坐标系约定，需要特别注意它们之间的转换(见第九章)

5.ROS1 TF 实践：命令与编程

5.1 命令行工具

5.1.1 查看TF树(view_frames）

bash 复制代码 
# 生成当前 TF 树的 PDF 可视化文件
rosrun tf view_frames

# 生成后查看（在命令运行目录会生成 frames.pdf）
evince frames.pdf
# 或
xdg-open frames.pdf

view_frames 会监听5秒钟的 /tf 数据，然后生成包含整个TF树的PDF文件，非常适合调试。

5.1.2 实时监听变换(tf_echo)

bash 复制代码 
# 格式：rosrun tf tf_echo <source_frame> <target_frame>
# 查看 base_link 相对于 map 的变换（机器人在地图中的位姿）
rosrun tf tf_echo /map /base_link

# 查看激光雷达相对于底盘的变换
rosrun tf tf_echo /base_link /laser

# 输出示例：
# At time 1620000000.123
# - Translation: [1.234, 0.567, 0.000]
# - Rotation: in Quaternion [0.000, 0.000, 0.707, 0.707]
#             in RPY (radian) [0.000, -0.000, 1.571]
#             in RPY (degree) [0.000, -0.000, 90.000]

5.1.3 静态变换发布(static_transform_publisher)

bash 复制代码 
# ROS1 格式（注意参数顺序！）：
# x y z yaw pitch roll frame_id child_frame_id period_ms

# 发布 base_link → laser 的静态变换
# 激光雷达在底盘前方 0.2m、上方 0.1m，无旋转
rosrun tf static_transform_publisher 0.2 0 0.1 0 0 0 base_link laser 100

# 或使用四元数版本：
# x y z qx qy qz qw frame_id child_frame_id period_ms
rosrun tf static_transform_publisher 0.2 0 0.1 0 0 0 1 base_link laser 100

⚠️1. ROS1 参数顺序陷阱 ：yaw pitch roll(不是 roll pitch yaw！)

2.ROS1 参数顺序：x y z yaw pitch roll frame_id child_frame_id period_ms(注意 yaw 在前！)

5.1.4 TF监控(tf_monitor)

bash 复制代码 
# 监控所有 TF 变换的发布频率和延迟
rosrun tf tf_monitor

# 监控特定两个坐标系
rosrun tf tf_monitor /base_link /laser

输出示例：

bash 复制代码 
RESULTS: for all Frames
Frames:
Frame: /laser published by unknown_publisher Average Delay: 0.00123 Max Delay: 0.00456
All Broadcasters:
Node: /robot_state_publisher 58.10 Hz, Max Delay: 0.00124

5.2 在 Launch文件中发布静态TF

xml 复制代码 
<!-- my_robot.launch -->
<launch>
  <!-- 方法一：直接在 launch 文件中发布静态变换 -->
  <node pkg="tf" type="static_transform_publisher"
        name="base_to_laser_broadcaster"
        args="0.2 0.0 0.1 0 0 0 base_link laser 100" />

  <!-- 方法二（推荐）：使用 tf2 的 static_transform_publisher -->
  <node pkg="tf2_ros" type="static_transform_publisher"
        name="base_to_camera_broadcaster"
        args="0.1 0.0 0.2 0 0 0 1 base_link camera_link" />

  <!-- 发布多个静态变换 -->
  <node pkg="tf2_ros" type="static_transform_publisher"
        name="camera_to_optical_broadcaster"
        args="0 0 0 -1.5707963 0 -1.5707963 camera_link camera_optical_frame" />
</launch>

5.3 C++ 编程：发布TF变换

5.3.1 广播静态变换

cpp 复制代码 
#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "static_tf_broadcaster");
    ros::NodeHandle nh;

    // 创建静态变换广播器
    static tf2_ros::StaticTransformBroadcaster static_broadcaster;

    geometry_msgs::TransformStamped static_tf;

    // 设置时间戳和坐标系名称
    static_tf.header.stamp = ros::Time::now();
    static_tf.header.frame_id = "base_link";    // 父坐标系
    static_tf.child_frame_id = "laser_link";    // 子坐标系

    // 设置平移（单位：米）
    static_tf.transform.translation.x = 0.2;
    static_tf.transform.translation.y = 0.0;
    static_tf.transform.translation.z = 0.1;

    // 设置旋转（四元数）
    tf2::Quaternion q;
    q.setRPY(0, 0, 0);  // roll, pitch, yaw（弧度）
    static_tf.transform.rotation.x = q.x();
    static_tf.transform.rotation.y = q.y();
    static_tf.transform.rotation.z = q.z();
    static_tf.transform.rotation.w = q.w();

    // 发布（只发一次，持续有效）
    static_broadcaster.sendTransform(static_tf);

    ROS_INFO("Static TF published: base_link -> laser_link");
    ros::spin();
    return 0;
}

5.3.2 广播动态变换

cpp 复制代码 
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <nav_msgs/Odometry.h>

class OdomTFBroadcaster {
public:
    OdomTFBroadcaster() : nh_() {
        odom_sub_ = nh_.subscribe("odom", 10,
            &OdomTFBroadcaster::odomCallback, this);
    }

    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
        geometry_msgs::TransformStamped tf_msg;

        // 从里程计消息填充 TF
        tf_msg.header.stamp = msg->header.stamp;
        tf_msg.header.frame_id = "odom";
        tf_msg.child_frame_id = "base_link";

        // 平移：从里程计位置获取
        tf_msg.transform.translation.x = msg->pose.pose.position.x;
        tf_msg.transform.translation.y = msg->pose.pose.position.y;
        tf_msg.transform.translation.z = msg->pose.pose.position.z;

        // 旋转：直接使用里程计的四元数
        tf_msg.transform.rotation = msg->pose.pose.orientation;

        // 发布变换
        br_.sendTransform(tf_msg);
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber odom_sub_;
    tf2_ros::TransformBroadcaster br_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "odom_tf_broadcaster");
    OdomTFBroadcaster broadcaster;
    ros::spin();
    return 0;
}

5.4 C++ 编程：监听TF变换

cpp 复制代码 
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

class TFListenerNode {
public:
    TFListenerNode() : nh_(), tf_buffer_(), tf_listener_(tf_buffer_) {
        timer_ = nh_.createTimer(ros::Duration(1.0),
            &TFListenerNode::timerCallback, this);
    }

    void timerCallback(const ros::TimerEvent&) {
        // 方法一：查询变换
        try {
            geometry_msgs::TransformStamped transform =
                tf_buffer_.lookupTransform(
                    "base_link",   // 目标坐标系（to）
                    "laser_link",  // 源坐标系（from）
                    ros::Time(0),  // 查询最新时刻的变换
                    ros::Duration(1.0) // 等待超时时间
                );

            ROS_INFO("laser_link in base_link: (%.3f, %.3f, %.3f)",
                transform.transform.translation.x,
                transform.transform.translation.y,
                transform.transform.translation.z);

        } catch (tf2::TransformException& ex) {
            ROS_WARN("TF lookup failed: %s", ex.what());
        }

        // 方法二：变换一个具体的点
        geometry_msgs::PointStamped point_in_laser;
        point_in_laser.header.frame_id = "laser_link";
        point_in_laser.header.stamp = ros::Time(0);
        point_in_laser.point.x = 1.5;
        point_in_laser.point.y = 0.0;
        point_in_laser.point.z = 0.0;

        try {
            geometry_msgs::PointStamped point_in_base;
            tf_buffer_.transform(point_in_laser, point_in_base, "base_link");

            ROS_INFO("Point in base_link: (%.3f, %.3f, %.3f)",
                point_in_base.point.x,
                point_in_base.point.y,
                point_in_base.point.z);
        } catch (tf2::TransformException& ex) {
            ROS_WARN("Point transform failed: %s", ex.what());
        }
    }

private:
    ros::NodeHandle nh_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    ros::Timer timer_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "tf_listener_demo");
    TFListenerNode node;
    ros::spin();
    return 0;
}

5.5 Python 编程(ROS1）

5.5.1 Python发布静态变换

python 复制代码 
#!/usr/bin/env python
import rospy
import tf2_ros
import geometry_msgs.msg
import tf.transformations

def publish_static_tf():
    rospy.init_node('static_tf_publisher')

    broadcaster = tf2_ros.StaticTransformBroadcaster()
    static_tf = geometry_msgs.msg.TransformStamped()

    static_tf.header.stamp = rospy.Time.now()
    static_tf.header.frame_id = "base_link"
    static_tf.child_frame_id = "camera_link"

    # 设置平移：相机在底盘前方 0.1m，上方 0.2m
    static_tf.transform.translation.x = 0.1
    static_tf.transform.translation.y = 0.0
    static_tf.transform.translation.z = 0.2

    # 使用 tf.transformations 将欧拉角转为四元数
    quat = tf.transformations.quaternion_from_euler(0, 0, 0)  # roll, pitch, yaw
    static_tf.transform.rotation.x = quat[0]
    static_tf.transform.rotation.y = quat[1]
    static_tf.transform.rotation.z = quat[2]
    static_tf.transform.rotation.w = quat[3]

    broadcaster.sendTransform(static_tf)
    rospy.loginfo("Static TF: base_link -> camera_link published")
    rospy.spin()

if __name__ == '__main__':
    publish_static_tf()

5.5.2 Python监听变换

python 复制代码 
#!/usr/bin/env python
import rospy
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import PointStamped

def main():
    rospy.init_node('tf_listener_py')

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

    rate = rospy.Rate(1.0)  # 1 Hz

    while not rospy.is_shutdown():
        try:
            # 等待变换可用
            tf_buffer.can_transform('base_link', 'camera_link',
                                    rospy.Time(0), rospy.Duration(1.0))

            # 查询变换
            trans = tf_buffer.lookup_transform(
                'base_link',    # 目标坐标系
                'camera_link',  # 源坐标系
                rospy.Time(0)   # 最新时刻
            )

            rospy.loginfo("camera_link in base_link:")
            rospy.loginfo("  Translation: (%.3f, %.3f, %.3f)",
                trans.transform.translation.x,
                trans.transform.translation.y,
                trans.transform.translation.z)
            rospy.loginfo("  Rotation (quat): (%.3f, %.3f, %.3f, %.3f)",
                trans.transform.rotation.x,
                trans.transform.rotation.y,
                trans.transform.rotation.z,
                trans.transform.rotation.w)

        except (tf2_ros.LookupException,
                tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException) as e:
            rospy.logwarn("TF lookup failed: %s", str(e))

        rate.sleep()

if __name__ == '__main__':
    main()

5.6 欧拉角与四元数转换(ROS1)

cpp 复制代码 
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

// ===== 欧拉角 → 四元数 =====
double roll  = 0.0;
double pitch = 0.0;
double yaw   = M_PI_2;  // 90度

tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
// 结果：q = (0, 0, 0.707, 0.707)

printf("Quaternion: (%.4f, %.4f, %.4f, %.4f)\n",
    q.x(), q.y(), q.z(), q.w());

// ===== 四元数 → 欧拉角 =====
tf2::Matrix3x3 rot_matrix(q);
double r, p, y;
rot_matrix.getRPY(r, p, y);

printf("RPY: (%.4f, %.4f, %.4f) rad\n", r, p, y);
printf("RPY: (%.2f, %.2f, %.2f) deg\n",
    r * 180/M_PI, p * 180/M_PI, y * 180/M_PI);

6.ROS2 TF2实践：命令与编程

6.1 命令行工具

6.1.1 查看TF树

bash 复制代码 
# 生成当前 TF 树 PDF（运行后等待几秒自动保存）
ros2 run tf2_tools view_frames

# 查看生成的文件（当前目录下的 frames_XXXX.pdf）
ls *.pdf

6.1.2 实时监听变换

bash 复制代码 
# 格式：ros2 run tf2_ros tf2_echo <source_frame> <target_frame>
# 查看 camera_link 在 base_link 中的变换
ros2 run tf2_ros tf2_echo camera_link base_link

# 输出示例：
# At time 1713000000.000000000
# - Translation: [0.100, 0.000, 0.200]
# - Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
#             in RPY (radian) [0.000, 0.000, 0.000]
#             in RPY (degree) [0.000, 0.000, 0.000]

6.1.3 命令行发布静态变换

bash 复制代码 
# ROS2 格式（参数顺序与 ROS1 不同！）
# ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
# 注意：ROS2 中参数是 x y z yaw pitch roll（弧度）

# 例：发布 base_link → lidar_link 的静态变换
ros2 run tf2_ros static_transform_publisher \
    0.0 0.0 0.3 0 0 0 base_link lidar_link

# 例：相机在前方 0.1m，上方 0.2m，绕 Z 轴旋转 90°（π/2）
ros2 run tf2_ros static_transform_publisher \
    0.1 0.0 0.2 1.5707963 0 0 base_link camera_link

6.1.4 查看 TF 话题

bash 复制代码 
# 查看动态变换话题
ros2 topic echo /tf

# 查看静态变换话题
ros2 topic echo /tf_static

# 查看发布频率
ros2 topic hz /tf

6.2 C++ 编程(ROS2)

6.2.1 发布静态变换

cpp 复制代码 
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>

class StaticTFPublisher : public rclcpp::Node {
public:
    StaticTFPublisher() : Node("static_tf_publisher") {
        tf_static_broadcaster_ =
            std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

        // 发布 base_link → laser_link
        publishStaticTransform(
            "base_link", "laser_link",
            0.2, 0.0, 0.1,  // x, y, z
            0.0, 0.0, 0.0   // roll, pitch, yaw
        );
    }

private:
    void publishStaticTransform(
        const std::string& parent, const std::string& child,
        double tx, double ty, double tz,
        double roll, double pitch, double yaw)
    {
        geometry_msgs::msg::TransformStamped t;
        t.header.stamp = this->get_clock()->now();
        t.header.frame_id = parent;
        t.child_frame_id = child;

        t.transform.translation.x = tx;
        t.transform.translation.y = ty;
        t.transform.translation.z = tz;

        tf2::Quaternion q;
        q.setRPY(roll, pitch, yaw);
        t.transform.rotation.x = q.x();
        t.transform.rotation.y = q.y();
        t.transform.rotation.z = q.z();
        t.transform.rotation.w = q.w();

        tf_static_broadcaster_->sendTransform(t);

        RCLCPP_INFO(this->get_logger(),
            "Static TF published: %s -> %s",
            parent.c_str(), child.c_str());
    }

    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<StaticTFPublisher>());
    rclcpp::shutdown();
    return 0;
}

6.2.2 发布动态变换(从里程计)

cpp 复制代码 
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2/LinearMath/Quaternion.h>

class OdomTFBroadcaster : public rclcpp::Node {
public:
    OdomTFBroadcaster() : Node("odom_tf_broadcaster") {
        tf_broadcaster_ =
            std::make_shared<tf2_ros::TransformBroadcaster>(this);

        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "odom", 10,
            std::bind(&OdomTFBroadcaster::odomCallback, this,
                      std::placeholders::_1));
    }

private:
    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
        geometry_msgs::msg::TransformStamped t;
        t.header.stamp = msg->header.stamp;
        t.header.frame_id = "odom";
        t.child_frame_id = "base_link";

        // 从里程计消息提取位置和姿态
        t.transform.translation.x = msg->pose.pose.position.x;
        t.transform.translation.y = msg->pose.pose.position.y;
        t.transform.translation.z = msg->pose.pose.position.z;
        t.transform.rotation = msg->pose.pose.orientation;

        tf_broadcaster_->sendTransform(t);
    }

    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<OdomTFBroadcaster>());
    rclcpp::shutdown();
    return 0;
}

6.2.3 监听变换(ROS2 C++)

cpp 复制代码 
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

class TFListenerNode : public rclcpp::Node {
public:
    TFListenerNode() : Node("tf_listener_node") {
        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(
            *tf_buffer_, this);

        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&TFListenerNode::timerCallback, this));
    }

private:
    void timerCallback() {
        // 查询变换
        try {
            geometry_msgs::msg::TransformStamped t =
                tf_buffer_->lookupTransform(
                    "base_link",               // target frame
                    "camera_link",             // source frame
                    tf2::TimePointZero         // latest available
                );

            RCLCPP_INFO(this->get_logger(),
                "camera_link in base_link: (%.3f, %.3f, %.3f)",
                t.transform.translation.x,
                t.transform.translation.y,
                t.transform.translation.z);

        } catch (const tf2::TransformException& ex) {
            RCLCPP_WARN(this->get_logger(),
                "TF lookup failed: %s", ex.what());
        }

        // 变换一个点
        geometry_msgs::msg::PointStamped pt_in;
        pt_in.header.frame_id = "camera_link";
        pt_in.header.stamp = this->get_clock()->now();
        pt_in.point.x = 0.5;
        pt_in.point.y = 0.0;
        pt_in.point.z = 1.0;

        try {
            geometry_msgs::msg::PointStamped pt_out;
            tf_buffer_->transform(pt_in, pt_out, "base_link");
            RCLCPP_INFO(this->get_logger(),
                "Point in base_link: (%.3f, %.3f, %.3f)",
                pt_out.point.x, pt_out.point.y, pt_out.point.z);
        } catch (const tf2::TransformException& ex) {
            RCLCPP_WARN(this->get_logger(),
                "Point transform failed: %s", ex.what());
        }
    }

    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TFListenerNode>());
    rclcpp::shutdown();
    return 0;
}

6.3 Python 编程(ROS2）

6.3.1 Python发布静态变换

python 复制代码 
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations  # pip install tf-transformations
import math

class StaticTFPublisher(Node):
    def __init__(self):
        super().__init__('static_tf_publisher')
        self.broadcaster = StaticTransformBroadcaster(self)
        self.publish_transforms()

    def publish_transforms(self):
        # 1. 发布 base_link → laser_link
        t1 = TransformStamped()
        t1.header.stamp = self.get_clock().now().to_msg()
        t1.header.frame_id = 'base_link'
        t1.child_frame_id = 'laser_link'
        t1.transform.translation.x = 0.0
        t1.transform.translation.y = 0.0
        t1.transform.translation.z = 0.3
        t1.transform.rotation.x = 0.0
        t1.transform.rotation.y = 0.0
        t1.transform.rotation.z = 0.0
        t1.transform.rotation.w = 1.0

        # 2. 发布 base_link → camera_link
        # 相机在前方 0.1m，上方 0.2m，俯角 15°（pitch = -15° = -π/12）
        t2 = TransformStamped()
        t2.header.stamp = self.get_clock().now().to_msg()
        t2.header.frame_id = 'base_link'
        t2.child_frame_id = 'camera_link'
        t2.transform.translation.x = 0.1
        t2.transform.translation.y = 0.0
        t2.transform.translation.z = 0.2
        # 俯角 15 度
        quat = tf_transformations.quaternion_from_euler(
            0, -math.pi/12, 0)  # roll, pitch, yaw
        t2.transform.rotation.x = quat[0]
        t2.transform.rotation.y = quat[1]
        t2.transform.rotation.z = quat[2]
        t2.transform.rotation.w = quat[3]

        # 同时发布多个静态变换
        self.broadcaster.sendTransform([t1, t2])
        self.get_logger().info('Static TFs published!')

def main():
    rclpy.init()
    node = StaticTFPublisher()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

6.3.2 Python发布动态变换(定时器驱动）

python 复制代码 
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
import math

class DynamicTFPublisher(Node):
    """模拟一个绕圆周运动的坐标系"""

    def __init__(self):
        super().__init__('dynamic_tf_publisher')
        self.broadcaster = TransformBroadcaster(self)
        self.timer = self.create_timer(0.05, self.publish_tf)  # 20Hz
        self.t = 0.0

    def publish_tf(self):
        self.t += 0.05  # 时间步进

        transform = TransformStamped()
        transform.header.stamp = self.get_clock().now().to_msg()
        transform.header.frame_id = 'odom'
        transform.child_frame_id = 'base_link'

        # 圆周运动轨迹：半径 1m，角速度 0.5 rad/s
        radius = 1.0
        omega = 0.5
        transform.transform.translation.x = radius * math.cos(omega * self.t)
        transform.transform.translation.y = radius * math.sin(omega * self.t)
        transform.transform.translation.z = 0.0

        # 朝向切线方向
        yaw = omega * self.t + math.pi / 2
        transform.transform.rotation.z = math.sin(yaw / 2)
        transform.transform.rotation.w = math.cos(yaw / 2)

        self.broadcaster.sendTransform(transform)

def main():
    rclpy.init()
    node = DynamicTFPublisher()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

6.3.3 Python监听变换(ROS2)

python 复制代码 
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener, TransformException
from geometry_msgs.msg import PointStamped
import tf2_geometry_msgs  # 必须导入此模块才能使用 do_transform_point

class TFListenerPy(Node):
    def __init__(self):
        super().__init__('tf_listener_py')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        # 查询变换
        try:
            now = rclpy.time.Time()
            t = self.tf_buffer.lookup_transform(
                'base_link',   # 目标坐标系
                'laser_link',  # 源坐标系
                now
            )
            self.get_logger().info(
                f'laser_link in base_link: '
                f'({t.transform.translation.x:.3f}, '
                f'{t.transform.translation.y:.3f}, '
                f'{t.transform.translation.z:.3f})'
            )
        except TransformException as ex:
            self.get_logger().warn(f'Cannot get transform: {ex}')
            return

        # 变换一个点
        pt = PointStamped()
        pt.header.frame_id = 'laser_link'
        pt.header.stamp = self.get_clock().now().to_msg()
        pt.point.x = 2.0
        pt.point.y = 0.0
        pt.point.z = 0.0

        try:
            pt_base = self.tf_buffer.transform(pt, 'base_link')
            self.get_logger().info(
                f'Point in base_link: '
                f'({pt_base.point.x:.3f}, '
                f'{pt_base.point.y:.3f}, '
                f'{pt_base.point.z:.3f})'
            )
        except TransformException as ex:
            self.get_logger().warn(f'Transform point failed: {ex}')

def main():
    rclpy.init()
    node = TFListenerPy()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

6.4 ROS2 Launch文件中发布静态TF

python 复制代码 
# my_robot.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # 方法一：直接在 launch 文件中发布静态 TF
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='base_to_laser_tf',
            arguments=[
                '0', '0', '0.3',  # x y z
                '0', '0', '0',    # yaw pitch roll
                'base_link', 'laser_link'
            ]
        ),

        # 方法二：发布相机 TF
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='base_to_camera_tf',
            arguments=[
                '0.1', '0', '0.2',  # x y z
                '0', '0', '0',      # yaw pitch roll
                'base_link', 'camera_link'
            ]
        ),

        # 方法三：相机 ROS 坐标系到光学坐标系的变换
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='camera_to_optical_tf',
            arguments=[
                '0', '0', '0',
                '-1.5707963', '0', '-1.5707963',  # yaw pitch roll
                'camera_link', 'camera_optical_frame'
            ]
        ),
    ])

7.静态变换vs动态变换

7.1 核心区别

对比维度 静态变换 动态变换
适用场景 固定安装的传感器、机器人刚性连接 移动关节、里程计、随时间变化的位置
发布频率 只发一次（或重启时） 持续发布（通常 10~100 Hz）
话题 /tf_static /tf
QoS TRANSIENT_LOCAL（新订阅者可获得历史消息） VOLATILE（只接收最新消息）
广播器类 StaticTransformBroadcaster TransformBroadcaster

7.2 静态变换的特殊性

/tf_static 话题使用TRANSIENT_LOCAL QoS，意味着新加入的订阅者可以立即获得之前发布的静态变换，不需要等待下一次发布。这非常重要：

cpp 复制代码 
场景：机器人已运行5分钟后，Rviz2 刚启动

/tf_static（TRANSIENT_LOCAL）：
  Rviz2 连接后立即收到所有静态TF → 立即显示坐标系 ✅

/tf（VOLATILE）：
  Rviz2 必须等待下一条动态TF消息 → 最多等待1帧时间 ⚠️

7.3 何时使用静态变换

应使用静态变换的情况：

  • 相机固定安装在机器人上（base_link → camera_link）
  • 激光雷达固定安装（base_link → laser_link）
  • IMU固定安装（base_link → imu_link）
  • URDF中固定关节（<joint type="fixed">）

应使用动态变换的情况：

  • 机器人里程计（odom → base_link）
  • SLAM 定位（map → odom）
  • 机械臂关节运动（link1 → link2，旋转关节）
  • 移动的夹具或工具

8.URDF与robot_state_publisher 自动发布TF

8.1 URDF中的TF关系

URDF(Unified Robot Description Format)文件描述了机器人的结构，robot_state_publisher 节点读取URDF并自动发布机器人各关节的 TF 变换。

8.1.1 典型的简单机器人URDF示例：

xml 复制代码 
<?xml version="1.0"?>
<robot name="my_robot">

  <!-- 底盘 -->
  <link name="base_link">
    <visual>
      <geometry><box size="0.5 0.3 0.1"/></geometry>
    </visual>
    <collision>
      <geometry><box size="0.5 0.3 0.1"/></geometry>
    </collision>
  </link>

  <!-- 激光雷达 Link -->
  <link name="laser_link">
    <visual>
      <geometry><cylinder radius="0.05" length="0.05"/></geometry>
    </visual>
  </link>

  <!-- 激光雷达与底盘的固定关节（生成静态 TF） -->
  <joint name="base_to_laser" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin xyz="0.0 0.0 0.3" rpy="0 0 0"/>
    <!-- xyz: 相对父坐标系的平移，rpy: roll pitch yaw 旋转 -->
  </joint>

  <!-- 相机 Link -->
  <link name="camera_link">
    <visual>
      <geometry><box size="0.04 0.1 0.04"/></geometry>
    </visual>
  </link>

  <!-- 相机光学坐标系（x右y下z前） -->
  <link name="camera_optical_frame"/>

  <!-- 相机与底盘的固定关节 -->
  <joint name="base_to_camera" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin xyz="0.1 0.0 0.2" rpy="0 0 0"/>
  </joint>

  <!-- 相机 ROS 坐标系到光学坐标系的变换 -->
  <!-- 注意：这个旋转将 ROS 约定(x前y左z上) 转为 光学约定(x右y下z前) -->
  <joint name="camera_to_optical" type="fixed">
    <parent link="camera_link"/>
    <child link="camera_optical_frame"/>
    <origin xyz="0 0 0" rpy="-1.5707963 0 -1.5707963"/>
  </joint>

  <!-- 可旋转关节示例（生成动态 TF，需要 joint_state 输入） -->
  <link name="wheel_left_link"/>

  <joint name="wheel_left_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_left_link"/>
    <origin xyz="0.0 0.15 -0.05" rpy="-1.5707963 0 0"/>
    <axis xyz="0 0 1"/>  <!-- 旋转轴 -->
  </joint>

</robot>

8.2 robot_state_publisher启动配置

python 复制代码 
# launch/display.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacro

def generate_launch_description():
    pkg_dir = get_package_share_directory('my_robot_description')

    # 处理 URDF/XACRO 文件
    xacro_file = os.path.join(pkg_dir, 'urdf', 'my_robot.urdf.xacro')
    robot_description = xacro.process_file(xacro_file).toxml()

    return LaunchDescription([
        # robot_state_publisher：读取 URDF，发布 TF
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            parameters=[{
                'robot_description': robot_description,
                'publish_frequency': 50.0,  # 发布频率 Hz
            }]
        ),

        # joint_state_publisher_gui：提供关节状态（用于调试）
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            name='joint_state_publisher_gui',
        ),

        # Rviz2 可视化
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
        ),
    ])

8.3TF发布流程

当使用URDF+ robot_state_publisher时，TF的发布机制如下：

cpp 复制代码 
URDF 文件
    │
    ↓ 解析
robot_state_publisher
    │
    ├─ 固定关节（fixed joints）
    │      → 直接发布到 /tf_static
    │
    └─ 活动关节（revolute/prismatic joints）
           ← 接收来自 /joint_states 的关节角度
           → 计算 TF 并发布到 /tf

9.相机 TF 发布详解

相机的 TF 发布是实际项目中最常遇到的场景之一，也是最容易出错的地方，因为相机存在两种不同的坐标系约定。

9.1 两种相机坐标系

相机有两种坐标系约定，这是最容易混淆的地方：

▲ 图：左侧 camera_link 遵循 ROS 机器人约定（X前Y左Z上），右侧 camera_optical_frame 遵循计算机视觉约定(X右Y下Z前)。两者之间需要旋转变换。

💡 两者的旋转关系:从camera_link变换到camera_optical_frame需要RPY = (-π/2, 0, -π/2)。

9.1.1 ROS 相机坐标系(camera_link）

遵循机器人标准约定(右手坐标系)：

cpp 复制代码 
         Z（向上）
         ↑
         │
         │
         └──────→ X（向前，相机镜头朝向）
        /
       /
      Y（向左）

9.1.2 光学相机坐标系(camera_optical_frame)

遵循计算机视觉/图像处理的标准(Z轴朝外）：

cpp 复制代码 
  ┌──────────────┐
  │              │
  │   相机镜头    │ → Z（视线方向/光轴方向）
  │              │
  └──────────────┘
  
  X 轴 → 向右（图像水平正方向）
  Y 轴 ↓ 向下（图像垂直正方向）
  Z 轴 → 向前（视线方向/深度方向）

9.1.3 两种坐标系之间的旋转关系

从 camera_link(ROS)变换到 camera_optical_frame(光学)需要：

  • 绕 Z 轴旋转 -90°（将 X-前 旋转到 Z-前）
  • 再绕 X 轴旋转 -90°（将 Z-上 变为 Y-下）

用 RPY 表示为：roll=-π/2, pitch=0, yaw=-π/2

bash 复制代码 
# 发布 camera_link → camera_optical_frame 的 TF
# ROS1
rosrun tf static_transform_publisher 0 0 0 -1.5707963 0 -1.5707963 \
    camera_link camera_optical_frame 100

# ROS2
ros2 run tf2_ros static_transform_publisher \
    0 0 0 -1.5707963 0 -1.5707963 \
    camera_link camera_optical_frame

9.2 常见相机坐标系命名规范

坐标系名称 描述
camera_link 相机物理位置，ROS 约定轴向
camera_optical_frame / camera_color_optical_frame 彩色相机光学坐标系（x右y下z前）
camera_depth_frame 深度相机坐标系
camera_depth_optical_frame 深度相机光学坐标系
camera_infra1_frame 红外相机1坐标系
camera_aligned_depth_to_color_frame 深度对齐到彩色的坐标系

9.3 Intel RealSense相机TF结构

Intel RealSense D435i等深度相机的完整TF树示例：

cpp 复制代码 
base_link
    └── camera_link
             ├── camera_color_frame
             │       └── camera_color_optical_frame
             ├── camera_depth_frame
             │       └── camera_depth_optical_frame
             ├── camera_infra1_frame
             │       └── camera_infra1_optical_frame
             ├── camera_infra2_frame
             │       └── camera_infra2_optical_frame
             └── camera_imu_optical_frame

在 ROS2中启动RealSense时，realsense2_camera节点会自动发布这些 TF：

bash 复制代码 
# 安装 RealSense ROS2 包
sudo apt install ros-humble-realsense2-camera

# 启动相机节点（自动发布 TF）
ros2 launch realsense2_camera rs_launch.py

# 查看生成的 TF 树
ros2 run tf2_tools view_frames

9.4 手动为相机发布TF(完整节点）

以下是一个完整的ROS2节点，演示如何为相机发布完整的TF链：

python 复制代码 
#!/usr/bin/env python3
"""
camera_tf_publisher.py
为相机发布完整的 TF 变换链：
  base_link → camera_link → camera_color_optical_frame
                           → camera_depth_optical_frame
"""
import rclpy
from rclpy.node import Node
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations
import math

class CameraTFPublisher(Node):
    def __init__(self):
        super().__init__('camera_tf_publisher')

        # 参数声明（可通过 launch 文件或命令行覆盖）
        self.declare_parameter('camera_x', 0.1)     # 相机在底盘前方 x 距离
        self.declare_parameter('camera_y', 0.0)     # 相机在底盘 y 偏移
        self.declare_parameter('camera_z', 0.2)     # 相机在底盘上方 z 高度
        self.declare_parameter('camera_roll', 0.0)
        self.declare_parameter('camera_pitch', 0.0) # 俯角（负值向下看）
        self.declare_parameter('camera_yaw', 0.0)
        self.declare_parameter('base_frame', 'base_link')
        self.declare_parameter('camera_frame', 'camera_link')

        # 读取参数
        cx = self.get_parameter('camera_x').value
        cy = self.get_parameter('camera_y').value
        cz = self.get_parameter('camera_z').value
        cr = self.get_parameter('camera_roll').value
        cp = self.get_parameter('camera_pitch').value
        cyw = self.get_parameter('camera_yaw').value
        base_frame = self.get_parameter('base_frame').value
        camera_frame = self.get_parameter('camera_frame').value

        self.static_broadcaster = StaticTransformBroadcaster(self)

        transforms = []

        # 1. base_link → camera_link（机器人安装位置）
        t_base_to_camera = TransformStamped()
        t_base_to_camera.header.stamp = self.get_clock().now().to_msg()
        t_base_to_camera.header.frame_id = base_frame
        t_base_to_camera.child_frame_id = camera_frame
        t_base_to_camera.transform.translation.x = cx
        t_base_to_camera.transform.translation.y = cy
        t_base_to_camera.transform.translation.z = cz
        quat = tf_transformations.quaternion_from_euler(cr, cp, cyw)
        t_base_to_camera.transform.rotation.x = quat[0]
        t_base_to_camera.transform.rotation.y = quat[1]
        t_base_to_camera.transform.rotation.z = quat[2]
        t_base_to_camera.transform.rotation.w = quat[3]
        transforms.append(t_base_to_camera)

        # 2. camera_link → camera_color_optical_frame（ROS坐标系→光学坐标系）
        # 旋转：-90° around Z, then -90° around X
        t_camera_to_optical = TransformStamped()
        t_camera_to_optical.header.stamp = self.get_clock().now().to_msg()
        t_camera_to_optical.header.frame_id = camera_frame
        t_camera_to_optical.child_frame_id = 'camera_color_optical_frame'
        t_camera_to_optical.transform.translation.x = 0.0
        t_camera_to_optical.transform.translation.y = 0.0
        t_camera_to_optical.transform.translation.z = 0.0
        # RPY = (-π/2, 0, -π/2) 将 ROS 坐标系转为光学坐标系
        quat_optical = tf_transformations.quaternion_from_euler(
            -math.pi/2, 0, -math.pi/2)
        t_camera_to_optical.transform.rotation.x = quat_optical[0]
        t_camera_to_optical.transform.rotation.y = quat_optical[1]
        t_camera_to_optical.transform.rotation.z = quat_optical[2]
        t_camera_to_optical.transform.rotation.w = quat_optical[3]
        transforms.append(t_camera_to_optical)

        # 3. camera_link → camera_depth_optical_frame（深度相机，假设与彩色共轴）
        t_camera_to_depth = TransformStamped()
        t_camera_to_depth.header.stamp = self.get_clock().now().to_msg()
        t_camera_to_depth.header.frame_id = camera_frame
        t_camera_to_depth.child_frame_id = 'camera_depth_optical_frame'
        t_camera_to_depth.transform.translation.x = 0.0
        t_camera_to_depth.transform.translation.y = 0.0
        t_camera_to_depth.transform.translation.z = 0.0
        t_camera_to_depth.transform.rotation.x = quat_optical[0]
        t_camera_to_depth.transform.rotation.y = quat_optical[1]
        t_camera_to_depth.transform.rotation.z = quat_optical[2]
        t_camera_to_depth.transform.rotation.w = quat_optical[3]
        transforms.append(t_camera_to_depth)

        # 一次发布所有静态变换
        self.static_broadcaster.sendTransform(transforms)

        self.get_logger().info(
            f'Camera TF published: {base_frame} -> {camera_frame} '
            f'at ({cx:.2f}, {cy:.2f}, {cz:.2f})'
        )

def main():
    rclpy.init()
    node = CameraTFPublisher()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

9.5 URDF中定义相机TF

在实际项目中，推荐将相机TF写入URDF文件，由robot_state_publisher 统一管理：

xml 复制代码 
<!-- 相机的完整 URDF 定义 -->
<link name="camera_link">
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <box size="0.025 0.09 0.025"/>
    </geometry>
    <material name="black">
      <color rgba="0.0 0.0 0.0 1.0"/>
    </material>
  </visual>
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <box size="0.025 0.09 0.025"/>
    </geometry>
  </collision>
</link>

<!-- base_link 到 camera_link 的安装变换 -->
<joint name="camera_joint" type="fixed">
  <parent link="base_link"/>
  <child link="camera_link"/>
  <origin xyz="0.064 -0.047 0.107" rpy="0 0 0"/>
</joint>

<!-- camera_link 到彩色光学坐标系 -->
<link name="camera_color_frame"/>
<joint name="camera_color_joint" type="fixed">
  <parent link="camera_link"/>
  <child link="camera_color_frame"/>
  <origin xyz="0 0.015 0" rpy="0 0 0"/>
</joint>

<link name="camera_color_optical_frame"/>
<joint name="camera_color_optical_joint" type="fixed">
  <parent link="camera_color_frame"/>
  <child link="camera_color_optical_frame"/>
  <origin xyz="0 0 0" rpy="-1.5707963 0 -1.5707963"/>
</joint>

9.6 深度相机点云坐标系变换

使用深度相机时，需要将点云从光学坐标系变换到机器人坐标系：

python 复制代码 
#!/usr/bin/env python3
"""将深度相机点云从光学坐标系变换到 base_link"""
import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener
import tf2_py as tf2
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2
import numpy as np

class PointCloudTransformer(Node):
    def __init__(self):
        super().__init__('pointcloud_transformer')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        self.sub = self.create_subscription(
            PointCloud2,
            '/camera/depth/points',
            self.cloud_callback,
            10
        )
        self.pub = self.create_publisher(PointCloud2, '/points_in_base', 10)

    def cloud_callback(self, msg):
        try:
            # 获取点云时间戳时的 TF 变换
            transform = self.tf_buffer.lookup_transform(
                'base_link',
                msg.header.frame_id,  # 通常是 camera_depth_optical_frame
                msg.header.stamp,
                timeout=rclpy.duration.Duration(seconds=0.1)
            )

            # 提取变换参数
            tx = transform.transform.translation.x
            ty = transform.transform.translation.y
            tz = transform.transform.translation.z
            rx = transform.transform.rotation.x
            ry = transform.transform.rotation.y
            rz = transform.transform.rotation.z
            rw = transform.transform.rotation.w

            self.get_logger().info(
                f'Transform from {msg.header.frame_id} to base_link: '
                f't=({tx:.3f}, {ty:.3f}, {tz:.3f})'
            )

            # 实际项目中使用 pcl 或 tf2_sensor_msgs 进行点云变换
            # 这里仅做示意，完整实现需要 do_transform_cloud

        except Exception as ex:
            self.get_logger().warn(f'TF lookup failed: {ex}')

def main():
    rclpy.init()
    node = PointCloudTransformer()
    rclpy.spin(node)
    rclpy.shutdown()

10.调试与可视化工具

10.1 view_frames:TF树可视化

bash 复制代码 
# ROS1
rosrun tf view_frames
# → 生成 frames.pdf

# ROS2
ros2 run tf2_tools view_frames
# → 生成 frames_XXXXXXXX_XXXXXXXX.pdf（时间戳命名）

生成的PDF展示：

  • 所有活跃坐标系节点
  • 坐标系之间的父子关系
  • 每个TF的发布频率
  • 最新TF的时间偏移

10.2 tf2_echo:实时变换监控

bash 复制代码 
# ROS1
rosrun tf tf_echo /base_link /camera_link

# ROS2
ros2 run tf2_ros tf2_echo base_link camera_link

# 带刷新频率
ros2 run tf2_ros tf2_echo --hz 10 base_link camera_link

# 输出示例（连续刷新）：
# At time 1713000000.000000000
# - Translation: [0.100, 0.000, 0.200]
# - Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
#             in RPY (radian) [0.000, 0.000, 0.000]
#             in RPY (degree) [0.000, 0.000, 0.000]

10.3 rqt_tf_tree:图形化TF树

bash 复制代码 
# ROS1
rosrun rqt_tf_tree rqt_tf_tree
# 或
rqt --standalone rqt_tf_tree

# ROS2（需要安装）
sudo apt install ros-humble-rqt-tf-tree
ros2 run rqt_tf_tree rqt_tf_tree

rqt_tf_tree 提供图形化界面，实时显示 TF 树，点击节点可查看详细信息。

10.4 Rviz/Rviz2:3D可视化

在Rviz中可视化TF：

  1. 启动 Rviz：rviz2
  2. 在左侧面板点击 Add
  3. 选择 TF 显示类型
  4. 调整参数：
    • Show Names：显示坐标系名称
    • Show Axes：显示坐标轴（X=红，Y=绿，Z=蓝）
    • Show Arrows：显示父子连接箭头
    • Marker Scale：坐标轴大小
    • Update Interval：更新频率

10.5 命令行快速排查

bash 复制代码 
# 检查特定 TF 是否存在
ros2 run tf2_ros tf2_echo base_link laser_link 2>&1 | head -20

# 查看所有活跃的 TF 话题
ros2 topic list | grep tf

# 查看 TF 消息内容
ros2 topic echo /tf --once

# 查看 TF 发布频率
ros2 topic hz /tf

# 检查某个坐标系是否在树中
ros2 run tf2_ros tf2_echo world camera_link
# 如果不存在，会报错：'camera_link' passed to lookupTransform...

11.ROS1→ ROS2 TF迁移指南

从ROS1迁移到ROS2时，TF相关代码需要注意以下变化：

11.1 API对比

功能 ROS1(tf2） ROS2(tf2）
头文件（C++） #include <tf2_ros/...> #include <tf2_ros/...>（基本相同）
节点初始化 ros::init() rclcpp::init()
时间 ros::Time::now() node->get_clock()->now()
最新时刻 ros::Time(0) tf2::TimePointZero
等待变换 waitForTransform() lookup_transform(..., timeout)
消息类型 geometry_msgs::TransformStamped geometry_msgs::msg::TransformStamped

11.2 命令行工具对比

功能 ROS1 ROS2
查看 TF 树 rosrun tf view_frames ros2 run tf2_tools view_frames
监听变换 rosrun tf tf_echo /A /B ros2 run tf2_ros tf2_echo A B
发布静态变换 rosrun tf static_transform_publisher x y z yaw pitch roll A B ms ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll A B
图形化 TF 树 rosrun rqt_tf_tree rqt_tf_tree ros2 run rqt_tf_tree rqt_tf_tree

11.3 静态变换参数顺序变化

⚠️ 重要变化：

bash 复制代码 
# ROS1：x y z yaw pitch roll frame_id child_frame_id period_ms
rosrun tf static_transform_publisher 0 0 0.3 0 0 0 base_link laser 100

# ROS2：x y z yaw pitch roll frame_id child_frame_id
ros2 run tf2_ros static_transform_publisher 0 0 0.3 0 0 0 base_link laser

主要区别：

  1. ROS2 无需 period_ms 参数(静态变换只发一次）
  2. 参数顺序完全相同(yaw pitch roll）

11.4 Python API迁移

python 复制代码 
# ===== ROS1 =====
import rospy, tf2_ros

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

# 等待变换
tf_buffer.can_transform('target', 'source', rospy.Time(0), rospy.Duration(1.0))

# 查询变换
t = tf_buffer.lookup_transform('target', 'source', rospy.Time(0))

# ===== ROS2 =====
import rclpy
from tf2_ros import Buffer, TransformListener

tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, self)  # 需要传入节点

# 查询变换（ROS2 中直接用 lookup_transform 并设置超时）
t = tf_buffer.lookup_transform(
    'target', 'source',
    rclpy.time.Time(),  # 对应 ROS1 的 rospy.Time(0)
    timeout=rclpy.duration.Duration(seconds=1.0)
)

12.常见问题与解决方案

12.1 "Could not find a connection between 'X' and 'Y'"

原因：两个坐标系不在同一棵 TF 树上（没有连接路径）。

排查步骤：

bash 复制代码 
# 1. 查看当前 TF 树
ros2 run tf2_tools view_frames

# 2. 检查是否有两棵独立的 TF 树
# 看 frames.pdf 中是否有断开的子树

# 3. 确认发布节点是否在运行
ros2 node list
ros2 topic echo /tf_static --once

# 解决方案：
# 添加缺失的 TF 变换，连接两棵子树

12.2 "Extrapolation into the future"

原因：查询的时间戳比 TF 缓存中最新的变换还新（时钟不同步或延迟）。

python 复制代码 
# 错误做法：用当前时间戳查询（可能超出缓存）
t = tf_buffer.lookup_transform('A', 'B', self.get_clock().now())

# 正确做法：查询最新可用时刻（rclpy.time.Time() 等价于 ROS1 的 Time(0)）
t = tf_buffer.lookup_transform('A', 'B', rclpy.time.Time())

# 或者带超时等待
from rclpy.duration import Duration
t = tf_buffer.lookup_transform('A', 'B', rclpy.time.Time(),
                                timeout=Duration(seconds=1.0))

12.3 "Extrapolation into the past"

原因：查询的时间戳比 TF 缓冲区保存的最早时间还旧（超过 10 秒缓冲）。

python 复制代码 
# 延长 TF 缓冲时间（ROS2）
from rclpy.duration import Duration
tf_buffer = Buffer(cache_time=Duration(seconds=30.0))

12.4 TF 树形成循环(Loop Detected）

原因：某个坐标系被发布了两个不同的父节点，形成循环。

bash 复制代码 
# 检查
ros2 run tf2_tools view_frames
# 查看 PDF 中是否有循环

# 常见原因：
# - URDF 定义了 map → odom，同时 SLAM 节点也发布 map → odom
# - 解决：确保每个 TF 只有一个发布者

12.5 TF 发布频率过低导致抖动

原因：动态 TF 发布频率太低，查询时需要大量插值。

python 复制代码 
# 建议频率：
# - 里程计 TF（odom → base_link）：≥ 20 Hz，推荐 50 Hz
# - SLAM TF（map → odom）：≥ 5 Hz，推荐 10 Hz
# - 传感器数据相关 TF：与传感器频率相匹配

# 提高发布频率示例（ROS2）
self.timer = self.create_timer(0.02, self.publish_tf)  # 50 Hz

12.6 时间戳不一致(Time Stamp Mismatch）

原因：传感器数据的时间戳与 TF 发布的时间戳不对齐。

python 复制代码 
# 错误：使用当前时间，不与传感器数据时间对齐
t.header.stamp = self.get_clock().now().to_msg()

# 正确：使用与传感器数据相同的时间戳
def sensor_callback(self, msg):
    t.header.stamp = msg.header.stamp  # 使用传感器时间戳
    self.tf_broadcaster.sendTransform(t)

12.7 静态TF在Rviz中不显示

可能原因及解决方案：

bash 复制代码 
# 1. 检查 /tf_static 是否有消息
ros2 topic echo /tf_static --once

# 2. 检查 Fixed Frame 设置是否正确（Rviz 中设为 TF 树的根节点）
# Rviz2 → Global Options → Fixed Frame → 改为 base_link 或 map

# 3. 确认静态 TF 节点是否在运行
ros2 node list | grep tf

# 4. 尝试重新发布
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link camera_link

13.综合实战:多传感器TF构建

13.1 场景描述

构建一个典型的自主移动机器人的完整TF系统，包含：

  • 激光雷达（顶部，固定安装）
  • RGB-D 深度相机(前方，固定安装，俯角 15°）
  • IMU(底盘内部，固定安装）
  • 里程计(驱动层发布）
  • SLAM 定位(发布 map → odom）

13.2 完整的URDF描述

xml 复制代码 
<?xml version="1.0"?>
<robot name="autonomous_robot"
       xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- 定义常量 -->
  <xacro:property name="PI" value="3.14159265358979"/>
  <xacro:property name="base_width"  value="0.4"/>
  <xacro:property name="base_length" value="0.5"/>
  <xacro:property name="base_height" value="0.15"/>

  <!-- ===== 底盘 ===== -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="blue"><color rgba="0.2 0.4 0.8 1.0"/></material>
    </visual>
    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="10.0"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
  </link>

  <!-- ===== 激光雷达 ===== -->
  <link name="laser_link">
    <visual>
      <geometry><cylinder radius="0.05" length="0.08"/></geometry>
      <material name="black"><color rgba="0.1 0.1 0.1 1.0"/></material>
    </visual>
  </link>

  <joint name="base_to_laser" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <!-- 激光雷达在底盘顶部中心，高 0.3m -->
    <origin xyz="0.0 0.0 0.3" rpy="0 0 0"/>
  </joint>

  <!-- ===== 深度相机 ===== -->
  <link name="camera_link">
    <visual>
      <geometry><box size="0.025 0.09 0.025"/></geometry>
      <material name="silver"><color rgba="0.8 0.8 0.8 1.0"/></material>
    </visual>
  </link>

  <joint name="base_to_camera" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <!-- 相机在底盘前方 0.2m，上方 0.1m，俯角 15° -->
    <origin xyz="0.2 0.0 0.1" rpy="0 ${-PI/12} 0"/>
  </joint>

  <!-- 相机光学坐标系 -->
  <link name="camera_depth_optical_frame"/>
  <joint name="camera_to_depth_optical" type="fixed">
    <parent link="camera_link"/>
    <child link="camera_depth_optical_frame"/>
    <origin xyz="0 0 0" rpy="${-PI/2} 0 ${-PI/2}"/>
  </joint>

  <link name="camera_color_optical_frame"/>
  <joint name="camera_to_color_optical" type="fixed">
    <parent link="camera_link"/>
    <child link="camera_color_optical_frame"/>
    <origin xyz="0 -0.015 0" rpy="${-PI/2} 0 ${-PI/2}"/>
  </joint>

  <!-- ===== IMU ===== -->
  <link name="imu_link"/>
  <joint name="base_to_imu" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <!-- IMU 在底盘中心 -->
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
  </joint>

  <!-- ===== 左右驱动轮 ===== -->
  <link name="wheel_left_link">
    <visual>
      <geometry><cylinder radius="0.1" length="0.05"/></geometry>
      <material name="dark"><color rgba="0.2 0.2 0.2 1.0"/></material>
    </visual>
  </link>

  <joint name="wheel_left_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_left_link"/>
    <origin xyz="0.0 0.2 -0.1" rpy="${-PI/2} 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="wheel_right_link">
    <visual>
      <geometry><cylinder radius="0.1" length="0.05"/></geometry>
      <material name="dark"><color rgba="0.2 0.2 0.2 1.0"/></material>
    </visual>
  </link>

  <joint name="wheel_right_joint" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_right_link"/>
    <origin xyz="0.0 -0.2 -0.1" rpy="${PI/2} 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

</robot>

13.3 完整的Launch文件

python 复制代码 
# launch/robot_tf_demo.launch.py
import os
import xacro
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    pkg_dir = get_package_share_directory('my_robot_bringup')

    # 处理 XACRO 文件
    urdf_file = os.path.join(pkg_dir, 'urdf', 'autonomous_robot.urdf.xacro')
    robot_description = xacro.process_file(urdf_file).toxml()

    return LaunchDescription([
        # 1. robot_state_publisher（发布 URDF 中定义的所有固定 TF）
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{
                'robot_description': robot_description,
                'publish_frequency': 50.0,
                'use_sim_time': False,
            }]
        ),

        # 2. joint_state_publisher（发布轮子关节状态，触发动态 TF）
        Node(
            package='joint_state_publisher',
            executable='joint_state_publisher',
            name='joint_state_publisher',
        ),

        # 3. 驱动节点（发布 odom → base_link 的动态 TF）
        # 实际项目中替换为真实的驱动节点
        Node(
            package='my_robot_driver',
            executable='diff_drive_controller',
            name='diff_drive_controller',
            output='screen',
        ),

        # 4. SLAM 节点（发布 map → odom 的动态 TF）
        # 实际项目中使用 slam_toolbox 或 cartographer
        Node(
            package='slam_toolbox',
            executable='async_slam_toolbox_node',
            name='slam_toolbox',
            output='screen',
            parameters=[{
                'use_sim_time': False,
            }]
        ),

        # 5. Rviz2 可视化
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', os.path.join(pkg_dir, 'config', 'robot.rviz')],
        ),
    ])

13.4 最终TF树结构

运行上述配置后，完整的 TF 树如下：

复制代码 
map (SLAM 定位)
  └── odom (里程计)
        └── base_link (机器人底盘)
              ├── laser_link (激光雷达)
              ├── camera_link (深度相机)
              │       ├── camera_color_optical_frame
              │       └── camera_depth_optical_frame
              ├── imu_link (IMU)
              ├── wheel_left_link (左轮，动态)
              └── wheel_right_link (右轮，动态)

13.5 验证TF树正确性

bash 复制代码 
# 步骤1：查看 TF 树结构
ros2 run tf2_tools view_frames
# 打开生成的 PDF，确认树结构正确

# 步骤2：验证关键变换链
# 激光雷达到地图的完整链
ros2 run tf2_ros tf2_echo map laser_link

# 相机到地图的完整链
ros2 run tf2_ros tf2_echo map camera_color_optical_frame

# 步骤3：在 Rviz2 中确认
rviz2
# Fixed Frame → map
# 添加 TF 显示
# 添加 LaserScan（frame = laser_link）
# 添加 PointCloud2（frame = camera_depth_optical_frame）
# 确认传感器数据在地图中位置正确

# 步骤4：数值验证
# 激光雷达应在 base_link 正上方 0.3m
ros2 run tf2_ros tf2_echo base_link laser_link
# 期望输出 Translation: [0.000, 0.000, 0.300]

# 相机应在 base_link 前方 0.2m、上方 0.1m
ros2 run tf2_ros tf2_echo base_link camera_link
# 期望输出 Translation: [0.200, 0.000, 0.100]

14.常用 TF 命令速查表

14.1 ROS1命令

命令 描述
rosrun tf view_frames 生成 TF 树 PDF
rosrun tf tf_echo /A /B 监听 A→B 变换
rosrun tf tf_monitor 监控所有 TF
rosrun tf static_transform_publisher x y z yaw pitch roll A B ms 发布静态变换
rosrun rqt_tf_tree rqt_tf_tree 图形化 TF 树
rostopic echo /tf 查看 TF 话题

14.2 ROS2命令

命令 描述
ros2 run tf2_tools view_frames 生成 TF 树 PDF
ros2 run tf2_ros tf2_echo A B 监听 A→B 变换
ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll A B 发布静态变换
ros2 run rqt_tf_tree rqt_tf_tree 图形化 TF 树
ros2 topic echo /tf 查看动态 TF 话题
ros2 topic echo /tf_static 查看静态 TF 话题
ros2 topic hz /tf 查看 TF 发布频率

14.3 常用坐标系名称参考

坐标系名称 含义
map 全局地图坐标系
odom 里程计坐标系
base_link 机器人底盘坐标系
base_footprint 机器人在地面上的投影坐标系
laser_link / scan 激光雷达坐标系
camera_link 相机物理安装坐标系（ROS 约定）
camera_optical_frame 相机光学坐标系（CV 约定）
imu_link IMU 坐标系
gps_link GPS 天线坐标系

14.4 包依赖速查(CMakeLists.txt)

cmake 复制代码 
# ROS1
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  tf2
  tf2_ros
  tf2_geometry_msgs
  geometry_msgs
)

# ROS2（CMakeLists.txt）
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
xml 复制代码 
<!-- ROS2（package.xml） -->
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>geometry_msgs</depend>

参考资源

相关推荐
报错小能手2 小时前
ios开发方向——swift并发进阶核心 Task、Actor、await 详解
开发语言·学习·ios·swift
EnglishJun2 小时前
ARM嵌入式学习（十九）--- 字符设备驱动的注册与调用流程
arm开发·学习
Engineer邓祥浩2 小时前
JVM学习笔记(8) 第三部分 虚拟机执行子系统 第7章 虚拟机类加载机制
jvm·笔记·学习
深蓝海拓2 小时前
基于QtPy (PySide6) 的PLC-HMI工程项目（七）上位机通信部分的初步建设：socket客户端
网络·笔记·python·学习·plc
送外卖的CV工程师2 小时前
STM32 CubeMX Makefile 工程编译 入门指南
stm32·单片机·嵌入式硬件·学习·makefile·stm32cubemx
喜欢吃燃面2 小时前
Linux 进程间通信：命名管道与 System V 共享内存深度解析
linux·运维·服务器·学习
承渊政道2 小时前
【递归、搜索与回溯算法】(递归问题拆解与经典模型实战大秘笈)
数据结构·c++·学习·算法·macos·dfs·bfs
xuhaoyu_cpp_java2 小时前
MySql学习(一)
经验分享·学习·mysql
爱上好庆祝2 小时前
clip-path裁剪，css的滤镜，动画时间线，css的变量和函数
前端·css·学习·html·css3
热门推荐
012026年4月技术前沿：AI大模型爆发、智能体革命与量子安全新纪元02GitHub 镜像站点03一周AI热点速览（2026.03.31-04.06）：GPT-6曝光、谷歌开源Gemma 4、资本狂飙与模型军备竞赛04GPT-6核心能力解析及与现有主流大模型对比05基于 Docker 部署 Hermes Agent 并接入飞书机器人的完整指南06零成本！Ollama本地部署国产大模型全指南（支持Kimi-K2.5/GLM-5/Qwen，新手秒上手）07AI Weekly | 2026年4月第二周 · GitHub热门项目与AI发展趋势深度解析08免费！不限量！用opencode接入英伟达（NVIDIA）大模型，轻松打造你的 AI 编程助手09从限购到畅通：GLM-5.1 Coding Plan接入攻略10CodeBuddy与WorkBuddy深度对比：腾讯两款AI工具差异及实操指南