【ROS2】中级:tf2-编写监听器（Python）

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

教程级别：中级

 时间：10 分钟

 目录

•  背景

•  先决条件

•  任务

  • 1. 编写监听节点

  • 2. 更新启动文件

  • 3. 构建

  • 4. 运行

•  摘要

 背景

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

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

 先决条件

本教程假设您已经完成了 tf2 静态广播器教程（Python）和 tf2 广播器教程（Python）。在上一个教程中，我们创建了一个 learning_tf2_py 包，我们将继续在此基础上进行工作。

 任务

1. 编写监听节点

让我们首先创建源文件。转到我们在上一个教程中创建的 learning_tf2_py 包。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载示例监听器代码：

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

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

# 版权 2021 开源机器人基金会，公司。
#
# 根据Apache许可证版本2.0（“许可证”）许可；
# 除非符合许可证，否则不得使用此文件。
# 您可以在以下位置获取许可证副本：
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# 除非适用法律要求或书面同意，否则在许可证下分发的软件
# 将按“原样”基础分发，无任何形式的明示或暗示
# 保证或条件。有关在许可证下管理权限和
# 限制的语言，请参阅许可证。import math  # 导入数学库from geometry_msgs.msg import Twist  # 从geometry_msgs包导入Twist消息import rclpy  # 导入ROS客户端库
from rclpy.node import Node  # 从rclpy.node导入Node类from tf2_ros import TransformException  # 从tf2_ros导入TransformException异常
from tf2_ros.buffer import Buffer  # 从tf2_ros.buffer导入Buffer类
from tf2_ros.transform_listener import TransformListener  # 从tf2_ros.transform_listener导入TransformListener类from turtlesim.srv import Spawn  # 从turtlesim.srv导入Spawn服务class FrameListener(Node):  # 定义FrameListener类，继承自Node类def __init__(self):  # 定义初始化函数super().__init__('turtle_tf2_frame_listener')  # 调用父类初始化函数，设置节点名为'turtle_tf2_frame_listener'# 声明并获取`target_frame`参数self.target_frame = self.declare_parameter('target_frame', 'turtle1').get_parameter_value().string_value  # 声明并获取'target_frame'参数，默认值为'turtle1'self.tf_buffer = Buffer()  # 创建tf缓冲区self.tf_listener = TransformListener(self.tf_buffer, self)  # 创建tf监听器# 创建一个客户端来生成一个turtle # 在ROS中创建了一个客户端，用于调用名为 'spawn' 的服务。self.spawner = self.create_client(Spawn, 'spawn')  # 创建一个Spawn服务的客户端，服务名为'spawn'# 布尔值用于存储信息# 如果生成turtle的服务可用self.turtle_spawning_service_ready = False  # 初始化turtle生成服务状态为不可用# 如果turtle成功生成self.turtle_spawned = False  # 初始化turtle生成状态为未生成# 创建turtle2速度发布器self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)  # 创建一个Twist消息的发布器，话题名为'turtle2/cmd_vel'，QoS设置为1# 每秒调用一次on_timer函数self.timer = self.create_timer(1.0, self.on_timer)  # 创建一个定时器，每秒调用一次on_timer函数def on_timer(self):  # 定义定时器回调函数# 将帧名存储在变量中，这些变量将用于# 计算变换from_frame_rel = self.target_frame  # 设置源帧为目标帧to_frame_rel = 'turtle2'  # 设置目标帧为'turtle2'if self.turtle_spawning_service_ready:  # 如果turtle生成服务可用if self.turtle_spawned:  # 如果turtle已经生成# 查找目标帧和turtle2帧之间的变换# 并发送速度命令使turtle2达到目标帧try:t = self.tf_buffer.lookup_transform(to_frame_rel,from_frame_rel,rclpy.time.Time())  # 尝试查找目标帧和turtle2帧之间的变换except TransformException as ex:  # 如果查找失败，抛出TransformException异常self.get_logger().info(f'无法将 {to_frame_rel} 变换到 {from_frame_rel}: {ex}')  # 记录无法进行变换的信息return  # 返回，不执行后续代码msg = Twist()  # 创建一个Twist消息scale_rotation_rate = 1.0  # 设置旋转速度比例因子msg.angular.z = scale_rotation_rate * math.atan2(t.transform.translation.y,t.transform.translation.x)  # 计算并设置z轴的角速度scale_forward_speed = 0.5  # 设置前进速度比例因子msg.linear.x = scale_forward_speed * math.sqrt(t.transform.translation.x ** 2 +t.transform.translation.y ** 2)  # 计算并设置x轴的线速度self.publisher.publish(msg)  # 发布Twist消息else:  # 如果turtle未生成if self.result.done():  # 如果服务调用完成self.get_logger().info(f'成功生成 {self.result.result().name}')  # 记录成功生成turtle的信息self.turtle_spawned = True  # 设置turtle生成状态为已生成else:  # 如果服务调用未完成self.get_logger().info('生成未完成')  # 记录生成未完成的信息else:  # 如果turtle生成服务不可用if self.spawner.service_is_ready():  # 如果服务可用# 使用turtle名和坐标初始化请求# 注意在turtlesim/srv/Spawn中，x、y和theta被定义为浮点数request = Spawn.Request()  # 创建一个Spawn请求request.name = 'turtle2'  # 设置turtle名为'turtle2'request.x = 4.0  # 设置turtle的x坐标为4.0request.y = 2.0  # 设置turtle的y坐标为2.0request.theta = 0.0  # 设置turtle的theta值为0.0# 调用请求self.result = self.spawner.call_async(request)  # 异步调用服务，发送请求self.turtle_spawning_service_ready = True  # 设置turtle生成服务状态为可用else:  # 如果服务不可用# 检查服务是否准备好self.get_logger().info('服务未准备好')  # 记录服务未准备好的信息def main():  # 定义主函数rclpy.init()  # 初始化ROS客户端库node = FrameListener()  # 创建一个FrameListener节点try:rclpy.spin(node)  # 让节点保持运行，处理回调函数except KeyboardInterrupt:  # 如果收到键盘中断信号pass  # 不做任何操作rclpy.shutdown()  # 关闭ROS客户端库

