- custom_action_cpp: 自定义动作创建与调用示例

以下为ROS2中同步订阅消息的C++与Python详细范例，包含核心机制、代码实现及运行步骤：

C++同步订阅示例（基于message_filters）

核心机制：使用message_filters库实现时间戳同步，支持ExactTime（严格匹配）和ApproximateTime（允许时间差）策略。

代码实现（C++）

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/imu.hpp>using namespace std::chrono_literals;
using MyPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Imu>;class SyncNode : public rclcpp::Node {
public:SyncNode() : Node("sync_node") {// 创建订阅器image_sub_.subscribe(this, "/camera/image");imu_sub_.subscribe(this, "/imu/data");// 初始化同步器（队列长度10，允许时间差0.1秒）sync_ = std::make_shared<message_filters::Synchronizer<MyPolicy>>(MyPolicy(10), image_sub_, imu_sub_);// 注册回调函数sync_->registerCallback(std::bind(&SyncNode::sync_callback, this, std::placeholders::_1, std::placeholders::_2));}private:void sync_callback(const sensor_msgs::msg::Image::ConstSharedPtr& image,const sensor_msgs::msg::Imu::ConstSharedPtr& imu) {// 计算时间差（单位：秒）double time_diff = (rclcpp::Time(image->header.stamp) - rclcpp::Time(imu->header.stamp)).seconds();RCLCPP_INFO(this->get_logger(), "同步消息时间差: %.6f秒", time_diff);}message_filters::Subscriber<sensor_msgs::msg::Image> image_sub_;message_filters::Subscriber<sensor_msgs::msg::Imu> imu_sub_;std::shared_ptr<message_filters::Synchronizer<MyPolicy>> sync_;
};int main(int argc, char **argv) {rclcpp::init(argc, argv);auto node = std::make_shared<SyncNode>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

配置与编译

1. CMakeLists.txt：

   find_package(ament_cmake REQUIRED)
   find_package(rclcpp REQUIRED)
   find_package(message_filters REQUIRED)
   find_package(sensor_msgs REQUIRED)add_executable(sync_node src/sync_node.cpp)
   ament_target_dependencies(sync_node rclcpp message_filters sensor_msgs)
   

2. 编译运行：

   colcon build --packages-select cpp_sync
   source install/setup.bash
   ros2 run cpp_sync sync_node
   

Python同步订阅示例（基于rclpy）

核心机制：使用rclpy创建订阅者，通过队列和线程实现消息同步，或利用message_filters的Python绑定（需安装ros2-message-filters）。

代码实现（Python）

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, Imu
from queue import Queue
import threadingclass SyncSubscriber(Node):def __init__(self):super().__init__('sync_subscriber')# 创建队列和线程锁self.image_queue = Queue()self.imu_queue = Queue()self.lock = threading.Lock()# 创建订阅者self.image_sub = self.create_subscription(Image, '/camera/image', self.image_callback, 10)self.imu_sub = self.create_subscription(Imu, '/imu/data', self.imu_callback, 10)# 启动同步线程threading.Thread(target=self.sync_thread).start()def image_callback(self, msg):self.image_queue.put(msg)def imu_callback(self, msg):self.imu_queue.put(msg)def sync_thread(self):while rclpy.ok():with self.lock:if not self.image_queue.empty() and not self.imu_queue.empty():image = self.image_queue.get()imu = self.imu_queue.get()self.process_sync_data(image, imu)def process_sync_data(self, image, imu):time_diff = abs(image.header.stamp.sec + image.header.stamp.nanosec*1e-9 - imu.header.stamp.sec - imu.header.stamp.nanosec*1e-9)self.get_logger().info(f'同步消息时间差: {time_diff:.6f}秒')def main():rclpy.init()node = SyncSubscriber()rclpy.spin(node)rclpy.shutdown()if __name__ == '__main__':main()

配置与运行

1. setup.py（Python包配置）：

   setup(name='py_sync',version='0.1.0',packages=['py_sync'],install_requires=['rclpy', 'sensor_msgs'],entry_points={'console_scripts': ['sync_node = py_sync.sync_subscriber:main']}
   )
   

2. 编译运行：

   colcon build --packages-select py_sync
   source install/setup.bash
   ros2 run py_sync sync_node
   

关键注意事项

1. 时间戳一致性：确保消息头包含有效时间戳（header.stamp），否则同步失效。
2. 队列大小：队列长度需根据消息频率和延迟调整，避免消息丢失。
3. 线程安全：Python中使用线程时需加锁，避免资源竞争。
4. 依赖安装：C++需安装libmessage-filters-dev，Python需安装ros2-message-filters（如使用其绑定）。

通过以上范例，可实现ROS2中多话题消息的同步订阅与处理。实际应用中需根据具体场景调整同步策略和参数。