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

目标：学习如何使用 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消息


        ps = PointStamped()  # 创建一个PointStamped消息对象
        ps.header.stamp = self.get_clock().now().to_msg()  # 设置时间戳为当前时间
        ps.header.frame_id = 'world'  # 设置坐标系ID为'world'
        ps.point.x = msg.x  # 设置点的x坐标为乌龟的x坐标
        ps.point.y = msg.y  # 设置点的y坐标为乌龟的y坐标
        ps.point.z = 0.0  # 设置点的z坐标为0.0
        self.pub.publish(ps)  # 发布PointStamped消息




def main():  # 主函数
    rclpy.init()  # 初始化rclpy
    node = PointPublisher()  # 创建一个PointPublisher节点
    try:
        rclpy.spin(node)  # 运行节点
    except KeyboardInterrupt:  # 捕捉键盘中断异常
        pass


    rclpy.shutdown()  # 关闭rclpy

1.1 检查代码

现在让我们看看代码。首先，在 on_timer 回调函数中，我们通过异步调用 turtlesim 的 Spawn 服务来生成 turtle3 ，并在乌龟生成服务准备就绪时将其位置初始化为(4, 2, 0)。

# 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 = 'turtle3'
request.x = 4.0
request.y = 2.0
request.theta = 0.0
# Call request
self.result = self.spawner.call_async(request)

之后，该节点发布主题 turtle3/cmd_vel 、主题 turtle3/turtle_point_stamped ，并订阅主题 turtle3/pose ，在每个传入消息上运行回调函数 handle_turtle_pose 。

self.vel_pub = self.create_publisher(Twist, '/turtle3/cmd_vel', 10)
self.sub = self.create_subscription(Pose, '/turtle3/pose', self.handle_turtle_pose, 10)
self.pub = self.create_publisher(PointStamped, '/turtle3/turtle_point_stamped', 10)

最后，在回调函数 handle_turtle_pose 中，我们初始化 turtle3 的 Twist 消息并发布它们，这将使 turtle3 沿着一个圆圈移动。然后我们用传入的 Pose 消息填充 turtle3 的 PointStamped 消息并发布它们。

vel_msg = Twist()
vel_msg.linear.x = 1.0
vel_msg.angular.z = 1.0
self.vel_pub.publish(vel_msg)


ps = PointStamped()
ps.header.stamp = self.get_clock().now().to_msg()
ps.header.frame_id = 'world'
ps.point.x = msg.x
ps.point.y = msg.y
ps.point.z = 0.0
self.pub.publish(ps)

1.2 编写启动文件

要运行此演示，我们需要在包 learning_tf2_py 的 launch 子目录中创建一个启动文件 turtle_tf2_sensor_message_launch.py ：

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node




def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument(
            'target_frame', default_value='turtle1',
            description='Target frame name.'
        ),
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim',
            output='screen'
        ),
        Node(
            package='turtle_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
        Node(
            package='turtle_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster2',
            parameters=[
                {'turtlename': 'turtle3'}
            ]
        ),
        Node(
            package='turtle_tf2_py',
            executable='turtle_tf2_message_broadcaster',
            name='message_broadcaster',
        ),
    ])

1.3 添加入口点

要允许 ros2 run 命令运行您的节点，您必须将入口点添加到 setup.py （位于 src/learning_tf2_py 目录中）。

在 'console_scripts': 括号之间添加以下行：

'turtle_tf2_message_broadcaster = learning_tf2_py.turtle_tf2_message_broadcaster:main',

 1.4 构建 

在工作区的根目录运行 rosdep 以检查缺失的依赖项。

rosdep install -i --from-path src --rosdistro jazzy -y

然后我们可以构建这个包：

colcon build --packages-select learning_tf2_py

2 编写消息过滤器/监听器节点

现在，为了可靠地获取 turtle1 框架中 turtle3 的流 PointStamped 数据，我们将创建消息过滤器/监听器节点的源文件。

转到我们在上一个教程中创建的 learning_tf2_cpp 包。在 src/learning_tf2_cpp/src 目录中，通过输入以下命令下载文件 turtle_tf2_message_filter.cpp ：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp

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

这段代码定义了一个名为 PoseDrawer 的类，它继承自 rclcpp::Node，用于在ROS 2中实现坐标变换和消息过滤。以下是代码的总结：

1. 包含头文件：代码包含了用于时间操作、智能指针、字符串操作、消息类型、消息过滤、ROS 2客户端库、tf2库等的头文件。

2. 命名空间：使用 std::chrono_literals 命名空间中的字面量。

3. 类定义：定义了一个名为 PoseDrawer 的类，继承自 rclcpp::Node。

4. 构造函数：

• 初始化节点名称为 "turtle_tf2_pose_drawer"。

