【ROS2学习笔记】 TF 坐标系

悠哉悠哉愿意2025-10-07 3:04

前言

本系列博文是本人的学习笔记，自用为主，不是教程，学习请移步其他大佬的相关教程。前几篇学习资源来自鱼香ROS大佬的详细教程，适合深入学习，但对本人这样的初学者不算友好，后续笔记将以@古月居的ROS2入门21讲为主，侵权即删。

一、学习目标

  1. 理解 TF（坐标系变换）的核心作用 ------ 解决机器人 "多个坐标系位置关系管理" 的痛点
  2. 认识机器人中常见的坐标系（如 base_link、odom、map），知道不同坐标系的用途
  3. 熟练使用 3 个 TF 命令行工具（view_frames、tf2_echo、RViz 可视化），直观查看坐标系关系
  4. 掌握 TF 编程的 3 个核心技能：静态 TF 广播、TF 监听、动态 TF 应用（海龟跟随）
  5. 理解海龟跟随的底层逻辑：通过 TF 获取坐标变换，计算速度指令实现跟随

二、先搞懂：什么是 TF？为什么需要它？（小白入门）

2.1 一句话定义：机器人的 "坐标系地图"

TF（Transform Frame，坐标系变换）是 ROS 提供的坐标系管理工具，它能实时跟踪机器人系统中所有坐标系的位置关系，就像给机器人画了一张 "坐标系地图"------ 比如 "相机在底盘的哪个位置""机器人在地图的哪个位置"，都能通过 TF 快速查询。

2.2 生活类比：为什么需要坐标系管理？

假设你在教室（世界坐标系）里，书包放在桌子上（桌子坐标系）：

  • 你想知道 "书包相对于你的位置"，需要先知道 "你相对于教室的位置" 和 "书包相对于桌子的位置"，再通过两次变换计算得出；
  • 机器人也是一样：比如 "激光雷达检测到障碍物，障碍物相对于底盘的位置是多少？"，需要通过 TF 将 "雷达坐标系" 的障碍物位置，变换到 "底盘坐标系"。

如果没有 TF，每个节点都要自己写变换计算，代码重复且容易出错；有了 TF，所有坐标系关系统一管理，节点直接调用即可。

2.3 机器人中的常见坐标系（小白必知）

不同类型的机器人有不同的坐标系，记住核心的几个，后续开发足够用：

机器人类型 坐标系名称 作用（小白理解） 是否固定（相对世界）
通用 world（世界坐标系） 整个系统的 "绝对参考系"（比如教室的墙角） 是（固定不动）
移动机器人 base_link（底盘坐标系） 机器人底盘的中心点（相当于你的身体中心） 否（随机器人移动）
移动机器人 odom（里程计坐标系） 基于里程计的参考系（记录机器人走了多远） 否（有累积误差，会漂移）
移动机器人 map（地图坐标系） 基于地图的绝对参考系（比如 GPS 定位的位置） 是（固定不动）
通用 laser_link（雷达坐标系） 激光雷达的中心点（雷达检测到的障碍物先在这个坐标系） 否（随底盘移动，但相对底盘位置固定）
机械臂 tool0（工具坐标系） 机械臂末端夹爪的中心点（夹爪抓东西的参考） 否（随机械臂运动）

三、TF 命令行工具（小白入门首选，直观又简单）

先用 ROS 自带的 "小海龟跟随" 例程，感受 TF 的作用，再学习命令行工具。

3.1 步骤 1：安装并启动小海龟跟随例程

1. 安装依赖包

打开终端，执行命令（以 ROS2 Humble 为例）：

复制代码 
# 安装小海龟TF相关包和变换库
sudo apt install ros-humble-turtle-tf2-py ros-humble-tf2-tools
sudo pip3 install transforms3d  # 用于坐标变换的Python库
2. 启动例程
复制代码 
# 启动小海龟TF跟随（会启动两个海龟和TF广播）
ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
# 新终端启动键盘控制（控制turtle1，turtle2会自动跟随）
ros2 run turtlesim turtle_teleop_key
  • 效果：按键盘方向键控制 turtle1 运动，turtle2 会自动跟着 turtle1 动 ------ 这就是 TF 的功劳：turtle2 通过 TF 获取 turtle1 的位置，计算自己的运动指令。

3.2 工具 1：view_frames------ 查看 TF 树（坐标系关系图）

作用：生成一张 PDF 图，展示所有坐标系的父子关系（谁是谁的 "参考系"）
操作步骤：

  1. 保持小海龟例程运行，新终端执行：

    复制代码 
    ros2 run tf2_tools view_frames

  2. 终端会提示 "Wrote frames to frames.pdf"，在当前目录下找到frames.pdf并打开；

  3. 看到的内容：world是父坐标系，turtle1和turtle2是子坐标系（两个海龟都以world为参考）。

小白解读：
  • TF 树是 "树形结构"，每个子坐标系只有一个父坐标系（比如turtle1的父是world）；
  • 不能有 "循环依赖"（比如turtle1的父是turtle2，turtle2的父又是turtle1），否则 TF 会报错。

3.3 工具 2：tf2_echo------ 查看两个坐标系的具体变换

作用：实时打印两个坐标系之间的 "平移"（x/y/z 距离）和 "旋转"（四元数 / 欧拉角）
操作步骤：

  1. 保持小海龟例程运行，新终端执行：

    复制代码 
    # 格式：ros2 run tf2_ros tf2_echo 目标坐标系 源坐标系
# 含义：查看"源坐标系"相对于"目标坐标系"的位置
ros2 run tf2_ros tf2_echo turtle2 turtle1

  2. 终端会循环打印类似内容：

    复制代码 
    At time 1690000000.123456789
- Translation: [x: 0.5, y: 0.3, z: 0.0]  # turtle1在turtle2的x方向0.5米，y方向0.3米处
- Rotation: [x: 0.0, y: 0.0, z: 0.707, w: 0.707]  # 旋转（四元数表示，对应45度）
小白解读：
  • 平移（Translation）：x/y/z 分别表示源坐标系相对于目标坐标系在三个轴上的距离（单位：米）；
  • 旋转（Rotation）：默认用四元数表示（避免 "万向锁" 问题），后续代码会讲如何转成更易理解的欧拉角（roll/pitch/yaw，即绕 x/y/z 轴的旋转角度，单位：弧度）。

3.4 工具 3：RViz 可视化 ------ 直观看到坐标系

作用：在 3D 窗口中显示坐标系的位置和姿态，比看数值更直观
操作步骤：

  1. 保持小海龟例程运行，新终端执行：

    复制代码 
    # 启动RViz并加载小海龟TF的配置文件
ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz

  2. RViz 窗口中会看到两个彩色的 "坐标轴"：

    • 红色：x 轴；绿色：y 轴；蓝色：z 轴；
    • 移动 turtle1，两个坐标轴的位置会变化，直观看到 turtle2 跟着 turtle1 动。

四、TF 编程实战（从简单到复杂，代码带详细注释）

命令行工具适合查看，编程才能实现自定义需求（比如 "发布雷达相对于底盘的坐标系""监听相机和底盘的变换"）。

案例 1：静态 TF 广播（相对位置不变的坐标系）

场景需求

机器人的激光雷达安装在底盘上，安装后两者的相对位置就固定了（比如雷达在底盘 x 方向 0.2 米、y 方向 0 米处）------ 这种 "相对位置不变" 的坐标系，用静态 TF发布。

