Skip to content
Anima Mundi Anima Mundi

TF2 监听器（Python）

目标： 学习如何使用 tf2 获取坐标系变换。

教程级别： 中级

预计时间： 10 分钟

背景

Section titled “背景”

在之前的教程中，我们创建了一个 tf2 广播器，将乌龟的位姿发布到 tf2。

在本教程中，我们将创建一个 tf2 监听器，开始实际使用 tf2。

前提条件

Section titled “前提条件”

本教程假设你已经完成了 tf2 静态广播器教程（Python）和 tf2 广播器教程（Python）。 在之前的教程中，我们创建了一个 learning_tf2_py 功能包（package），接下来我们将继续在该功能包中开发。

任务

Section titled “任务”

1 编写监听器节点

Section titled “1 编写监听器节点”

首先创建源文件。 进入之前教程中创建的 learning_tf2_py 功能包。 在 src/learning_tf2_py/learning_tf2_py 目录下，运行以下命令下载示例监听器代码：

Linux/macOS：

Terminal window
wget https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py

Windows：

在 Windows 命令行提示符中：

Terminal window
curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py -o turtle_tf2_listener.py

或在 PowerShell 中：

Terminal window
curl https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py -o turtle_tf2_listener.py

使用你喜欢的文本编辑器打开名为 turtle_tf2_listener.py 的文件。

import math


from geometry_msgs.msg import Twist


import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node


from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener


from turtlesim_msgs.srv import Spawn




class FrameListener(Node):


    def __init__(self):
        super().__init__('turtle_tf2_frame_listener')


        # Declare and acquire `target_frame` parameter
        self.target_frame = self.declare_parameter(
          'target_frame', 'turtle1').get_parameter_value().string_value


        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)


        # Create a client to spawn a turtle
        self.spawner = self.create_client(Spawn, 'spawn')
        # Boolean values to store the information
        # if the service for spawning turtle is available
        self.turtle_spawning_service_ready = False
        # if the turtle was successfully spawned
        self.turtle_spawned = False


        # Create turtle2 velocity publisher
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)


        # Call on_timer function every second
        self.timer = self.create_timer(1.0, self.on_timer)


    def on_timer(self):
        # Store frame names in variables that will be used to
        # compute transformations
        from_frame_rel = self.target_frame
        to_frame_rel = 'turtle2'


        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                # Look up for the transformation between target_frame and turtle2 frames
                # and send velocity commands for turtle2 to reach target_frame
                try:
                    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():
                # Initialize request with turtle name and coordinates
                # Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn
                request = Spawn.Request()
                request.name = 'turtle2'
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)
                # Call request
                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                # Check if the service is ready
                self.get_logger().info('Service is not ready')




def main():
    try:
        with rclpy.init():
            node = FrameListener()
            rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass

1.1 代码解析

Section titled “1.1 代码解析”

要了解生成乌龟所用服务的工作原理，请参阅”编写简单的服务与客户端（Python）“教程。

接下来看与获取坐标系变换相关的代码。 tf2_ros 包提供了 TransformListener 的实现，可以帮助简化接收变换的工作。

from tf2_ros.transform_listener import TransformListener

这里创建了一个 TransformListener 对象。 监听器一旦创建，就会在后台接收网络上的 tf2 变换，并缓存最多 10 秒。

self.tf_listener = TransformListener(self.tf_buffer, self)

最后，向监听器查询特定的变换。 调用 lookup_transform 方法，参数如下：

  1. 目标坐标系（target frame）
  2. 源坐标系（source frame）
  3. 变换的目标时间点

传入 rclpy.time.Time() 即可获取最新的可用变换。 上述调用包裹在 try-except 块中，以处理可能出现的异常。

t = self.tf_buffer.lookup_transform(
    to_frame_rel,
    from_frame_rel,
    rclpy.time.Time())

1.2 添加入口点

Section titled “1.2 添加入口点”

要让 ros2 run 命令能够运行你的节点，必须在 setup.py（位于 src/learning_tf2_py 目录下）中添加入口点。

