第2章 TF广播器与监听器

这一章我们直接动手。TF广播器和监听器，说白了就是ROS里坐标变换的“嘴”和“耳朵”。你想想看，机器人身上那么多坐标系——激光雷达、摄像头、机械臂末端——它们之间怎么知道彼此的位置？靠的就是广播器不停地“喊”，监听器安静地“听”。

我个人习惯把TF广播器想象成一个实时广播电台，而监听器就是调频收音机。频率对上了，数据就通了。好，咱们从最简单的静态广播开始。

2.1 编写静态TF广播器

静态TF广播器，顾名思义，发布的是固定不变的变换。比如机器人底座到激光雷达的偏移，装好就不会动了。我建议你用 static_transform_publisher 命令行工具，一行搞定，但为了深入理解，咱们还是手写一个节点。

// C++ 示例：静态TF广播器
#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, 1.57); // 绕Z轴旋转90度
    transform.setRotation(q);

    ros::Rate rate(10);
    while (nh.ok()) {
        // 注意：静态变换虽然不变，但需要持续发布才能被缓存
        broadcaster.sendTransform(
            tf::StampedTransform(transform, ros::Time::now(),
                                 "base_link", "laser_frame"));
        rate.sleep();
    }
    return 0;
}

你看，代码很简单。但有个坑——时间戳。我曾经在项目里用了 ros::Time(0)，结果监听器那边一直拿不到最新变换，折腾了半天。记住：静态广播虽然内容不变，但时间戳必须用 ros::Time::now()，否则tf缓存会认为数据过期。

2.2 编写动态TF广播器

动态广播器就更有意思了。比如机械臂每个关节的角度在变，末端执行器的位姿就得实时广播。说白了，就是每算出一个新位置，就“喊”一嗓子。

# Python 示例：动态TF广播器（关节状态发布）
#!/usr/bin/env python3
import rospy
import tf2_ros
import geometry_msgs.msg
import math

def dynamic_broadcaster():
    rospy.init_node('dynamic_tf_broadcaster')
    broadcaster = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

    rate = rospy.Rate(30)  # 30Hz
    angle = 0.0
    while not rospy.is_shutdown():
        angle += 0.02
        t.header.stamp = rospy.Time.now()
        t.header.frame_id = "base_link"
        t.child_frame_id = "arm_end"
        t.transform.translation.x = 0.5 * math.cos(angle)
        t.transform.translation.y = 0.5 * math.sin(angle)
        t.transform.translation.z = 0.2
        q = tf.transformations.quaternion_from_euler(0, 0, angle)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        broadcaster.sendTransform(t)
        rate.sleep()

if __name__ == '__main__':
    try:
        dynamic_broadcaster()
    except rospy.ROSInterruptException:
        pass

这里我用了 tf2_ros，它是ROS的升级版tf库。我个人更推荐用tf2，因为它的API更清晰，而且支持缓冲查询。嗯，动态广播的关键是每次都要更新header.stamp，不然监听器那边会报“extrapolation”错误。

2.3 编写TF监听器

监听器就是“收音机”。它订阅 /tf 话题，然后把所有变换缓存起来。你可以随时查询任意两个坐标系之间的关系。

// C++ 监听器示例
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle nh;
    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener listener(tfBuffer);

    ros::Rate rate(10);
    while (nh.ok()) {
        geometry_msgs::TransformStamped transformStamped;
        try {
            transformStamped = tfBuffer.lookupTransform(
                "base_link", "laser_frame", ros::Time(0));
            // ros::Time(0) 表示获取最新可用的变换
            ROS_INFO("平移: %.2f %.2f %.2f",
                     transformStamped.transform.translation.x,
                     transformStamped.transform.translation.y,
                     transformStamped.transform.translation.z);
        } catch (tf2::TransformException &ex) {
            ROS_WARN("%s", ex.what());
        }
        rate.sleep();
    }
    return 0;
}

写监听器的时候，一定要用 try-catch。为什么？因为如果两个坐标系之间还没有建立连接，或者时间不同步，tf2会抛出异常。我曾经在调试时没加异常处理，程序直接崩溃，后来养成了习惯——所有 lookupTransform 都包在 try 里。

2.4 理解坐标变换流程

整个流程其实就三步：

1. 广播器 发布变换树的一个“枝干”（父子坐标系关系 + 时间戳）。
2. tf缓存 在后台把所有广播的变换拼成一棵树。
3. 监听器 从缓存里查路径，算出任意两个坐标系间的变换矩阵。

你想想看，这就像你手里有一张地图（变换树），广播器负责更新地图上的路标，监听器则根据路标规划路线。我习惯用 tf_echo 命令行工具先验证一下广播对不对，再写代码。

组件	职责	常见坑点
静态广播器	发布固定变换（如传感器安装偏移）	时间戳必须更新，否则缓存丢弃
动态广播器	发布时变变换（如关节位置）	频率与控制周期匹配，避免丢帧
TF监听器	查询坐标系变换，支持时间回溯	必须处理 TransformException

最后说一句，理解坐标变换流程，最好的办法就是打开 rviz，把广播的坐标系显示出来。眼见为实，比看代码直观多了。好，这一章的内容就到这儿，动手试试吧。