📚 课程目录第4章 TF2消息与数据类型

4. TF2消息与数据类型

好，这一章我们直接上手写代码。动态广播器，说白了就是让坐标变换随着时间动起来。你想想看，机器人轮子一转，odom 到 base_link 的关系肯定得跟着变。静态广播器只能发布固定不变的变换，而动态广播器才是真正驱动机器人运动学的那根线。

4.1 为什么需要动态广播？

我记得第一次做差速底盘时，以为只要发布一次 odom→base_link 就够了。结果小车一转弯，TF树直接报错——坐标变换时间戳对不上。嗯，这里要注意：里程计数据是连续变化的，每一帧都要告诉TF系统“我现在在哪儿”。

里程计积分：轮式编码器每来一次，位置就更新一点。
传感器融合：IMU、激光雷达都会修正位姿，必须实时广播。
导航堆栈：全局规划器依赖最新的 odom→base_link，延迟一帧都可能撞墙。

💡 我个人习惯：动态广播器的发布频率至少和里程计更新频率一致，通常是 50~100 Hz。别偷懒用 10 Hz，控制周期会抖。

4.2 核心流程：从里程计到TF变换

先画一张图，把逻辑理清楚。下面这个流程我几乎在每个机器人项目里都用，算是标准模板了。

流程不复杂：编码器给速度 → 积分得到位姿 → 广播器打包成 TransformStamped 发出去。我在项目里经常在积分后面加一个滤波器，不然轮子打滑时位姿会飘得离谱。

4.3 编写动态广播节点（C++ 示例）

下面是一个完整的动态广播节点。我习惯用 nav_msgs/msg/Odometry 作为输入，但实际项目中你也可以直接订阅 /joint_states 或者编码器原始消息。

// dynamic_tf_broadcaster.cpp
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>

class DynamicTfBroadcaster : public rclcpp::Node
{
public:
    DynamicTfBroadcaster() : Node("dynamic_tf_broadcaster")
    {
        // 初始化广播器
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        // 订阅里程计话题 (通常来自 robot_localization 或编码器节点)
        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10,
            std::bind(&DynamicTfBroadcaster::odomCallback, this, std::placeholders::_1));
    }

private:
    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        // 构建变换
        geometry_msgs::msg::TransformStamped t;
        t.header.stamp = msg->header.stamp;
        t.header.frame_id = "odom";
        t.child_frame_id = "base_link";

        // 位置直接从里程计消息中取
        t.transform.translation.x = msg->pose.pose.position.x;
        t.transform.translation.y = msg->pose.pose.position.y;
        t.transform.translation.z = msg->pose.pose.position.z;

        // 姿态 (四元数)
        t.transform.rotation = msg->pose.pose.orientation;

        // 发布变换
        tf_broadcaster_->sendTransform(t);
    }

    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DynamicTfBroadcaster>());
    rclcpp::shutdown();
    return 0;
}

            关键点： 时间戳必须用 msg->header.stamp，而不是 now()。为什么？因为里程计数据本身有延迟，用当前时间会让TF插值错乱。我曾经在这个坑里趴了一整天……
        

4.4 处理里程计数据：常见陷阱

里程计数据不是拿来就能用的。下面几个问题我几乎在每个项目里都遇到过：

陷阱	表现	我的解法
坐标系不统一	odom 和 base_link 反了	先确认 `frame_id` 和 `child_frame_id`，用 `tf2_echo` 验证
时间戳跳变	TF 报 extrapolation error	检查里程计发布频率是否稳定，加异常值过滤
Z轴高度乱飘	base_link 忽高忽低	差速机器人通常固定 Z=0，直接硬编码
四元数非法	TF 树显示 nan	归一化检查，或者用 `tf2::Quaternion::normalize()`

⚠️ 我曾经在调试AGV时，里程计消息的协方差矩阵全是零，结果定位模块直接罢工。记得给协方差赋合理值，哪怕单位阵也比全零强。

4.5 启动与验证

编译运行后，用命令行工具看一眼：

# 终端1 启动节点
ros2 run your_package dynamic_tf_broadcaster

# 终端2 查看TF树
ros2 run tf2_tools view_frames

# 或者实时监听
ros2 run tf2_ros tf2_echo odom base_link

如果看到变换的平移和旋转随着机器人移动而变化，说明动态广播器跑起来了。我习惯同时打开 rviz2，添加TF显示，这样更直观。

🧪 小技巧：在 odomCallback 里加一行 RCLCPP_INFO 打印位置，方便调试。但记得上线前关掉，不然日志刷屏。

4.6 扩展：从编码器直接积分

有时候你拿不到现成的 /odom 话题，只有轮子速度。那就自己写一个积分器。核心公式很简单：

// 差速模型积分 (简化版)
double delta_x = (v_left + v_right) / 2.0 * cos(theta) * dt;
double delta_y = (v_left + v_right) / 2.0 * sin(theta) * dt;
double delta_theta = (v_right - v_left) / wheel_separation * dt;

x += delta_x;
y += delta_y;
theta += delta_theta;

然后把 x, y, theta 打包成 TransformStamped。嗯，这里要注意：积分会累积误差，所以实际产品中一定要配合视觉或激光的闭环校正。我早期做的一个小机器人，纯靠编码器积分，跑十米能偏半米……后来加了UKF才稳住。

4.7 本章小结

动态广播器是连接里程计和TF树的桥梁。说白了，就是每来一帧数据，就发一次变换。代码量不大，但细节很多：时间戳、坐标系、异常处理。我个人建议：先把基础版本跑通，再逐步加入滤波和异常检测。