Implement service introspection.

To do this, we add a new method on the Client and
Service classes that allows the user to change the
introspection method at runtime.  These end up calling
into the rcl layer to do the actual configuration,
at which point service introspection messages will be
sent as configured.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>

rclpy/rclpy/__init__.py

@@ -143,8 +143,7 @@ def create_node(
     start_parameter_services: bool = True,
     parameter_overrides: List[Parameter] = None,
     allow_undeclared_parameters: bool = False,
-    automatically_declare_parameters_from_overrides: bool = False,
-    enable_service_introspection: bool = False,
+    automatically_declare_parameters_from_overrides: bool = False
 ) -> 'Node':
     """
     Create an instance of :class:`.Node`.
@@ -166,7 +165,6 @@ def create_node(
         This option doesn't affect `parameter_overrides`.
     :param automatically_declare_parameters_from_overrides: If True, the "parameter overrides" will
         be used to implicitly declare parameters on the node during creation, default False.
-    :param enable_service_introspection: Flag to enable service introspection, default False.
     :return: An instance of the newly created node.
     """
     # imported locally to avoid loading extensions on module import
@@ -180,8 +178,7 @@ def create_node(
         allow_undeclared_parameters=allow_undeclared_parameters,
         automatically_declare_parameters_from_overrides=(
             automatically_declare_parameters_from_overrides
-        ),
-        enable_service_introspection=enable_service_introspection)
+        ))
 
 
 def spin_once(node: 'Node', *, executor: 'Executor' = None, timeout_sec: float = None) -> None:

rclpy/rclpy/client.py

@@ -18,9 +18,11 @@
 from typing import TypeVar
 
 from rclpy.callback_groups import CallbackGroup
+from rclpy.clock import Clock
 from rclpy.context import Context
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.qos import QoSProfile
+from rclpy.service_introspection import ServiceIntrospectionState
 from rclpy.task import Future
 
 # Used for documentation purposes only
@@ -180,6 +182,23 @@ def wait_for_service(self, timeout_sec: float = None) -> bool:
 
         return self.service_is_ready()
 
+    def configure_introspection(
+        self, clock: Clock,
+        service_event_qos_profile: QoSProfile,
+        introspection_state: ServiceIntrospectionState
+    ) -> None:
+        """
+        Configure client introspection.
+
+        :param clock: Clock to use for generating timestamps.
+        :param service_event_qos_profile: QoSProfile to use when creating service event publisher.
+        :param introspection_state: ServiceIntrospectionState to set introspection.
+        """
+        with self.handle:
+            self.__client.configure_introspection(clock.handle,
+                                                  service_event_qos_profile.get_c_qos_profile(),
+                                                  introspection_state)
+
     @property
     def handle(self):
         return self.__client

rclpy/rclpy/node.py

@@ -66,7 +66,6 @@
 from rclpy.publisher import Publisher
 from rclpy.qos import qos_profile_parameter_events
 from rclpy.qos import qos_profile_services_default
-from rclpy.qos import qos_profile_system_default
 from rclpy.qos import QoSProfile
 from rclpy.qos_event import PublisherEventCallbacks
 from rclpy.qos_event import SubscriptionEventCallbacks
@@ -127,8 +126,7 @@ def __init__(
         start_parameter_services: bool = True,
         parameter_overrides: List[Parameter] = None,
         allow_undeclared_parameters: bool = False,
-        automatically_declare_parameters_from_overrides: bool = False,
-        enable_service_introspection: bool = False,
+        automatically_declare_parameters_from_overrides: bool = False
     ) -> None:
         """
         Create a Node.
@@ -152,8 +150,6 @@ def __init__(
             This flag affects the behavior of parameter-related operations.
         :param automatically_declare_parameters_from_overrides: If True, the "parameter overrides"
             will be used to implicitly declare parameters on the node during creation.
-        :param enable_service_introspection: If True, the node will enable introspection of
-            services.
         """
         self.__handle = None
         self._context = get_default_context() if context is None else context
