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

十年一梦实验室

于 2024-07-11 18:48:18 发布

阅读量571

点赞数 19

文章标签： python 开发语言

本文链接：https://blog.csdn.net/cxyhjl/article/details/140366429

版权

目标：学习如何使用 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.0
                request.y = 2.0  # 设置turtle的y坐标为2.0
                request.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 方法，传入以下参数：

目标框架
来源框架
我们想要转变的时间

提供 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 演示。

十年一梦实验室

关注

19
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
【ROS2】中级:tf2-编写监听器（Python）

目标：学习如何使用 tf2 获取帧变换。教程级别：中级时间：10 分钟目录背景先决条件任务1. 编写监听节点2. 更新启动文件3. 构建4. 运行摘要背景在之前的教程中，我们创建了一个 tf2 广播器来发布乌龟的姿态到 tf2。在本教程中，我们将创建一个 tf2 监听器以开始使用 tf2。先决条件本教程假设您已经完成了 tf2 静态广播器教程（Python）和 tf2 广播器教程（...
复制链接

扫一扫