Fix QoS incompatibility camera_calibration ROS2

Currently if QoS of camera topic doesn't match, the calibration won't work. For that request the QoS of the topics at runtime to resolve that.

camera_calibration/src/camera_calibration/camera_calibrator.py

@@ -48,6 +48,9 @@
 from std_msgs.msg import String
 from std_srvs.srv import Empty
 
+from rclpy.qos import qos_profile_system_default
+from rclpy.qos import QoSProfile
+
 class SpinThread(threading.Thread):
     """
     Thread that spins the ros node, while imshow runs in the main thread
@@ -111,12 +114,12 @@ def __init__(self, name, boards, service_check = True, synchronizer = message_fi
         self._pattern = pattern
         self._camera_name = camera_name
         self._max_chessboard_speed = max_chessboard_speed
-        lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'left')
-        rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'right')
+        lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'left', qos_profile=self.get_topic_qos("left"))
+        rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'right', qos_profile=self.get_topic_qos("left"))
         ts = synchronizer([lsub, rsub], 4)
         ts.registerCallback(self.queue_stereo)
 
-        msub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'image')
+        msub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'image', qos_profile=self.get_topic_qos("image"))
         msub.registerCallback(self.queue_monocular)
 
         self.q_mono = deque([], 1)
@@ -211,6 +214,21 @@ def do_upload(self):
             response = self.set_right_camera_info_service.call_async(req)
             rv = rv and self.check_set_camera_info(response)
         return rv
+
+    def get_topic_qos(self, topic_name: str) -> QoSProfile:
+        """! 
+        Given a topic name, get the QoS profile with which it is being published
+        @param topic_name (str) the topic name
+        @return QosProfile the qos profile with which the topic is published. If no publishers exist 
+        for the given topic, it returns the sensor data QoS. returns None in case ROS1 is being used
+        """
+        topic_name = self.resolve_topic_name(topic_name)
+        topic_info = self.get_publishers_info_by_topic(topic_name=topic_name)
+        if len(topic_info):
+            return topic_info[0].qos_profile
+        else:
+            self.get_logger().warn(f"No publishers available for topic {topic_name}. Using system default QoS for subscriber.")
+            return qos_profile_system_default
 
 
 class OpenCVCalibrationNode(CalibrationNode):