【ROS2】中级:tf2-使用带有 tf2_ros::MessageFilter 的已标记数据类型-优快云博客

本文链接：https://blog.youkuaiyun.com/cxyhjl/article/details/140411646

目标：学习如何使用 tf2_ros::MessageFilter 处理标记的数据类型。

教程级别：中级

时间：10 分钟

背景
任务
- 1. 编写 PointStamped 消息的广播节点
- 1.1 检查代码
- 1.2 编写启动文件
- 1.3 添加入口点
- 1.4 构建
- 2. 编写消息过滤器/监听器节点
- 2.1 检查代码
- 2.2 添加依赖项
- 2.3 CMakeLists.txt
- 2.4 构建
- 3 运行
摘要

背景

本教程解释了如何使用 tf2 处理传感器数据。一些传感器数据的实际例子是：

相机，包括单目Mono和双目Stereo

激光扫描

假设创建了一只名为 turtle3 的新乌龟，它的里程计不好，但有一个高架摄像头跟踪它的位置，并将其作为 PointStamped 消息发布到 world 框架中。

turtle1 想知道 turtle3 相对于自己在哪里。

要做到这一点， turtle1 必须监听发布 turtle3 姿态的主题，等待转换到所需框架的准备就绪，然后执行其操作。为了使这更容易， tf2_ros::MessageFilter 非常有用。 tf2_ros::MessageFilter 将订阅任何带有标头的 ROS 2 消息并将其缓存，直到可以将其转换为目标框架。

任务

1 编写 PointStamped 消息的广播节点

在本教程中，我们将设置一个演示应用程序，该应用程序有一个节点（用 Python 编写）来广播 PointStamped 位置消息的 turtle3 。

首先，让我们创建源文件。

转到我们在上一个教程中创建的 learning_tf2_py 包https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Static-Broadcaster-Py.html 。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载示例传感器消息广播代码：

cxy@ubuntu2404-cxy:~/ros2_ws/src/learning_tf2_py/learning_tf2_py$ wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py

使用您喜欢的文本编辑器打开文件。

from geometry_msgs.msg import PointStamped  # 从geometry_msgs.msg导入PointStamped消息类型
from geometry_msgs.msg import Twist  # 从geometry_msgs.msg导入Twist消息类型


import rclpy  # 导入rclpy库
from rclpy.node import Node  # 从rclpy.node导入Node类


from turtlesim.msg import Pose  # 从turtlesim.msg导入Pose消息类型
from turtlesim.srv import Spawn  # 从turtlesim.srv导入Spawn服务类型




class PointPublisher(Node):  # 定义一个名为PointPublisher的类，继承自Node类


    def __init__(self):  # 初始化方法
        super().__init__('turtle_tf2_message_broadcaster')  # 调用父类的初始化方法，并设置节点名称为'turtle_tf2_message_broadcaster'


        # 创建一个客户端以生成乌龟
        self.spawner = self.create_client(Spawn, 'spawn')  # 创建一个客户端，服务类型为Spawn，服务名称为'spawn'
        # 布尔值存储信息
        # 如果生成乌龟的服务可用
        self.turtle_spawning_service_ready = False  # 初始化布尔变量，表示生成乌龟的服务是否可用
        # 如果乌龟成功生成
        self.turtle_spawned = False  # 初始化布尔变量，表示乌龟是否成功生成
        # 如果可以订阅turtle3的话题
        self.turtle_pose_cansubscribe = False  # 初始化布尔变量，表示是否可以订阅turtle3的话题


        self.timer = self.create_timer(1.0, self.on_timer)  # 创建一个定时器，每1秒调用一次on_timer方法


    def on_timer(self):  # 定时器回调方法
        if self.turtle_spawning_service_ready:  # 如果生成乌龟的服务可用
            if self.turtle_spawned:  # 如果乌龟已生成
                self.turtle_pose_cansubscribe = True  # 可以订阅turtle3的话题
            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():  # 如果生成乌龟的服务已准备好
                # 初始化请求，设置乌龟名称和坐标
                # 注意x, y和theta在turtlesim/srv/Spawn中定义为浮点数
                request = Spawn.Request()  # 创建一个Spawn请求对象
                request.name = 'turtle3'  # 设置乌龟名称为'turtle3'
                request.x = 4.0  # 设置乌龟的x坐标为4.0
                request.y = 2.0  # 设置乌龟的y坐标为2.0
                request.theta = 0.0  # 设置乌龟的theta角度为0.0
                # 发送请求
                self.result = self.spawner.call_async(request)  # 异步调用生成乌龟的服务
                self.turtle_spawning_service_ready = True  # 设置生成乌龟的服务已准备好
            else:
                # 检查服务是否准备好
                self.get_logger().info('Service is not ready')  # 记录日志，表示服务未准备好


        if self.turtle_pose_cansubscribe:  # 如果可以订阅turtle3的话题
            self.vel_pub = self.create_publisher(Twist, 'turtle3/cmd_vel', 10)  # 创建一个发布者，发布Twist消息到'turtle3/cmd_vel'话题
            self.sub = self.create_subscription(Pose, 'turtle3/pose', self.handle_turtle_pose, 10)  # 创建一个订阅者，订阅'turtle3/pose'话题，并指定回调函数为handle_turtle_pose
            self.pub = self.create_publisher(PointStamped, 'turtle3/turtle_point_stamped', 10)  # 创建一个发布者，发布PointStamped消息到'turtle3/turtle_point_stamped'话题


    def handle_turtle_pose(self, msg):  # 处理乌龟位姿的回调函数
        vel_msg = Twist()  # 创建一个Twist消息对象
        vel_msg.linear.x = 1.0  # 设置线速度为1.0
        vel_msg.angular.z = 1.0  # 设置角速度为1.0
        self.vel_pub.publish(vel_msg)  # 发布Twist消息