核心概念：静态 TF vs 动态 TF
类型 特点 应用场景
静态 TF 父子坐标系相对位置不变 雷达 - 底盘、相机 - 底盘
动态 TF 父子坐标系相对位置随时间变化 底盘 - 世界、海龟 - 世界
步骤 1：创建功能包
复制代码 
cd dev_ws/src
# 创建功能包learning_tf，依赖rclpy、tf2_ros、geometry_msgs（坐标变换消息）
ros2 pkg create learning_tf --build-type ament_python --dependencies rclpy tf2_ros geometry_msgs turtlesim transforms3d
步骤 2：静态 TF 广播代码（learning_tf/static_tf_broadcaster.py）
python 复制代码 
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
ROS2静态TF广播示例：
- 发布"world"（父坐标系）到"house"（子坐标系）的静态变换
- 两者相对位置固定：house在world的x=10米、y=5米处，无旋转
"""

# 1. 导入必需的库
import rclpy                                                                 # ROS2 Python核心库
from rclpy.node import Node                                                  # ROS2节点类
from geometry_msgs.msg import TransformStamped                               # 坐标变换消息（TF的核心消息）
import tf_transformations                                                    # TF变换工具库（欧拉角转四元数等）
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster  # 静态TF广播器类（专门用于静态TF）

# 2. 定义静态TF广播节点类
class StaticTFBroadcaster(Node):
    def __init__(self, name):
        super().__init__(name)  # 初始化节点，节点名=static_tf_broadcaster

        # 3. 创建静态TF广播器对象（核心对象，用于发布静态TF）
        self.tf_broadcaster = StaticTransformBroadcaster(self)

        # 4. 创建坐标变换消息（TransformStamped），设置变换内容
        static_transform = TransformStamped()

        # 4.1 设置消息时间戳（用当前ROS时间）
        static_transform.header.stamp = self.get_clock().now().to_msg()
        # 4.2 设置父坐标系（谁是参考系？这里是world）
        static_transform.header.frame_id = 'world'
        # 4.3 设置子坐标系（相对于父坐标系的哪个坐标系？这里是house）
        static_transform.child_frame_id = 'house'

        # 4.4 设置平移（子坐标系相对于父坐标系的x/y/z距离，单位：米）
        static_transform.transform.translation.x = 10.0  # house在world的x方向10米处
        static_transform.transform.translation.y = 5.0   # y方向5米处
        static_transform.transform.translation.z = 0.0   # z方向0米（2D场景，z=0）

        # 4.5 设置旋转（子坐标系相对于父坐标系的姿态，用四元数表示）
        # 步骤：先将欧拉角（roll, pitch, yaw）转成四元数
        # 欧拉角含义：roll（绕x轴转）、pitch（绕y轴转）、yaw（绕z轴转），这里都为0（无旋转）
        quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0)
        # 将四元数赋值给消息
        static_transform.transform.rotation.x = quat[0]
        static_transform.transform.rotation.y = quat[1]
        static_transform.transform.rotation.z = quat[2]
        static_transform.transform.rotation.w = quat[3]

        # 5. 发布静态TF（静态TF只需发布一次，后续会一直生效，直到节点关闭）
        self.tf_broadcaster.sendTransform(static_transform)
        self.get_logger().info("静态TF发布成功：world → house（x=10, y=5, 无旋转）")

# 3. 主入口函数
def main(args=None):
    rclpy.init(args=args)  # 初始化ROS2
    node = StaticTFBroadcaster("static_tf_broadcaster")  # 创建节点
    rclpy.spin(node)       # 启动节点循环（静态TF发布一次就够，但spin保持节点运行）
    node.destroy_node()    # 销毁节点
    rclpy.shutdown()       # 关闭ROS2
步骤 3：配置节点入口（修改learning_tf/setup.py）

在entry_points的console_scripts中添加静态 TF 广播器的入口：

python 复制代码 
entry_points={
    'console_scripts': [
        # 格式："命令名 = 包名.文件名:main函数"
        'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main',
    ],
},
步骤 4：编译与运行

  1. 编译：

    复制代码 
    cd dev_ws
colcon build --packages-select learning_tf
source install/setup.bash  # 加载环境变量

  2. 运行静态 TF 广播器：

    复制代码 
    ros2 run learning_tf static_tf_broadcaster

  3. 验证（新终端）：

    复制代码 
    # 查看TF树，能看到world→house的关系
ros2 run tf2_tools view_frames
# 查看world到house的具体变换
ros2 run tf2_ros tf2_echo world house

案例 2：TF 监听（查询两个坐标系的变换）

场景需求

发布静态 TF 后，如何在代码中查询 "house 相对于 world 的位置"？用TF 监听器实现 ------ 监听 TF 树中的变换，实时获取两个坐标系的位置关系。

TF 监听代码（learning_tf/tf_listener.py）
python 复制代码 
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
ROS2 TF监听示例：
- 监听"source_frame"（默认world）到"target_frame"（默认house）的变换
- 每1秒打印一次变换的平移和旋转（欧拉角）
"""

