夜雨聆风目标： 学习如何使用 tf2 获取帧变换。
教程级别： 中级
时间： 10 分钟
背景
在之前的教程中，我们创建了一个 tf2 广播器来向 tf2 发布乌龟的位姿。
在本教程中，我们将创建一个 tf2 监听器来开始使用 tf2。
前置条件
本教程假定您已完成“tf2 静态广播器教程（Python）”和“tf2 广播器教程（Python）”。在之前的教程中，我们创建了一个 learning_tf2_py 包，我们将继续在其中工作。
任务
1 编写监听器节点
首先创建源文件。转到我们在上一个教程中创建的 learning_tf2_py 包。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载示例监听器代码：
Linux：
wget https://raw.githubusercontent.com/ros/geometry_tutorials/lyrical/turtle_tf2_py/turtle_tf2_py/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):
pass1.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 方法：
- 目标帧
- 源帧
- 我们想要变换的时间
提供 rclpy.time.Time() 将只获取最新的可用变换。所有这些都包装在 try-except 块中，以处理可能的异常。
t = self.tf_buffer.lookup_transform(
to_frame_rel,
from_frame_rel,
rclpy.time.Time())1.2 添加入口点
为了允许 ros2 run 命令运行您的节点，您必须将入口点添加到位于 src/learning_tf2_py 目录中的 setup.py。
在 'console_scripts': 括号之间添加以下行：
'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',2 更新启动文件
使用文本编辑器打开位于 src/learning_tf2_py/launch 目录中的启动文件 turtle_tf2_demo_launch（扩展名为 .py、.xml 或 .yaml），向启动描述中添加两个新节点，添加一个启动参数，并添加相应的 import 语句。生成的文件应如下所示：
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 启动参数，启动一个用于我们将生成的第二只乌龟的广播器，以及一个将订阅这些变换的监听器。
3 构建
在工作空间根目录运行 rosdep 来检查缺少的依赖项。
rosdep install -i --from-path src --rosdistro lyrical -y仍在工作空间根目录中，构建您的包：
colcon build --packages-select learning_tf2_py打开一个新终端，导航到工作空间根目录，并加载设置文件：
. install/setup.bash4 运行
现在您已准备好启动完整的乌龟演示：
ros2 launch learning_tf2_py turtle_tf2_demo_launch.py您应该会看到带有两只乌龟的 turtlesim。在第二个终端窗口中输入以下命令：
ros2 run turtlesim turtle_teleop_key要查看是否正常工作，只需使用箭头键驾驶第一只乌龟移动（确保您的终端窗口处于活动状态，而不是您的模拟器窗口），您将看到第二只乌龟跟随第一只！
总结
在本教程中，您学习了如何使用 tf2 获取帧变换。您也完成了编写自己的 turtlesim 演示，该演示首次在“tf2 简介”教程中尝试。
