Service introspection (#988)

* 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.

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: 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
@@ -190,6 +191,7 @@ if(BUILD_TESTING)
       test/test_rate.py
       test/test_rosout_subscription.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/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/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>
 
@@ -44,6 +45,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"
@@ -119,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);

rclpy/src/rclpy/client.cpp

@@ -17,13 +17,15 @@
 #include <rcl/client.h>
 #include <rcl/error_handling.h>
 #include <rcl/graph.h>
+#include <rcl/service_introspection.h>
 #include <rosidl_runtime_c/service_type_support_struct.h>
 #include <rmw/types.h>
 
 #include <memory>
 #include <string>
 
 #include "client.hpp"
+#include "clock.hpp"
 #include "exceptions.hpp"
 #include "node.hpp"
 #include "python_allocator.hpp"
@@ -40,12 +42,11 @@ Client::destroy()
 }
 
 Client::Client(
-  Node & node, py::object pysrv_type, const char * service_name, py::object pyqos_profile)
+  Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos_profile)
 : node_(node)
 {
-  auto srv_type = static_cast<rosidl_service_type_support_t *>(
-    common_get_type_support(pysrv_type));
-  if (!srv_type) {
+  srv_type_ = static_cast<rosidl_service_type_support_t *>(common_get_type_support(pysrv_type));
+  if (nullptr == srv_type_) {
     throw py::error_already_set();
   }
 
@@ -55,7 +56,6 @@ Client::Client(
     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),
@@ -77,7 +77,7 @@ Client::Client(
   *rcl_client_ = rcl_get_zero_initialized_client();
 
   rcl_ret_t ret = rcl_client_init(
-    rcl_client_.get(), node_.rcl_ptr(), srv_type, service_name, &client_ops);
+    rcl_client_.get(), node_.rcl_ptr(), srv_type_, service_name.c_str(), &client_ops);
   if (RCL_RET_OK != ret) {
     if (RCL_RET_SERVICE_NAME_INVALID == ret) {
       std::string error_text{"failed to create client due to invalid service name '"};
@@ -146,11 +146,29 @@ Client::take_response(py::object pyresponse_type)
   return result_tuple;
 }
 
+void
+Client::configure_introspection(
+  Clock & clock, py::object pyqos_service_event_pub,
+  rcl_service_introspection_state_t introspection_state)
+{
+  rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+  pub_opts.qos =
+    pyqos_service_event_pub.is_none() ? rcl_publisher_get_default_options().qos :
+    pyqos_service_event_pub.cast<rmw_qos_profile_t>();
+
+  rcl_ret_t ret = rcl_client_configure_service_introspection(
+    rcl_client_.get(), node_.rcl_ptr(), clock.rcl_ptr(), srv_type_, pub_opts, introspection_state);
+
+  if (RCL_RET_OK != ret) {
+    throw RCLError("failed to configure client introspection");
+  }
+}
+
 void
 define_client(py::object module)
 {
   py::class_<Client, Destroyable, std::shared_ptr<Client>>(module, "Client")
-  .def(py::init<Node &, py::object, const char *, py::object>())
+  .def(py::init<Node &, py::object, const std::string &, py::object>())
   .def_property_readonly(
     "pointer", [](const Client & client) {
       return reinterpret_cast<size_t>(client.rcl_ptr());
@@ -164,6 +182,9 @@ define_client(py::object module)
     "Return true if the service server is available")
   .def(
     "take_response", &Client::take_response,
-    "Take a received response from an earlier request");
+    "Take a received response from an earlier request")
+  .def(
+    "configure_introspection", &Client::configure_introspection,
+    "Configure whether introspection is enabled");
 }
 }  // namespace rclpy

rclpy/src/rclpy/client.hpp

@@ -18,9 +18,13 @@
 #include <pybind11/pybind11.h>
 
 #include <rcl/client.h>
+#include <rcl/service_introspection.h>
+#include <rmw/types.h>
 
 #include <memory>
+#include <string>
 
+#include "clock.hpp"
 #include "destroyable.hpp"
 #include "node.hpp"
 
@@ -43,9 +47,9 @@ 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
    */
-  Client(Node & node, py::object pysrv_type, const char * service_name, py::object pyqos);
+  Client(Node & node, py::object pysrv_type, const std::string & service_name, py::object pyqos);
 
   ~Client() = default;
 
@@ -86,13 +90,25 @@ class Client : public Destroyable, public std::enable_shared_from_this<Client>
     return rcl_client_.get();
   }
 
+  /// Configure introspection.
+  /**
+   * \param[in] clock clock to use for service event timestamps
+   * \param[in] pyqos_service_event_pub QoSProfile python object for the service event publisher
+   * \param[in] introspection_state which state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock & clock, py::object pyqos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state);
+
   /// Force an early destruction of this object
   void
   destroy() override;
 
 private:
   Node node_;
   std::shared_ptr<rcl_client_t> rcl_client_;
+  rosidl_service_type_support_t * srv_type_;
 };
 
 /// Define a pybind11 wrapper for an rclpy::Client

rclpy/src/rclpy/service.cpp

@@ -16,12 +16,14 @@
 
 #include <rcl/error_handling.h>
 #include <rcl/service.h>
+#include <rcl/service_introspection.h>
 #include <rosidl_runtime_c/service_type_support_struct.h>
 #include <rmw/types.h>
 
 #include <memory>
 #include <string>
 
+#include "clock.hpp"
 #include "exceptions.hpp"
 #include "node.hpp"
 #include "service.hpp"
@@ -38,13 +40,13 @@ Service::destroy()
 }
 
 Service::Service(
-  Node & node, py::object pysrv_type, std::string service_name,
+  Node & node, py::object pysrv_type, const std::string & service_name,
   py::object pyqos_profile)
 : node_(node)
 {
-  auto srv_type = static_cast<rosidl_service_type_support_t *>(
+  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();
   }
 
@@ -54,7 +56,7 @@ Service::Service(
     service_ops.qos = pyqos_profile.cast<rmw_qos_profile_t>();
   }
 
-  // Create a client
+  // Create a service
   rcl_service_ = std::shared_ptr<rcl_service_t>(
     new rcl_service_t,
     [node](rcl_service_t * service)
@@ -75,7 +77,7 @@ Service::Service(
   *rcl_service_ = rcl_get_zero_initialized_service();
 
   rcl_ret_t ret = rcl_service_init(
-    rcl_service_.get(), node_.rcl_ptr(), srv_type,
+    rcl_service_.get(), node_.rcl_ptr(), srv_type_,
     service_name.c_str(), &service_ops);
   if (RCL_RET_OK != ret) {
     if (ret == RCL_RET_SERVICE_NAME_INVALID) {
@@ -144,11 +146,29 @@ Service::get_qos_profile()
   return rclpy::convert_to_qos_dict(&options->qos);
 }
 
+void
+Service::configure_introspection(
+  Clock & clock, py::object pyqos_service_event_pub,
+  rcl_service_introspection_state_t introspection_state)
+{
+  rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+  pub_opts.qos =
+    pyqos_service_event_pub.is_none() ? rcl_publisher_get_default_options().qos :
+    pyqos_service_event_pub.cast<rmw_qos_profile_t>();
+
+  rcl_ret_t ret = rcl_service_configure_service_introspection(
+    rcl_service_.get(), node_.rcl_ptr(), clock.rcl_ptr(), srv_type_, pub_opts, introspection_state);
+
+  if (RCL_RET_OK != ret) {
+    throw RCLError("failed to configure service introspection");
+  }
+}
+
 void
 define_service(py::object module)
 {
   py::class_<Service, Destroyable, std::shared_ptr<Service>>(module, "Service")
-  .def(py::init<Node &, py::object, std::string, py::object>())
+  .def(py::init<Node &, py::object, const std::string &, py::object>())
   .def_property_readonly(
     "pointer", [](const Service & service) {
       return reinterpret_cast<size_t>(service.rcl_ptr());
@@ -165,6 +185,9 @@ define_service(py::object module)
     "Send a response")
   .def(
     "service_take_request", &Service::service_take_request,
-    "Take a request from a given service");
+    "Take a request from a given service")
+  .def(
+    "configure_introspection", &Service::configure_introspection,
+    "Configure whether introspection is enabled");
 }
 }  // namespace rclpy