From 96db1e4fec1c42305963aba4ad44bc1042bac38e Mon Sep 17 00:00:00 2001 From: Nour Saeed Date: Wed, 1 Sep 2021 21:58:05 +0200 Subject: [PATCH] Add liveliness and lease duration to pub and echo Closes Issue #614 Signed-off-by: Nour Saeed --- ros2topic/ros2topic/api/__init__.py | 26 ++++++++++++++++++-- ros2topic/ros2topic/verb/echo.py | 27 +++++++++++++++----- ros2topic/ros2topic/verb/pub.py | 34 ++++++++++++-------------- ros2topic/test/test_qos_conversions.py | 6 ++++- 4 files changed, 66 insertions(+), 27 deletions(-) diff --git a/ros2topic/ros2topic/api/__init__.py b/ros2topic/ros2topic/api/__init__.py index c9688071c..f79b2510e 100644 --- a/ros2topic/ros2topic/api/__init__.py +++ b/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 diff --git a/ros2topic/ros2topic/verb/echo.py b/ros2topic/ros2topic/verb/echo.py index bfae04abf..ff1fd1b16 100644 --- a/ros2topic/ros2topic/verb/echo.py +++ b/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 diff --git a/ros2topic/ros2topic/verb/pub.py b/ros2topic/ros2topic/verb/pub.py index 7a05f030a..1f5fba339 100644 --- a/ros2topic/ros2topic/verb/pub.py +++ b/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: diff --git a/ros2topic/test/test_qos_conversions.py b/ros2topic/test/test_qos_conversions.py index 80ab3f2e6..d3a5ebb68 100644 --- a/ros2topic/test/test_qos_conversions.py +++ b/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)