@@ -186,8 +182,7 @@ def __init__(
                     self._context.handle,
                     cli_args,
                     use_global_arguments,
-                    enable_rosout,
-                    enable_service_introspection
+                    enable_rosout
                 )
             except ValueError:
                 # these will raise more specific errors if the name or namespace is bad
@@ -236,18 +231,6 @@ def __init__(
         if start_parameter_services:
             self._parameter_service = ParameterService(self)
 
-        if enable_service_introspection:
-            self.declare_parameters(
-                namespace='',
-                parameters=[
-                    (_rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER,  # noqa E501
-                     'off', ParameterDescriptor()),
-                    (_rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER,  # noqa E501
-                     'off', ParameterDescriptor()),
-                ])
-            self.add_on_set_parameters_callback(self._check_service_introspection_parameters)
-            self.add_post_set_parameters_callback(self._configure_service_introspection)
-
     @property
     def publishers(self) -> Iterator[Publisher]:
         """Get publishers that have been created on this node."""
@@ -1593,102 +1576,22 @@ def create_subscription(
 
         return subscription
 
-    def _check_service_introspection_parameters(
-        self, parameters: List[Parameter]
-    ) -> SetParametersResult:
-        result = SetParametersResult(successful=True)
-        for param in parameters:
-            if param.name not in (
-                    _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER,  # noqa: E501
-                    _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER):  # noqa: E501
-                continue
-
-            if param.type_ != Parameter.Type.STRING:
-                result.successful = False
-                result.reason = 'Parameter type must be string'
-                break
-
-            if param.value not in ('off', 'metadata', 'contents'):
-                result.successful = False
-                result.reason = "Value must be one of 'off', 'metadata', or 'contents'"
-                break
-
-        return result
-
-    def _configure_service_introspection(self, parameters: List[Parameter]):
-        for param in parameters:
-            if param.name == \
-                    _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER:  # noqa: E501
-
-                value = param.value
-
-                for cli in self.clients:
-                    should_enable_service_events = False
-                    should_enable_contents = False
-                    if value == 'off':
-                        should_enable_service_events = False
-                        should_enable_contents = False
-                    elif value == 'metadata':
-                        should_enable_service_events = True
-                        should_enable_contents = False
-                    elif value == 'contents':
-                        should_enable_service_events = True
-                        should_enable_contents = True
-
-                    _rclpy.service_introspection.configure_client_events(
-                        cli.handle.pointer,
-                        self.handle.pointer,
-                        should_enable_service_events)
-                    _rclpy.service_introspection.configure_client_message_payload(
-                        cli.handle.pointer,
-                        should_enable_contents)
-            elif param.name == \
-                    _rclpy.service_introspection.RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER:  # noqa: E501
-
-                value = param.value
-
-                for srv in self.services:
-                    should_enable_service_events = False
-                    should_enable_contents = False
-                    if value == 'off':
-                        should_enable_service_events = False
-                        should_enable_contents = False
-                    elif value == 'metadata':
-                        should_enable_service_events = True
-                        should_enable_contents = False
-                    elif value == 'contents':
-                        should_enable_service_events = True
-                        should_enable_contents = True
-
-                    _rclpy.service_introspection.configure_service_events(
-                        srv.handle.pointer,
-                        self.handle.pointer,
-                        should_enable_service_events)
-                    _rclpy.service_introspection.configure_service_message_payload(
-                        srv.handle.pointer,
-                        should_enable_contents)
-
     def create_client(
         self,
         srv_type,
         srv_name: str,
         *,
         qos_profile: QoSProfile = qos_profile_services_default,
-        callback_group: CallbackGroup = None,
-        service_event_qos_profile: QoSProfile = qos_profile_system_default
+        callback_group: CallbackGroup = None
     ) -> Client:
         """
         Create a new service client.
 
         :param srv_type: The service type.
         :param srv_name: The name of the service.
         :param qos_profile: The quality of service profile to apply the service client.
-        :param service_event_publisher_qos_profile: The quality of service
-            profile to apply the service event publisher.
         :param callback_group: The callback group for the service client. If ``None``, then the
             default callback group for the node is used.
-        :param service_event_qos_profile: The quality of service profile to apply to service
-            introspection (if enabled).
         """
         if callback_group is None:
             callback_group = self.default_callback_group
@@ -1700,9 +1603,7 @@ def create_client(
                     self.handle,
                     srv_type,
                     srv_name,
-                    qos_profile.get_c_qos_profile(),
-                    service_event_qos_profile.get_c_qos_profile(),
-                    self._clock.handle)
+                    qos_profile.get_c_qos_profile())
         except ValueError:
             failed = True
         if failed:
@@ -1724,8 +1625,7 @@ def create_service(
         callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse],
         *,
         qos_profile: QoSProfile = qos_profile_services_default,
-        callback_group: CallbackGroup = None,
-        service_event_qos_profile: QoSProfile = qos_profile_system_default
+        callback_group: CallbackGroup = None
     ) -> Service:
         """
         Create a new service server.
@@ -1735,12 +1635,8 @@ def create_service(
         :param callback: A user-defined callback function that is called when a service request
             received by the server.
         :param qos_profile: The quality of service profile to apply the service server.
-        :param service_event_publisher_qos_profile: The quality of service
-            profile to apply the service event publisher.
         :param callback_group: The callback group for the service server. If ``None``, then the
             default callback group for the node is used.
-        :param service_event_qos_profile: The quality of service profile to apply to service
-            introspection (if enabled).
         """
         if callback_group is None:
             callback_group = self.default_callback_group
