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

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

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

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

代码实现（C++）

cpp 复制代码

#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;
}

配置与编译

CMakeLists.txt ：

cmake 复制代码

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)

编译运行 ：

bash 复制代码

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）

python 复制代码

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, Imu
from queue import Queue
import threading

class 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()

配置与运行

setup.py （Python包配置）：

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'
        ]
    }
)

编译运行 ：

bash 复制代码

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

关键注意事项

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

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