4、坐标变换基础（TF2）：TF2树的概念、静态与动态坐标变换发布、坐标监听与变换计算、常见坐标系（map, odom, base_link, laser）定义

各位同学，欢迎来到第四章。

说实话，坐标变换是自动驾驶里最容易让人头晕的部分。我当年刚入行时，调试一个定位模块，发现小车明明在往前走，地图上的点却在往后飘。折腾了两天，最后发现是TF树里一个坐标系的父节点挂错了。嗯，从那以后，我对TF2就格外上心。

4.1 TF2树的概念

TF2，全称是Transform Library 2。说白了，它就是一个帮你管理坐标系之间关系的工具包。

你想想看，一辆自动驾驶车上有多少个坐标系？激光雷达有自己的坐标系，IMU有自己的坐标系，车轮中心也有自己的坐标系。这些坐标系之间怎么互相转换？靠手算？不现实。靠猜？更不行。

TF2用一棵树来组织这些关系。这棵树叫TF树。

我个人习惯把TF树想象成家族谱系。map是老祖宗，odom是儿子，base_link是孙子，laser是曾孙。每个节点都知道自己相对于父节点的位置和姿态。

为什么是树而不是网？因为如果允许循环依赖，变换计算就会陷入死循环。ROS2的设计者很聪明，强制要求无环结构，保证了计算的稳定性和实时性。

4.2 静态坐标变换发布

静态变换，就是那些永远不会变的变换关系。比如激光雷达安装在车顶，它的位置相对于车体是固定的。你不可能开着开着，激光雷达自己挪了个位置——除非它松了，那叫故障。

发布静态变换有两种方式：

1. 命令行方式：适合快速测试
2. 代码方式：适合正式部署

4.2.1 命令行发布静态变换

ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

举个例子，激光雷达相对于base_link的位置是(0.5, 0, 0.3)米，姿态是(0, 0, 0)弧度：

ros2 run tf2_ros static_transform_publisher 0.5 0 0.3 0 0 0 base_link laser

4.2.2 代码发布静态变换

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/static_transform_broadcaster.h>

class StaticTFPublisher : public rclcpp::Node
{
public:
    StaticTFPublisher() : Node("static_tf_publisher")
    {
        broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
        
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = this->get_clock()->now();
        transform.header.frame_id = "base_link";
        transform.child_frame_id = "laser";
        
        transform.transform.translation.x = 0.5;
        transform.transform.translation.y = 0.0;
        transform.transform.translation.z = 0.3;
        
        transform.transform.rotation.x = 0.0;
        transform.transform.rotation.y = 0.0;
        transform.transform.rotation.z = 0.0;
        transform.transform.rotation.w = 1.0;
        
        broadcaster_->sendTransform(transform);
    }
    
private:
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
};

注意看，静态变换只需要发布一次。TF2内部会缓存它，后续查询时直接返回。这比动态变换省资源得多。

4.3 动态坐标变换发布

动态变换，就是那些随时间变化的变换关系。最典型的就是odom到base_link的变换——车在动，这个变换每时每刻都在变。

我曾经犯过一个错误：把里程计的动态变换用静态方式发布。结果小车明明在转弯，TF树里却显示它还在原地。定位模块直接崩溃。

发布动态变换的代码示例：

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.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(50),  // 20Hz
            std::bind(&DynamicTFPublisher::publish_tf, this));
    }
    
private:
    void publish_tf()
    {
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = this->get_clock()->now();
        transform.header.frame_id = "odom";
        transform.child_frame_id = "base_link";
        
        // 这里假设从里程计获取到了位置和姿态
        transform.transform.translation.x = odom_x_;
        transform.transform.translation.y = odom_y_;
        transform.transform.translation.z = 0.0;
        
        // 四元数表示姿态
        transform.transform.rotation.x = 0.0;
        transform.transform.rotation.y = 0.0;
        transform.transform.rotation.z = sin(yaw_ / 2);
        transform.transform.rotation.w = cos(yaw_ / 2);
        
        broadcaster_->sendTransform(transform);
    }
    
    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
    double odom_x_ = 0.0, odom_y_ = 0.0, yaw_ = 0.0;
};

4.4 坐标监听与变换计算

发布变换只是第一步。更关键的是，其他节点怎么拿到这些变换？这就需要用到TF2的Listener。

我见过很多新手，自己手动算坐标变换。明明有TF2这么好的工具不用，非要造轮子。结果算出来的坐标经常差个几厘米，定位精度直接拉胯。

正确的做法是使用TF2的TransformListener：

#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

class TransformListenerNode : public rclcpp::Node
{
public:
    TransformListenerNode() : Node("transform_listener")
    {
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
        
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&TransformListenerNode::lookup_transform, this));
    }
    
private:
    void lookup_transform()
    {
        try {
            // 查询从laser到map的变换
            geometry_msgs::msg::TransformStamped transform =
                tf_buffer_->lookupTransform(
                    "map",           // 目标坐标系
                    "laser",         // 源坐标系
                    tf2::TimePointZero);  // 获取最新变换
            
            double x = transform.transform.translation.x;
            double y = transform.transform.translation.y;
            RCLCPP_INFO(this->get_logger(), 
                "Laser in map: (%.2f, %.2f)", x, y);
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "Could not get transform: %s", ex.what());
        }
    }
    
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    rclcpp::TimerBase::SharedPtr timer_;
};

4.5 常见坐标系定义

在自动驾驶中，有四个坐标系你必须要搞清楚。我按照TF树的层级顺序来讲：

坐标系	父节点	作用	更新频率
map	无（根节点）	全局地图坐标系，固定不变	静态
odom	map	里程计坐标系，用于局部定位	动态（高频）
base_link	odom	车辆本体坐标系，原点在车辆中心	动态（高频）
laser	base_link	激光雷达坐标系	静态

4.5.1 map坐标系

map是TF树的根节点。它代表全局地图的参考系，一旦建立就不会改变。所有的全局规划、定位结果最终都要转换到map坐标系下。

4.5.2 odom坐标系

odom是map的子节点。它由里程计（轮式编码器、IMU等）维护，表示车辆相对于起点的位置。注意，odom会随时间漂移，所以它和map之间需要定位模块来修正。

4.5.3 base_link坐标系

base_link是车辆的本体坐标系。原点通常在后轴中心，x轴指向车头，y轴指向左侧，z轴向上。所有的传感器数据最终都要转换到base_link下才能被控制模块使用。

4.5.4 laser坐标系

laser是激光雷达的坐标系。它相对于base_link是固定的。我在项目中遇到过一个问题：激光雷达安装时有点歪，导致点云数据整体偏了2度。如果不发布正确的静态变换，建图效果会一塌糊涂。

4.6 本章知识体系图

下面这张图展示了TF2的核心结构和数据流向。我画这张图时，特意把坐标系层级和发布/监听关系放在一起，方便你理解整体框架。

这张图里，从上到下是TF树的层级关系。map在最顶层，laser在最底层。左侧的Broadcaster负责把变换数据发布到TF树中，右侧的Listener则从树中查询所需的变换。

嗯，到这里，坐标变换的基础内容就讲完了。记住一句话：TF2的核心就是管理坐标系之间的父子关系，让数据在不同坐标系之间自由流转。你只要把树搭对了，剩下的计算TF2会帮你搞定。

可可精品资料库