Service introspection

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