ROS2-消息同步订阅

ROS2 消息同步订阅

在机器人系统中，经常需要同时处理来自多个传感器的数据，这些数据需要在时间上同步才能进行有效的融合处理。ROS2 提供了消息同步机制来解决这一问题。

1. 核心函数与类

1.1 message_filters 库导入

使用消息同步功能前需要导入相关模块：

from message_filters import Subscriber, TimeSynchronizer, ApproximateTimeSynchronizer

1.2 Subscriber 函数

用于创建消息订阅者，是消息同步的基础组件。

函数签名：

Subscriber(node, msg_type, topic_name)

参数说明:

• node: ROS2 节点实例
• msg_type: 消息类型（如 sensor_msgs.msg.Image）
• topic_name: 要订阅的话题名称

返回值:

• Subscriber 对象，用于后续同步处理

使用示例:

from sensor_msgs.msg import Image
from message_filters import Subscriber

# 创建图像消息订阅者
image_sub = Subscriber(node, Image, '/camera/image_raw')

1.3 TimeSynchronizer 函数

用于实现精确时间同步，仅当所有订阅者在同一时间戳都有消息时才触发回调。

函数签名：

TimeSynchronizer(subscribers, queue_size)

参数说明:

• subscribers: Subscriber 对象列表
• queue_size: 同步队列大小（建议设置为10-30）

使用示例:

from message_filters import TimeSynchronizer

# 创建时间同步器
sync = TimeSynchronizer([subscriber1, subscriber2], 10)

1.4 ApproximateTimeSynchronizer 函数

用于实现近似时间同步，允许一定时间差内的消息同步。

函数签名：

ApproximateTimeSynchronizer(subscribers, queue_size, slop)

参数说明:

• subscribers: Subscriber 对象列表
• queue_size: 同步队列大小
• slop: 允许的最大时间差（秒），例如0.1表示100毫秒

使用示例:

from message_filters import ApproximateTimeSynchronizer

# 创建近似时间同步器，允许最大时间差0.1秒
approx_sync = ApproximateTimeSynchronizer(
    [subscriber1, subscriber2], 
    queue_size=10, 
    slop=0.1
)

1.5 registerCallback 函数

注册同步回调函数，当所有订阅者的消息同步后调用该函数。

函数签名：

sync.registerCallback(callback_function)

参数说明:

• callback_function: 回调函数，参数数量与订阅者数量一致

使用示例:

# 注册回调函数
sync.registerCallback(self.sync_callback)

# 回调函数定义（两个订阅者的情况）
def sync_callback(self, msg1, msg2):
    # 处理同步消息
    pass

2. 完整示例代码

以下是一个完整的ROS2节点示例，展示如何使用消息同步功能：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from message_filters import Subscriber, TimeSynchronizer
from cv_bridge import CvBridge

class SyncImageSubscriber(Node):
    def __init__(self):
        super().__init__('sync_image_subscriber')
        
        # 创建CvBridge实例
        self.bridge = CvBridge()
        
        # 创建两个订阅者
        self.sub_left = Subscriber(self, Image, '/left/camera/image_raw')
        self.sub_right = Subscriber(self, Image, '/right/camera/image_raw')
        
        # 创建时间同步器
        self.sync = TimeSynchronizer([self.sub_left, self.sub_right], 10)
        
        # 注册回调函数
        self.sync.registerCallback(self.sync_callback)
        
    def sync_callback(self, left_msg, right_msg):
        # 处理同步的图像消息
        try:
            # 将ROS图像消息转换为OpenCV图像
            left_image = self.bridge.imgmsg_to_cv2(left_msg, "bgr8")
            right_image = self.bridge.imgmsg_to_cv2(right_msg, "bgr8")
            
            # 在这里处理图像数据
            # 例如：立体匹配、图像融合等
            
        except Exception as e:
            self.get_logger().error(f'Error processing images: {e}')

def main():
    rclpy.init()
    node = SyncImageSubscriber()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()