'Synchronization problem with ROS and four subscribers
I'm writing a ROS node in python that implements a specific application that takes as input for processing 4 different images. In my main thread I use OpenCV's cv2.imshow() to show results (I can't put it elsewhere since it doesn't work outside of the main thread). I have these four pictures published by another node not written by me, and they're published with a rate of 15Hz, all four together.
In the main thread, other than the image showing methods, I also do some lengthy image processing. This might take more or less time than the images publication period (approx. 66ms), but it usually takes more time than that. The fact is, for the processing I need the latest available images, and all from the same time window. I tried to implement this with a LIFO queue for each image (or better, data msg) recieved. Here is a (much reduced) example:
import cv2
from cv_bridge import CvBridge
import numpy as np
from queue import LifoQueue
import rospy
from sensor_msgs.msg import Image
class ImageProc():
def __init__(self):
self.cv_bridge = CvBridge()
# Placeholders
self.image_1 = dummyImage()
self.image_2 = dummyImage()
self.image_3 = dummyImage()
self.image_4 = dummyImage()
self.image_1_queue = LifoQueue(QUEUE_SIZE)
self.image_2_queue = LifoQueue(QUEUE_SIZE)
self.image_3_queue = LifoQueue(QUEUE_SIZE)
self.image_4_queue = LifoQueue(QUEUE_SIZE)
rospy.init_node('image_proc', anonymous=True)
# 15 Hz
rospy.Subscriber('/subscribe/image_a',
Image, self.image_1_callback)
# 15 Hz
rospy.Subscriber('/subscribe/image_b',
Image, self.image_2_callback)
# 15 Hz
rospy.Subscriber('/subscribe/image_c',
Image, self.image_3_callback)
# 15 Hz
rospy.Subscriber('/subscribe/image_d',
Image, self.image_4_callback)
def image_1_callback(self, data):
if not self.image_1_queue.full():
self.image_1_queue.put(data)
def image_2_callback(self, data):
if not self.image_2_queue.full():
self.image_2_queue.put(data)
def image_3_callback(self, data):
if not self.image_3_queue.full():
self.image_3_queue.put(data)
def image_4_callback(self, data):
if not self.image_4_queue.full():
self.image_4_queue.put(data)
def main():
instance = ImageProc()
cv2.namedWindow('image_1', cv2.WINDOW_NORMAL)
cv2.namedWindow('image_2', cv2.WINDOW_NORMAL)
cv2.namedWindow('image_3', cv2.WINDOW_NORMAL)
cv2.namedWindow('image_4', cv2.WINDOW_NORMAL)
while True:
image_1 = np.copy(instance.cv_bridge.imgmsg_to_cv2(
instance.image_1_queue.get()))
image_2 = np.copy(instance.cv_bridge.imgmsg_to_cv2(
instance.image_2_queue.get()))
image_3 = np.copy(instance.cv_bridge.imgmsg_to_cv2(
instance.image_3_queue.get()))
image_4 = np.copy(instance.cv_bridge.imgmsg_to_cv2(
instance.image_4_queue.get()))
###
# Lengthy image processing stuff
###
cv2.imshow('image_1', image_1)
cv2.imshow('image_2', image_2)
cv2.imshow('image_3', image_3)
cv2.imshow('image_4', image_4)
k = cv2.waitKey(1) & 0xFF
if k == ord('q'):
cv2.destroyAllWindows()
break
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass
The problems I encounter are: How do I know if two image messages are from the same time stamp? With the information from the header.stamp, and by checking if they're all within a certain time delta? Or maybe the header.seq? How do I synchronize this perfectly such that there is no occasion that (for example) in the middle of the image copying (just after the while True), a new image message arrives and gets written to one of the LIFOs, and I have images from different time periods during processing? Also, I'm using a LIFO because if my processing takes a lot of time, when the cycle starts again I need the latest data from the ROS topics, and not the data stored in a queue. And in the remote occasion that my processing takes less than 66ms, how do I prevent this small niche case?
Any help/insight is much appreciated!
Sources
This article follows the attribution requirements of Stack Overflow and is licensed under CC BY-SA 3.0.
Source: Stack Overflow
| Solution | Source |
|---|
