机器人系统ros2-开发实践08-了解如何使用 tf2 来访问坐标帧转换(Python)-编程知识

tf2 库允许你在 ROS 节点中查询两个帧之间的转换。这个查询可以是阻塞的，也可以是非阻塞的，取决于你的需求。下面是一个基本的 Python 示例，展示如何在 ROS 节点中使用 tf2 查询帧转换。

本教程假设您已完成tf2 静态广播器教程 (Python)和tf2 广播器教程 (Python)。在上一个教程中，我们创建了一个learning_tf2_py包。机器人系统ros2-开发实践06-将静态坐标系广播到 tf2(Python)-定义机器人底座与其传感器或非移动部件之间的关系

步骤1：

1 编写监听节点

我们首先创建源文件。转到learning_tf2_py我们在上一教程中创建的包。在src/learning_tf2_py/learning_tf2_py目录中新建turtle_tf2_listener.py，用vscode 打开文件，将下面的代码贴入文件中。

代码如下：

import mathfrom geometry_msgs.msg import Twistimport rclpy
from rclpy.node import Nodefrom tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListenerfrom turtlesim.srv import Spawnclass FrameListener(Node):def __init__(self):super().__init__('turtle_tf2_frame_listener')# Declare and acquire `target_frame` parameterself.target_frame = self.declare_parameter('target_frame', 'turtle1').get_parameter_value().string_valueself.tf_buffer = Buffer()self.tf_listener = TransformListener(self.tf_buffer, self)# Create a client to spawn a turtleself.spawner = self.create_client(Spawn, 'spawn')# Boolean values to store the information# if the service for spawning turtle is availableself.turtle_spawning_service_ready = False# if the turtle was successfully spawnedself.turtle_spawned = False# Create turtle2 velocity publisherself.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)# Call on_timer function every secondself.timer = self.create_timer(1.0, self.on_timer)def on_timer(self):# Store frame names in variables that will be used to# compute transformationsfrom_frame_rel = self.target_frameto_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_frametry: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}')returnmsg = Twist()scale_rotation_rate = 1.0msg.angular.z = scale_rotation_rate * math.atan2(t.transform.translation.y,t.transform.translation.x)scale_forward_speed = 0.5msg.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 = Trueelse: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/srv/Spawnrequest = Spawn.Request()request.name = 'turtle2'request.x = float(4)request.y = float(2)request.theta = float(0)# Call requestself.result = self.spawner.call_async(request)self.turtle_spawning_service_ready = Trueelse:# Check if the service is readyself.get_logger().info('Service is not ready')def main():rclpy.init()node = FrameListener()try:rclpy.spin(node)except KeyboardInterrupt:passrclpy.shutdown()

2 代码说明：

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())

3 新增启动入口点

要允许命令运行您的节点，您必须将入口点添加到src/learning_tf2_py 的setup.py ，在setup.py 文件里找到console_scripts，在括号之间添加以下行

'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',

在这里插入图片描述

3.1 更新启动文件

使用文本编辑器打开目录中调用的启动文件src/learning_tf2_py/launch，向启动描述turtle_tf2_demo.launch.py添加两个新节点，添加启动参数，然后添加导入。生成的新的文件应如下所示：

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfigurationfrom launch_ros.actions import Nodedef 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 humble -y

在这里插入图片描述

仍然在工作区的根目录中构建您的包：

colcon build --packages-select learning_tf2_py

在这里插入图片描述

打开一个新终端，导航到工作区的根目录，然后获取安装文件：

. install/setup.bash

4 运行

现在您已准备好开始完整的海龟演示：
在工作区的根目录执行

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

看到有两个海龟，另外一个会向其中一个自动靠齐

在这里插入图片描述

您应该会看到海龟模拟卡上有两只海龟。在第二个终端窗口中键入以下命令：

ros2 run turtlesim turtle_teleop_key

在这里插入图片描述

要查看是否有效，只需使用箭头键围绕第一只乌龟行驶（确保您的终端窗口处于活动状态，而不是模拟器窗口），您将看到第二只乌龟紧随第一只乌龟！