I found a method from this discussion How to subscribe to two image topics in ROS using Python and I did more research on that method using the official documentation on how to synchronize multiple topic subscriptions https://wiki.ros.org/message_filters#ApproximateTime_Policy. And I tried to use it when I am developing a GUI to show some details on top of ROS. For that I had to use QThread in pyqt5 and Here's how I did it.
class QNode_upgraded1(QThread):
rosShutdown = pyqtSignal() ## signal the gui for a shutdown
def __init__(self, queue_total):
QThread.__init__(self)
rospy.init_node('listenerim',anonymous=True)
self.queue = queue_total
self.dic = {'img':None, 'msg':None, 'img_count':0, 'msg_count':0}
#self.model = QStringListModel(self)
#self.list = []
#self.model.setStringList(self.list)
#self.text_window.setModel(self.model)
##connect signal
def __del__(self):
if not rospy.is_shutdown():
#rospy.shutdown('shutdown reason')
self.wait()
def callback(self, data_img, data_msg):
br = CvBridge()
#rospy.loginfo('receiving image')
dx = br.imgmsg_to_cv2(data_img)
self.dic["img"] = dx
self.dic['img_count'] += 1
#self.queue.put(frame)
if data_msg.data != 'None':
self.dic['msg'] = 'None'
else:
self.dic['msg'] = data_msg.data
self.dic['msg_count'] += 1
if max(self.dic['img_count'], self.dic['msg_count']) > 1000:
self.dic['img_count'] = 0
self.dic['msg_count'] = 0
self.queue.put(self.dic)
def run(self):
img_=message_filters.Subscriber('/reolink/image_raw', Image) #2hz
msg_=message_filters.Subscriber('/reolink/periodic_msg', String)
#sync topic subscriptions
ts = message_filters.ApproximateTimeSynchronizer([img_,msg_], queue_size=10, slop=0.5, allow_headerless=True)
ts.registerCallback(self.callback)
rospy.spin()
self.rosShutdown.emit() #emit signal for shutdown
But when I did this it did not subscribed to multiple topics. But when I use two callback methods to subscribe two topics seperately, it worked. But the topic subscriptions are not synchronized.
class QNode_upgraded(QThread):
rosShutdown = pyqtSignal() ## signal the gui for a shutdown
def __init__(self, queue_total):
QThread.__init__(self)
rospy.init_node('listenerim',anonymous=True)
self.queue = queue_total
self.dic = {'img':None, 'msg':None, 'img_count':0, 'msg_count':0}
##connect signal
def __del__(self):
if not rospy.is_shutdown():
#rospy.shutdown('shutdown reason')
self.wait()
def callback1(self, data):
br = CvBridge()
#rospy.loginfo('receiving image')
dx = br.imgmsg_to_cv2(data)
self.dic["img"] = dx
self.dic['img_count'] += 1
#self.queue.put(frame)
def callback2(self, msg_data):
if msg_data.data != 'None':
self.dic['msg'] = 'None'
else:
self.dic['msg'] = msg_data.data
self.dic['msg_count'] += 1
if max(self.dic['img_count'], self.dic['msg_count']) > 1000:
self.dic['img_count'] = 0
self.dic['msg_count'] = 0
self.queue.put(self.dic)
def run(self):
rospy.Subscriber('/reolink/image_raw', Image, self.callback1) #2hz
rospy.Subscriber('/reolink/periodic_msg', String, self.callback2)
rospy.spin()
self.rosShutdown.emit() #emit signal for shutdown
For further clarification what I try to do here is subscribing two topics and the subscribed data is put into a dictionary and they are appended to a python queue, so that I can use it in main program. Can someone point out why the first method is not working and what I am doing wrong?
I'll throw it out there, your first code looks pretty close to right, and if your second is receiving data, then it might be in the tuning / frequencies. For example, if they're not close enough in occurrence, then the approximate time won't pick them up. Here's a few ways you can check what your timings are.
ros topic hz: While running your sensor, use this to see what rates your topics are actually coming out at; if they're too far apart, then they'll only alias some of the time.rospy.get_time() can get you the current time, and if you save the times for each callback in a global variable, you can print out the deltas between them everytime one of them gets updated (for precise logging... just not too much).