# 1. 导入必需的库
import rclpy                                              # ROS2核心库
from rclpy.node import Node                               # 节点类
import tf_transformations                                 # TF变换工具库（四元数转欧拉角）
from tf2_ros import TransformException                    # TF变换异常类（处理变换失败）
from tf2_ros.buffer import Buffer                         # TF缓冲器（存储最近的TF变换数据）
from tf2_ros.transform_listener import TransformListener  # TF监听器（监听TF变换并存入缓冲器）

# 2. 定义TF监听器节点类
class TFListener(Node):
    def __init__(self, name):
        super().__init__(name)  # 初始化节点，节点名=tf_listener

        # 3. 声明参数（让用户可以通过命令行修改监听的坐标系，默认world→house）
        # 3.1 声明源坐标系参数（默认world）
        self.declare_parameter('source_frame', 'world')
        self.source_frame = self.get_parameter(
            'source_frame').get_parameter_value().string_value
        # 3.2 声明目标坐标系参数（默认house）
        self.declare_parameter('target_frame', 'house')
        self.target_frame = self.get_parameter(
            'target_frame').get_parameter_value().string_value

        # 4. 创建TF缓冲器和监听器（核心对象）
        # 缓冲器：存储最近10秒的TF变换（默认缓存10秒）
        self.tf_buffer = Buffer()
        # 监听器：实时监听TF变换，自动存入缓冲器
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # 5. 创建定时器：每1秒执行一次on_timer函数（定期查询TF变换）
        self.timer = self.create_timer(1.0, self.on_timer)
        self.get_logger().info(f"TF监听器启动：监听 {self.source_frame} → {self.target_frame} 的变换")

    def on_timer(self):
        """定时器回调函数：查询并打印TF变换"""
        try:
            # 6. 查询当前时刻的TF变换
            # 格式：lookup_transform(目标坐标系, 源坐标系, 时间戳)
            # 含义：获取"源坐标系相对于目标坐标系"的位置
            now = rclpy.time.Time()  # 当前ROS时间
            trans = self.tf_buffer.lookup_transform(
                self.target_frame,  # 目标坐标系（参考系）
                self.source_frame,  # 源坐标系（要查询的坐标系）
                now                 # 时间戳（now表示当前时刻）
            )

        # 7. 处理变换失败的情况（比如坐标系不存在）
        except TransformException as ex:
            self.get_logger().warn(f"无法获取 {self.source_frame} → {self.target_frame} 的变换：{ex}")
            return

        # 8. 提取变换数据（平移和旋转）
        # 8.1 平移（x/y/z）
        pos = trans.transform.translation
        # 8.2 旋转（四元数转欧拉角，方便理解）
        quat = trans.transform.rotation
        # 四元数转欧拉角：顺序是roll(x)、pitch(y)、yaw(z)
        euler = tf_transformations.euler_from_quaternion(
            [quat.x, quat.y, quat.z, quat.w]
        )

        # 9. 打印变换结果（日志输出）
        self.get_logger().info(
            f"[{self.source_frame} → {self.target_frame}] "
            f"平移：(x:{pos.x:.2f}, y:{pos.y:.2f}, z:{pos.z:.2f}) "
            f"旋转（欧拉角，弧度）：(roll:{euler[0]:.2f}, pitch:{euler[1]:.2f}, yaw:{euler[2]:.2f})"
        )

