编写tf2监听程序 (Python) [待校准@8691]

Goal目标: Learn了解如何使用tf2访问帧转换。 [待校准@8674]

Tutorial教程级别: Intermediate中级 [待校准@6713]

时间: 10分钟 [Alyssa@7452]

背景

在之前的教程中，我们创建了一个tf2广播器，将海龟的姿势发布到tf2。 [待校准@8675]

在本教程中，我们将创建一个tf2监听程序以开始使用tf2。 [待校准@8676]

先决条件

本教程假设您已经完成了 tf2 broadcaster tutorial (Python) 。在之前的教程中，我们创建了一个 learning_tf2_py 包，在这里我们将继续工作。 [待校准@8692]

任务

1写入监听节点 [待校准@8678]

让我们先创建源文件。转到我们在上一教程中创建的 learning_tf2_py 包。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载示例侦听器代码: [待校准@8693]

LinuxmacOSWindows

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

使用您喜欢的文本编辑器打开文件。 [待校准@8591]

import math

from geometry_msgs.msg import Twist

import rclpy
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.srv import Spawn


class FrameListener(Node):

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

        # Declare and acquire `target_frame` parameter
        self.declare_parameter('target_frame', 'turtle1')
        self.target_frame = self.get_parameter(
            'target_frame').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:
                    now = rclpy.time.Time()
                    trans = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        now)
                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(
                    trans.transform.translation.y,
                    trans.transform.translation.x)

                scale_forward_speed = 0.5
                msg.linear.x = scale_forward_speed * math.sqrt(
                    trans.transform.translation.x ** 2 +
                    trans.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/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():
    rclpy.init()
    node = FrameListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

1.1检查代码 [待校准@8367]

要了解产卵海龟背后的服务是如何工作的，请参考 writing a simple service and client (Python) 教程。 [待校准@8694]

现在，让我们看一下与访问帧转换相关的代码。 tf2_ros 包提供了 TransformListener 的实现，以帮助简化接收转换的任务。 [待校准@8695]

from tf2_ros.transform_listener import TransformListener

在这里，我们创建一个 TransformListener 对象。创建侦听器后，它开始通过导线接收tf2转换，并将其缓冲长达10秒。 [待校准@8682]

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

最后，我们向侦听器查询特定转换。我们使用以下参数调用 lookup_transform 方法: [待校准@8683]

1. 目标帧 [待校准@8559]

2. 源帧 [待校准@8561]

3. 我们想要转换的时间 [待校准@8684]

提供 rclpy.time.Time() 将为我们提供最新的转换。所有这些都包含在一个try-exception块中，以处理可能的异常。 [待校准@8696]

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

2构建和运行 [待校准@7729]

使用文本编辑器，打开调用 turtle_tf2_demo.launch.py 的launch文件，并在第一个 turtle1 广播器节点后添加以下行。此外，在文件开头包括 DeclareLaunchArgument 和 LaunchConfiguration 的进口: [待校准@8686]

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([
        ...,
        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的论点，为第二只乌龟启动一个广播器，我们将产卵，听众将订阅这些转变。现在，您可以开始完整的turtle例程了: [待校准@8687]

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

你应该看看有两只海龟的海龟sim。在第二个终端窗口中，键入以下命令: [待校准@8688]

ros2 run turtlesim turtle_teleop_key

3检查结果 [待校准@8476]

要查看是否正常工作，只需使用箭头键绕开第一只乌龟 (确保终端窗口处于活动状态，而不是模拟器窗口)，你会看到第二只乌龟跟着第一只! [待校准@8689]

总结

在本教程中，您学习了如何使用tf2访问帧转换。你也已经完成了你自己的turtlesim例程，这是你第一次在 Introduction to tf2 教程中尝试。 [待校准@8690]