📚 课程目录第3章多机器人TF树构建

3. 多机器人TF树构建

任务目标

在ROS2中，当多个机器人共享同一套坐标系命名空间时，TF树会发生冲突。本案例为每个机器人构建独立的TF树，通过命名空间隔离各机器人的坐标系，并发布全局坐标系（map）到各机器人基座（base_link）的变换关系。

实现方案

采用命名空间前缀方式隔离TF树：每个机器人的所有坐标系都以robotX/为前缀（如robot1/base_link），全局坐标系map和odom不添加前缀，作为所有机器人共享的全局参考。使用tf2_ros::TransformBroadcaster分别发布各机器人的静态变换和动态变换。

环境依赖

ROS2 Humble或更高版本
已安装tf2_ros、geometry_msgs、rclcpp

关键完整代码

// multi_robot_tf_publisher.cpp
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <chrono>

using namespace std::chrono_literals;

class MultiRobotTfPublisher : public rclcpp::Node
{
public:
    MultiRobotTfPublisher() : Node("multi_robot_tf_publisher")
    {
        // 创建两个机器人的TF广播器
        tf_broadcaster_1_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        tf_broadcaster_2_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        // 发布静态变换：map -> odom（全局坐标系）
        publish_static_transform("map", "odom", 0.0, 0.0, 0.0);

        // 定时发布动态变换
        timer_ = this->create_wall_timer(100ms, [this]() {
            publish_robot_transforms();
        });
    }

private:
    void publish_static_transform(const std::string& parent, const std::string& child,
                                  double x, double y, double z)
    {
        geometry_msgs::msg::TransformStamped ts;
        ts.header.stamp = this->now();
        ts.header.frame_id = parent;
        ts.child_frame_id = child;
        ts.transform.translation.x = x;
        ts.transform.translation.y = y;
        ts.transform.translation.z = z;
        ts.transform.rotation.w = 1.0;
        tf_broadcaster_1_->sendTransform(ts);
    }

    void publish_robot_transforms()
    {
        auto now = this->now();

        // Robot1: odom -> robot1/base_link（动态变换，模拟移动）
        geometry_msgs::msg::TransformStamped t1;
        t1.header.stamp = now;
        t1.header.frame_id = "odom";
        t1.child_frame_id = "robot1/base_link";
        t1.transform.translation.x = 1.0 + 0.1 * sin(now.seconds());
        t1.transform.translation.y = 0.5;
        t1.transform.translation.z = 0.0;
        t1.transform.rotation.w = 1.0;
        tf_broadcaster_1_->sendTransform(t1);

        // Robot1: robot1/base_link -> robot1/laser（静态偏移）
        geometry_msgs::msg::TransformStamped t1_laser;
        t1_laser.header.stamp = now;
        t1_laser.header.frame_id = "robot1/base_link";
        t1_laser.child_frame_id = "robot1/laser";
        t1_laser.transform.translation.x = 0.2;
        t1_laser.transform.translation.y = 0.0;
        t1_laser.transform.translation.z = 0.1;
        t1_laser.transform.rotation.w = 1.0;
        tf_broadcaster_1_->sendTransform(t1_laser);

        // Robot2: odom -> robot2/base_link（动态变换）
        geometry_msgs::msg::TransformStamped t2;
        t2.header.stamp = now;
        t2.header.frame_id = "odom";
        t2.child_frame_id = "robot2/base_link";
        t2.transform.translation.x = -1.0;
        t2.transform.translation.y = -0.5 + 0.1 * cos(now.seconds());
        t2.transform.translation.z = 0.0;
        t2.transform.rotation.w = 1.0;
        tf_broadcaster_2_->sendTransform(t2);

        // Robot2: robot2/base_link -> robot2/camera（静态偏移）
        geometry_msgs::msg::TransformStamped t2_cam;
        t2_cam.header.stamp = now;
        t2_cam.header.frame_id = "robot2/base_link";
        t2_cam.child_frame_id = "robot2/camera";
        t2_cam.transform.translation.x = 0.15;
        t2_cam.transform.translation.y = 0.0;
        t2_cam.transform.translation.z = 0.3;
        t2_cam.transform.rotation.w = 1.0;
        tf_broadcaster_2_->sendTransform(t2_cam);
    }

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

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

编译与运行

# 在CMakeLists.txt中添加：
# find_package(rclcpp REQUIRED)
# find_package(tf2_ros REQUIRED)
# find_package(geometry_msgs REQUIRED)
# add_executable(multi_robot_tf_publisher src/multi_robot_tf_publisher.cpp)
# target_link_libraries(multi_robot_tf_publisher ${rclcpp_LIBRARIES} ${tf2_ros_LIBRARIES} ${geometry_msgs_LIBRARIES})

colcon build --packages-select your_package
source install/setup.bash
ros2 run your_package multi_robot_tf_publisher

验证方法

在另一个终端运行ros2 run tf2_ros tf2_echo map robot1/base_link，应持续输出平移分量（x在1.0附近正弦变化，y=0.5），证明robot1的TF树已连接到全局坐标系。
运行ros2 run tf2_ros tf2_echo map robot2/base_link，应输出x=-1.0，y在-0.5附近余弦变化。
运行ros2 run tf2_ros tf2_echo robot1/base_link robot1/laser，应输出固定平移(0.2, 0.0, 0.1)，证明机器人内部TF树完整。
运行ros2 run tf2_ros tf2_echo robot1/base_link robot2/base_link，应报错Could not transform，证明两机器人TF树相互隔离。

TF树结构示意

图中红色分支为robot1的TF树，绿色分支为robot2的TF树，两树仅在全局坐标系odom处交汇，各机器人内部坐标系互不可见，实现了TF树隔离。