How to subscribe to two image topics in ROS using Python

user_13725889 0
Viewed 826

I am trying to read left and right camera images from a vehicle to do some image processing. I am unsure of how to use the same callback function to process each image. I have seen examples where the data type was different, but not where both are of the same type, such as images. In this example, separate callbacks were used.

Here is my current attempt:

import rospy
import cv2
import os
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import message_filters

def read_cameras():
    imageL = rospy.Subscriber("/camera/left/image_raw", Image, image_callback)
    imageR = rospy.Subscriber("/camera/right/image_raw", Image, image_callback)
    # Synch images here ? 
    ts = message_filters.ApproximateTimeSynchronizer([imageL, imageR], 10)
    rospy.spin()
    
def image_callback(imageL, imageR):
    br = CvBridge()
    rospy.loginfo("receiving frame")
    imageLeft = br.imgmsg_to_cv2(imageL)
    imageRight = br.imgmsg_to_cv2(imageR)
    # Do some fancy computations on each image
    
if __name__ == '__main__':
    try:
        read_cameras()
    except rospy.ROSInterruptException:
        pass

Additionaly, even after reading the documentation on approximate time synchronization, I still don't understand the parameters I need to input. Each camera should be outputting images at 24fps.

ros callback python
1 Answers

Each node needs to initialize itself first before it can interact with the ROS network using rospy.init_node().

Also, you need to provide two message_filter.Subscriber instead of rospy.Subscriber to the message_filter.ApproximateTimeSynchronizer. Finally, instead of setting the callback for each subscriber you should set it once for the ApproximateTimeSynchronizer using registerCallback.

Here is a working example:

#! /usr/bin/env python2

import rospy
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import message_filters

def read_cameras():
    imageL = message_filters.Subscriber("/camera/left/image_raw", Image)
    imageR = message_filters.Subscriber("/camera/right/image_raw", Image)

    # Synchronize images
    ts = message_filters.ApproximateTimeSynchronizer([imageL, imageR], queue_size=10, slop=0.5)
    ts.registerCallback(image_callback)
    rospy.spin()

def image_callback(imageL, imageR):
    br = CvBridge()
    rospy.loginfo("receiving frame")
    imageLeft = br.imgmsg_to_cv2(imageL)
    imageRight = br.imgmsg_to_cv2(imageR)
    # Process images...

if __name__ == '__main__':
    rospy.init_node('my_node')
    try:
        read_cameras()
    except rospy.ROSInterruptException:
        pass

Be sure to check the delay (slop) for your ApproximateTimeSynchronizer and set it according to your data.

user_61056920
Related