Support QoS options for ros2 service call (#966)

* add QoS option for ros2 service call.

Signed-off-by: Tomoya Fujita <[email protected]>

* add unit test for ros2 service call with all QoS options.

Signed-off-by: Tomoya Fujita <[email protected]>

---------

Signed-off-by: Tomoya Fujita <[email protected]>

Author: fujitatomoya

ros2service/package.xml

@@ -20,6 +20,7 @@
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>rclpy</exec_depend>
   <exec_depend>ros2cli</exec_depend>
+  <exec_depend>ros2topic</exec_depend>
   <exec_depend>rosidl_runtime_py</exec_depend>
 
   <test_depend>ament_copyright</test_depend>

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()
 

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
+        )

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})'))

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=(