Multiple Camera Sub
Hi, I am trying to create a code for multiple camera subscriber, I usualy use global variable to subscriber the RGBD topic from camera, but I want to make cleaner code.
Here is my code:
class CameraSub:
def __init__(self, rgb_topic, d_topic):
self.rgb_img = None
self.d_img = None
self.rgb_topic = rgb_topic
self.d_topic = d_topic
self.bridge = cv_bridge.CvBridge()
self.loop_rate = rospy.Rate(10)
self.rgb_sub = rospy.Subscriber(self.rgb_topic,
Image, self.RGBimage_callback)
self.d_sub = rospy.Subscriber(self.d_topic,
Image, self.depthImage_callback)
def RGBimage_callback(self, img):
self.rgb_img = self.bridge.imgmsg_to_cv2(img, desired_encoding='bgr8')
def depthImage_callback(self, img_d):
self.d_img = self.bridge.imgmsg_to_cv2(img_d, )
self.d_img = np.array(self.d_img, dtype=np.float32)
self.d_img = cv2.normalize(self.d_img, self.d_img, 0, 255, cv2.NORM_MINMAX)
def RGBD_output(self):
if self.rgb_img is not None and self.d_img is not None:
return self.rgb_img, self.d_img
else:
return 0,0
rospy.init_node('dualCameraRGBD')
camera_1 = CameraSub('camera1/color/image_raw', 'camera1/depth/image_raw')
rgb_img_c1, d_img_c1 = camera_1.RGBD_output()
cv2.imshow("rgb_img_c1", rgb_img_c1)
cv2.imshow("d_img_c1", d_img_c1)
Thank you for the help !
Asked by irfanrah on 2022-08-03 21:30:52 UTC
Answers
Looking at your code, it seems you want to subscribe to RGB and depth images. Please use the message_filters. A sample code taken from the wiki page is shown below:
import message_filters
from sensor_msgs.msg import Image
def callback(rgb_image, depth_image):
# Solve all of perception here...
image_sub = message_filters.Subscriber('rgb_topic_here', Image)
depth_sub = message_filters.Subscriber('depth_topic_here', Image)
ts = message_filters.TimeSynchronizer([image_sub, depth_sub], 10)
ts.registerCallback(callback)
rospy.spin()
I recommend checking the message_filters to apply additional filters such as Time Synchronizer etc.
Asked by ravijoshi on 2022-08-03 22:15:54 UTC
Comments
Hi, Thanks for your answer and suggestion. But I think I ask how to utilize ROS code with Python OOP Class, so it will make the code more clean.
As you can see in the part
camera_1 = CameraSub('camera1/color/image_raw', 'camera1/depth/image_raw')
rgb_img_c1, d_img_c1 = camera_1.RGBD_output()
I want to make the code work, and we can get the rgb and depth camera with this setting.
So, if we want to sub other camera topic we just need to create some other function like this
camera_10 = CameraSub('camera10/color/image_raw, 'camera10/depth/image_raw')
rgb_img_c10, d_img_c10 = camera_10.RGBD_output()
Asked by irfanrah on 2022-08-04 19:36:20 UTC
That's how it is done. Please see a similar answer here. However, instead of defining two subscribers and corresponding callbacks, you may use message_filters. On a side note, self.loop_rate
is an unused variable.
Asked by ravijoshi on 2022-08-04 21:00:03 UTC
Thank you, Really appreciate your answer !
Asked by irfanrah on 2022-08-08 20:14:26 UTC
You are welcome. It is advised to click on the "accept" button to mark the question as solved.
Asked by ravijoshi on 2022-08-08 21:26:59 UTC
Comments
It is advised to keep your code in your question. That way, a reader doesn't need to look at an external website.
Asked by ravijoshi on 2022-08-03 22:09:25 UTC
Thanks ! I am new in this forum
Asked by irfanrah on 2022-08-04 19:34:17 UTC