4、动态TF发布器：TransformBroadcaster的API详解与实战

动态TF发布器，说白了就是让坐标变换“活”起来。上一章我们聊了静态发布器，它适合固定不变的变换——比如激光雷达装在车体上，位置焊死了。但机器人动起来之后呢？关节在转，轮子在滚，里程计在不断更新……这时候就需要动态TF发布器登场了。

我个人习惯把动态TF发布器比作“实时坐标广播电台”。它每隔几十毫秒就把当前最新的坐标关系扔到TF树里，监听器那边才能拿到新鲜的数据。你想想看，如果机器人走了一米，但TF还停在原地，那导航肯定翻车。

4.1 TransformBroadcaster 核心API

在ROS里，动态TF发布靠的是 tf2_ros::TransformBroadcaster 这个类。别看它名字长，用起来其实就三板斧：创建对象、填充消息、发送。我最早接触它的时候，觉得跟发布普通topic没啥两样——嗯，本质上确实差不多，但有几个坑得注意。

核心API速览：

TransformBroadcaster() —— 构造函数，一般直接实例化就行
sendTransform(const geometry_msgs::TransformStamped & transform) —— 发送单条变换
sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms) —— 批量发送，一次调用发多条

你可能会问：就这？对，就这。但真正的功夫在消息填充上。 TransformStamped 里包含三个关键部分：header（时间戳+父坐标系）、child_frame_id（子坐标系）、transform（平移+旋转）。我见过不少新手把父子关系搞反，结果机器人在地上“漂移”。

4.2 实战：发布一个动态旋转的坐标系

咱们直接上代码。假设你有一个移动机器人，轮子每时每刻都在转。下面这个例子发布一个 wheel_link 相对于 base_link 的动态变换，角度随时间变化。

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "dynamic_tf_broadcaster");
    ros::NodeHandle nh;
    tf2_ros::TransformBroadcaster br;

    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.frame_id = "base_link";
    transformStamped.child_frame_id  = "wheel_link";

    ros::Rate rate(30);  // 30 Hz 发布

    while (ros::ok()) {
        // 时间戳必须更新！不然TF树会认为数据过期
        transformStamped.header.stamp = ros::Time::now();

        // 让轮子绕 Z 轴匀速旋转
        double angle = ros::Time::now().toSec() * 1.5;  // 1.5 rad/s
        tf2::Quaternion q;
        q.setRPY(0, 0, angle);
        transformStamped.transform.rotation.x = q.x();
        transformStamped.transform.rotation.y = q.y();
        transformStamped.transform.rotation.z = q.z();
        transformStamped.transform.rotation.w = q.w();

        // 平移量：轮子中心在 base_link 前方 0.3 米，左侧 0.2 米
        transformStamped.transform.translation.x = 0.3;
        transformStamped.transform.translation.y = 0.2;
        transformStamped.transform.translation.z = 0.0;

        br.sendTransform(transformStamped);
        rate.sleep();
    }
    return 0;
}

我的经验： 发布频率别太低。低于10Hz的话，监听器那边容易报“Lookup would require extrapolation into the past”的警告。我一般设30~50Hz，既平滑又不占太多CPU。

4.3 批量发送：一次搞定多个关节

如果你的机器人有6个关节，难道要写6个 sendTransform ？当然不用。 TransformBroadcaster 支持一次传入一个vector，内部帮你批量发布。这在多关节机器人里特别实用。

std::vector<geometry_msgs::TransformStamped> transforms;
// 填充关节1
geometry_msgs::TransformStamped t1;
t1.header.frame_id = "base_link";
t1.child_frame_id  = "joint1";
// ... 设置平移旋转 ...
transforms.push_back(t1);

// 填充关节2
geometry_msgs::TransformStamped t2;
t2.header.frame_id = "joint1";
t2.child_frame_id  = "joint2";
// ... 设置平移旋转 ...
transforms.push_back(t2);

// 一次性发送
br.sendTransform(transforms);

这样做的好处是减少ROS消息开销，而且所有变换的时间戳可以统一管理。我记得有一次做六轴机械臂的仿真，用批量发送之后CPU占用直接降了15%。

4.4 常见陷阱与避坑指南

我曾经踩过的坑：

时间戳忘了更新 —— 如果你在循环外用同一个 TransformStamped 对象，每次发送前一定要重新赋值 header.stamp。否则TF树里全是旧时间戳，监听器一查就报错。
父子坐标系命名搞反 —— 记住：frame_id 是父，child_frame_id 是子。比如轮子装在车体上，父是 base_link，子是 wheel_link。
旋转用错了四元数 —— 别手写四元数！用 tf2::Quaternion::setRPY() 或者 tf2::createQuaternionMsgFromYaw() 这类工具函数，不容易出错。

4.5 发布频率与性能的平衡

动态TF发布器不是越快越好。我见过有人设200Hz，结果CPU飙到80%，TF树里全是冗余数据。一般来说：

应用场景	推荐频率	说明
轮式里程计	30~50 Hz	轮子转速变化快，但没必要超过编码器采样率
机械臂关节	20~30 Hz	关节角度变化相对平滑，太高反而浪费
无人机姿态	50~100 Hz	姿态变化快，但也要看IMU输出率
仿真环境	与仿真步长一致	比如Gazebo通常用1000 Hz，但TF发布可以降采样到100 Hz

说白了，发布频率跟你的传感器更新率匹配就好。你想想看，里程计如果只有20Hz，你TF发到100Hz也没意义——数据都是重复的。

4.6 调试小技巧

写完动态TF发布器，怎么验证对不对？我一般用两个命令：

rosrun tf tf_echo base_link wheel_link —— 实时看两个坐标系之间的变换，数值在变就对了。
rosrun rqt_tf_tree rqt_tf_tree —— 图形化看TF树，能直观看到发布频率和最新时间戳。

有一次我调试AGV小车，发现TF树里 odom 到 base_link 的变换一直不变。查了半天，原来是里程计回调里忘了调用 sendTransform。嗯，这种低级错误，犯过一次就长记性了。