Add parameter to change QoS on ros2 service call similar to `ros2 t…

…opic pub`

Signed-off-by: Bastian <[email protected]>

ros2service/ros2service/api/__init__.py

@@ -12,6 +12,10 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from typing import Optional
+
+import rclpy
+
 from rclpy.topic_or_service_is_hidden import topic_or_service_is_hidden
 from ros2cli.node.strategy import NodeStrategy
 from rosidl_runtime_py import get_service_interfaces
@@ -84,3 +88,33 @@ def __init__(self, *, service_type_key=None):
     def __call__(self, prefix, parsed_args, **kwargs):
         service = get_service(getattr(parsed_args, self.service_type_key))
         return [message_to_yaml(service.Request())]
+
+
+def profile_configure_short_keys(
+    profile: rclpy.qos.QoSProfile = None, reliability: str = None,
+    durability: str = None, depth: Optional[int] = None, history: str = None,
+) -> rclpy.qos.QoSProfile:
+    """Configure a QoSProfile given a profile, and optional overrides."""
+    if history:
+        profile.history = rclpy.qos.QoSHistoryPolicy.get_from_short_key(history)
+    if durability:
+        profile.durability = rclpy.qos.QoSDurabilityPolicy.get_from_short_key(durability)
+    if reliability:
+        profile.reliability = rclpy.qos.QoSReliabilityPolicy.get_from_short_key(reliability)
+    if depth and depth >= 0:
+        profile.depth = depth
+    else:
+        if (profile.durability == rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL
+                and profile.depth == 0):
+            profile.depth = 1
+
+
+def qos_profile_from_short_keys(
+    preset_profile: str, reliability: str = None, durability: str = None,
+    depth: Optional[int] = None, history: str = 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)
+    return profile

ros2service/ros2service/verb/call.py

@@ -16,15 +16,26 @@
 import time
 
 import rclpy
+from rclpy.qos import QoSDurabilityPolicy
+from rclpy.qos import QoSProfile
+from rclpy.qos import QoSReliabilityPolicy
 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 ros2service.api import profile_configure_short_keys
 from rosidl_runtime_py import set_message_fields
 import yaml
 
 
+def get_pub_qos_profile():
+    return QoSProfile(
+        reliability=QoSReliabilityPolicy.RELIABLE,
+        durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+        depth=1)
+
+
 class CallVerb(VerbExtension):
     """Call a service."""
 
@@ -49,17 +60,53 @@ 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')
+        parser.add_argument(
+            '--qos-profile',
+            choices=rclpy.qos.QoSPresetProfiles.short_keys(),
+            help='Quality of service preset profile to publish)')
+        default_profile = get_pub_qos_profile()
+        parser.add_argument(
+            '--qos-depth', metavar='N', type=int, default=-1,
+            help='Queue size setting to publish with '
+                 '(overrides depth value of --qos-profile option)')
+        parser.add_argument(
+            '--qos-history',
+            choices=rclpy.qos.QoSHistoryPolicy.short_keys(),
+            help='History of samples setting to publish with '
+                 '(overrides history value of --qos-profile option, default: {})'
+                 .format(default_profile.history.short_key))
+        parser.add_argument(
+            '--qos-reliability',
+            choices=rclpy.qos.QoSReliabilityPolicy.short_keys(),
+            help='Quality of service reliability setting to publish with '
+                 '(overrides reliability value of --qos-profile option, default: {})'
+                 .format(default_profile.reliability.short_key))
+        parser.add_argument(
+            '--qos-durability',
+            choices=rclpy.qos.QoSDurabilityPolicy.short_keys(),
+            help='Quality of service durability setting to publish with '
+                 '(overrides durability value of --qos-profile option, default: {})'
+                 .format(default_profile.durability.short_key))
 
     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
 
+        qos_profile = get_pub_qos_profile()
+
+        qos_profile_name = args.qos_profile
+        if 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)
+
         return requester(
-            args.service_type, args.service_name, args.values, period)
+            args.service_type, args.service_name, args.values, period, qos_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('/')
@@ -83,7 +130,11 @@ def requester(service_type, service_name, values, period):
 
     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_type=srv_module,
+        srv_name=service_name,
+        qos_profile=qos_profile
+    )
 
     request = srv_module.Request()