Merge pull request #104 from kiwicampus/auto-qos

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