# 50 · TF2 坐标变换(完全指南)

> **目标**:吃透 TF2(坐标系管理),能做"机械臂末端在哪"、"相机看到的点在机器人哪"、"手眼标定",为 MoveIt2 / ros2_control / 抓取 打基础。

---

## 目录

- [1. TF 是什么](#1-tf-是什么)
- [2. 关键概念](#2-关键概念)
- [3. TF tree 典型结构](#3-tf-tree-典型结构)
- [4. C++ API 详解](#4-c--api-详解)
- [5. Python API 详解](#5-python-api-详解)
- [6. 静态 TF vs 动态 TF](#6-静态-tf-vs-动态-tf)
- [7. URDF + JointState → TF 工作链](#7-urdf--jointstate--tf-工作链)
- [8. 调试命令大全](#8-调试命令大全)
- [9. VLA / 机器人实战](#9-vla--机器人实战)
- [10. 在本仓库里跑](#10-在本仓库里跑)
- [11. 常见坑 + 排错](#11-常见坑--排错)
- [12. 进阶:手眼标定 + 时间同步](#12-进阶手眼标定--时间同步)

---

## 1. TF 是什么

**TF (Transform Library)** = ROS 里管理"所有坐标系之间相对位姿"的工具。
**TF2** = ROS2 的下一代实现。

机器人身上有几十个坐标系(世界、底盘、激光雷达、相机、机械臂每个关节、夹爪):
- TF2 帮你算任意两个之间的相对位姿,**实时更新**
- TF2 维护一棵 **TF tree**,节点之间路径唯一
- TF2 通过 `/tf` topic 广播,通过 `/tf_static` 静态广播

---

## 2. 关键概念

### 2.1 Frame ID
每个坐标系有个名字:
- `world`、`map`、`odom`、`base_link`、`base_footprint`
- `camera_optical_frame`、`laser_frame`
- `arm_base`、`shoulder`、`elbow`、`wrist`、`gripper`

### 2.2 Transform(变换)
```
parent_frame ──transform──> child_frame
```
包含:
- **translation**(Vector3: x, y, z) - 平移
- **rotation**(Quaternion: x, y, z, w) - 旋转
- **header.stamp**(时间戳)

### 2.3 时间
- `tf2::TimePointZero` = "最新可用"
- 也可以指定具体时间戳查历史(`lookupTransform(target, source, time)`)
- 必须保证时间戳 ≤ buffer 当前时间

### 2.4 Buffer + Listener 模式

```cpp
tf2_ros::Buffer buffer(clock);
tf2_ros::TransformListener listener(buffer, node);

// lookup
auto tf = buffer.lookupTransform(target, source, tf2::TimePointZero);
```

后台 `TransformListener` 订阅 `/tf` + `/tf_static`,自动把变换塞进 buffer。

---

## 3. TF tree 典型结构

### 3.1 移动底盘 + 机械臂

```
world                                  ← 全局参考
└─ map                                  ← SLAM 输出
   └─ odom                              ← 里程计 / AMCL
      └─ base_link                       ← 底盘中心
         ├─ base_footprint
         ├─ imu_link
         ├─ laser_frame                   ← 2D 激光
         ├─ camera_optical_frame          ← RGB 相机
         └─ arm_base                      ← 机械臂底座
            └─ shoulder_link              ← 关节 1
               └─ upper_arm_link          ← 关节 2
                  └─ elbow_link           ← 关节 3
                     └─ forearm_link
                        └─ wrist_link
                           └─ gripper_link ← 末端
```

### 3.2 树 vs 图
TF 是**树**结构:任意两个 frame 间路径唯一。
不能有闭环(两个 link 间只能有一条路径)。

---

## 4. C++ API 详解

### 4.1 Listener + Buffer

```cpp
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

class TfListenerNode : public rclcpp::Node {
public:
  TfListenerNode() : rclcpp::Node("tf_listener") {
    buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, this);
  }

  void lookup() {
    try {
      auto tf = buffer_->lookupTransform(
        "base_link",            // target
        "gripper",              // source
        tf2::TimePointZero);    // latest
      RCLCPP_INFO(get_logger(),
        "gripper in base_link: x=%.3f y=%.3f z=%.3f",
        tf.transform.translation.x,
        tf.transform.translation.y,
        tf.transform.translation.z);
    } catch (const tf2::TransformException& ex) {
      RCLCPP_WARN(get_logger(), "lookup failed: %s", ex.what());
    }
  }

private:
  std::shared_ptr<tf2_ros::Buffer> buffer_;
  std::shared_ptr<tf2_ros::TransformListener> listener_;
};
```

### 4.2 几何变换(把点变换到其他系)

```cpp
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"

geometry_msgs::msg::PointStamped p_in, p_out;
p_in.header.frame_id = "world";
p_in.point.x = 1.0; p_in.point.y = 2.0; p_in.point.z = 3.0;

p_out = buffer_->transform(p_in, "robot");
// p_out 在 robot 系下
```

也支持 PoseStamped、Vector3、Quaternion。

### 4.3 Broadcast(自己发布 TF)

```cpp
#include "tf2_ros/transform_broadcaster.h"

geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = node->now();
tf.header.frame_id = "world";
tf.child_frame_id = "robot";
tf.transform.translation.set__data(1.0, 2.0, 3.0);
tf.transform.rotation.set__data(0, 0, 0, 1.0);
broadcaster.sendTransform(tf);
```

### 4.4 Static Broadcast(只发一次)

```cpp
#include "tf2_ros/static_transform_broadcaster.h"
// 用 sendTransform 后就永久保留,ROS 系统不删
static_broadcaster.sendTransform(tf);
```

### 4.5 关键 API 速查

| API | 用途 |
|---|---|
| `tf2_ros::Buffer(clock)` | 创建时间缓冲(默认 10s 历史) |
| `tf2_ros::TransformListener(buffer, node)` | 订阅 /tf /tf_static |
| `buffer.lookupTransform(target, source, time)` | 查变换(失败抛异常) |
| `buffer.canTransform(target, source, time, timeout)` | 预检 |
| `buffer.transform(p_in, target_frame)` | 几何变换 |
| `tf2_ros::TransformBroadcaster` | 动态广播 |
| `tf2_ros::StaticTransformBroadcaster` | 静态广播 |

---

## 5. Python API 详解

```python
from tf2_ros import Buffer, TransformListener, TransformBroadcaster, StaticTransformBroadcaster
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped

class TfNode(Node):
    def __init__(self):
        super().__init__('tf_node_py')
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.broadcaster = TransformBroadcaster(self)
        self.static_broadcaster = StaticTransformBroadcaster(self)

    def lookup(self):
        try:
            tf = self.buffer.lookup_transform(
                'base_link', 'gripper',
                rclpy.time.Time(),
                timeout=rclpy.duration.Duration(seconds=1.0))
            print(f'gripper in base_link: {tf.transform.translation}')
        except Exception as e:
            self.get_logger().warn(f'lookup failed: {e}')

    def broadcast(self):
        tf = TransformStamped()
        tf.header.stamp = self.get_clock().now().to_msg()
        tf.header.frame_id = 'world'
        tf.child_frame_id = 'robot'
        tf.transform.translation.x = 1.0
        tf.transform.translation.y = 2.0
        tf.transform.translation.z = 3.0
        tf.transform.rotation.w = 1.0
        self.broadcaster.sendTransform(tf)
```

---

## 6. 静态 TF vs 动态 TF

### 6.1 静态 TF(不变的)
- 激光雷达装在底盘上,位置不变
- 相机装在机械臂末端,相对末端不变
- **关键**: 只发一次,ROS 永久保留(用 latched topic)

```bash
ros2 run tf2_ros static_transform_publisher \
  --x 0.1 --y 0 --z 0.2 --roll 0 --pitch 0 --yaw 0 \
  --frame-id base_link --child-frame-id laser_frame
```

或在 launch 里:
```python
Node(package='tf2_ros', executable='static_transform_publisher',
     arguments=['--x', '0.1', '--y', '0', '--z', '0.2',
                '--frame-id', 'base_link', '--child-frame-id', 'laser_frame'])
```

### 6.2 动态 TF(随时间变)
- 机械臂关节 → 末端位姿实时变
- 通过 URDF + JointState + robot_state_publisher 自动算

---

## 7. URDF + JointState → TF 工作链

```
URDF (XML)         JointState (msg)
   │                    │
   │                    │ /joint_states (sensor_msgs/JointState)
   │                    │
   ▼                    ▼
        robot_state_publisher (ROS2 系统包)
                 │
                 │ 计算各 link 之间的变换
                 ▼
             /tf (geometry_msgs/TransformStamped)
                 │
                 ▼
        tf2::Buffer (订阅)
                 │
                 ▼
        buffer.lookupTransform(target, source, time)
                 │
                 ▼
            "gripper in base_link: x=..., y=..., z=..."
```

### 7.1 安装 robot_state_publisher

```bash
sudo apt install ros-humble-robot-state-publisher
```

### 7.2 启动

```bash
# 读 URDF + JointState
ros2 run robot_state_publisher robot_state_publisher \
  --ros-args -p robot_description:="$(xacro arm.urdf)"

# 发布 JointState(本仓库 cpp_robot_tf2 就是这个,executable 名 joint_state_publisher_cpp)
ros2 run cpp_robot_tf2 joint_state_publisher_cpp
```

### 7.3 看 TF 树

```bash
ros2 run tf2_tools view_frames
# 生成 frames_<timestamp>.pdf
```

---

## 8. 调试命令大全

```bash
# 实时打印某变换
ros2 run tf2_ros tf2_echo base_link gripper

# 看 /tf topic 流量
ros2 topic hz /tf
ros2 topic info /tf_static -v

# 看完整 TF tree(生成 PDF)
ros2 run tf2_tools view_frames

# 在 RViz 看(可视化)
rviz2
# Add → TF

# 静态 TF publisher(临时)
ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0.5 \
  --frame-id world --child-frame-id robot

# 看 /tf_static 列表
ros2 topic info /tf_static -v
```

---

## 9. VLA / 机器人实战

### 9.1 VLA 抓取的工作流

```
[RGB Camera] → /camera/color/image_raw
                    ↓
[GraspNet / 6-DoF pose estimator]
                    ↓ publish /grasp_pose (PoseStamped)
                    ↓
[TF 工具:把 grasp_pose 从 camera_optical_frame 转到 base_link]
                    ↓
[MoveIt2 算轨迹 → /joint_trajectory]
                    ↓
[ros2_control + 电机]
```

### 9.2 关键代码片段

```python
# 把图像里的目标点转到 base_link
def transform_grasp_to_base(camera_pose, buffer):
    p_in = PoseStamped()
    p_in.header.frame_id = 'camera_optical_frame'
    p_in.pose = camera_pose
    p_base = buffer.transform(p_in, 'base_link')
    return p_base.pose
```

### 9.3 真实项目必备

| 任务 | TF 用法 |
|---|---|
| 视觉抓取 | camera 点 → base_link 点 |
| 手眼标定 | camera_link ↔ gripper 静态 TF |
| SLAM | map → odom → base_link 动态 TF |
| 机械臂正运动学 | lookup gripper 在 base_link 下 |
| 移动底盘导航 | base_link 在 map 下 |
| 多传感器融合 | 把所有数据投到 base_link |

---

## 10. 在本仓库里跑

### 10.1 启 3 关节机械臂 demo

```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup robot_launch.py"
```

**预期日志**(launch 用 `name=` 重命名去掉了 `_cpp` 后缀,所以节点名是 joint_state_publisher / tf2_listener,而 `[INFO]` 前缀里显示的是 launch 起的 process 名,带 `_cpp`):
```
[joint_state_publisher_cpp-1] [INFO] [...] JointStatePublisher started, joints: 3
[tf2_listener_cpp-2] [INFO] [...] Tf2Listener started: from=base_link to=gripper
[robot_state_publisher-3] [INFO] [...] got segment base_link
[robot_state_publisher-3] [INFO] [...] got segment link1
[robot_state_publisher-3] [INFO] [...] got segment link2
[robot_state_publisher-3] [INFO] [...] got segment gripper
[tf2_listener_cpp-2] [INFO] [...] gripper in base_link: x=0.013 y=0.004 z=0.299
[tf2_listener_cpp-2] [INFO] [...] gripper in base_link: x=0.019 y=0.009 z=0.298
```

### 10.2 看 TF 树(PDF)

```bash
docker exec ros2_dev bash -lc "source /opt/ros/humble/setup.bash && source /root/ros2_ws/install/setup.bash && ros2 run tf2_tools view_frames"
# 生成 frames_<时间>.pdf
```

### 10.3 源码
- JointState pub: [`src/cpp_robot_tf2/src/joint_state_publisher.cpp`](../src/cpp_robot_tf2/src/joint_state_publisher.cpp)
- TF 监听: [`src/cpp_robot_tf2/src/tf2_listener.cpp`](../src/cpp_robot_tf2/src/tf2_listener.cpp)
- URDF: [`src/cpp_robot_tf2/urdf/simple_arm.urdf`](../src/cpp_robot_tf2/urdf/simple_arm.urdf)
- 测试: [`src/cpp_robot_tf2/test/test_tf2_lookup.cpp`](../src/cpp_robot_tf2/test/test_tf2_lookup.cpp)

### 10.4 端到端日志
[`docker/robot_e2e.log`](../docker/robot_e2e.log)

---

## 11. 常见坑 + 排错

### 11.1 "lookup failed: frame does not exist"

**原因**:该 frame 没发布。

**排错**:
```bash
# 看有哪些 frame
ros2 run tf2_tools view_frames   # 生成 PDF,看完整树
# 或
ros2 topic echo /tf | head -50  # 看具体发布的变换
```

**常见原因**:
- robot_state_publisher 没起 → 启它
- JointState 没发布 → 检查 publisher
- 静态 TF publisher 没起

### 11.2 lookup 超时

**原因**: buffer 里没该变换(还没发布)。

**排错**:
- 等 2-3 秒再查
- 加 retry:
```cpp
for (int i = 0; i < 5; i++) {
    try {
        auto tf = buffer.lookupTransform(target, source, tf2::TimePointZero, 1s);
        break;
    } catch (const tf2::TransformException& ex) {
        // 等
    }
}
```

### 11.3 TF 跨机器漂移

**原因**: 不同机器墙钟时间不同步。

**解决**:
- 三机装 NTP,误差 < 10ms
- 用 `use_sim_time` 统一时钟源
- 用 ROS Time API 而非 `chrono::system_clock`

### 11.4 lookupTransform 报"context is not valid"

**原因**: rclpy 已经 shutdown,在 callback 里调用。

**解决**: callback 里用 try/catch,且确保 ROS 仍 alive。

### 11.5 大 TF 树性能

**原因**: 10万 frame 时 lookup 慢。

**解决**:
- 用静态 TF 减少动态节点数
- 拆分成多棵子树
- buffer cache 调大

---

## 12. 进阶:手眼标定 + 时间同步

### 12.1 手眼标定

相机装在机械臂末端(eye-in-hand)或固定在外部(eye-to-hand),需要求:
- camera_link ↔ gripper 的静态变换(eye-in-hand)
- camera_link ↔ base_link 的静态变换(eye-to-hand)

```bash
sudo apt install ros-humble-easy-handeye

# 流程:
# 1) 启动相机 + MoveIt2 + 标定板
# 2) MoveIt2 移动机械臂到多个 pose,采集数据
# 3) easy_handeye 离线计算 camera_link → gripper TF
# 4) 写进 URDF
# 5) 重启 robot_state_publisher,验证
```

### 12.2 时间同步

ROS2 默认 `use_sim_time: false`(wall clock)。多机时:
```bash
# 装 NTP
sudo apt install chrony
sudo systemctl enable chrony

# 验证
chronyc tracking
```

---

## 接下来读

| 主题 | 文档 |
|---|---|
| URDF 模型 | [`60-urdf.md`](60-urdf.md) |
| ros2_control | 在 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| MoveIt2 | 在 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |
| VLA 应用 | [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 5 |

---

---

## 📖 阅读路径导航

> 💡 这是仓库 `doc/` 下所有文档的推荐阅读顺序。[返回 README 总导航](../README.md#-23-篇文档怎么读)
>
> ⏱ **本文预计阅读时间**: 50 分钟
> 📍 **当前位置**: 第 16 / 24 篇

- ⏮ **上一篇**: [Action 深度](40-actions.md)
- ⏭ **下一篇**: [机器人模型描述](60-urdf.md)