ROS2学习（15）------ROS2 TF2 机器人坐标系管理器

• 操作系统：ubuntu22.04
• IDE:Visual Studio Code
• 编程语言：C++11
• ROS版本：2

在 ROS 2 中，TF2（Transform Library, v2） 是一个非常核心的工具库，用于管理多个坐标系之间的 变换关系（translation + rotation）。它广泛应用于机器人导航、SLAM、机械臂控制等场景。

什么是 TF2？

简单来说：

TF2 是 ROS 2 中用于实时跟踪和转换多个坐标系之间位置与姿态关系的系统。

你可以把它想象成机器人的"空间感知大脑"，帮助你在不同坐标系之间进行数据对齐和坐标变换。

常见应用场景

• 将激光雷达的数据从传感器坐标系转换到机器人底盘坐标系。
• 在导航中将地图坐标系（map）下的路径点转换为机器人当前坐标系（base_link）下的相对位置。
• 控制机械臂末端执行器时，需要知道其相对于世界坐标系的位置。
• 多传感器融合（如 IMU + 激光雷达 + 视觉）时，需要统一坐标系。

核心概念

名称	含义
frame_id	坐标系名称，例如 /base_link, /laser, /map
transform	描述两个坐标系之间的平移和旋转（translation + rotation）
tf2_ros::TransformBroadcaster	发布坐标变换信息
tf2_ros::TransformListener	订阅并缓存坐标变换信息
tf2::Buffer	存储所有已知的坐标变换，支持查询历史数据
static_transform_publisher	用于发布静态坐标变换（常用于 URDF）

使用 TF2 的基本流程

1. 安装依赖包（Ubuntu）

 sudo apt install ros-humble-tf2-tools ros-humble-tf2-ros ros-humble-geometry-msgs

2. 示例：发布和监听坐标变换

下面是一个简单的 C++ 示例，演示如何使用 TF2 来广播和监听坐标变换。

创建 my_tf2_demo 包的命令如下：

ros2 pkg  create --build-type ament_cmake my_tf2_demo

文件结构：

3.示例一：发布坐标变换（tf2_broadcaster.cpp）

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

class FramePublisher : public rclcpp::Node {
public:
    FramePublisher() : Node("tf2_broadcaster") {
        broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            [this]() { this->timer_callback(); });
    }

private:
    void timer_callback() {
        geometry_msgs::msg::TransformStamped t;

        t.header.stamp = this->now();
        t.header.frame_id = "world";
        t.child_frame_id = "robot_base";

        // 位置
        t.transform.translation.x = 1.0;
        t.transform.translation.y = 0.5;
        t.transform.translation.z = 0.0;

        // 四元数（绕 Z 轴旋转 30 度）
        double angle = 0.5236; // radians
        t.transform.rotation.w = cos(angle / 2);
        t.transform.rotation.x = 0.0;
        t.transform.rotation.y = 0.0;
        t.transform.rotation.z = sin(angle / 2);

        broadcaster_->sendTransform(t);
    }

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

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<FramePublisher>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

4.示例二：监听并查询坐标变换（tf2_listener.cpp）

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

class FrameListener : public rclcpp::Node {
public:
    FrameListener() : Node("tf2_listener") {
        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
        listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            [this]() { this->timer_callback(); });
    }

private:
    void timer_callback() {
        try {
            geometry_msgs::msg::TransformStamped transform =
                tf_buffer_->lookupTransform("world", "robot_base", tf2::TimePointZero);

            RCLCPP_INFO(this->get_logger(), "Translation: (%.2f, %.2f, %.2f)",
                        transform.transform.translation.x,
                        transform.transform.translation.y,
                        transform.transform.translation.z);

            RCLCPP_INFO(this->get_logger(), "Rotation (quat): (%.2f, %.2f, %.2f, %.2f)",
                        transform.transform.rotation.x,
                        transform.transform.rotation.y,
                        transform.transform.rotation.z,
                        transform.transform.rotation.w);

        } catch (const tf2::TransformException & ex) {
            RCLCPP_WARN(this->get_logger(), "Could not get transform: %s", ex.what());
        }
    }

    rclcpp::TimerBase::SharedPtr timer_;
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> listener_;
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<FrameListener>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