# 3. 主入口函数
def main(args=None):
    rclpy.init(args=args)  # 初始化ROS2
    node = TFListener("tf_listener")  # 创建节点
    rclpy.spin(node)       # 启动节点循环
    node.destroy_node()    # 销毁节点
    rclpy.shutdown()       # 关闭ROS2
配置入口与运行

  1. 修改setup.py，添加监听器入口：

    python 复制代码 
    entry_points={
    'console_scripts': [
        'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main',
        'tf_listener = learning_tf.tf_listener:main',  # 新增监听器入口
    ],
},

  2. 编译与运行：

    复制代码 
    # 终端1：启动静态TF广播器
ros2 run learning_tf static_tf_broadcaster
# 终端2：启动TF监听器（默认监听world→house）
ros2 run learning_tf tf_listener
# （可选）终端2：监听house→world（交换源和目标坐标系）
ros2 run learning_tf tf_listener --ros-args -p source_frame:=house -p target_frame:=world

  3. 预期输出：每 1 秒打印一次world→house的变换（平移 x=10、y=5，旋转 0）。

案例 3：综合应用 ------ 海龟跟随（动态 TF 实战）

场景需求

实现 "turtle2 跟随 turtle1"：

  1. 发布两个动态 TF：world→turtle1（turtle1 的位置）和world→turtle2（turtle2 的位置）；
  2. 监听turtle1→turtle2的变换，计算 turtle2 的速度指令，让它跟着 turtle1 动。
步骤 1：动态 TF 广播代码（learning_tf/turtle_tf_broadcaster.py）

作用：订阅海龟的/turtlename/pose话题（海龟位置），将位置转成world→turtlename的动态 TF。

python 复制代码 
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
ROS2动态TF广播示例：
- 订阅海龟的pose话题（位置），发布world→turtlename的动态TF
- 支持通过参数指定海龟名（turtle1或turtle2）
"""

# 1. 导入库
import rclpy                                       # ROS2核心库
from rclpy.node import Node                        # 节点类
from geometry_msgs.msg import TransformStamped     # TF变换消息
import tf_transformations                          # TF变换工具库
from tf2_ros import TransformBroadcaster           # 动态TF广播器（区别于静态）
from turtlesim.msg import Pose                     # 海龟位置消息（turtlesim的核心消息）

# 2. 定义动态TF广播节点类
class TurtleTFBroadcaster(Node):
    def __init__(self, name):
        super().__init__(name)  # 初始化节点，节点名=turtle_tf_broadcaster

        # 3. 声明参数：海龟名（默认turtle，可通过参数修改为turtle1或turtle2）
        self.declare_parameter('turtlename', 'turtle')
        self.turtlename = self.get_parameter(
            'turtlename').get_parameter_value().string_value

        # 4. 创建动态TF广播器（动态TF需要频繁发布，用这个类）
        self.tf_broadcaster = TransformBroadcaster(self)

        # 5. 订阅海龟的pose话题（获取海龟当前位置）
        # 话题名：/turtlename/pose（比如/turtle1/pose）
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',  # 动态生成话题名（根据海龟名）
            self.turtle_pose_callback,   # 收到pose后的回调函数
            10                           # 队列长度
        )
        self.get_logger().info(f"动态TF广播器启动：订阅/{self.turtlename}/pose，发布world→{self.turtlename}")

    def turtle_pose_callback(self, msg):
        """pose话题回调函数：将海龟位置转成TF变换并发布"""
        # 6. 创建TF变换消息
        transform = TransformStamped()

        # 6.1 设置时间戳（用当前ROS时间，确保和pose消息同步）
        transform.header.stamp = self.get_clock().now().to_msg()
        # 6.2 父坐标系（world）
        transform.header.frame_id = 'world'
        # 6.3 子坐标系（当前海龟名，比如turtle1）
        transform.child_frame_id = self.turtlename

        # 6.4 平移（从pose消息中获取x/y，z=0）
        transform.transform.translation.x = msg.x
        transform.transform.translation.y = msg.y
        transform.transform.translation.z = 0.0

        # 6.5 旋转（从pose消息的theta（yaw角）转成四元数）
        # pose.theta：海龟绕z轴的旋转角度（yaw角），roll和pitch都为0
        quat = tf_transformations.quaternion_from_euler(0.0, 0.0, msg.theta)
        transform.transform.rotation.x = quat[0]
        transform.transform.rotation.y = quat[1]
        transform.transform.rotation.z = quat[2]
        transform.transform.rotation.w = quat[3]

        # 7. 发布动态TF（每次收到pose消息都发布一次，确保实时更新）
        self.tf_broadcaster.sendTransform(transform)

# 3. 主入口函数
def main(args=None):
    rclpy.init(args=args)  # 初始化ROS2
    node = TurtleTFBroadcaster("turtle_tf_broadcaster")  # 创建节点
    rclpy.spin(node)       # 启动节点循环
    node.destroy_node()    # 销毁节点
    rclpy.shutdown()       # 关闭ROS2
步骤 2：海龟跟随控制代码（learning_tf/turtle_following.py）

作用：

  1. 调用turtlesim的spawn服务，生成 turtle2；
  2. 监听turtle1→turtle2的变换，计算 turtle2 的线速度和角速度；
  3. 发布/turtle2/cmd_vel话题，控制 turtle2 跟随。
python 复制代码 
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
ROS2海龟跟随示例：
- 生成turtle2，监听turtle1→turtle2的变换
- 计算速度指令，让turtle2跟着turtle1动
"""

