xs
14 KiB
14 KiB
50 · TF2 坐标变换(完全指南)
目标:吃透 TF2(坐标系管理),能做"机械臂末端在哪"、"相机看到的点在机器人哪"、"手眼标定",为 MoveIt2 / ros2_control / 抓取 打基础。
目录
- 1. TF 是什么
- 2. 关键概念
- 3. TF tree 典型结构
- 4. C++ API 详解
- 5. Python API 详解
- 6. 静态 TF vs 动态 TF
- 7. URDF + JointState → TF 工作链
- 8. 调试命令大全
- 9. VLA / 机器人实战
- 10. 在本仓库里跑
- 11. 常见坑 + 排错
- 12. 进阶:手眼标定 + 时间同步
1. TF 是什么
TF (Transform Library) = ROS 里管理"所有坐标系之间相对位姿"的工具。 TF2 = ROS2 的下一代实现。
机器人身上有几十个坐标系(世界、底盘、激光雷达、相机、机械臂每个关节、夹爪):
- TF2 帮你算任意两个之间的相对位姿,实时更新
- TF2 维护一棵 TF tree,节点之间路径唯一
- TF2 通过
/tftopic 广播,通过/tf_static静态广播
2. 关键概念
2.1 Frame ID
每个坐标系有个名字:
world、map、odom、base_link、base_footprintcamera_optical_frame、laser_framearm_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 模式
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
#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 几何变换(把点变换到其他系)
#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)
#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(只发一次)
#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 详解
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)
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 里:
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
sudo apt install ros-humble-robot-state-publisher
7.2 启动
# 读 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 树
ros2 run tf2_tools view_frames
# 生成 frames_<timestamp>.pdf
8. 调试命令大全
# 实时打印某变换
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 关键代码片段
# 把图像里的目标点转到 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
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)
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 - TF 监听:
src/cpp_robot_tf2/src/tf2_listener.cpp - URDF:
src/cpp_robot_tf2/urdf/simple_arm.urdf - 测试:
src/cpp_robot_tf2/test/test_tf2_lookup.cpp
10.4 端到端日志
11. 常见坑 + 排错
11.1 "lookup failed: frame does not exist"
原因:该 frame 没发布。
排错:
# 看有哪些 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:
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)
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)。多机时:
# 装 NTP
sudo apt install chrony
sudo systemctl enable chrony
# 验证
chronyc tracking
接下来读
| 主题 | 文档 |
|---|---|
| URDF 模型 | 60-urdf.md |
| ros2_control | 在 99-embodied-ai.md 阶段 2 |
| MoveIt2 | 在 99-embodied-ai.md 阶段 2 |
| 三机部署 | 100-embedded-deployment.md |
| VLA 应用 | 99-embodied-ai.md 阶段 5 |