ros订阅多个topic并同步时间戳 |
您所在的位置：网站首页 › rosbag时间戳 › ros订阅多个topic并同步时间戳 |
|
参考资料： csdn博客：ROS节点同时订阅多个话题并进行消息时间同步（非常值得参考） ros中同时订阅两个topic（2张图像）合并成一个topic（1张图像） 官方文档 官方函数说明 问题背景 ros在订阅传感器topic时，往往需要同时订阅多个传感器，并且在数据处理之间会存在相互调用结果的情况。因此在编写程序时，最好能够同时处理两个传感器的数据。在采集机器人传感器数据时，往往涉及到多种传感器进行环境感知和信息采集，因此不仅需要一个节点同时订阅多个topic，而且需要将传感器时间戳保持一致。问题解决方案 从上述问题出发，查找了相关的资料后，看了官方的参考资料；ros提供了一个函数message_filters，可以解决上述问题。并且这个方式可以避免写过多的callback函数，如下提供一个小测试程序。该程序可以在turtlebot仿真中验证，程序订阅了turtlebot中的rgb图像以及depth图像，同时只使用一个反馈函数进行数据处理（将图像分别同时显示在窗口）。 大致思路 初始化节点名称，使用message_filters.Subscriber分别订阅rgb和depth节点，message_filters.Subscriber参数说明在官方文档中，不需要直接写反馈函数。同步两节点时间戳，message_filters.ApproximateTimeSynchronizer是官方提供的函数以对齐多种传感器时间戳。参数参考函数说明。执行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()遗留问题 上述代码测试成功应该没问题，但是在gazebo中仿真中出现一个小问题，即窗口显示视频总是出现一段时间后卡死黑框，类似于程序突然卡死的情况。查了一些说是电脑处理速度的问题，把waitKey时间加长即可，但不符合我的原意，故舍弃。神奇的是，我在实际硬件中实验的时候并未出现卡死现象（求解）。 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |