init: ROS2 learning suite

2026-08-03 18:09:35 +08:00
commit 5ef38ab508
95 changed files with 13322 additions and 0 deletions
@@ -0,0 +1,517 @@
+# 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 就是这个)
+ros2 run cpp_robot_tf2 joint_state_publisher
+```
+
+### 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"
+```
+
+**预期日志**:
+```
+[joint_state_publisher_cpp]: joint_state_publisher_cpp started, joints: 3
+[tf2_listener_cpp]: tf2_listener_cpp started
+[robot_state_publisher]: got segment base_link
+[robot_state_publisher]: got segment link1
+[robot_state_publisher]: got segment link2
+[robot_state_publisher]: got segment gripper
+[tf2_listener_cpp]: gripper in base_link: x=0.013 y=0.004 z=0.299
+[tf2_listener_cpp]: 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 |