import math  # 用于计算角度和距离
import rclpy                                              # ROS2核心库
from rclpy.node import Node                               # 节点类
import tf_transformations                                 # TF变换工具库
from tf2_ros import TransformException                    # TF异常类
from tf2_ros.buffer import Buffer                         # TF缓冲器
from tf2_ros.transform_listener import TransformListener  # TF监听器
from geometry_msgs.msg import Twist                       # 速度控制消息（控制海龟运动）
from turtlesim.srv import Spawn                           # 海龟生成服务（生成turtle2）

# 2. 定义海龟跟随节点类
class TurtleFollowing(Node):
    def __init__(self, name):
        super().__init__(name)  # 初始化节点，节点名=turtle_following

        # 3. 声明参数：源坐标系（默认turtle1，即跟随目标）
        self.declare_parameter('source_frame', 'turtle1')
        self.source_frame = self.get_parameter(
            'source_frame').get_parameter_value().string_value

        # 4. 创建TF缓冲器和监听器
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # 5. 创建服务客户端：调用spawn服务生成turtle2
        self.spawn_client = self.create_client(Spawn, 'spawn')
        # 标志位：服务是否准备就绪、turtle2是否生成
        self.service_ready = False
        self.turtle2_spawned = False

        # 6. 创建速度发布者：发布/turtle2/cmd_vel控制turtle2
        self.vel_publisher = self.create_publisher(Twist, '/turtle2/cmd_vel', 10)

        # 7. 创建定时器：每1秒执行一次跟随逻辑
        self.timer = self.create_timer(1.0, self.follow_logic)
        self.get_logger().info("海龟跟随节点启动：准备生成turtle2并跟随turtle1")

    def follow_logic(self):
        """跟随逻辑：生成turtle2 → 监听TF → 发布速度指令"""
        # 8. 第一步：生成turtle2（如果还没生成）
        if not self.service_ready:
            # 检查spawn服务是否就绪
            if self.spawn_client.service_is_ready():
                # 创建服务请求：生成turtle2在(4,2)位置，角度0
                spawn_req = Spawn.Request()
                spawn_req.name = 'turtle2'
                spawn_req.x = 4.0
                spawn_req.y = 2.0
                spawn_req.theta = 0.0

                # 异步发送请求（不阻塞节点）
                self.spawn_result = self.spawn_client.call_async(spawn_req)
                self.service_ready = True  # 标记服务已请求
                self.get_logger().info("已发送生成turtle2的请求")
            else:
                self.get_logger().warn("spawn服务未就绪，等待中...")
            return

        # 9. 第二步：检查turtle2是否生成成功
        if not self.turtle2_spawned:
            if self.spawn_result.done():
                # 获取服务结果，确认生成成功
                result = self.spawn_result.result()
                self.get_logger().info(f"turtle2生成成功！名称：{result.name}")
                self.turtle2_spawned = True
            else:
                self.get_logger().info("等待turtle2生成...")
            return

        # 10. 第三步：监听turtle1→turtle2的TF变换，计算速度
        try:
            now = rclpy.time.Time()
            # 监听turtle2（目标坐标系）相对于turtle1（源坐标系）的变换
            trans = self.tf_buffer.lookup_transform(
                'turtle2',    # 目标坐标系（turtle2的视角）
                self.source_frame,  # 源坐标系（turtle1的位置）
                now
            )
        except TransformException as ex:
            self.get_logger().warn(f"无法获取TF变换：{ex}")
            return

        # 11. 计算速度指令（核心：根据TF变换算线速度和角速度）
        vel_msg = Twist()
        # 11.1 线速度：与turtle1和turtle2的距离成正比（距离越远，速度越快）
        distance = math.sqrt(trans.transform.translation.x**2 + trans.transform.translation.y**2)
        vel_msg.linear.x = 0.5 * distance  # 比例系数0.5，避免速度太快
        vel_msg.linear.y = 0.0  # 2D海龟只能沿x轴运动
        vel_msg.linear.z = 0.0

        # 11.2 角速度：朝向turtle1的方向（用atan2算角度）
        angle = math.atan2(trans.transform.translation.y, trans.transform.translation.x)
        vel_msg.angular.z = 1.0 * angle  # 比例系数1.0，控制转向速度
        vel_msg.angular.x = 0.0
        vel_msg.angular.y = 0.0

        # 12. 发布速度指令，控制turtle2跟随
        self.vel_publisher.publish(vel_msg)
        self.get_logger().info(
            f"发布速度：线速度x={vel_msg.linear.x:.2f}，角速度z={vel_msg.angular.z:.2f}"
        )

