[ROS2 学习指南4] --- 手写一个 TF2 监听器（python版）

一、创建监听器节点

1、下载代码

2、逐行拆解

3、添加入口点

二、更新启动文件

三、编译运行

四、核心总结

前言：上篇实现了一个 TF2 静态广播器，创建了一个learning_tf2_py 功能包。

一、创建监听器节点

1、下载代码

进入之前创建的 learning_tf2_py 功能包的 src/learning_tf2_py/learning_tf2_py/ 目录下，下载示例监听器代码：

python 复制代码

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py

2、逐行拆解

python 复制代码

import math
from geometry_msgs.msg import Twist    # 速度消息类型
import rclpy                           #  ROS 2的Python客户端库
from rclpy.node import Node
# TF2的异常类型，用于捕获变换查询失败的情况
from tf2_ros import TransformException
# 变换缓冲区，用来存储和管理所有接收到的坐标变换
from tf2_ros.buffer import Buffer
# 监听器
from tf2_ros.transform_listener import TransformListener
# 海龟模拟器的生成服务，用来在仿真环境中"生"出第二只海龟
from turtlesim.srv import Spawn




# 定义类
class FrameListener(Node):
    def __init__(self):
        super().__init__('turtle_tf2_frame_listener')
        
        # 声明并获取 target_frame 参数（默认指向 turtle1）
        self.target_frame = self.declare_parameter(
            'target_frame', 'turtle1'
        ).get_parameter_value().string_value
        
        # 创建变换缓冲区
        self.tf_buffer = Buffer()
        # 创建变换监听器------它会自动接收并缓存所有TF变换
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # 创建一个客户端，用于调用"生成海龟"的服务
        self.spawner = self.create_client(Spawn, 'spawn')
        
        # 两个状态标记：服务是否就绪、海龟是否已生成
        self.turtle_spawning_service_ready = False
        self.turtle_spawned = False
        
        # 创建发布器，用于向 turtle2 发送速度指令
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)
        
        # 创建定时器，每秒调用一次 on_timer 函数
        self.timer = self.create_timer(1.0, self.on_timer)



def on_timer(self):
    from_frame_rel = self.target_frame   # 源坐标系：要跟踪的目标（如 turtle1）
    to_frame_rel = 'turtle2'             # 目标坐标系：我们控制的乌龟
    
    if self.turtle_spawning_service_ready:
        if self.turtle_spawned:
            try:
                # 查询从 turtle2 到 target_frame 的变换
                t = self.tf_buffer.lookup_transform(
                    to_frame_rel,      # 目标坐标系
                    from_frame_rel,    # 源坐标系
                    rclpy.time.Time()  # 时间戳------传空值表示获取最新数据
                )
            except TransformException as ex:
                self.get_logger().info(
                    f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}'
                )
                return
            
            # 计算角速度：使用反正切函数计算朝向误差
            msg = Twist()
            scale_rotation_rate = 1.0
            msg.angular.z = scale_rotation_rate * math.atan2(
                t.transform.translation.y,
                t.transform.translation.x
            )
            
            # 计算线速度：距离越远，速度越快
            scale_forward_speed = 0.5
            msg.linear.x = scale_forward_speed * math.sqrt(
                t.transform.translation.x ** 2 + t.transform.translation.y ** 2
            )
            
            # 发布速度指令
            self.publisher.publish(msg)
        else:
            # 等待生成完成
            if self.result.done():
                self.get_logger().info(f'Successfully spawned {self.result.result().name}')
                self.turtle_spawned = True
            else:
                self.get_logger().info('Spawn is not finished')
    else:
        # 检查生成服务是否就绪
        if self.spawner.service_is_ready():
            request = Spawn.Request()
            request.name = 'turtle2'
            request.x = float(4)
            request.y = float(2)
            request.theta = float(0)
            self.result = self.spawner.call_async(request)
            self.turtle_spawning_service_ready = True
        else:
            self.get_logger().info('Service is not ready')

3、添加入口点

要让ros2 run 能够找到并运行我们的节点，需要在setup.py 中添加入口点。

打开src/learning_tf2_py/setup.py，在console_scripts 括号中添加：

python 复制代码

'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',

二、更新启动文件

我们需要一个启动文件来一键拉起所有节点。打开src/learning_tf2_py/launch/turtle_tf2_demo.launch.py，修改为以下内容：

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([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[{'turtlename': 'turtle1'}]
        ),
        DeclareLaunchArgument(
            'target_frame',
            default_value='turtle1',
            description='Target frame name.'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster2',
            parameters=[{'turtlename': 'turtle2'}]
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_listener',
            name='listener',
            parameters=[{'target_frame': LaunchConfiguration('target_frame')}]
        ),
    ])

这个启动文件按顺序做了四件事：

启动turtlesim 仿真界面
启动第一个广播器（发布turtle1 的位置）
声明一个启动参数target_frame，默认指向turtle1
启动第二个广播器（发布turtle2 的位置）
启动监听器（订阅变换并控制turtle2 跟踪目标）

三、编译运行

1、检查依赖

在工作空间根目录下运行：

python 复制代码

rosdep install -i --from-path src --rosdistro humble -y

2、编译

python 复制代码

colcon build --packages-select learning_tf2_py

3、运行

打开一个新终端，进入工作空间根目录，加载环境变量：

python 复制代码

source install/setup.bash

启动演示：

python 复制代码

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

应该会看到 turtlesim 窗口中出现两只海龟。

打开另一个终端（注意保持焦点在终端窗口，而不是仿真器窗口），运行键盘控制节点：

python 复制代码

ros2 run turtlesim turtle_teleop_key

用方向键控制海龟1号移动------你会发现海龟2号像跟屁虫一样紧紧跟在后面！

四、核心总结

核心要点回顾：

TransformListener 一旦创建就会在后台持续工作，你不需要手动处理网络通信；
lookup_transform 是获取变换的核心API，三个参数分别是：目标坐标系、源坐标系、时间戳；
传入rclpy.time.Time() 表示获取最新可用的变换------这是最常用的用法；
所有查询操作都应该包裹在try-except 中，因为变换可能暂时不可用。