Ensure compatible QoS in ros2 subscriptions

rosboard/rosboard.py

@@ -12,6 +12,7 @@
     import rospy # ROS1
 elif os.environ.get("ROS_VERSION") == "2":
     import rosboard.rospy2 as rospy # ROS2
+    from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy
 else:
     print("ROS not detected. Please source your ROS environment\n(e.g. 'source /opt/ros/DISTRO/setup.bash')")
     exit(1)
@@ -122,6 +123,30 @@ def get_msg_class(self, msg_type):
             rospy.logerr(str(e))
             return None
 
+    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
+        """
+        if rospy.__name__ == "rospy2":
+            topic_info = rospy._node.get_publishers_info_by_topic(topic_name=topic_name)
+            if len(topic_info):
+                return topic_info[0].qos_profile
+            else:
+                rospy.logwarn(f"No publishers available for topic {topic_name}. Returning sensor data QoS")
+                return QoSProfile(
+                        depth=10,
+                        reliability=QoSReliabilityPolicy.BEST_EFFORT,
+                        # reliability=QoSReliabilityPolicy.RELIABLE,
+                        durability=QoSDurabilityPolicy.VOLATILE,
+                        # durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+                    )
+        else:
+            rospy.logwarn("QoS profiles are only used in ROS2")
+            return None
+
     def pingpong_loop(self):
         """
         Loop to send pings to all active sockets every 5 seconds.
@@ -226,11 +251,18 @@ def sync_subs(self):
 
                     rospy.loginfo("Subscribing to %s" % topic_name)
 
+                    kwargs = {}
+                    if rospy.__name__ == "rospy2":
+                        # In ros2 we also can pass QoS parameters to the subscriber.
+                        # To avoid incompatibilities we subscribe using the same Qos
+                        # of the topic's publishers
+                        kwargs = {"qos": self.get_topic_qos(topic_name)}
                     self.local_subs[topic_name] = rospy.Subscriber(
                         topic_name,
                         self.get_msg_class(topic_type),
                         self.on_ros_msg,
                         callback_args = (topic_name, topic_type),
+                        **kwargs
                     )
 
             # clean up local subscribers for which remote clients have lost interest

rosboard/rospy2/__init__.py

@@ -213,7 +213,7 @@ def unregister(self):
         _node.destroy_publisher(self._pub)
 
 class Subscriber(object):
-    def __init__(self, topic_name, topic_type, callback, callback_args = None):
+    def __init__(self, topic_name, topic_type, callback, callback_args = None, qos=10):
         global _node
         self.reg_type = "sub"
         self.data_class = topic_type
@@ -222,7 +222,7 @@ def __init__(self, topic_name, topic_type, callback, callback_args = None):
         self.type = _ros2_type_to_type_name(topic_type)
         self.callback = callback
         self.callback_args = callback_args
-        self._sub = _node.create_subscription(topic_type, topic_name, self._ros2_callback, 10, event_callbacks = rclpy.qos_event.SubscriptionEventCallbacks())
+        self._sub = _node.create_subscription(topic_type, topic_name, self._ros2_callback, qos, event_callbacks = rclpy.qos_event.SubscriptionEventCallbacks())
         _node.guards
         self.get_num_connections = lambda: 1 # No good ROS2 equivalent