# 3. 主入口函数
def main(args=None):
    rclpy.init(args=args)  # 初始化ROS2
    node = TurtleFollowing("turtle_following")  # 创建节点
    rclpy.spin(node)       # 启动节点循环
    node.destroy_node()    # 销毁节点
    rclpy.shutdown()       # 关闭ROS2
步骤 3：Launch 文件（一键启动所有节点）

创建learning_tf/launch/turtle_following_demo.launch.py，一次性启动小海龟仿真器、两个动态 TF 广播器、跟随节点：

python 复制代码 
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # 1. 启动小海龟仿真器
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        # 2. 启动turtle1的动态TF广播器（参数turtlename=turtle1）
        Node(
            package='learning_tf',
            executable='turtle_tf_broadcaster',
            name='broadcaster1',
            parameters=[{'turtlename': 'turtle1'}]
        ),
        # 3. 启动turtle2的动态TF广播器（参数turtlename=turtle2）
        Node(
            package='learning_tf',
            executable='turtle_tf_broadcaster',
            name='broadcaster2',
            parameters=[{'turtlename': 'turtle2'}]
        ),
        # 4. 启动海龟跟随节点
        Node(
            package='learning_tf',
            executable='turtle_following',
            name='follower',
            parameters=[{'source_frame': 'turtle1'}]  # 跟随turtle1
        ),
    ])
