第四章：动态变换发布器——让坐标动起来

各位同学，欢迎来到动态变换的世界。

上一章我们聊了静态变换，说白了就是「焊死」的坐标关系。但自动驾驶里，哪有东西是静止的？车辆在跑，行人会动，就连路边的树，在激光雷达看来都在「移动」。所以这一章，我们来解决核心问题：如何发布一个会随时间变化的TF变换。

4.1 动态变换 vs 静态变换：本质区别在哪？

静态变换发布一次，广播出去就完事了。动态变换呢？你得持续不断地发布。

我刚开始接触Autoware时，犯过一个低级错误——在循环里只发布了一次变换，结果定位模块死活不认。后来才明白，TF树要求每个时间戳都有对应的变换数据。你想想看，如果车辆在动，但TF里「base_link」到「map」的关系还是3秒前的，那规划出来的路径能对吗？

4.2 动手写一个动态TF广播器

好，我们直接上代码。我个人习惯用C++写TF广播，因为性能更可控。但Python版本我也会给出，方便大家调试。

4.2.1 C++版本：一个会转圈的「虚拟障碍物」

假设我们要模拟一个在停车场里绕圈的行人。这个行人每帧都在移动，所以它的「odom」到「pedestrian」的变换必须实时更新。

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

class DynamicTfBroadcaster : public rclcpp::Node
{
public:
  DynamicTfBroadcaster() : Node("dynamic_tf_broadcaster")
  {
    broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(50),  // 20Hz
      std::bind(&DynamicTfBroadcaster::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 = "pedestrian";

    // 模拟一个半径为3米的圆周运动
    double angle = this->now().seconds() * 0.5;
    t.transform.translation.x = 3.0 * cos(angle);
    t.transform.translation.y = 3.0 * sin(angle);
    t.transform.translation.z = 0.0;

    // 让行人始终面朝运动方向
    tf2::Quaternion q;
    q.setRPY(0, 0, angle + M_PI/2);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    broadcaster_->sendTransform(t);
  }

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

嗯，这里要注意：时间戳必须用当前时间。我曾经在调试时用了rclcpp::Time(0)，结果TF查询一直报「Lookup would require extrapolation into the past」。说白了，TF2靠时间戳来插值，你给个0时刻，它当然找不到。

4.2.2 Python版本：快速验证用

import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
import math

class DynamicTfBroadcasterPy(Node):
    def __init__(self):
        super().__init__('dynamic_tf_broadcaster_py')
        self.br = TransformBroadcaster(self)
        self.timer = self.create_timer(0.05, self.publish_tf)  # 20Hz

    def publish_tf(self):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'odom'
        t.child_frame_id = 'pedestrian'

        angle = self.get_clock().now().nanoseconds * 1e-9 * 0.5
        t.transform.translation.x = 3.0 * math.cos(angle)
        t.transform.translation.y = 3.0 * math.sin(angle)
        t.transform.translation.z = 0.0

        # 四元数计算
        q = self.euler_to_quaternion(0, 0, angle + math.pi/2)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        self.br.sendTransform(t)

4.3 核心逻辑：动态变换的知识体系

为了让大家更直观地理解，我画了一张流程图。它展示了从传感器数据到TF广播的完整链路。

4.4 避坑指南：我踩过的三个坑

1. 时间戳不同步：我曾经把传感器的时间戳直接塞进TF，结果传感器是100Hz，TF广播是20Hz，导致时间戳跳跃。解决方案：统一用this->now()。
2. 坐标系命名冲突：两个节点同时广播同一个child_frame_id，TF树直接崩溃。我建议在命名时加上模块前缀，比如lidar_front、camera_rear。
3. 频率过高导致CPU飙升：有次我把广播频率设到200Hz，结果工控机直接卡死。动态TF不是越快越好，20-50Hz足够满足大多数规划控制需求。

4.5 实战：让动态TF跑起来

写完了代码，怎么验证？我一般用三步法：

• 第一步：启动广播节点
  ros2 run your_package dynamic_tf_broadcaster
• 第二步：查看TF树
  ros2 run tf2_tools view_frames
  打开生成的frames.pdf，确认pedestrian节点在持续出现。
• 第三步：实时监听
  ros2 run tf2_ros tf2_echo odom pedestrian
  你会看到平移和旋转数据在不停变化。如果数据不动，检查一下时间戳是不是当前时间。

我个人习惯在第三步用tf2_echo配合watch -n 0.1，这样每100ms刷新一次，能明显看到数值变化。你想想看，如果数值一直不变，那肯定是有问题的。

4.6 本章小结

动态变换发布器，说白了就是「定时器 + TransformBroadcaster」的组合。核心就三点：

• 用定时器保证发布频率
• 每帧更新时间戳和位姿数据
• 保持父坐标系稳定，子坐标系随动

嗯，掌握了这些，你就可以让任何物体在TF树里「活」起来。下一章我们会聊如何接收这些动态变换，并在代码里查询它们。但那是后话了，先把今天的代码跑通再说。