Subscription to multiple ROS topics fixed

Author: ezemskov

CollisionAvoidanceManager.py

@@ -47,9 +47,9 @@ def __init__(self):
 
         Cfg.RTODCfg["callback"] = self.updateRTDetection    #NB modifying a global/static dictionary, but who cares
         self._rtod = RTOD(Cfg.RTODCfg)
-        self._rosSubColor = ImageSubscriberWrapper('/color/image_raw', self._rtod.ProcessNumpyImage)
-
-        self._rosSubDepth = ImageSubscriberWrapper('/depth/image_rect_raw', self.processDepthImage)
+        self._rosSub = ImageSubscriberWrapper()
+        self._rosSub.subscribe('/color/image_raw', self._rtod.ProcessNumpyImage)
+        self._rosSub.subscribe('/depth/image_rect_raw', self.processDepthImage)
 
         self._steeringCmdRel = None
         self._speedCmdRel = None

ROS_ImageSubscriber.py

@@ -5,46 +5,54 @@
 from sensor_msgs.msg import Image # Image is the message type
 from cv_bridge import CvBridge, CvBridgeError # Package to convert between ROS and OpenCV Images
 
+from functools import partial #std::bind for python
+
 from threading import Thread
 
 class ImageSubscriber(Node):
-  def __init__(self, ros_topic_name, callback_):
-    """
-    Class constructor to set up the node
-    """
+  def __init__(self):
     # Initiate the Node class's constructor and give it a name
-    super().__init__('image_subscriber')
+    super().__init__('ImageSubscriber')
+
+    self._callbacks = {}
+
+  def subscribe(self, ros_topic_name, callback):
+      self._callbacks[ros_topic_name] = callback
+      subscriber_callback_bound = partial(self.subscriber_callback, ros_topic_name_=ros_topic_name)
 
-    self.callback = callback_
-    # Create the subscriber. This subscriber will receive an Image
-    # from the video_frames topic. The queue size is 10 messages.
-    self.sub = self.create_subscription(
-      Image, 
-      ros_topic_name, 
-      self.subscriber_callback, 
-      qos_profile_sensor_data)
-    self.get_logger().info('Subscribed on ' + ros_topic_name)
-
-    # Used to convert between ROS and OpenCV images
-    self.br = CvBridge()
-   
-  def subscriber_callback(self, data): 
+      # Create the subscriber. This subscriber will receive an Image from the topic. The queue size is 10 messages.
+      self.create_subscription(
+            Image, 
+            ros_topic_name, 
+            subscriber_callback_bound, 
+            qos_profile_sensor_data)
+
+      self.get_logger().info('Subscribed on ' + ros_topic_name)
+
+
+  def subscriber_callback(self, data, ros_topic_name_): 
     try:
       # Convert ROS Image message to OpenCV/numpy image
       imageNumpy = CvBridge().imgmsg_to_cv2(data)
       self.get_logger().info('Received video frame dimensions {0}x{1}'.format(imageNumpy.shape[1], imageNumpy.shape[0]))
-      if (self.callback is not None):
-        self.callback(imageNumpy)
+      
+      callback = self._callbacks.get(ros_topic_name_)
+      if (callback is not None):
+        callback(imageNumpy)
     except CvBridgeError as exc:
           self.get_logger().error(exc)
 
 class ImageSubscriberWrapper:
-    def __init__(self, ros_topic_name, callback = None):
+
+    def __init__(self):
         rclpy.init()
-        self.node = ImageSubscriber(ros_topic_name, callback)
+        self.node = ImageSubscriber()
         self.thread = Thread(target=self._threadFunc)
         self.thread.start()
 
+    def subscribe(self, ros_topic_name, callback = None):
+        self.node.subscribe(ros_topic_name, callback)
+
     def _threadFunc(self):
         rclpy.spin(self.node)
         rclpy.shutdown()