• 声明并获取 target_frame 参数，默认值为 "turtle1"。

• 创建一个 tf2_ros::Buffer 对象，并设置定时器接口。

• 创建一个 tf2_ros::TransformListener 对象，用于监听坐标变换。

• 订阅 /turtle3/turtle_point_stamped 话题。

• 创建一个 tf2_ros::MessageFilter 对象，用于过滤消息，并注册回调函数。

回调函数：

• 定义了一个名为 msgCallback 的回调函数，用于处理接收到的 PointStamped 消息。

• 在回调函数中，进行坐标变换，并输出变换后的坐标。如果发生异常，打印异常信息。

成员变量：

• target_frame_：目标坐标系。

• tf2_buffer_：tf2缓冲区。

• tf2_listener_：tf2变换监听器。

• point_sub_：消息订阅器。

• tf2_filter_：消息过滤器。

主函数：

• 初始化ROS 2。

• 创建 PoseDrawer 节点并进入循环。

• 关闭ROS 2。

这段代码的主要功能是订阅一个 PointStamped 消息，并将其坐标变换到指定的目标坐标系，然后输出变换后的坐标。

// 引入所需的头文件
#include <chrono>  // 用于处理时间
#include <memory>  // 用于处理内存  包含用于智能指针的头文件
#include <string>  // 用于处理字符串


#include "geometry_msgs/msg/point_stamped.hpp"  // 用于处理带时间戳的点消息
#include "message_filters/subscriber.h"  // 用于订阅消息
#include "rclcpp/rclcpp.hpp"  // ROS2的核心库
#include "tf2_ros/buffer.h"  // 用于处理tf2的缓冲区
#include "tf2_ros/create_timer_ros.h"  // 用于创建tf2的定时器
#include "tf2_ros/message_filter.h"  // 用于过滤tf2的消息
#include "tf2_ros/transform_listener.h"  // 用于监听tf2的转换
#ifdef TF2_CPP_HEADERS
  #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"  // 用于处理tf2的几何消息
#else
  #include "tf2_geometry_msgs/tf2_geometry_msgs.h"  // 用于处理tf2的几何消息
#endif


using namespace std::chrono_literals;  // 使用标准库中的时间字面量


// 定义一个名为PoseDrawer的类，继承自rclcpp::Node
class PoseDrawer : public rclcpp::Node
{
public:
  // 构造函数
  PoseDrawer()
  : Node("turtle_tf2_pose_drawer")  // 初始化节点名为"turtle_tf2_pose_drawer"
  {
    // 声明并获取`target_frame`参数
    target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");


    std::chrono::duration<int> buffer_timeout(1);  // 设置缓冲区超时为1秒


    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());  // 创建tf2的缓冲区
    // 在调用waitForTransform之前创建定时器接口，
    // 以避免tf2_ros::CreateTimerInterfaceException异常
    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
      this->get_node_base_interface(),
      this->get_node_timers_interface());
    tf2_buffer_->setCreateTimerInterface(timer_interface);// 设置定时器接口
    tf2_listener_ =
      std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);  // 创建tf2的转换监听器


    point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");  // 订阅"/turtle3/turtle_point_stamped"主题
    tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
      point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
      this->get_node_clock_interface(), buffer_timeout);  // 创建tf2的消息过滤器
    // 使用tf2_ros::MessageFilter注册一个回调函数，当转换可用时调用
    tf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);
  }


private:
  // 消息回调函数
  void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr)
{
    geometry_msgs::msg::PointStamped point_out;  // 输出的带时间戳的点消息
    try {
      tf2_buffer_->transform(*point_ptr, point_out, target_frame_);  // 尝试进行转换
      RCLCPP_INFO(
        this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n",
        point_out.point.x,
        point_out.point.y,
        point_out.point.z);  // 打印转换后的点坐标
    } catch (const tf2::TransformException & ex) {
      RCLCPP_WARN(
        // 打印捕获的异常
        this->get_logger(), "Failure %s\n", ex.what());
    }
  }
  std::string target_frame_;  // 目标帧
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;  // tf2的缓冲区
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;  // tf2的转换监听器
  message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;  // 点消息的订阅者
  std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;  // tf2的消息过滤器
};


// 主函数
int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);  // 初始化ROS
  rclcpp::spin(std::make_shared<PoseDrawer>());  // 运行PoseDrawer节点
  rclcpp::shutdown();  // 关闭ROS
  return 0;  // 返回0表示程序正常结束
}

2.1 检查代码 

首先，您必须包含来自 tf2_ros 包的 tf2_ros::MessageFilter 头文件，以及先前使用的 tf2 和 ros2 相关头文件。

