From b52abaf3b2d5072ece4651d313d48c597b47e242 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 31 Jan 2025 16:56:34 -0800 Subject: [PATCH 1/2] add QoS option for ros2 service call. Signed-off-by: Tomoya Fujita --- ros2service/package.xml | 1 + ros2service/ros2service/verb/call.py | 17 +++++++++++--- ros2topic/ros2topic/api/__init__.py | 34 ++++++++++++++++------------ ros2topic/ros2topic/verb/echo.py | 4 +++- 4 files changed, 38 insertions(+), 18 deletions(-) diff --git a/ros2service/package.xml b/ros2service/package.xml index ada9cfa0d..26262ded8 100644 --- a/ros2service/package.xml +++ b/ros2service/package.xml @@ -20,6 +20,7 @@ python3-yaml rclpy ros2cli + ros2topic rosidl_runtime_py ament_copyright diff --git a/ros2service/ros2service/verb/call.py b/ros2service/ros2service/verb/call.py index 7ee5dd850..a3cbf9dfc 100644 --- a/ros2service/ros2service/verb/call.py +++ b/ros2service/ros2service/verb/call.py @@ -16,12 +16,14 @@ import time import rclpy +from rclpy.qos import QoSPresetProfiles from ros2cli.helpers import collect_stdin from ros2cli.node import NODE_NAME_PREFIX from ros2service.api import ServiceNameCompleter from ros2service.api import ServicePrototypeCompleter from ros2service.api import ServiceTypeCompleter from ros2service.verb import VerbExtension +from ros2topic.api import add_qos_arguments, profile_configure_short_keys from rosidl_runtime_py import set_message_fields import yaml @@ -54,22 +56,28 @@ def add_arguments(self, parser, cli_name): parser.add_argument( '-r', '--rate', metavar='N', type=float, help='Repeat the call at a specific rate in Hz') + add_qos_arguments(parser, 'service client', 'services_default') def main(self, *, args): if args.rate is not None and args.rate <= 0: raise RuntimeError('rate must be greater than zero') period = 1. / args.rate if args.rate else None + default_profile = QoSPresetProfiles.get_from_short_key(args.qos_profile) + profile_configure_short_keys( + default_profile, args.qos_reliability, args.qos_durability, + args.qos_depth, args.qos_history) + if args.stdin: values = collect_stdin() else: values = args.values return requester( - args.service_type, args.service_name, values, period) + args.service_type, args.service_name, values, period, default_profile) -def requester(service_type, service_name, values, period): +def requester(service_type, service_name, values, period, qos_profile): # TODO(wjwwood) this logic should come from a rosidl related package try: parts = service_type.split('/') @@ -92,7 +100,10 @@ def requester(service_type, service_name, values, period): with rclpy.init(): node = rclpy.create_node(NODE_NAME_PREFIX + '_requester_%s_%s' % (package_name, srv_name)) - cli = node.create_client(srv_module, service_name) + cli = node.create_client( + srv_module, + service_name, + qos_profile=qos_profile) request = srv_module.Request() diff --git a/ros2topic/ros2topic/api/__init__.py b/ros2topic/ros2topic/api/__init__.py index 7b5b4f94a..62ebd92a4 100644 --- a/ros2topic/ros2topic/api/__init__.py +++ b/ros2topic/ros2topic/api/__init__.py @@ -173,7 +173,7 @@ def profile_configure_short_keys( profile: rclpy.qos.QoSProfile = None, reliability: Optional[str] = None, durability: Optional[str] = None, depth: Optional[int] = None, history: Optional[str] = None, liveliness: Optional[str] = None, liveliness_lease_duration_s: Optional[int] = None, -) -> rclpy.qos.QoSProfile: +) -> None: """Configure a QoSProfile given a profile, and optional overrides.""" if history: profile.history = rclpy.qos.QoSHistoryPolicy.get_from_short_key(history) @@ -206,50 +206,56 @@ def qos_profile_from_short_keys( return profile -def add_qos_arguments(parser: ArgumentParser, subscribe_or_publish: str, default_profile_str): +def add_qos_arguments( + parser: ArgumentParser, entity_type: str, + default_profile_str: str = 'default', extra_message: str = '' +) -> None: parser.add_argument( '--qos-profile', choices=rclpy.qos.QoSPresetProfiles.short_keys(), help=( - f'Quality of service preset profile to {subscribe_or_publish} with' + f'Quality of service preset profile to {entity_type} with' f' (default: {default_profile_str})'), default=default_profile_str) default_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(default_profile_str) parser.add_argument( '--qos-depth', metavar='N', type=int, help=( - f'Queue size setting to {subscribe_or_publish} with ' - '(overrides depth value of --qos-profile option)')) + f'Queue size setting to {entity_type} with ' + '(overrides depth value of --qos-profile option, default: ' + f'{default_profile.depth})')) parser.add_argument( '--qos-history', choices=rclpy.qos.QoSHistoryPolicy.short_keys(), help=( - f'History of samples setting to {subscribe_or_publish} with ' + f'History of samples setting to {entity_type} with ' '(overrides history value of --qos-profile option, default: ' f'{default_profile.history.short_key})')) parser.add_argument( '--qos-reliability', choices=rclpy.qos.QoSReliabilityPolicy.short_keys(), help=( - f'Quality of service reliability setting to {subscribe_or_publish} with ' + f'Quality of service reliability setting to {entity_type} with ' '(overrides reliability value of --qos-profile option, default: ' - 'Compatible profile with running endpoints )')) + f'{default_profile.reliability.short_key} {extra_message})')) parser.add_argument( '--qos-durability', choices=rclpy.qos.QoSDurabilityPolicy.short_keys(), help=( - f'Quality of service durability setting to {subscribe_or_publish} with ' + f'Quality of service durability setting to {entity_type} with ' '(overrides durability value of --qos-profile option, default: ' - 'Compatible profile with running endpoints )')) + f'{default_profile.durability.short_key} {extra_message})')) parser.add_argument( '--qos-liveliness', choices=rclpy.qos.QoSLivelinessPolicy.short_keys(), help=( - f'Quality of service liveliness setting to {subscribe_or_publish} with ' - '(overrides liveliness value of --qos-profile option')) + f'Quality of service liveliness setting to {entity_type} with ' + '(overrides liveliness value of --qos-profile option, default ' + f'{default_profile.liveliness.short_key})')) parser.add_argument( '--qos-liveliness-lease-duration-seconds', type=float, help=( - f'Quality of service liveliness lease duration setting to {subscribe_or_publish} ' - 'with (overrides liveliness lease duration value of --qos-profile option')) + f'Quality of service liveliness lease duration setting to {entity_type} ' + 'with (overrides liveliness lease duration value of --qos-profile option, default: ' + f'{default_profile.liveliness_lease_duration})')) diff --git a/ros2topic/ros2topic/verb/echo.py b/ros2topic/ros2topic/verb/echo.py index db72b51ea..56693ecfc 100644 --- a/ros2topic/ros2topic/verb/echo.py +++ b/ros2topic/ros2topic/verb/echo.py @@ -57,7 +57,9 @@ def add_arguments(self, parser, cli_name): parser.add_argument( 'message_type', nargs='?', help="Type of the ROS message (e.g. 'std_msgs/msg/String')") - add_qos_arguments(parser, 'subscribe', 'sensor_data') + add_qos_arguments( + parser, 'subscribe', 'sensor_data', + ' / compatible profile with running endpoints') parser.add_argument( '--csv', action='store_true', help=( From 2f1f3be775428340aadaa4f40d243631c5426054 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 31 Jan 2025 17:12:54 -0800 Subject: [PATCH 2/2] add unit test for ros2 service call with all QoS options. Signed-off-by: Tomoya Fujita --- ros2service/test/test_cli.py | 37 ++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/ros2service/test/test_cli.py b/ros2service/test/test_cli.py index 8333ddf41..d9fb9d2ad 100644 --- a/ros2service/test/test_cli.py +++ b/ros2service/test/test_cli.py @@ -326,3 +326,40 @@ def test_repeated_call(self): bool_value=True, int32_value=1, float64_value=1.0, string_value='foobar' ) ), timeout=10) + + @launch_testing.markers.retry_on_failure(times=5, delay=1) + def test_call_with_qos_option(self): + with self.launch_service_command( + arguments=[ + 'call', + '--qos-profile', + 'system_default', + '--qos-depth', + '5', + '--qos-history', + 'system_default', + '--qos-reliability', + 'system_default', + '--qos-durability', + 'system_default', + '--qos-liveliness', + 'system_default', + '--qos-liveliness-lease-duration', + '0', + '/my_ns/echo', + 'test_msgs/srv/BasicTypes', + '{bool_value: false, int32_value: -1, float64_value: 0.1, string_value: bazbar}' + ] + ) as service_command: + assert service_command.wait_for_shutdown(timeout=10) + assert service_command.exit_code == launch_testing.asserts.EXIT_OK + assert launch_testing.tools.expect_output( + expected_lines=get_echo_call_output( + bool_value=False, + int32_value=-1, + float64_value=0.1, + string_value='bazbar' + ), + text=service_command.output, + strict=True + )