Add liveliness and lease duration to pub and echo

Closes Issue #614

Signed-off-by: Nour Saeed <[email protected]>

ros2topic/ros2topic/api/__init__.py

@@ -18,6 +18,7 @@
 
 import rclpy
 
+from rclpy.duration import Duration
 from rclpy.expand_topic_name import expand_topic_name
 from rclpy.topic_or_service_is_hidden import topic_or_service_is_hidden
 from rclpy.validate_full_topic_name import validate_full_topic_name
@@ -37,6 +38,20 @@ def unsigned_int(string):
     return value
 
 
+def positive_float(inval):
+    try:
+        value = float(inval)
+    except ValueError:
+        value = -1
+    if value <= 0.0:
+        raise ArgumentTypeError('value must be a non-negative positive float')
+    return value
+
+
+def duration_ns(inval):
+    return Duration(nanoseconds=unsigned_int(inval))
+
+
 def get_topic_names_and_types(*, node, include_hidden_topics=False):
     topic_names_and_types = node.get_topic_names_and_types()
     if not include_hidden_topics:
@@ -160,6 +175,7 @@ def __call__(self, prefix, parsed_args, **kwargs):
 def profile_configure_short_keys(
     profile: rclpy.qos.QoSProfile = None, reliability: str = None,
     durability: str = None, depth: Optional[int] = None, history: str = None,
+    liveliness: str = None, liveliness_lease_duration: Duration = None
 ) -> rclpy.qos.QoSProfile:
     """Configure a QoSProfile given a profile, and optional overrides."""
     if history:
@@ -168,6 +184,10 @@ def profile_configure_short_keys(
         profile.durability = rclpy.qos.QoSDurabilityPolicy.get_from_short_key(durability)
     if reliability:
         profile.reliability = rclpy.qos.QoSReliabilityPolicy.get_from_short_key(reliability)
+    if liveliness:
+        profile.liveliness = rclpy.qos.LivelinessPolicy.get_from_short_key(liveliness)
+    if liveliness_lease_duration:
+        profile.liveliness_lease_duration = liveliness_lease_duration
     if depth and depth >= 0:
         profile.depth = depth
     else:
@@ -178,10 +198,12 @@ def profile_configure_short_keys(
 
 def qos_profile_from_short_keys(
     preset_profile: str, reliability: str = None, durability: str = None,
-    depth: Optional[int] = None, history: str = None,
+    depth: Optional[int] = None, history: str = None, liveliness: str = None,
+    liveliness_lease_duration: Duration = None
 ) -> rclpy.qos.QoSProfile:
     """Construct a QoSProfile given the name of a preset, and optional overrides."""
     # Build a QoS profile based on user-supplied arguments
     profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(preset_profile)
-    profile_configure_short_keys(profile, reliability, durability, depth, history)
+    profile_configure_short_keys(
+        profile, reliability, durability, depth, history, liveliness, liveliness_lease_duration)
     return profile

ros2topic/ros2topic/verb/echo.py

@@ -27,6 +27,7 @@
 from rclpy.task import Future
 from ros2cli.node.strategy import add_arguments as add_strategy_node_arguments
 from ros2cli.node.strategy import NodeStrategy
+from ros2topic.api import duration_ns
 from ros2topic.api import get_msg_class
 from ros2topic.api import qos_profile_from_short_keys
 from ros2topic.api import TopicNameCompleter
@@ -83,6 +84,15 @@ def add_arguments(self, parser, cli_name):
             help='Quality of service durability setting to subscribe with '
                  '(overrides durability value of --qos-profile option, default: '
                  'Automatically match existing publishers )')
+        parser.add_argument(
+            '--qos-liveliness',
+            choices=rclpy.qos.LivelinessPolicy.short_keys(),
+            help='Quality of service liveliness setting to subscribe with '
+                 '(overrides liveliness value of --qos-profile option, default: {})'
+                 .format(default_profile.liveliness.short_key))
+        parser.add_argument(
+            '--qos-liveliness-lease-duration', metavar='N', type=duration_ns,
+            help='Quality of service liveliness lease duration in nanoseconds')
         parser.add_argument(
             '--csv', action='store_true',
             help='Output all recursive fields separated by commas (e.g. for '
@@ -129,15 +139,20 @@ def choose_qos(self, node, args):
                 args.qos_reliability is not None or
                 args.qos_durability is not None or
                 args.qos_depth is not None or
-                args.qos_history is not None):
+                args.qos_history is not None or
+                args.qos_liveliness is not None or
+                args.qos_liveliness_lease_duration is not None):
 
             if args.qos_profile is None:
                 args.qos_profile = default_profile_str