5. 配置 CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(my_tf2_demo)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)

message(STATUS "rclcpp include dirs: ${rclcpp_INCLUDE_DIRS}")
message(STATUS "rclcpp libraries: ${rclcpp_LIBRARIES}")

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()


add_executable(tf2_broadcaster src/tf2_broadcaster.cpp)
target_link_libraries(tf2_broadcaster PRIVATE rclcpp::rclcpp tf2_ros::tf2_ros)

add_executable(tf2_listener src/tf2_listener.cpp)
target_link_libraries(tf2_listener PRIVATE rclcpp::rclcpp tf2_ros::tf2_ros)

install(TARGETS
  tf2_broadcaster
  tf2_listener
  DESTINATION lib/${PROJECT_NAME})

ament_package()

6. 编译并运行

colcon build --packages-select my_tf2_demo
source install/setup.bash

# 启动广播节点
ros2 run my_tf2_demo tf2_broadcaster

# 另开终端启动监听节点
ros2 run my_tf2_demo tf2_listener

7.运行结果

ros2 run my_tf2_demo tf2_listener
[INFO] [1748338687.574480189] [tf2_listener]: Translation: (1.00, 0.50, 0.00)
[INFO] [1748338687.574690392] [tf2_listener]: Rotation (quat): (0.00, 0.00, 0.26, 0.97)
[INFO] [1748338688.574395561] [tf2_listener]: Translation: (1.00, 0.50, 0.00)
[INFO] [1748338688.574498723] [tf2_listener]: Rotation (quat): (0.00, 0.00, 0.26, 0.97)
[INFO] [1748338689.574409312] [tf2_listener]: Translation: (1.00, 0.50, 0.00)
[INFO] [1748338689.574533292] [tf2_listener]: Rotation (quat): (0.00, 0.00, 0.26, 0.97)
[INFO] [1748338690.574416727] [tf2_listener]: Translation: (1.00, 0.50, 0.00)
[INFO] [1748338690.574547269] [tf2_listener]: Rotation (quat): (0.00, 0.00, 0.26, 0.97)
[INFO] [1748338691.574411090] [tf2_listener]: Translation: (1.00, 0.50, 0.00)
[INFO] [1748338691.574529887] [tf2_listener]: Rotation (quat): (0.00, 0.00, 0.26, 0.97)
[INFO] [1748338692.574424152] [tf2_listener]: Translation: (1.00, 0.50, 0.00)
[INFO] [1748338692.574548332] [tf2_listener]: Rotation (quat): (0.00, 0.00, 0.26, 0.97)
[INFO] [1748338693.574500531] [tf2_listener]: Translation: (1.00, 0.50, 0.00)
[INFO] [1748338693.574624469] [tf2_listener]: Rotation (quat): (0.00, 0.00, 0.26, 0.97)

8.查看 TF 变换树

你也可以使用命令行工具查看整个变换树：

ros2 run tf2_tools view_frames

输出：

[INFO] [1748338803.412777413] [view_frames]: Listening to tf data for 5.0 seconds...
[INFO] [1748338808.497500320] [view_frames]: Generating graph in frames.pdf file...
[INFO] [1748338808.499981451] [view_frames]: Result:tf2_msgs.srv.FrameGraph_Response(frame_yaml="robot_base: \n  parent: 'world'\n  broadcaster: 'default_authority'\n  rate: 10.200\n  most_recent_transform: 1748338808.495811\n  oldest_transform: 1748338803.495771\n  buffer_length: 5.000\n")

总结

功能	工具
广播坐标变换	tf2_ros::TransformBroadcaster
监听坐标变换	tf2_ros::TransformListener + tf2::Buffer
查询坐标变换	lookupTransform()
静态变换	static_transform_publisher
查看变换树	view_frames
与 URDF 结合	robot_state_publisher