1.1 检查代码

要了解生成海龟服务背后的工作原理，请参阅编写一个简单的服务和客户端（Python）教程 https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Service-And-Client.html 。

现在，让我们来看一下与获取帧转换相关的代码。 tf2_ros 包提供了一个 TransformListener 的实现，以帮助简化接收转换的任务。

from tf2_ros.transform_listener import TransformListener

在这里，我们创建一个 TransformListener 对象。监听器创建后，它开始通过网络接收 tf2 变换，并将它们缓冲最多 10 秒。

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

最后，我们查询监听器以获取特定转换。我们调用 lookup_transform 方法，传入以下参数：

1.  目标框架

2.  来源框架

3. 我们想要转变的时间

提供 rclpy.time.Time() 将只会得到最新可用的转换。所有这些都包装在一个 try-except 块中，以处理可能的异常。

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

1.2 添加一个入口点 

要允许 ros2 run 命令运行您的节点，您必须将入口点添加到 setup.py （位于 src/learning_tf2_py 目录中）。

在 'console_scripts': 括号之间添加以下行：

'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',

2. 更新启动文件

打开位于 src/learning_tf2_py/launch 目录中名为 turtle_tf2_demo_launch.py 的启动文件，用你的文本编辑器，向启动描述中添加两个新节点，添加一个启动参数，并添加导入。结果文件应该看起来像：

from launch import LaunchDescription  # 从launch包导入LaunchDescription类
from launch.actions import DeclareLaunchArgument  # 从launch.actions导入DeclareLaunchArgument类
from launch.substitutions import LaunchConfiguration  # 从launch.substitutions导入LaunchConfiguration类from launch_ros.actions import Node  # 从launch_ros.actions导入Node类def generate_launch_description():  # 定义generate_launch_description函数return LaunchDescription([  # 返回一个LaunchDescription对象Node(  # 创建一个Node对象package='turtlesim',  # 设置包名为'turtlesim'executable='turtlesim_node',  # 设置可执行文件名为'turtlesim_node'name='sim'  # 设置节点名为'sim'),Node(  # 创建一个Node对象package='learning_tf2_py',  # 设置包名为'learning_tf2_py'executable='turtle_tf2_broadcaster',  # 设置可执行文件名为'turtle_tf2_broadcaster'name='broadcaster1',  # 设置节点名为'broadcaster1'parameters=[  # 设置参数{'turtlename': 'turtle1'}  # 设置'turtlename'参数为'turtle1']),DeclareLaunchArgument(  # 创建一个DeclareLaunchArgument对象'target_frame', default_value='turtle1',  # 设置参数名为'target_frame'，默认值为'turtle1'description='Target frame name.'  # 设置参数描述为'Target frame name.'),Node(  # 创建一个Node对象package='learning_tf2_py',  # 设置包名为'learning_tf2_py'executable='turtle_tf2_broadcaster',  # 设置可执行文件名为'turtle_tf2_broadcaster'name='broadcaster2',  # 设置节点名为'broadcaster2'parameters=[  # 设置参数{'turtlename': 'turtle2'}  # 设置'turtlename'参数为'turtle2']),Node(  # 创建一个Node对象package='learning_tf2_py',  # 设置包名为'learning_tf2_py'executable='turtle_tf2_listener',  # 设置可执行文件名为'turtle_tf2_listener'name='listener',  # 设置节点名为'listener'parameters=[  # 设置参数{'target_frame': LaunchConfiguration('target_frame')}  # 设置'target_frame'参数为LaunchConfiguration对象，参数名为'target_frame']),])

这将声明一个 target_frame 启动参数，启动第二只乌龟的广播器，我们将生成这只乌龟，并启动一个监听器来订阅那些变换。

 3.构建

在工作区的根目录运行 rosdep 以检查缺失的依赖项。

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

仍在工作区的根目录下，构建您的包：

colcon build --packages-select learning_tf2_py

打开一个新的终端，导航到工作区的根目录，然后加载设置文件：

. install/setup.bash

4. 运行

现在您已准备好开始完整的海龟演示：

ros2 launch learning_tf2_py turtle_tf2_demo_launch.py

您应该看到有两只乌龟的 turtle sim。在第二个终端窗口中输入以下命令：

ros2 run turtlesim turtle_teleop_key

要查看事物是否正常工作，只需使用箭头键驾驶第一只乌龟（确保您的终端窗口处于活动状态，而不是您的模拟器窗口），您会看到第二只乌龟跟随第一只乌龟！

 摘要

在本教程中，您学习了如何使用 tf2 来获取帧变换的访问权限。您还完成了编写您在 tf2 教程https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Introduction-To-Tf2.html 介绍中首次尝试的 turtlesim 演示。