diff --git a/ros2service/ros2service/api/__init__.py b/ros2service/ros2service/api/__init__.py index 9143ca163..cfd99d1d5 100644 --- a/ros2service/ros2service/api/__init__.py +++ b/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 name of the service. + :param include_hidden_services: Whether to include hidden services while finding the + list of currently running services. + :return: + """ + 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("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("No types associated with '{service_name}'") + if len(types) > 1: + raise RuntimeError("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 = [] diff --git a/ros2service/ros2service/verb/echo.py b/ros2service/ros2service/verb/echo.py new file mode 100644 index 000000000..a2669aaf7 --- /dev/null +++ b/ros2service/ros2service/verb/echo.py @@ -0,0 +1,144 @@ +# 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 argparse import ArgumentTypeError +from typing import TypeVar + +import rclpy + +from rclpy.qos import QoSPresetProfiles +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_yaml +from rosidl_runtime_py.utilities import get_service +from service_msgs.msg import ServiceEventInfo + +DEFAULT_TRUNCATE_LENGTH = 128 +MsgType = TypeVar('MsgType') + + +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 + + +class EchoVerb(VerbExtension): + """Echo a service.""" + + def __init__(self): + self.event_type_map = [ + (v, k) for k, v in ServiceEventInfo._Metaclass_ServiceEventInfo__constants.items()] + + 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("The service name '%s' is invalid" % args.service_name) + 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: + to_print = message_to_yaml(msg, truncate_length=self.truncate_length, + no_arr=self.no_arr, no_str=self.no_str, + flow_style=self.flow_style) + to_print += '---' + + print(to_print) diff --git a/ros2service/setup.py b/ros2service/setup.py index e9d3e0510..286ec0797 100644 --- a/ros2service/setup.py +++ b/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',