- 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;
}
配置与编译
  1. 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)
  2. 编译运行

    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()
配置与运行
  1. 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'
            ]
        }
    )
  2. 编译运行

    bash 复制代码
    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中多话题消息的同步订阅与处理。实际应用中需根据具体场景调整同步策略和参数。

相关推荐
kalvin_y_liu21 小时前
Lumi 具神智能机器人 SDK说明和ACT算法中的学习与推理
人工智能·ai·ros
lihongli0007 天前
ros中的Navigation导航系统
自动驾驶·ros
lihongli0007 天前
ROS与Qt结合开发CAN控制界面(发布自定义的truck_send_can1消息)
开发语言·qt·ros
酌量9 天前
从 ROS 订阅视频话题到本地可视化与 RTMP 推流全流程实战
经验分享·笔记·ffmpeg·音视频·ros
lihongli00011 天前
修改ros工作空间名称方法与步骤
ubuntu·ros
lihongli00012 天前
CAN、ROS数据录制与rqt图形化显示
自动驾驶·ros·激光雷达
Mr.Winter`15 天前
深度强化学习 | 基于SAC算法的动态避障(ROS C++仿真)
人工智能·深度学习·神经网络·机器人·自动驾驶·ros·具身智能
老黄编程18 天前
ros2 中 CMakeLists.txt 的 ament_package 有什么用?有什么使用约束?必须放置尾部吗?
ros·cmake
老黄编程18 天前
ros2 功能包 package.xml 结构详细解释
ros
老黄编程19 天前
ros2 自定义消息、服务、动作接口详细范例
ros