#include "geometry_msgs/msg/point_stamped.hpp"
#include "message_filters/subscriber.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_listener.h"
#ifdef TF2_CPP_HEADERS
  #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#else
  #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#endif

其次，需要有持续的 tf2_ros::Buffer 、 tf2_ros::TransformListener 和 tf2_ros::MessageFilter 实例。

std::string target_frame_;
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;

第三，ROS 2 message_filters::Subscriber 必须使用主题进行初始化。而 tf2_ros::MessageFilter 必须使用该 Subscriber 对象进行初始化。 MessageFilter 构造函数中的其他参数是 target_frame 和回调函数。目标帧是它将确保 canTransform 成功的帧。回调函数是在数据准备好时将被调用的函数。

PoseDrawer()
: Node("turtle_tf2_pose_drawer")
{
  // Declare and acquire `target_frame` parameter
  target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");


  std::chrono::duration<int> buffer_timeout(1);


  tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
  // Create the timer interface before call to waitForTransform,
  // to avoid a tf2_ros::CreateTimerInterfaceException exception
  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
    this->get_node_base_interface(),
    this->get_node_timers_interface());
  tf2_buffer_->setCreateTimerInterface(timer_interface);
  tf2_listener_ =
    std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);


  point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");
  tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
    point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
    this->get_node_clock_interface(), buffer_timeout);
  // Register a callback with tf2_ros::MessageFilter to be called when transforms are available
  tf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);
}

最后，当数据准备好时，回调方法将调用 tf2_buffer_->transform 并将输出打印到控制台。

private:
  void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr)
  {
    geometry_msgs::msg::PointStamped point_out;
    try {
      tf2_buffer_->transform(*point_ptr, point_out, target_frame_);
      RCLCPP_INFO(
        this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n",
        point_out.point.x,
        point_out.point.y,
        point_out.point.z);
    } catch (const tf2::TransformException & ex) {
      RCLCPP_WARN(
        // Print exception which was caught
        this->get_logger(), "Failure %s\n", ex.what());
    }
  }

2.2 添加依赖项

在构建包 learning_tf2_cpp 之前，请在此包的 package.xml 文件中添加另外两个依赖项

<depend>message_filters</depend>
<depend>tf2_geometry_msgs</depend>

2.3 CMakeLists.txt

并在 CMakeLists.txt 文件中，在现有依赖项下方添加两行：