-            return qos_profile_from_short_keys(args.qos_profile,
-                                               reliability=args.qos_reliability,
-                                               durability=args.qos_durability,
-                                               depth=args.qos_depth,
-                                               history=args.qos_history)
+            return qos_profile_from_short_keys(
+                args.qos_profile,
+                reliability=args.qos_reliability,
+                durability=args.qos_durability,
+                depth=args.qos_depth,
+                history=args.qos_history,
+                liveliness=args.qos_liveliness,
+                liveliness_lease_duration=args.qos_liveliness_lease_duration)
 
         qos_profile = QoSPresetProfiles.get_from_short_key(default_profile_str)
         reliability_reliable_endpoints_count = 0

ros2topic/ros2topic/verb/pub.py

@@ -22,6 +22,9 @@
 from rclpy.qos import QoSProfile
 from rclpy.qos import QoSReliabilityPolicy
 from ros2cli.node.direct import DirectNode
+from ros2topic.api import duration_ns
+from ros2topic.api import nonnegative_int
+from ros2topic.api import positive_float
 from ros2topic.api import profile_configure_short_keys
 from ros2topic.api import TopicMessagePrototypeCompleter
 from ros2topic.api import TopicNameCompleter
@@ -34,22 +37,6 @@
 MsgType = TypeVar('MsgType')
 
 
-def nonnegative_int(inval):
-    ret = int(inval)
-    if ret < 0:
-        # The error message here gets completely swallowed by argparse
-        raise ValueError('Value must be positive or zero')
-    return ret
-
-
-def positive_float(inval):
-    ret = float(inval)
-    if ret <= 0.0:
-        # The error message here gets completely swallowed by argparse
-        raise ValueError('Value must be positive')
-    return ret
-
-
 def get_pub_qos_profile():
     return QoSProfile(
         reliability=QoSReliabilityPolicy.RELIABLE,
@@ -130,6 +117,15 @@ def add_arguments(self, parser, cli_name):
             help='Quality of service durability setting to publish with '
                  '(overrides durability value of --qos-profile option, default: {})'
                  .format(default_profile.durability.short_key))
+        parser.add_argument(
+            '--qos-liveliness',
+            choices=rclpy.qos.LivelinessPolicy.short_keys(),
+            help='Quality of service liveliness setting to publish with '
+                 '(overrides liveliness value of --qos-profile option, default: {})'
+                 .format(default_profile.liveliness.short_key))
+        parser.add_argument(
+            '--qos-liveliness-lease-duration', metavar='N', type=duration_ns,
+            help='Quality of service liveliness lease duration in nanoseconds')
 
     def main(self, *, args):
         return main(args)
@@ -140,10 +136,12 @@ def main(args):
 
     qos_profile_name = args.qos_profile
     if qos_profile_name:
-        qos_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(qos_profile_name)
+        qos_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(
+            qos_profile_name)
     profile_configure_short_keys(
         qos_profile, args.qos_reliability, args.qos_durability,
-        args.qos_depth, args.qos_history)
+        args.qos_depth, args.qos_history, args.qos_liveliness,
+        args.qos_liveliness_lease_duration)
 
     times = args.times
     if args.once:

ros2topic/test/test_qos_conversions.py

@@ -14,14 +14,18 @@
 
 import rclpy
 
+from ros2topic.api import duration_ns
 from ros2topic.api import qos_profile_from_short_keys
 
 
 def test_profile_conversion():
     profile = qos_profile_from_short_keys(
         'sensor_data', reliability='reliable', durability='transient_local',
-        depth=10, history='keep_last')
+        depth=10, history='keep_last', liveliness='automatic',
+        liveliness_lease_duration=duration_ns(1e9))
     assert profile.durability == rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL
     assert profile.reliability == rclpy.qos.QoSReliabilityPolicy.RELIABLE
     assert profile.depth == 10
     assert profile.history == rclpy.qos.QoSHistoryPolicy.KEEP_LAST
+    assert profile.liveliness == rclpy.qos.QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
+    assert profile.liveliness_lease_duration == rclpy.duration.Duration(nanoseconds=1e9)