Service introspection (#1985)

* Implementation of 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@openrobotics.org>
Signed-off-by: Brian Chen <brian.chen@openrobotics.org>

rclcpp/include/rclcpp/client.hpp

@@ -16,23 +16,26 @@
 #define RCLCPP__CLIENT_HPP_
 
 #include <atomic>
+#include <functional>
 #include <future>
-#include <unordered_map>
 #include <memory>
 #include <mutex>
 #include <optional>  // NOLINT, cpplint doesn't think this is a cpp std header
 #include <sstream>
 #include <string>
 #include <tuple>
+#include <unordered_map>
 #include <utility>
 #include <variant>  // NOLINT
 #include <vector>
 
 #include "rcl/client.h"
 #include "rcl/error_handling.h"
 #include "rcl/event_callback.h"
+#include "rcl/service_introspection.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/clock.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
@@ -467,15 +470,13 @@ class Client : public ClientBase
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & service_name,
     rcl_client_options_t & client_options)
-  : ClientBase(node_base, node_graph)
+  : ClientBase(node_base, node_graph),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
-    using rosidl_typesupport_cpp::get_service_type_support_handle;
-    auto service_type_support_handle =
-      get_service_type_support_handle<ServiceT>();
     rcl_ret_t ret = rcl_client_init(
       this->get_client_handle().get(),
       this->get_rcl_node_handle(),
-      service_type_support_handle,
+      srv_type_support_handle_,
       service_name.c_str(),
       &client_options);
     if (ret != RCL_RET_OK) {
@@ -781,6 +782,33 @@ class Client : public ClientBase
     return old_size - pending_requests_.size();
   }
 
+  /// Configure client introspection.
+  /**
+   * \param[in] clock clock to use to generate introspection timestamps
+   * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
+   * \param[in] introspection_state the state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock::SharedPtr clock, const QoS & qos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state)
+  {
+    rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+    pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
+
+    rcl_ret_t ret = rcl_client_configure_service_introspection(
+      client_handle_.get(),
+      node_handle_.get(),
+      clock->get_clock_handle(),
+      srv_type_support_handle_,
+      pub_opts,
+      introspection_state);
+
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure client introspection");
+    }
+  }
+
 protected:
   using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
   using CallbackWithRequestTypeValueVariant = std::tuple<
@@ -831,6 +859,9 @@ class Client : public ClientBase
       CallbackInfoVariant>>
   pending_requests_;
   std::mutex pending_requests_mutex_;
+
+private:
+  const rosidl_service_type_support_t * srv_type_support_handle_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/service.hpp

@@ -26,6 +26,7 @@
 #include "rcl/error_handling.h"
 #include "rcl/event_callback.h"
 #include "rcl/service.h"
+#include "rcl/service_introspection.h"
 
 #include "rmw/error_handling.h"
 #include "rmw/impl/cpp/demangle.hpp"
@@ -34,6 +35,7 @@
 #include "tracetools/tracetools.h"
 
 #include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/clock.hpp"
 #include "rclcpp/detail/cpp_callback_trampoline.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
@@ -308,11 +310,9 @@ class Service
     const std::string & service_name,
     AnyServiceCallback<ServiceT> any_callback,
     rcl_service_options_t & service_options)
-  : ServiceBase(node_handle), any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
-    using rosidl_typesupport_cpp::get_service_type_support_handle;
-    auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
-
     // rcl does the static memory allocation here
     service_handle_ = std::shared_ptr<rcl_service_t>(
       new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service)
@@ -331,7 +331,7 @@ class Service
     rcl_ret_t ret = rcl_service_init(
       service_handle_.get(),
       node_handle.get(),
-      service_type_support_handle,
+      srv_type_support_handle_,
       service_name.c_str(),
       &service_options);
     if (ret != RCL_RET_OK) {
@@ -371,8 +371,8 @@ class Service
     std::shared_ptr<rcl_node_t> node_handle,
     std::shared_ptr<rcl_service_t> service_handle,
     AnyServiceCallback<ServiceT> any_callback)
-  : ServiceBase(node_handle),
-    any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
     // check if service handle was initialized
     if (!rcl_service_is_valid(service_handle.get())) {
@@ -406,8 +406,8 @@ class Service
     std::shared_ptr<rcl_node_t> node_handle,
     rcl_service_t * service_handle,
     AnyServiceCallback<ServiceT> any_callback)
-  : ServiceBase(node_handle),
-    any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback),
+    srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
   {
     // check if service handle was initialized
     if (!rcl_service_is_valid(service_handle)) {
@@ -487,10 +487,39 @@ class Service
     }
   }
 
+  /// Configure client introspection.
+  /**
+   * \param[in] clock clock to use to generate introspection timestamps
+   * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
+   * \param[in] introspection_state the state to set introspection to
+   */
+  void
+  configure_introspection(
+    Clock::SharedPtr clock, const QoS & qos_service_event_pub,
+    rcl_service_introspection_state_t introspection_state)
+  {
+    rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
+    pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
+
+    rcl_ret_t ret = rcl_service_configure_service_introspection(
+      service_handle_.get(),
+      node_handle_.get(),
+      clock->get_clock_handle(),
+      srv_type_support_handle_,
+      pub_opts,
+      introspection_state);
+
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure service introspection");
+    }
+  }
+
 private:
   RCLCPP_DISABLE_COPY(Service)
 
   AnyServiceCallback<ServiceT> any_callback_;
+
+  const rosidl_service_type_support_t * srv_type_support_handle_;
 };
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/client.cpp

@@ -23,9 +23,11 @@
 #include "rcl/graph.h"
 #include "rcl/node.h"
 #include "rcl/wait.h"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/logging.hpp"
 
@@ -241,7 +243,6 @@ ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const vo
     user_data);
 
   if (RCL_RET_OK != ret) {
-    using rclcpp::exceptions::throw_from_rcl_error;
     throw_from_rcl_error(ret, "failed to set the on new response callback for client");
   }
 }

rclcpp/src/rclcpp/service.cpp

@@ -22,6 +22,7 @@
 
 #include "rclcpp/any_service_callback.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -131,7 +132,7 @@ ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const vo
     user_data);
 
   if (RCL_RET_OK != ret) {
-    using rclcpp::exceptions::throw_from_rcl_error;
-    throw_from_rcl_error(ret, "failed to set the on new request callback for service");
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to set the on new request callback for service");
   }
 }

rclcpp/test/rclcpp/CMakeLists.txt

@@ -508,6 +508,17 @@ if(TARGET test_service)
   )
   target_link_libraries(test_service ${PROJECT_NAME} mimick)
 endif()
+ament_add_gmock(test_service_introspection test_service_introspection.cpp)
+if(TARGET test_service)
+  ament_target_dependencies(test_service_introspection
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick)
+endif()
 # Creating and destroying nodes is slow with Connext, so this needs larger timeout.
 ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120)
 if(TARGET test_subscription)