find_package(message_filters REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

以下内容将讨论 ROS 发行版之间的差异：

if(TARGET tf2_geometry_msgs::tf2_geometry_msgs)
  get_target_property(_include_dirs tf2_geometry_msgs::tf2_geometry_msgs INTERFACE_INCLUDE_DIRECTORIES)
else()
  set(_include_dirs ${tf2_geometry_msgs_INCLUDE_DIRS})
endif()


find_file(TF2_CPP_HEADERS
  NAMES tf2_geometry_msgs.hpp
  PATHS ${_include_dirs}
  NO_CACHE
  PATH_SUFFIXES tf2_geometry_msgs
)

之后，添加可执行文件并将其命名为 turtle_tf2_message_filter ，稍后将与 ros2 run 一起使用。

add_executable(turtle_tf2_message_filter src/turtle_tf2_message_filter.cpp)
ament_target_dependencies(
  turtle_tf2_message_filter
  geometry_msgs
  message_filters
  rclcpp
  tf2
  tf2_geometry_msgs
  tf2_ros
)


if(EXISTS ${TF2_CPP_HEADERS})
  target_compile_definitions(turtle_tf2_message_filter PUBLIC -DTF2_CPP_HEADERS)
endif()

最后，添加 install(TARGETS…) 部分（在其他现有节点下面），以便 ros2 run 可以找到您的可执行文件

install(TARGETS
  turtle_tf2_message_filter
  DESTINATION lib/${PROJECT_NAME})

find_package(message_filters REQUIRED) # 查找并引入message_filters包，标记为必需
find_package(tf2_geometry_msgs REQUIRED) # 查找并引入tf2_geometry_msgs包，标记为必需


if(TARGET tf2_geometry_msgs::tf2_geometry_msgs) # 如果目标tf2_geometry_msgs::tf2_geometry_msgs存在
  get_target_property(_include_dirs tf2_geometry_msgs::tf2_geometry_msgs INTERFACE_INCLUDE_DIRECTORIES) # 获取目标的包含目录属性，并赋值给_include_dirs
else() # 否则
  set(_include_dirs ${tf2_geometry_msgs_INCLUDE_DIRS}) # 将tf2_geometry_msgs_INCLUDE_DIRS赋值给_include_dirs
endif()


find_file(TF2_CPP_HEADERS # 查找文件TF2_CPP_HEADERS
  NAMES tf2_geometry_msgs.hpp # 文件名为tf2_geometry_msgs.hpp
  PATHS ${_include_dirs} # 在_include_dirs路径中查找
  NO_CACHE # 不缓存查找结果
  PATH_SUFFIXES tf2_geometry_msgs # 路径后缀为tf2_geometry_msgs
)


add_executable(turtle_tf2_message_filter src/turtle_tf2_message_filter.cpp) # 添加可执行文件turtle_tf2_message_filter，源文件为src/turtle_tf2_message_filter.cpp
ament_target_dependencies( # 设置目标的依赖项
  turtle_tf2_message_filter # 目标为turtle_tf2_message_filter
  geometry_msgs # 依赖geometry_msgs
  message_filters # 依赖message_filters
  rclcpp # 依赖rclcpp
  tf2 # 依赖tf2
  tf2_geometry_msgs # 依赖tf2_geometry_msgs
  tf2_ros # 依赖tf2_ros
)


if(EXISTS ${TF2_CPP_HEADERS}) # 如果TF2_CPP_HEADERS文件存在
  target_compile_definitions(turtle_tf2_message_filter PUBLIC -DTF2_CPP_HEADERS) # 为目标turtle_tf2_message_filter定义编译宏TF2_CPP_HEADERS
endif()


install(TARGETS # 安装目标
  turtle_tf2_message_filter # 目标为turtle_tf2_message_filter
  DESTINATION lib/${PROJECT_NAME}) # 安装路径为lib/${PROJECT_NAME}

 2.4 构建 

在工作区的根目录运行 rosdep 以检查缺失的依赖项。

rosdep install -i --from-path src --rosdistro jazzy -y

现在打开一个新的终端，导航到工作区的根目录，并使用以下命令重新构建包：

colcon build --packages-select learning_tf2_cpp

打开一个新的终端，导航到工作区的根目录，然后加载设置文件：

. install/setup.bash

 3 运行 

首先，我们需要通过启动启动文件 turtle_tf2_sensor_message_launch.py 来运行几个节点（包括 PointStamped 消息的广播节点）：

ros2 launch learning_tf2_py turtle_tf2_sensor_message_launch.py

这将弹出 turtlesim 窗口，其中有两只乌龟，其中 turtle3 沿着圆圈移动，而 turtle1 一开始没有移动。但你可以在另一个终端运行 turtle_teleop_key 节点来驱动 turtle1 移动：

ros2 run turtlesim turtle_teleop_key

ros2 run tf2_tools view_frames

现在，如果你回显主题 turtle3/turtle_point_stamped :

ros2 topic echo /turtle3/turtle_point_stamped

然后会有这样的输出：

header:
  stamp:
    sec: 1720851780
    nanosec: 71402015
  frame_id: world
point:
  x: 3.667041063308716
  y: 2.054239273071289
  z: 0.0
---
header:
  stamp:
    sec: 1720851780
    nanosec: 84285703
  frame_id: world
point:
  x: 3.68221378326416
  y: 2.049161195755005
  z: 0.0
---
header:
  stamp:
    sec: 1720851780
    nanosec: 84484752
  frame_id: world
point:
  x: 3.68221378326416
  y: 2.049161195755005
  z: 0.0
---

当演示运行时，打开另一个终端并运行消息过滤器/监听器节点：

cxy@ubuntu2404-cxy:~/ros2_ws$ . install/setup.bash
cxy@ubuntu2404-cxy:~/ros2_ws$ ros2 run learning_tf2_cpp turtle_tf2_message_filter
[INFO] [1720851894.641150110] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-5.121535 y:0.618463 z:0.000000


[INFO] [1720851894.641507911] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-5.121535 y:0.618463 z:0.000000


[INFO] [1720851894.641594495] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-5.121535 y:0.618463 z:0.000000

 摘要

在本教程中，您学习了如何在 tf2 中使用传感器数据/消息。具体来说，您学习了如何在一个主题上发布 PointStamped 消息，以及如何监听该主题并使用 tf2_ros::MessageFilter 转换 PointStamped 消息的框架。

笔记

详细对比 ROS2 中的 tf2_geometry_msgs 与 geometry_msgs

创建一个 tf2_ros::MessageFilter 对象，用于过滤和处理 PointStamped 类型的消息

tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
      point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
      this->get_node_clock_interface(), buffer_timeout);

tf2_buffer_ 是一个 tf2_ros::Buffer 对象的智能指针

tf2_buffer_ 是一个 tf2_ros::Buffer 对象，用于存储和查询坐标变换信息。它在ROS 2中起到了关键作用，帮助节点在不同坐标系之间进行坐标变换。通过与 tf2_ros::TransformListener 和 tf2_ros::MessageFilter 等组件的配合使用，可以实现复杂的坐标变换和消息过滤功能。