4. 动态变换发布器：编写C++节点发布随时间变化的坐标变换

好，咱们进入第四章。上一章我们搞定了静态变换，说白了就是“焊死”的坐标关系。但机器人动起来之后，轮子转、关节动、激光雷达跟着晃——这些变换每一刻都在变。你想想看，如果还用静态发布，那导航肯定乱套。所以这一章，我带你手写一个动态变换发布器，用C++节点让坐标变换“活”起来。

4.1 动态变换 vs 静态变换 —— 本质区别

静态变换只发一次，或者频率极低（比如机器人底座到激光雷达的偏移）。动态变换则要求高频、持续地发布，比如每10ms更新一次里程计到base_link的位姿。我个人习惯把动态变换看作“会呼吸的TF”——它必须跟着传感器数据或运动学模型走。

特性	静态变换	动态变换
发布频率	通常1~10 Hz	50~200 Hz (甚至更高)
坐标关系	固定不变	随时间连续变化
典型用途	传感器安装偏移	里程计、关节状态、SLAM位姿
发布器类型	StaticTransformBroadcaster	TransformBroadcaster

嗯，这里要注意：动态变换发布器虽然叫“动态”，但发布的内容仍然是一个瞬时快照。只不过我们以很高的频率不断更新这个快照，让下游节点感觉坐标在连续运动。

4.2 编写动态变换发布节点（C++）

我会用一个最简单的例子：模拟一个旋转的激光雷达。假设雷达装在机器人顶部，它自身以恒定角速度旋转。我们发布 base_link 到 laser_frame 的变换，其中旋转角随时间线性增加。

先看完整代码，然后我拆开讲。

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>

class DynamicTfPublisher : public rclcpp::Node
{
public:
    DynamicTfPublisher() : Node("dynamic_tf_publisher")
    {
        broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(20),  // 50Hz
            std::bind(&DynamicTfPublisher::publish_tf, this));
        RCLCPP_INFO(this->get_logger(), "动态变换发布器已启动，50Hz");
    }

private:
    void publish_tf()
    {
        geometry_msgs::msg::TransformStamped t;
        t.header.stamp = this->now();
        t.header.frame_id = "base_link";
        t.child_frame_id = "laser_frame";

        // 模拟旋转：随时间改变偏航角
        double yaw = this->now().seconds() * 0.5;  // 每秒转0.5弧度
        tf2::Quaternion q;
        q.setRPY(0, 0, yaw);
        t.transform.rotation.x = q.x();
        t.transform.rotation.y = q.y();
        t.transform.rotation.z = q.z();
        t.transform.rotation.w = q.w();

        // 平移固定：雷达在机器人中心上方0.2米
        t.transform.translation.x = 0.0;
        t.transform.translation.y = 0.0;
        t.transform.translation.z = 0.2;

        broadcaster_->sendTransform(t);
    }

    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
};

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

4.3 代码拆解 —— 每一行都在干什么

• 创建广播器：tf2_ros::TransformBroadcaster 是动态发布的核心对象。我把它声明为成员变量，避免每次发布都重新构造。
• 定时器驱动：用 create_wall_timer 以固定频率触发发布。20ms对应50Hz，对于大多数导航场景足够了。如果你做高速运动，可以提到100Hz甚至200Hz。
• 填充TransformStamped：注意 frame_id 是父坐标系，child_frame_id 是子坐标系。这里 base_link → laser_frame。
• 旋转计算：我用当前时间（秒）乘以角速度得到偏航角。实际项目中，你应该从里程计或编码器读取真实角度。
• 发送：broadcaster_->sendTransform(t) 会把变换发布到 /tf 话题。所有监听器都会收到。

4.4 动态变换的核心逻辑图

下面这张SVG图帮你理清整个数据流：

4.5 编译与运行

把上面的代码放到你的ROS2工作包里，比如 src/tf_demo/src/dynamic_tf_publisher.cpp。然后在 CMakeLists.txt 里添加：

add_executable(dynamic_tf_publisher src/dynamic_tf_publisher.cpp)
ament_target_dependencies(dynamic_tf_publisher rclcpp tf2_ros geometry_msgs tf2)

编译：

cd ~/ros2_ws
colcon build --packages-select tf_demo
source install/setup.bash

运行：

ros2 run tf_demo dynamic_tf_publisher

然后用 ros2 topic echo /tf 就能看到变换数据源源不断输出。你也可以打开 rviz2，添加TF显示，观察 laser_frame 是否绕着 base_link 旋转。

4.6 动态发布中的时间策略

你可能会问：时间戳到底用 this->now() 还是用传感器数据的时间？我的建议是：尽量使用数据源的时间。比如你从激光雷达驱动拿到了一帧点云，点云的时间戳是 scan->header.stamp，那么发布对应的TF时，也应该用那个时间戳。这样才能保证TF和传感器数据在时间上对齐。

但如果你只是做纯运动学模拟（比如我们上面的旋转雷达），直接用 now() 没问题。记住一个原则：TF的时间戳代表“这个变换在什么时刻有效”。

4.7 扩展：发布多个动态变换

一个节点可以同时发布多个动态变换。你只需要多次调用 sendTransform，或者使用 sendTransform 的重载版本传入 vector。我个人习惯在同一个定时器回调里构建多个 TransformStamped，然后一起发送，减少锁竞争。

std::vector<geometry_msgs::msg::TransformStamped> transforms;
// 填充 transforms[0] (base_link → laser_frame)
// 填充 transforms[1] (base_link → camera_frame)
broadcaster_->sendTransform(transforms);

嗯，这里要注意：每个变换的 child_frame_id 必须唯一，否则TF树会混乱。

---

好了，动态变换发布器就讲到这里。说白了，它就是让TF“动起来”的引擎。但别急，先把今天的代码跑通，感受一下坐标变换在时间轴上的流动。