第4章 · TF编程基础（C++）

Broadcaster · Listener · TransformListener · TransformBroadcaster API

TF C++ API TransformBroadcaster TransformListener 发布坐标变换 · 监听坐标变换 · 时间戳与缓存 核心：sendTransform / lookupTransform / canTransform

这一章我们正式进入 C++ 层面的 TF 编程。说实话，命令行工具虽然方便，但真正要把 TF 集成到你的导航节点、控制节点里，还是得靠 API。我个人习惯先把广播器和监听器分开讲，因为它们在工程里各司其职——一个负责“告诉世界我在哪”，一个负责“世界你告诉我他在哪”。

4.1 创建 TF 广播器 (TransformBroadcaster)

广播器说白了就是发布坐标变换的节点。你想想看，机器人每个关节、每个传感器都有自己的坐标系，你得不停地告诉 TF 树：base_link 相对于 odom 的位置和姿态是多少。在 C++ 里，我们用 tf2_ros::TransformBroadcaster 这个类。

核心步骤：
  1. 包含头文件 #include <tf2_ros/transform_broadcaster.h>
  2. 创建广播器对象 tf2_ros::TransformBroadcaster broadcaster(node);
  3. 构造 geometry_msgs::msg::TransformStamped 消息
  4. 填充 header (frame_id, child_frame_id, timestamp)
  5. 调用 broadcaster.sendTransform(transform);

我刚开始写的时候，老是把 frame_id 和 child_frame_id 搞反。记住：frame_id 是父坐标系，child_frame_id 是子坐标系。比如你要发布 base_link 在 odom 下的位姿，那么 frame_id = "odom"，child_frame_id = "base_link"。

// 示例：发布 odom → base_link 变换
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/msg/transform_stamped.hpp"

class OdomBroadcaster : public rclcpp::Node {
public:
    OdomBroadcaster() : Node("odom_broadcaster") {
        broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(50),
            std::bind(&OdomBroadcaster::publish_tf, this));
    }

private:
    void publish_tf() {
        geometry_msgs::msg::TransformStamped t;
        t.header.stamp = this->now();
        t.header.frame_id = "odom";
        t.child_frame_id = "base_link";
        t.transform.translation.x = 1.0;
        t.transform.translation.y = 0.5;
        t.transform.translation.z = 0.0;
        t.transform.rotation.x = 0.0;
        t.transform.rotation.y = 0.0;
        t.transform.rotation.z = 0.0;
        t.transform.rotation.w = 1.0;
        broadcaster_->sendTransform(t);
    }
    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
};
💡 我的经验： 广播频率一般 20~50 Hz 就够了。导航里 odom→base_link 通常 50 Hz，但静态变换（比如 lidar→base_link）用 static broadcaster 一次搞定，别用普通广播器浪费资源。

4.2 创建 TF 监听器 (TransformListener)

监听器就是“偷听”广播器发出来的变换。你写导航节点、规划节点时，经常需要知道当前机器人相对于地图的位姿，或者激光雷达相对于基座的偏移。这时候 tf2_ros::TransformListener 就派上用场了。

注意，监听器需要配合 tf2_ros::Buffer 使用。Buffer 会缓存所有收到的 TF 数据，你可以查询任意两个坐标系之间的变换。我习惯把 Buffer 和 Listener 声明成类的成员变量，这样整个节点都能用。

// 示例：监听 base_link 在 map 下的位姿
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/msg/transform_stamped.hpp"

class TfListenerNode : public rclcpp::Node {
public:
    TfListenerNode() : Node("tf_listener_node") {
        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&TfListenerNode::lookup_tf, this));
    }

