[service introspection] ros2 service echo (#745)

* Implement ros2 service echo.

Signed-off-by: deepanshu <[email protected]>
Signed-off-by: Chris Lalancette <[email protected]>

* Add test for service echo verb

Launch a test fixture with an introspectable client repeatedly sending requests to an introspectable service.
The test checks that we can see all four expected service events sequentially using the new 'echo' verb.

Signed-off-by: Jacob Perron <[email protected]>

* Fixes from review.

Signed-off-by: Chris Lalancette <[email protected]>

* Fixes from review.

Signed-off-by: Chris Lalancette <[email protected]>

* Switch to meaningful names for the event_type.

Signed-off-by: Chris Lalancette <[email protected]>

---------

Signed-off-by: deepanshu <[email protected]>
Signed-off-by: Chris Lalancette <[email protected]>
Signed-off-by: Jacob Perron <[email protected]>
Co-authored-by: deepanshu <[email protected]>
Co-authored-by: Jacob Perron <[email protected]>
Co-authored-by: Chris Lalancette <[email protected]>

ros2cli/ros2cli/helpers.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from argparse import ArgumentTypeError
 import functools
 import inspect
 import os
@@ -96,3 +97,13 @@ def wrapper(*args, **kwargs):
             return func(*args, **kwargs)
     wrapper.__signature__ = inspect.signature(func)
     return wrapper
+
+
+def unsigned_int(string):
+    try:
+        value = int(string)
+    except ValueError:
+        value = -1
+    if value < 0:
+        raise ArgumentTypeError('value must be non-negative integer')
+    return value

ros2service/ros2service/api/__init__.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from rclpy.node import Node
 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
@@ -34,6 +35,60 @@ def get_service_names(*, node, include_hidden_services=False):
     return [n for (n, t) in service_names_and_types]
 
 
+def get_service_class(node: Node, service_name: str, include_hidden_services: bool):
+    """
+    Load service type module for the given service.
+
+    The service should be running for this function to find the service type.
+    :param node: The node object of rclpy Node class.
+    :param service_name: The fully-qualified name of the service.
+    :param include_hidden_services: Whether to include hidden services while finding the
+    list of currently running services.
+    :return: the service class or None
+    """
+    service_names_and_types = get_service_names_and_types(
+        node=node,
+        include_hidden_services=include_hidden_services)
+
+    # get_service_names_and_types() returns a list of lists, like the following:
+    #  [
+    #    ['/service1', ['service/srv/Type1]],
+    #    ['/service2', ['service/srv/Type2]],
+    #  ]
+    #
+    # If there are more than one server for a service with the same type, that is only represented
+    # once.  If there are more than one server for a service name with different types, those are
+    # represented like:
+    #
+    # [
+    #  ['/service1', ['service/srv/Type1', 'service/srv/Type2']],
+    # ]
+    matched_names_and_types = list(filter(lambda x: x[0] == service_name, service_names_and_types))
+    if len(matched_names_and_types) < 1:
+        raise RuntimeError(f"Cannot find type for '{service_name}'")
+    if len(matched_names_and_types) > 1:
+        raise RuntimeError(f"Unexpectedly saw more than one entry for service '{service_name}'")
+
+    # Now check whether there are multiple types associated with this service, which is unsupported
+    service_name_and_types = matched_names_and_types[0]
+
+    types = service_name_and_types[1]
+    if len(types) < 1:
+        raise RuntimeError(f"No types associated with '{service_name}'")
+    if len(types) > 1:
+        raise RuntimeError(f"More than one type associated with service '{service_name}'")
+
+    service_type = types[0]
+
+    if service_type is None:
+        return None
+
+    try:
+        return get_service(service_type)
+    except (AttributeError, ModuleNotFoundError, ValueError):
+        raise RuntimeError(f"The service type '{service_type}' is invalid")
+
+
 def service_type_completer(**kwargs):
     """Callable returning a list of service types."""
     service_types = []

ros2service/ros2service/verb/echo.py

@@ -0,0 +1,162 @@
+# Copyright 2022 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from collections import OrderedDict
+import sys
+from typing import TypeVar
+
+import rclpy
+
+from rclpy.qos import QoSPresetProfiles
+from ros2cli.helpers import unsigned_int
+from ros2cli.node.strategy import NodeStrategy
+from ros2service.api import get_service_class
+from ros2service.api import ServiceNameCompleter
+from ros2service.api import ServiceTypeCompleter
+from ros2service.verb import VerbExtension
+from rosidl_runtime_py import message_to_csv
+from rosidl_runtime_py import message_to_ordereddict
+from rosidl_runtime_py.utilities import get_service
+from service_msgs.msg import ServiceEventInfo
+
+import yaml
+
+
+DEFAULT_TRUNCATE_LENGTH = 128
+MsgType = TypeVar('MsgType')
+
+
+class EchoVerb(VerbExtension):
+    """Echo a service."""
+
+    # Custom representer for getting clean YAML output that preserves the order in an OrderedDict.
+    # Inspired by: http://stackoverflow.com/a/16782282/7169408
+    def __represent_ordereddict(self, dumper, data):
+        items = []
+        for k, v in data.items():
+            items.append((dumper.represent_data(k), dumper.represent_data(v)))
+        return yaml.nodes.MappingNode(u'tag:yaml.org,2002:map', items)
+
+    def __init__(self):
+        self._event_number_to_name = {}
+        for k, v in ServiceEventInfo._Metaclass_ServiceEventInfo__constants.items():
+            self._event_number_to_name[v] = k
+
+        yaml.add_representer(OrderedDict, self.__represent_ordereddict)
+
+    def add_arguments(self, parser, cli_name):
+        arg = parser.add_argument(
+            'service_name',
+            help="Name of the ROS service to echo (e.g. '/add_two_ints')")
+        arg.completer = ServiceNameCompleter(
+            include_hidden_services_key='include_hidden_services')
+        arg = parser.add_argument(
+            'service_type', nargs='?',
+            help="Type of the ROS service (e.g. 'example_interfaces/srv/AddTwoInts')")
+        arg.completer = ServiceTypeCompleter(service_name_key='service_name')
+        parser.add_argument(
+            '--csv', action='store_true', default=False,
+            help=(
+                'Output all recursive fields separated by commas (e.g. for plotting).'
+            ))
+        parser.add_argument(
+            '--full-length', '-f', action='store_true',
+            help='Output all elements for arrays, bytes, and string with a '
+                 "length > '--truncate-length', by default they are truncated "
+                 "after '--truncate-length' elements with '...''")
+        parser.add_argument(
+            '--truncate-length', '-l', type=unsigned_int, default=DEFAULT_TRUNCATE_LENGTH,
+            help='The length to truncate arrays, bytes, and string to '
+                 '(default: %d)' % DEFAULT_TRUNCATE_LENGTH)
+        parser.add_argument(
+            '--no-arr', action='store_true', help="Don't print array fields of messages")
+        parser.add_argument(
+            '--no-str', action='store_true', help="Don't print string fields of messages")
+        parser.add_argument(
+            '--flow-style', action='store_true',
+            help='Print collections in the block style (not available with csv format)')
+
+    def main(self, *, args):
+        if args.service_type is None:
+            with NodeStrategy(args) as node:
+                try:
+                    srv_module = get_service_class(
+                        node, args.service_name, include_hidden_services=True)
+                except (AttributeError, ModuleNotFoundError, ValueError):
+                    raise RuntimeError(f"The service name '{args.service_name}' is invalid")
+        else:
+            try:
+                srv_module = get_service(args.service_type)
+            except (AttributeError, ModuleNotFoundError, ValueError):
+                raise RuntimeError(f"The service type '{args.service_type}' is invalid")
+
+        if srv_module is None:
+            raise RuntimeError('Could not load the type for the passed service')
+
+        event_msg_type = srv_module.Event
+
+        # TODO(clalancette): We should probably expose this postfix from rclpy
+        event_topic_name = args.service_name + '/_service_event'
+
+        self.csv = args.csv
+        self.truncate_length = args.truncate_length if not args.full_length else None
+        self.flow_style = args.flow_style
+        self.no_arr = args.no_arr
+        self.no_str = args.no_str
+
+        with NodeStrategy(args) as node:
+            sub = node.create_subscription(
+                event_msg_type,
+                event_topic_name,
+                self._subscriber_callback,
+                QoSPresetProfiles.get_from_short_key('services_default'))
+
+            have_printed_warning = False
+            executor = rclpy.get_global_executor()
+            try:
+                executor.add_node(node)
+                while executor.context.ok():
+                    if not have_printed_warning and sub.get_publisher_count() < 1:
+                        print(f"No publishers on topic '{event_topic_name}'; "
+                              'is service introspection on the client or server enabled?')
+                        have_printed_warning = True
+                    executor.spin_once()
+            finally:
+                executor.remove_node(node)
+
+            sub.destroy()
+
+    def _subscriber_callback(self, msg):
+        if self.csv:
+            to_print = message_to_csv(msg, truncate_length=self.truncate_length,
+                                      no_arr=self.no_arr, no_str=self.no_str)
+        else:
+            # The "easy" way to print out a representation here is to call message_to_yaml().
+            # However, the message contains numbers for the event type, but we want to show
+            # meaningful names to the user.  So we call message_to_ordereddict() instead,
+            # and replace the numbers with meaningful names before dumping to YAML.
+            msgdict = message_to_ordereddict(msg, truncate_length=self.truncate_length,
+                                             no_arr=self.no_arr, no_str=self.no_str)
+
+            if 'info' in msgdict:
+                info = msgdict['info']
+                if 'event_type' in info:
+                    info['event_type'] = self._event_number_to_name[info['event_type']]
+
+            to_print = yaml.dump(msgdict, allow_unicode=True, width=sys.maxsize,
+                                 default_flow_style=self.flow_style)
+
+            to_print += '---'
+
+        print(to_print)

ros2service/setup.py

@@ -41,6 +41,7 @@
         ],
         'ros2service.verb': [
             'call = ros2service.verb.call:CallVerb',
+            'echo = ros2service.verb.echo:EchoVerb',
             'find = ros2service.verb.find:FindVerb',
             'list = ros2service.verb.list:ListVerb',
             'type = ros2service.verb.type:TypeVerb',

ros2service/test/fixtures/introspectable.py

@@ -0,0 +1,93 @@
+# Copyright 2023 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rclpy
+from rclpy.executors import ExternalShutdownException
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
+from rclpy.qos import qos_profile_system_default
+from rclpy.service_introspection import ServiceIntrospectionState
+
+from test_msgs.srv import BasicTypes
+
+
+class IntrospectableService(Node):
+
+    def __init__(self):
+        super().__init__('introspectable_service')
+        self.service = self.create_service(BasicTypes, 'test_introspectable', self.callback)
+        self.service.configure_introspection(
+            self.get_clock(), qos_profile_system_default, ServiceIntrospectionState.CONTENTS)
+
+    def callback(self, request, response):
+        for field_name in request.get_fields_and_field_types():
+            setattr(response, field_name, getattr(request, field_name))
+        return response
+
+
+class IntrospectableClient(Node):
+
+    def __init__(self):
+        super().__init__('introspectable_client')
+        self.client = self.create_client(BasicTypes, 'test_introspectable')
+        self.client.configure_introspection(
+            self.get_clock(), qos_profile_system_default, ServiceIntrospectionState.CONTENTS)
+
+        self.timer = self.create_timer(0.1, self.timer_callback)
+        self.future = None
+
+    def timer_callback(self):
+        if not self.client.service_is_ready():
+            return
+
+        if self.future is None:
+            request = BasicTypes.Request()
+            request.bool_value = True
+            request.int32_value = 42
+            request.string_value = 'test_string_value'
+            self.future = self.client.call_async(request)
+            return
+
+        if not self.future.done():
+            return
+
+        if self.future.result() is None:
+            self.get_logger().error(f'Exception calling service: {self.future.exception()!r}')
+
+        self.future = None
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    service_node = IntrospectableService()
+    client_node = IntrospectableClient()
+
+    executor = SingleThreadedExecutor()
+    executor.add_node(service_node)
+    executor.add_node(client_node)
+
+    try:
+        executor.spin()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        executor.remove_node(client_node)
+        executor.remove_node(service_node)
+        executor.shutdown()
+        service_node.destroy_node()
+        client_node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()