4. 末端位姿采集：TF树监听、位姿变换计算、坐标系管理

各位同学，欢迎来到第四章。

上一章我们聊了怎么把关节数据读出来。但说实话，光有关节角度还不够。你想让机械臂去抓东西，总得知道它的“手”在哪儿吧？这个“手”的位置和姿态，就是末端位姿。

在ROS2里，获取末端位姿的核心手段就是TF树。这一章，我就带你把它彻底搞明白。

4.1 为什么需要TF树？

想象一下，你的机械臂有6个关节，每个关节都有自己的坐标系。基座是一个坐标系，第一个关节是一个，第二个又是一个……一直到末端执行器。这些坐标系之间，通过旋转和平移相互关联。

TF树，说白了就是一张“坐标系关系网”。它记录了每个坐标系之间的变换关系。你只要告诉TF树“我想知道末端在基座坐标系下的位姿”，它就能沿着树一路算过去。

我在项目里见过有人自己手动算变换矩阵，结果算错了一个符号，整个臂撞到工件上。嗯，从那以后，我再也不敢手动算了。TF树虽然是个“黑盒”，但它可靠得多。

核心概念： TF树是一个树状结构，每个节点代表一个坐标系，每条边代表一个变换（平移+旋转）。根节点通常是 world 或 base_link。

4.2 TF树监听：获取实时变换

在ROS2里，监听TF树用的是 tf2_ros::TransformListener。它会自动订阅 /tf 和 /tf_static 话题，把变换数据缓存起来。

我个人习惯把监听器放在类的构造函数里初始化，这样整个生命周期都能用。

// C++ 示例
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

class EndEffectorPoseCollector {
public:
  EndEffectorPoseCollector() : buffer_(rclcpp::get_node_base_interface()),
                               listener_(buffer_) {
    // 监听器会自动开始工作
  }

  geometry_msgs::msg::TransformStamped get_pose() {
    // 等待变换可用
    return buffer_.lookupTransform(
      "base_link", "tool0", tf2::TimePointZero);
  }

private:
  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener listener_;
};

这里有个坑：lookupTransform 可能会抛出异常，比如变换还没发布，或者坐标系不存在。我建议你加个 try-catch，不然程序会直接崩掉。

注意： lookupTransform 的第三个参数是时间戳。如果你传 tf2::TimePointZero，它会返回最新可用的变换。如果你需要特定时刻的变换，可以传具体时间戳，但要注意缓存可能已经过期。

4.3 位姿变换计算：从关节空间到笛卡尔空间

TF树监听拿到的是 TransformStamped 消息。它包含平移（x, y, z）和旋转（四元数 qx, qy, qz, qw）。

但很多时候，我们需要的是6D位姿（x, y, z, roll, pitch, yaw）。这就涉及到四元数到欧拉角的转换。

我个人习惯用 tf2::Quaternion 来做转换，它自带 getRPY() 方法。

# Python 示例
import tf2_ros
import tf_transformations

buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)

try:
    trans = buffer.lookup_transform('base_link', 'tool0', rclpy.time.Time())
    # 提取平移
    x = trans.transform.translation.x
    y = trans.transform.translation.y
    z = trans.transform.translation.z

    # 提取旋转并转成欧拉角
    quat = trans.transform.rotation
    roll, pitch, yaw = tf_transformations.euler_from_quaternion(
        [quat.x, quat.y, quat.z, quat.w])

    print(f"末端位姿: x={x:.3f}, y={y:.3f}, z={z:.3f}, "
          f"roll={roll:.3f}, pitch={pitch:.3f}, yaw={yaw:.3f}")
except Exception as e:
    print(f"获取变换失败: {e}")

你想想看，为什么这里要用四元数而不是直接传欧拉角？因为四元数没有万向锁问题。我在做姿态插补时吃过这个亏，用欧拉角算出来的轨迹，在某个角度突然跳变。后来全改成四元数，问题就解决了。

4.4 坐标系管理：别让你的树长歪了

TF树虽然好用，但如果你不管理好坐标系，树就会变得一团糟。我见过一个项目，里面同时出现了 base_link、base_footprint、base_laser，三个坐标系之间还没发布变换，结果整个系统直接罢工。

这里我总结了几条坐标系管理的铁律：

原则 说明 我的经验
命名规范 所有坐标系名称使用小写+下划线，如 base_link、shoulder_pan_link 我曾经用过大写，结果和某个驱动库冲突，查了半天
单一父节点 每个坐标系只能有一个父节点，不能出现多父情况 多父会导致TF树变成图，监听器会报错
静态变换优先 固定不变的变换（如相机到基座）用 /tf_static 发布 静态变换只发布一次，节省带宽
定期检查 用 tf2_echo 或 view_frames 工具定期检查树结构 我每周跑一次 view_frames，确保树没长歪
小技巧： 在启动文件里，用 static_transform_publisher 节点发布静态变换。比如相机到基座的变换，写死在 launch 文件里，比在代码里硬编码更灵活。

4.5 知识体系总览

为了让你更直观地理解这一章的知识结构，我画了一张图。它展示了从TF树监听到位姿计算，再到坐标系管理的完整流程。

末端位姿采集知识体系 TF树监听 TransformListener lookupTransform 异常处理 位姿变换计算 坐标系管理 四元数→欧拉角 命名规范 静态变换 核心：TF树是坐标系关系的唯一真相来源，不要绕过它手动计算

4.6 实战：采集末端位姿并保存

理论说完了，我们来点实际的。假设你要采集机械臂运动过程中的末端位姿，每隔10毫秒记录一次，存到CSV文件里。

# Python 完整示例
import rclpy
from rclpy.node import Node
import tf2_ros
import tf_transformations
import csv
import time

class PoseCollector(Node):
    def __init__(self):
        super().__init__('pose_collector')
        self.buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.buffer, self)
        self.timer = self.create_timer(0.01, self.timer_callback)  # 10ms
        self.csv_file = open('end_effector_poses.csv', 'w', newline='')
        self.csv_writer = csv.writer(self.csv_file)
        self.csv_writer.writerow(['timestamp', 'x', 'y', 'z', 'roll', 'pitch', 'yaw'])

    def timer_callback(self):
        try:
            trans = self.buffer.lookup_transform('base_link', 'tool0', rclpy.time.Time())
            x = trans.transform.translation.x
            y = trans.transform.translation.y
            z = trans.transform.translation.z
            quat = trans.transform.rotation
            roll, pitch, yaw = tf_transformations.euler_from_quaternion(
                [quat.x, quat.y, quat.z, quat.w])
            now = self.get_clock().now().nanoseconds / 1e9
            self.csv_writer.writerow([now, x, y, z, roll, pitch, yaw])
        except Exception as e:
            self.get_logger().warn(f"采集失败: {e}")

    def __del__(self):
        self.csv_file.close()

def main(args=None):
    rclpy.init(args=args)
    node = PoseCollector()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

这段代码里，我用了 create_timer(0.01) 来保证10ms的采集频率。但要注意，如果TF树更新频率低于100Hz，你可能会采到重复数据。嗯，这时候可以加个时间戳去重逻辑。

建议： 采集数据时，最好同时记录关节角度和末端位姿。这样后期做标定或训练模型时，数据才完整。我一般会在CSV里加一列关节角度向量。

好了，这一章的内容就到这里。TF树监听、位姿变换计算、坐标系管理，这三块是末端位姿采集的基石。你只要把树管好，把变换算对，数据自然就准了。

← 上一章 📖 返回目录 下一章 →