private:
    void lookup_tf() {
        try {
            geometry_msgs::msg::TransformStamped transformStamped =
                tf_buffer_->lookupTransform("map", "base_link", tf2::TimePointZero);
            RCLCPP_INFO(this->get_logger(), 
                "base_link 在 map 下的位置: (%.2f, %.2f, %.2f)",
                transformStamped.transform.translation.x,
                transformStamped.transform.translation.y,
                transformStamped.transform.translation.z);
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "无法获取变换: %s", ex.what());
        }
    }
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    rclcpp::TimerBase::SharedPtr timer_;
};
⚠️ 我曾经踩过的坑： 调用 lookupTransform 时，时间戳参数如果传 tf2::TimePointZero 表示获取最新可用的变换。但如果你传了一个具体的历史时间，而 buffer 里没有缓存那么久的数据，就会抛出异常。所以生产代码里一定要 try-catch，不然节点会崩溃。

4.3 TransformListener 与 TransformBroadcaster API 详解

这两个 API 是 TF2 的基石。我整理了一个常用方法表格，方便你快速查阅。

类 / 方法说明常用参数
TransformBroadcaster()构造广播器，绑定节点rclcpp::Node 共享指针
sendTransform(TransformStamped)发布单个变换填充好的 TransformStamped 消息
sendTransform(vector<TransformStamped>)批量发布（效率更高）变换列表
TransformListener(buffer)构造监听器，绑定 buffertf2_ros::Buffer 引用
buffer.lookupTransform(target, source, time)查询 target→source 变换帧ID字符串 + 时间戳
buffer.canTransform(target, source, time)检查变换是否可用返回 bool，避免异常
buffer.waitForTransform(...)等待变换可用（可设超时）帧ID + 时间 + 超时时间

这里我想多说一句 canTransform。很多新手直接调用 lookupTransform 不做检查，一旦 TF 树还没建好或者帧名写错，节点就抛异常。我个人习惯在循环里先用 canTransform 判断一下，如果返回 false 就 skip 这次查询，等下一帧。这样代码更健壮。

🔧 实用技巧： 如果你需要频繁查询同一组变换（比如每个控制周期都要查 odom→base_link），可以考虑把 lookupTransform 的结果缓存起来，设置一个有效期（比如 100ms），避免每毫秒都去 buffer 里查，降低 CPU 开销。我在做多机器人项目时就是这么干的，效果很明显。

4.4 时间戳与 TF 的“时间哲学”

TF2 里时间戳是个微妙的东西。广播器发布变换时，header.stamp 应该填当前时间还是传感器时间？我建议：如果是里程计或仿真数据，用 now() 没问题；但如果你在处理传感器数据（比如相机图像时间戳），最好用图像对应的时间，这样监听器才能做时间对齐。

另外，监听器 buffer 默认缓存 10 秒的数据。如果你需要更长的历史，可以在构造 Buffer 时设置 cache_time 参数。嗯，这里要注意：缓存时间越长，内存占用越大，别设成 100 年。

⏱ 我的习惯： 在导航应用中，我通常把广播器的时间戳设为 node->now()，监听器查询时用 tf2::TimePointZero 获取最新变换。只有在做数据回放或离线分析时，我才会用到具体的历史时间戳。

4.5 完整示例：广播 + 监听 联合工作

下面是一个极简的联合示例，一个节点同时广播 odom→base_link，并监听 base_link→laser 的变换。你可以把它当作模板，快速集成到自己的项目里。

// 联合示例 (伪代码结构)
class CombinedTfNode : public rclcpp::Node {
    // 1. 创建 broadcaster 和 buffer + listener
    // 2. 定时器1: 发布 odom→base_link (模拟里程计)
    // 3. 定时器2: 查询 base_link→laser 并打印
    // 注意: 两个定时器频率可以不同
};

实际代码我就不贴完整了，上面两个小节已经给出了核心片段。你只需要把广播器和监听器放在同一个节点里，注意不要搞混 frame_id 就行。

🚨 避坑指南： 我曾经在同一个节点里同时创建了广播器和监听器，但忘了给监听器单独分配一个 executor 线程，导致回调阻塞，TF 一直超时。后来我把监听器放在另一个节点里，或者用 rclcpp::executors::MultiThreadedExecutor 才解决。记住：监听器的回调是异步的，别在主线程里做阻塞查询。

好了，这一章的内容就是这些。C++ 的 TF API 其实不复杂，多写几遍就熟了。

上一章 返回目录 下一章