步骤 4：配置入口与运行

  1. 修改setup.py，添加动态 TF 和跟随节点的入口：

    python 复制代码 
    entry_points={
    'console_scripts': [
        'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main',
        'tf_listener = learning_tf.tf_listener:main',
        'turtle_tf_broadcaster = learning_tf.turtle_tf_broadcaster:main',  # 动态TF入口
        'turtle_following = learning_tf.turtle_following:main',  # 跟随入口
    ],
},

  2. 配置 Launch 文件路径（修改setup.py的data_files）：

    python 复制代码 
    data_files=[
    ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
    ('share/' + package_name, ['package.xml']),
    # 添加Launch文件路径
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
],

  3. 编译与运行：

    复制代码 
    # 编译
cd dev_ws
colcon build --packages-select learning_tf
source install/setup.bash
# 启动跟随例程
ros2 launch learning_tf turtle_following_demo.launch.py
# 新终端启动键盘控制（控制turtle1）
ros2 run turtlesim turtle_teleop_key

  4. 效果：按方向键控制 turtle1，turtle2 会自动跟着 turtle1 运动。

五、复习要点总结（小白必背）

  1. TF 核心作用：管理机器人所有坐标系的位置关系，提供 "查询" 和 "广播" 接口，避免重复写变换计算；
  2. 关键概念 ：
    • 静态 TF：父子坐标系相对位置不变（雷达 - 底盘）；
    • 动态 TF：父子坐标系相对位置变化（海龟 - 世界）；
    • TF 树：坐标系的父子关系，不能循环依赖；
  3. 命令行工具 ：
    • view_frames：生成 TF 树 PDF；
    • tf2_echo 目标坐标系 源坐标系：打印具体变换；
    • RViz：可视化坐标系；
  4. 编程核心流程 ：
    • 广播 TF：创建TransformBroadcaster（动态）或StaticTransformBroadcaster（静态），发布TransformStamped消息；
    • 监听 TF：创建Buffer+TransformListener，调用lookup_transform查询变换；
  5. 海龟跟随原理 ：
    1. 广播 turtle1 和 turtle2 的动态 TF；
    2. 监听 turtle1→turtle2 的变换，用math.atan2算转向角度，math.sqrt算距离；
    3. 发布速度指令，让 turtle2 朝 turtle1 运动。

掌握这些内容，就能应对机器人中大部分坐标系相关的开发需求，比如雷达数据转底盘坐标、相机目标位置转世界坐标等

相关推荐
代码萌新知4 小时前
设计模式学习（五）装饰者模式、桥接模式、外观模式
java·学习·设计模式·桥接模式·装饰器模式·外观模式
驱动探索者6 小时前
find 命令使用介绍
java·linux·运维·服务器·前端·学习·microsoft
小雨凉如水6 小时前
k8s学习-pod的生命周期
java·学习·kubernetes
li星野6 小时前
打工人日报#20251005
笔记·程序人生·fpga开发·学习方法
charlie1145141917 小时前
理解C++20的革命特性——协程支持2：编写简单的协程调度器
c++·学习·算法·设计模式·c++20·协程·调度器
文火冰糖的硅基工坊7 小时前
[人工智能-综述-21]：学习人工智能的路径
大数据·人工智能·学习·系统架构·制造
JJJJ_iii7 小时前
【深度学习01】快速上手 PyTorch：环境 + IDE+Dataset
pytorch·笔记·python·深度学习·学习·jupyter
好奇龙猫8 小时前
日语学习-日语知识点小记-进阶-JLPT-N1阶段应用练习（5）：语法 ＋考え方１8+2022年7月N1
学习
。TAT。8 小时前
Linux - 进程状态
linux·学习
热门推荐
01KGG转MP3工具|非KGM文件|解密音频02BongoCat - 跨平台键盘猫动画工具03jdk21下载、安装（Windows、Linux、macOS）04GitHub 镜像站点05UV安装并设置国内源06HarmonyOS NEXT开发进阶（十四）：HarmonyOS应用开发者基础认证试题集汇总及答案解析07零基础搭建赛博朋克个人主页：蓝耘Claude Code完整实战教程08adb安装教程（附adb命令大全详解）adb环境配置教程0946个Nano-banana 精选提示词，持续更新中10Linux下V2Ray安装配置指南