在 'console_scripts': 的方括号中添加以下行：

'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',

2 更新 launch 文件

Section titled “2 更新 launch 文件”

使用文本编辑器打开 src/learning_tf2_py/launch 目录下名为 turtle_tf2_demo_launch 的 launch 文件（扩展名为 .py、.xml 或 .yaml），向 launch 描述中添加两个新节点和一个 launch 参数，并添加对应的导入语句。最终文件应如下所示：

XML：

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <node pkg="turtlesim" exec="turtlesim_node" name="sim" />
  <node pkg="learning_tf2_py" exec="turtle_tf2_broadcaster" name="broadcaster1">
    <param name="turtlename" value="turtle1" />
  </node>
  <arg name="target_frame" default="turtle1" description="Target frame name." />
  <node pkg="learning_tf2_py" exec="turtle_tf2_broadcaster" name="broadcaster2">
    <param name="turtlename" value="turtle2" />
  </node>
  <node pkg="learning_tf2_py" exec="turtle_tf2_listener" name="listener">
    <param name="target_frame" value="$(var target_frame)" />
  </node>
</launch>

YAML：

%YAML 1.2
---
launch:
  - node:
      pkg: "turtlesim"
      exec: "turtlesim_node"
      name: "sim"
  - node:
      pkg: "learning_tf2_py"
      exec: "turtle_tf2_broadcaster"
      name: "broadcaster1"
      param:
      - name: "turtlename"
        value: "turtle1"
  - arg:
      name: "target_frame"
      default: "turtle1"
      description: "Target frame name."
  - node:
      pkg: "learning_tf2_py"
      exec: "turtle_tf2_broadcaster"
      name: "broadcaster2"
      param:
      - name: "turtlename"
        value: "turtle2"
  - node:
      pkg: "learning_tf2_py"
      exec: "turtle_tf2_listener"
      name: "listener"
      param:
      - name: "target_frame"
        value: "$(var target_frame)"

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')}
            ]
        ),
    ])

这将声明一个 target_frame launch 参数，启动一个用于第二只乌龟的广播器，以及一个订阅这些变换的监听器节点。

3 构建

Section titled “3 构建”

在工作空间根目录运行 rosdep 来检查缺失的依赖。

Linux：

Terminal window
rosdep install -i --from-path src --rosdistro {DISTRO} -y

macOS/Windows：

rosdep 仅在 Linux 上可用，可直接跳到下一步。

仍在工作空间根目录下构建功能包：

Linux/macOS：

Terminal window
colcon build --packages-select learning_tf2_py

Windows：

Terminal window
colcon build --merge-install --packages-select learning_tf2_py

打开一个新终端，导航到工作空间根目录，并 source 环境配置文件：

Linux/macOS：

Terminal window
. install/setup.bash

Windows：

在 Windows 命令行提示符中：

Terminal window
call install\setup.bat

或在 PowerShell 中：

Terminal window
.\install\setup.ps1

4 运行

Section titled “4 运行”

现在可以启动完整的乌龟演示了：

XML：

Terminal window
ros2 launch learning_tf2_py turtle_tf2_demo_launch.xml

YAML：

Terminal window
ros2 launch learning_tf2_py turtle_tf2_demo_launch.yaml

Python：

Terminal window
ros2 launch learning_tf2_py turtle_tf2_demo_launch.py

你应该能看到带有两只乌龟的 turtlesim。 在第二个终端窗口中输入以下命令：

Terminal window
ros2 run turtlesim turtle_teleop_key

要验证是否正常工作，只需用方向键驱动第一只乌龟移动（确保当前活动窗口是运行 turtle_teleop_key 的终端，而不是仿真器窗口），你会看到第二只乌龟紧紧跟随第一只乌龟！

总结

Section titled “总结”

在本教程中，你学习了如何使用 tf2 获取坐标系变换。 至此，你也完成了 tf2 简介教程中开始的 turtlesim 演示。