4. TF广播器编写：C++与Python实现静态与动态广播

说到写广播器，很多新手第一反应就是「调个API不就行了？」
嗯，确实，API 就那么几个。但我在项目里见过太多因为广播器写得太随意，导致整个 TF 树乱成一锅粥的情况。
说白了，广播器是 TF 树的「水源」，源头一旦浑浊，下游监听器全得遭殃。

这一章，我带你手写两种广播器：静态广播 和 动态广播。
分别用 C++ 和 Python 实现。你想想看，同一个功能，两种语言，背后逻辑完全一致，但踩坑点却不太一样。

4.1 静态广播器 — 本质就是「发一次，管到死」

静态变换，比如激光雷达装在机器人底座前方 0.2 米处，这种关系装好就不会变了。
我个人习惯用 static_transform_publisher 命令行工具快速验证，但正式项目里还是得自己写节点。

C++ 静态广播示例

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "static_tf_broadcaster");
    ros::NodeHandle nh;
    tf::TransformBroadcaster broadcaster;

    tf::Transform transform;
    transform.setOrigin( tf::Vector3(0.2, 0.0, 0.1) );
    tf::Quaternion q;
    q.setRPY(0, 0, 0); // 无旋转
    transform.setRotation(q);

    // 注意：静态变换只需广播一次，但为了保险，我通常放在循环外
    broadcaster.sendTransform(
        tf::StampedTransform(transform, ros::Time::now(), "base_link", "laser_frame")
    );

    ros::spin(); // 保持节点存活
    return 0;
}

Python 静态广播示例

#!/usr/bin/env python3
import rospy
import tf

if __name__ == '__main__':
    rospy.init_node('static_tf_broadcaster_py')
    br = tf.TransformBroadcaster()
    # 静态变换，只发一次
    br.sendTransform((0.2, 0.0, 0.1),
                     (0.0, 0.0, 0.0, 1.0),
                     rospy.Time.now(),
                     "laser_frame",
                     "base_link")
    rospy.spin()

4.2 动态广播器 — 每一帧都在「变」

动态广播才是真正体现机器人实时状态的地方。比如移动机器人的里程计坐标系 odom → base_footprint，每来一次里程计数据就要更新一次。
你想想看，如果广播频率跟不上控制频率，TF 树就会出现断层，导航直接原地懵圈。

C++ 动态广播（循环发布）

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->pose.pose.position.x,
                                     msg->pose.pose.position.y,
                                     0.0) );
    transform.setRotation( tf::Quaternion(
        msg->pose.pose.orientation.x,
        msg->pose.pose.orientation.y,
        msg->pose.pose.orientation.z,
        msg->pose.pose.orientation.w) );
    // 使用里程计的时间戳，而不是 ros::Time::now()
    br.sendTransform(tf::StampedTransform(transform, msg->header.stamp,
                                          "odom", "base_footprint"));
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "dynamic_tf_broadcaster");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/odom", 10, odomCallback);
    ros::spin();
    return 0;
}

Python 动态广播（循环发布）

#!/usr/bin/env python3
import rospy
import tf
from nav_msgs.msg import Odometry

def odom_callback(msg):
    br = tf.TransformBroadcaster()
    # 注意：这里每次回调都创建新的 broadcaster，但 ROS 内部会复用
    br.sendTransform(
        (msg.pose.pose.position.x, msg.pose.pose.position.y, 0.0),
        (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
         msg.pose.pose.orientation.z, msg.pose.pose.orientation.w),
        msg.header.stamp,
        "base_footprint",
        "odom"
    )

if __name__ == '__main__':
    rospy.init_node('dynamic_tf_broadcaster_py')
    rospy.Subscriber('/odom', Odometry, odom_callback)
    rospy.spin()

4.3 静态 vs 动态 — 一张表说清楚

对比项	静态广播	动态广播
发布频率	一次（或极低频率）	随数据源频率（10~100Hz）
典型场景	传感器安装偏移、固定支架	里程计、关节状态、SLAM位姿
时间戳策略	可用 ros::Time::now()	务必使用数据源时间戳
常见错误	忘记设置 child_frame_id	时间戳跳跃或滞后
推荐实现	tf2_ros::StaticTransformBroadcaster	tf::TransformBroadcaster + 回调

嗯，到这里你应该能自己写出两种广播器了。我个人建议你从 Python 版本开始测试，因为改起来快，等逻辑确认无误再转成 C++ 部署到真机上。