Implement service introspection in rclpy.

Add in the parameters to control it, as well as the correct
calls into the underlying rcl implementation.  Also add in
tests.

Signed-off-by: Brian Chen <brian.chen@openrobotics.org>
Signed-off-by: Chris Lalancette <clalancette@gmail.com>

rclpy/CMakeLists.txt

@@ -96,6 +96,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED
   src/rclpy/serialization.cpp
   src/rclpy/service.cpp
   src/rclpy/service_info.cpp
+  src/rclpy/service_introspection.cpp
   src/rclpy/signal_handler.cpp
   src/rclpy/subscription.cpp
   src/rclpy/time_point.cpp
@@ -189,6 +190,7 @@ if(BUILD_TESTING)
       test/test_qos_overriding_options.py
       test/test_rate.py
       test/test_serialization.py
+      test/test_service_introspection.py
       test/test_subscription.py
       test/test_task.py
       test/test_time_source.py

rclpy/rclpy/__init__.py

@@ -143,7 +143,8 @@ 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
+    automatically_declare_parameters_from_overrides: bool = False,
+    enable_service_introspection: bool = False,
 ) -> 'Node':
     """
     Create an instance of :class:`.Node`.
@@ -165,6 +166,7 @@ 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
@@ -178,7 +180,8 @@ 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/node.py

@@ -66,6 +66,7 @@
 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
@@ -126,7 +127,8 @@ def __init__(
         start_parameter_services: bool = True,
         parameter_overrides: List[Parameter] = None,
         allow_undeclared_parameters: bool = False,
-        automatically_declare_parameters_from_overrides: bool = False
+        automatically_declare_parameters_from_overrides: bool = False,
+        enable_service_introspection: bool = False,
     ) -> None:
         """
         Create a Node.
@@ -150,6 +152,8 @@ 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
@@ -182,7 +186,8 @@ def __init__(
                     self._context.handle,
                     cli_args,
                     use_global_arguments,
-                    enable_rosout
+                    enable_rosout,
+                    enable_service_introspection
                 )
             except ValueError:
                 # these will raise more specific errors if the name or namespace is bad
@@ -231,6 +236,18 @@ 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."""
@@ -1576,22 +1593,102 @@ 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
+        callback_group: CallbackGroup = None,
+        service_event_qos_profile: QoSProfile = qos_profile_system_default
     ) -> 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
@@ -1603,7 +1700,9 @@ def create_client(
                     self.handle,
                     srv_type,
                     srv_name,
-                    qos_profile.get_c_qos_profile())
+                    qos_profile.get_c_qos_profile(),
+                    service_event_qos_profile.get_c_qos_profile(),
+                    self._clock.handle)
         except ValueError:
             failed = True
         if failed:
@@ -1625,7 +1724,8 @@ def create_service(
         callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse],
         *,
         qos_profile: QoSProfile = qos_profile_services_default,
-        callback_group: CallbackGroup = None
+        callback_group: CallbackGroup = None,
+        service_event_qos_profile: QoSProfile = qos_profile_system_default
     ) -> Service:
         """
         Create a new service server.
@@ -1635,8 +1735,12 @@ 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
@@ -1648,7 +1752,9 @@ def create_service(
                     self.handle,
                     srv_type,
                     srv_name,
-                    qos_profile.get_c_qos_profile())
+                    qos_profile.get_c_qos_profile(),
+                    service_event_qos_profile.get_c_qos_profile(),
+                    self._clock.handle)
         except ValueError:
             failed = True
         if failed:

rclpy/src/rclpy/_rclpy_pybind11.cpp

@@ -44,6 +44,7 @@
 #include "serialization.hpp"
 #include "service.hpp"
 #include "service_info.hpp"
+#include "service_introspection.hpp"
 #include "signal_handler.hpp"
 #include "subscription.hpp"
 #include "time_point.hpp"
@@ -240,4 +241,5 @@ 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);
 }

rclpy/src/rclpy/client.cpp

@@ -22,8 +22,10 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "client.hpp"
+#include "clock.hpp"
 #include "exceptions.hpp"
 #include "node.hpp"
 #include "python_allocator.hpp"
