From df1c3f4454daddb1a630320041a6ee83b4a83a80 Mon Sep 17 00:00:00 2001 From: charlielito Date: Thu, 9 Dec 2021 18:09:20 -0500 Subject: [PATCH] Add ros2 QoS BestEffort and Volatile by default --- rosboard/rosboard.py | 13 +++++++++++++ rosboard/rospy2/__init__.py | 4 ++-- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/rosboard/rosboard.py b/rosboard/rosboard.py index 34dc3b94..61ca2307 100755 --- a/rosboard/rosboard.py +++ b/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) @@ -219,11 +220,23 @@ 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. + # Hardcoding for now to BestEffort and Volatile. + kwargs = {"qos": QoSProfile( + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + # reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + # durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + )} 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 diff --git a/rosboard/rospy2/__init__.py b/rosboard/rospy2/__init__.py index 28dbfe49..c0ebc2f8 100644 --- a/rosboard/rospy2/__init__.py +++ b/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