@@ -1752,9 +1648,7 @@ def create_service(
                     self.handle,
                     srv_type,
                     srv_name,
-                    qos_profile.get_c_qos_profile(),
-                    service_event_qos_profile.get_c_qos_profile(),
-                    self._clock.handle)
+                    qos_profile.get_c_qos_profile())
         except ValueError:
             failed = True
         if failed:

rclpy/rclpy/service.py

@@ -16,8 +16,10 @@
 from typing import TypeVar
 
 from rclpy.callback_groups import CallbackGroup
+from rclpy.clock import Clock
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.qos import QoSProfile
+from rclpy.service_introspection import ServiceIntrospectionState
 
 # Used for documentation purposes only
 SrvType = TypeVar('SrvType')
@@ -79,6 +81,23 @@ def send_response(self, response: SrvTypeResponse, header) -> None:
             else:
                 raise TypeError()
 
+    def configure_introspection(
+        self, clock: Clock,
+        service_event_qos_profile: QoSProfile,
+        introspection_state: ServiceIntrospectionState
+    ) -> None:
+        """
+        Configure service introspection.
+
+        :param clock: Clock to use for generating timestamps.
+        :param service_event_qos_profile: QoSProfile to use when creating service event publisher.
+        :param introspection_state: ServiceIntrospectionState to set introspection.
+        """
+        with self.handle:
+            self.__service.configure_introspection(clock.handle,
+                                                   service_event_qos_profile.get_c_qos_profile(),
+                                                   introspection_state)
+
     @property
     def handle(self):
         return self.__service

rclpy/rclpy/service_introspection.py

@@ -0,0 +1,17 @@
+# 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.
+
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+
+ServiceIntrospectionState = _rclpy.service_introspection.ServiceIntrospectionState

rclpy/src/rclpy/_rclpy_pybind11.cpp

@@ -15,6 +15,7 @@
 #include <pybind11/pybind11.h>
 
 #include <rcl/domain_id.h>
+#include <rcl/service_introspection.h>
 #include <rcl/time.h>
 #include <rcl_action/types.h>
 
@@ -120,6 +121,8 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
   py::register_exception<rclpy::InvalidHandle>(
     m, "InvalidHandle", PyExc_RuntimeError);
 
+  rclpy::define_service_introspection(m);
+
   rclpy::define_client(m);
 
   rclpy::define_context(m);
@@ -241,5 +244,4 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
   rclpy::define_signal_handler_api(m);
   rclpy::define_clock_event(m);
   rclpy::define_lifecycle_api(m);
-  rclpy::define_service_introspection(m);
 }