@@ -40,22 +42,29 @@ Client::destroy()
 }
 
 Client::Client(
-  Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile)
+  Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile,
+  py::object pyqos_service_event_pub, Clock & clock)
 : node_(node)
 {
   auto srv_type = static_cast<rosidl_service_type_support_t *>(
     common_get_type_support(pysrv_type));
-  if (!srv_type) {
+  if (nullptr == srv_type) {
     throw py::error_already_set();
   }
 
   rcl_client_options_t client_ops = rcl_client_get_default_options();
 
+  if (rcl_node_get_options(node.rcl_ptr())->enable_service_introspection) {
+    client_ops.clock = clock.rcl_ptr();
+    client_ops.enable_service_introspection = true;
+    client_ops.event_publisher_options.qos = pyqos_service_event_pub.is_none() ?
+      rcl_publisher_get_default_options().qos : pyqos_service_event_pub.cast<rmw_qos_profile_t>();
+  }
+
   if (!pyqos_profile.is_none()) {
     client_ops.qos = pyqos_profile.cast<rmw_qos_profile_t>();
   }
 
-
   // Create a client
   rcl_client_ = std::shared_ptr<rcl_client_t>(
     PythonAllocator<rcl_client_t>().allocate(1),
@@ -150,7 +159,7 @@ void
 define_client(py::object module)
 {
   py::class_<Client, Destroyable, std::shared_ptr<Client>>(module, "Client")
-  .def(py::init<Node &, py::object, const std::string &, py::object>())
+  .def(py::init<Node &, py::object, const std::string &, py::object, py::object, Clock &>())
   .def_property_readonly(
     "pointer", [](const Client & client) {
       return reinterpret_cast<size_t>(client.rcl_ptr());

rclpy/src/rclpy/client.hpp

@@ -18,10 +18,12 @@
 #include <pybind11/pybind11.h>
 
 #include <rcl/client.h>
+#include <rmw/types.h>
 
 #include <memory>
 #include <string>
 
+#include "clock.hpp"
 #include "destroyable.hpp"
 #include "node.hpp"
 
@@ -44,9 +46,13 @@ class Client : public Destroyable, public std::enable_shared_from_this<Client>
    * \param[in] node Node to add the client to
    * \param[in] pysrv_type Service module associated with the client
    * \param[in] service_name The service name
-   * \param[in] pyqos rmw_qos_profile_t object for this client
+   * \param[in] pyqos QoSProfile python object for this client
+   * \param[in] pyqos_service_event_pub QoSProfile python object for the service event publisher
+   * \param[in] clock Clock to use for service event timestamps
    */
-  Client(Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos);
+  Client(
+    Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos,
+    py::object pyqos_service_event_pub, Clock & clock);
 
   ~Client() = default;
 

rclpy/src/rclpy/node.cpp

@@ -16,6 +16,7 @@
 
 #include <rcl_action/rcl_action.h>
 #include <rcl/error_handling.h>
+#include <rcl/service_introspection.h>
 #include <rcl/graph.h>
 #include <rcl/types.h>
 #include <rcl_interfaces/msg/parameter_type.h>
@@ -382,7 +383,8 @@ Node::Node(
   Context & context,
   py::object pycli_args,
   bool use_global_arguments,
-  bool enable_rosout)
+  bool enable_rosout,
+  bool enable_service_introspection)
 : context_(context)
 {
   rcl_ret_t ret;
@@ -458,6 +460,7 @@ Node::Node(
   options.use_global_arguments = use_global_arguments;
   options.arguments = arguments;
   options.enable_rosout = enable_rosout;
+  options.enable_service_introspection = enable_service_introspection;
 
   {
     rclpy::LoggingGuard scoped_logging_guard;
@@ -531,11 +534,12 @@ Node::get_action_names_and_types()
   return convert_to_py_names_and_types(&names_and_types);
 }
 
+
 void
 define_node(py::object module)
 {
   py::class_<Node, Destroyable, std::shared_ptr<Node>>(module, "Node")
-  .def(py::init<const char *, const char *, Context &, py::object, bool, bool>())
+  .def(py::init<const char *, const char *, Context &, py::object, bool, bool, bool>())
   .def_property_readonly(
     "pointer", [](const Node & node) {
       return reinterpret_cast<size_t>(node.rcl_ptr());

rclpy/src/rclpy/node.hpp

@@ -52,7 +52,8 @@ class Node : public Destroyable, public std::enable_shared_from_this<Node>
     Context & context,
     py::object pycli_args,
     bool use_global_arguments,
-    bool enable_rosout);
+    bool enable_rosout,
+    bool enable_service_introspection);
 
   /// Get the fully qualified name of the node.
   /**