xs
532 lines
14 KiB
Markdown
532 lines
14 KiB
Markdown
# 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 |
|
|
|
|
---
|
|
|
|
---
|
|
|
|
## 📖 阅读路径导航
|
|
|
|
> 💡 这是仓库 `doc/` 下所有文档的推荐阅读顺序。[返回 README 总导航](../README.md#-23-篇文档怎么读)
|
|
>
|
|
> ⏱ **本文预计阅读时间**: 50 分钟
|
|
> 📍 **当前位置**: 第 16 / 24 篇
|
|
|
|
- ⏮ **上一篇**: [Action 深度](../40-actions.md)
|
|
- ⏭ **下一篇**: [机器人模型描述](../60-urdf.md)
|