ros订阅多个topic并同步时间戳_ros中topic同步

参考资料：
csdn博客：ROS节点同时订阅多个话题并进行消息时间同步（非常值得参考）
ros中同时订阅两个topic（2张图像）合并成一个topic（1张图像）
官方文档
官方函数说明

问题背景

1. ros在订阅传感器topic时，往往需要同时订阅多个传感器，并且在数据处理之间会存在相互调用结果的情况。因此在编写程序时，最好能够同时处理两个传感器的数据。
2. 在采集机器人传感器数据时，往往涉及到多种传感器进行环境感知和信息采集，因此不仅需要一个节点同时订阅多个topic，而且需要将传感器时间戳保持一致。

问题解决方案
从上述问题出发，查找了相关的资料后，看了官方的参考资料；ros提供了一个函数message_filters，可以解决上述问题。并且这个方式可以避免写过多的callback函数，如下提供一个小测试程序。该程序可以在turtlebot仿真中验证，程序订阅了turtlebot中的rgb图像以及depth图像，同时只使用一个反馈函数进行数据处理（将图像分别同时显示在窗口）。

大致思路

1. 初始化节点名称，使用message_filters.Subscriber分别订阅rgb和depth节点，message_filters.Subscriber参数说明在官方文档中，不需要直接写反馈函数。
2. 同步两节点时间戳，message_filters.ApproximateTimeSynchronizer是官方提供的函数以对齐多种传感器时间戳。参数参考函数说明。
3. 执行callback函数，registerCallback是固定的调用反馈函数的用法。在其中主要把图像在两个窗口中显示出来，不需要过多解释。

测试代码

#!/usr/bin/env python
# coding=UTF-8
'''
此程序为测试测试程序， 用以测试message_filters同时
订阅两个topic，可以同时进行数据处理。
'''
import rospy, math, random, cv_bridge, cv2
import message_filters
from cv_bridge import CvBridge,CvBridgeError
from sensor_msgs.msg import Image, CameraInfo

def multi_callback(Subcriber_laser, Subcriber_pose):
    bridge1 = cv_bridge.CvBridge()
    color = bridge1.imgmsg_to_cv2(Subcriber_laser, 'bgr8')#常规操作
    depth = bridge1.imgmsg_to_cv2(Subcriber_pose,desired_encoding='8UC1')
    print "同步完成！"
    cv2.imshow("window", color)
    cv2.imshow("window2", depth)
    cv2.waitKey(1)


if __name__ == '__main__':
    rospy.init_node('two_TOPIC', anonymous=True)#初始化节点

    subcriber_laser = message_filters.Subscriber('/camera/rgb/image_raw', Image)#订阅第一个话题，rgb图像
    subcriber_pose = message_filters.Subscriber('/camera/depth/image_raw', Image)#订阅第二个话题，depth图像
 
    sync = message_filters.ApproximateTimeSynchronizer([subcriber_laser, subcriber_pose], 10,1)#同步时间戳，具体参数含义需要查看官方文档。
    sync.registerCallback(multi_callback)#执行反馈函数
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("over!")
        cv2.destroyAllWindows()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

遗留问题
上述代码测试成功应该没问题，但是在gazebo中仿真中出现一个小问题，即窗口显示视频总是出现一段时间后卡死黑框，类似于程序突然卡死的情况。查了一些说是电脑处理速度的问题，把waitKey时间加长即可，但不符合我的原意，故舍弃。神奇的是，我在实际硬件中实验的时候并未出现卡死现象（求解）。