configure service introspection via node options & parameters

Signed-off-by: Brian Chen <brian.chen@openrobotics.org>

Author: ihasdapie

rclcpp/CMakeLists.txt

@@ -78,6 +78,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/node_interfaces/node_logging.cpp
   src/rclcpp/node_interfaces/node_parameters.cpp
   src/rclcpp/node_interfaces/node_services.cpp
+  src/rclcpp/node_interfaces/node_service_introspection.cpp
   src/rclcpp/node_interfaces/node_time_source.cpp
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.cpp

rclcpp/include/rclcpp/client.hpp

@@ -16,14 +16,15 @@
 #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>

rclcpp/include/rclcpp/create_client.hpp

@@ -19,6 +19,7 @@
 #include <string>
 
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/qos.hpp"
 #include "rmw/rmw.h"
@@ -44,15 +45,15 @@ create_client(
   std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
   std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
   const std::string & service_name,
-  const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
-  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+  const rclcpp::QoS & qos,
+  rclcpp::CallbackGroup::SharedPtr group,
+  bool enable_service_introspection)
 {
   return create_client<ServiceT>(
-    node_base, node_graph, node_services,
-    service_name,
-    qos.get_rmw_qos_profile(),
-    group);
+    node_base, node_graph, node_services, node_clock, service_name,
+    qos.get_rmw_qos_profile(), group, enable_service_introspection);
 }
 
 /// Create a service client with a given type.
@@ -63,12 +64,39 @@ create_client(
   std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
   std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
+  const std::string & service_name,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::CallbackGroup::SharedPtr group,
+  bool enable_service_introspection)
+{
+  return create_client<ServiceT>(
+    node_base, node_graph, node_services, node_clock, service_name, qos_profile,
+    rcl_publisher_get_default_options().qos, group, enable_service_introspection);
+}
+
+/// Create a service client with a given type and qos profiles
+/// \internal
+template<typename ServiceT>
+typename rclcpp::Client<ServiceT>::SharedPtr
+create_client(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
   const std::string & service_name,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
+  const rmw_qos_profile_t & service_event_publisher_qos_profile,
+  rclcpp::CallbackGroup::SharedPtr group,
+  bool enable_service_introspection)
 {
   rcl_client_options_t options = rcl_client_get_default_options();
   options.qos = qos_profile;
+  if (enable_service_introspection) {
+    options.enable_service_introspection = enable_service_introspection;
+    options.clock = node_clock->get_clock()->get_clock_handle();
+    options.event_publisher_options.qos = service_event_publisher_qos_profile;
+  }
 
   auto cli = rclcpp::Client<ServiceT>::make_shared(
     node_base.get(),

rclcpp/include/rclcpp/create_service.hpp

@@ -20,6 +20,7 @@
 #include <utility>
 
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/rmw.h"
@@ -43,33 +44,65 @@ typename rclcpp::Service<ServiceT>::SharedPtr
 create_service(
   std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
   const std::string & service_name,
   CallbackT && callback,
   const rclcpp::QoS & qos,
-  rclcpp::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group,
+  bool enable_service_introspection)
 {
   return create_service<ServiceT, CallbackT>(
-    node_base, node_services, service_name,
-    std::forward<CallbackT>(callback), qos.get_rmw_qos_profile(), group);
+    node_base, node_services, node_clock, service_name, std::forward<CallbackT>(callback),
+    qos.get_rmw_qos_profile(), group, enable_service_introspection);
 }
 
-/// Create a service with a given type.
+
+/// Create a service with a given type
+/// \internal
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+create_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
+  const std::string & service_name,
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::CallbackGroup::SharedPtr group,
+  bool enable_service_introspection
+)
+{
+  return create_service<ServiceT, CallbackT>(
+    node_base, node_services, node_clock, service_name, std::forward<CallbackT>(callback),
+    qos_profile, rcl_publisher_get_default_options().qos, group, enable_service_introspection);
+}
+
+/// Create a service with a given type and qos profiles
 /// \internal
 template<typename ServiceT, typename CallbackT>
 typename rclcpp::Service<ServiceT>::SharedPtr
 create_service(
   std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
   const std::string & service_name,
   CallbackT && callback,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
+  const rmw_qos_profile_t & service_event_publisher_qos_profile,
+  rclcpp::CallbackGroup::SharedPtr group,
+  bool enable_service_introspection
+)
 {
   rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
   any_service_callback.set(std::forward<CallbackT>(callback));
 
   rcl_service_options_t service_options = rcl_service_get_default_options();
   service_options.qos = qos_profile;
+  if (enable_service_introspection) {
+    service_options.enable_service_introspection = enable_service_introspection;
+    service_options.clock = node_clock->get_clock()->get_clock_handle();
+    service_options.event_publisher_options.qos = service_event_publisher_qos_profile;
+  }
 
   auto serv = Service<ServiceT>::make_shared(
     node_base->get_shared_rcl_node_handle(),

rclcpp/include/rclcpp/node.hpp

@@ -53,6 +53,7 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_service_introspection_interface.hpp"
 #include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
@@ -283,6 +284,22 @@ class Node : public std::enable_shared_from_this<Node>
     const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
+  /// Create and return a Client.
+  /**
+   * \param[in] service_name The name on which the service is accessible.
+   * \param[in] qos Quality of service profile for client.
+   * \param[in] service_event_publisher_qos QOS profile for the service event publisher.
+   * \param[in] group Callback group to handle the reply to service calls.
+   * \return Shared pointer to the created client.
+   */
+  template<typename ServiceT>
+  typename Client<ServiceT>::SharedPtr
+  create_client(
+    const std::string & service_name,
+    const rclcpp::QoS & qos,
+    const rclcpp::QoS & service_event_publisher_qos,
+    rclcpp::CallbackGroup::SharedPtr group);
+
   /// Create and return a Service.
   /**
    * \param[in] service_name The topic to service on.
@@ -317,6 +334,25 @@ class Node : public std::enable_shared_from_this<Node>
     const rclcpp::QoS & qos = rclcpp::ServicesQoS(),
     rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
+
+  /// Create and return a Service.
+  /**
+   * \param[in] service_name The topic to service on.
+   * \param[in] callback User-defined callback function.
+   * \param[in] qos Quality of service profile for the service.
+   * \param[in] service_event_publisher_qos QOS profile for the service event publisher.
+   * \param[in] group Callback group to call the service.
+   * \return Shared pointer to the created service.
+   */
+  template<typename ServiceT, typename CallbackT>
+  typename rclcpp::Service<ServiceT>::SharedPtr
+  create_service(
+    const std::string & service_name,
+    CallbackT && callback,
+    const rclcpp::QoS & qos,
+    const rclcpp::QoS & service_event_publisher_qos,
+    rclcpp::CallbackGroup::SharedPtr group);
+
   /// Create and return a GenericPublisher.
   /**
    * The returned pointer will never be empty, but this function can throw various exceptions, for
@@ -1439,6 +1475,11 @@ class Node : public std::enable_shared_from_this<Node>
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
   get_node_services_interface();
 
+  /// Return the Node's internal NodeServicesInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeServiceIntrospectionInterface::SharedPtr
+  get_node_service_introspection_interface();
+
   /// Return the Node's internal NodeWaitablesInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
@@ -1587,6 +1628,7 @@ class Node : public std::enable_shared_from_this<Node>
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
+  rclcpp::node_interfaces::NodeServiceIntrospectionInterface::SharedPtr node_service_introspection_;
 
   const rclcpp::NodeOptions node_options_;
   const std::string sub_namespace_;

rclcpp/include/rclcpp/node_impl.hpp

@@ -143,13 +143,41 @@ Node::create_client(
   const rclcpp::QoS & qos,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_client<ServiceT>(
+  typename Client<ServiceT>::SharedPtr cli = rclcpp::create_client<ServiceT>(
     node_base_,
     node_graph_,
     node_services_,
+    node_clock_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     qos,
-    group);
+    group,
+    node_options_.enable_service_introspection());
+
+  node_service_introspection_->register_client(cli);
+  return cli;
+}
+
+template<typename ServiceT>
+typename Client<ServiceT>::SharedPtr
+Node::create_client(
+  const std::string & service_name,
+  const rclcpp::QoS & qos,
+  const rclcpp::QoS & service_event_publisher_qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  typename Client<ServiceT>::SharedPtr cli = rclcpp::create_client<ServiceT>(
+    node_base_,
+    node_graph_,
+    node_services_,
+    node_clock_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    qos.get_rmw_qos_profile(),
+    service_event_publisher_qos.get_rmw_qos_profile(),
+    group,
+    node_options_.enable_service_introspection());
+
+  node_service_introspection_->register_client(cli);
+  return cli;
 }
 
 template<typename ServiceT>
@@ -159,13 +187,18 @@ Node::create_client(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_client<ServiceT>(
+  typename Client<ServiceT>::SharedPtr cli = rclcpp::create_client<ServiceT>(
     node_base_,
     node_graph_,
     node_services_,
+    node_clock_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     qos_profile,
-    group);
+    group,
+    node_options_.enable_service_introspection());
+
+  node_service_introspection_->register_client(cli);
+  return cli;
 }
 
 template<typename ServiceT, typename CallbackT>
@@ -176,13 +209,42 @@ Node::create_service(
   const rclcpp::QoS & qos,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_service<ServiceT, CallbackT>(
+  typename rclcpp::Service<ServiceT>::SharedPtr serv = rclcpp::create_service<ServiceT, CallbackT>(
     node_base_,
     node_services_,
+    node_clock_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     std::forward<CallbackT>(callback),
     qos,
-    group);
+    group,
+    node_options_.enable_service_introspection());
+
+  node_service_introspection_->register_service(serv);
+  return serv;
+}
+
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+Node::create_service(
+  const std::string & service_name,
+  CallbackT && callback,
+  const rclcpp::QoS & qos,
+  const rclcpp::QoS & service_event_publisher_qos,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  typename rclcpp::Service<ServiceT>::SharedPtr serv = rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_,
+    node_services_,
+    node_clock_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
+    qos.get_rmw_qos_profile(),
+    service_event_publisher_qos.get_rmw_qos_profile(),
+    group,
+    node_options_.enable_service_introspection());
+
+  node_service_introspection_->register_service(serv);
+  return serv;
 }
 
 template<typename ServiceT, typename CallbackT>
@@ -193,13 +255,19 @@ Node::create_service(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_service<ServiceT, CallbackT>(
+  typename rclcpp::Service<ServiceT>::SharedPtr serv = rclcpp::create_service<ServiceT, CallbackT>(
     node_base_,
     node_services_,
+    node_clock_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     std::forward<CallbackT>(callback),
     qos_profile,
-    group);
+    group,
+    node_options_.enable_service_introspection()
+  );
+
+  node_service_introspection_->register_service(serv);
+  return serv;
 }
 
 template<typename AllocatorT>

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -108,7 +108,8 @@ class NodeParameters : public NodeParametersInterface
     const rclcpp::QoS & parameter_event_qos,
     const rclcpp::PublisherOptionsBase & parameter_event_publisher_options,
     bool allow_undeclared_parameters,
-    bool automatically_declare_parameters_from_overrides);
+    bool automatically_declare_parameters_from_overrides,
+    bool enable_service_introspection_for_parameter_service);
 
   RCLCPP_PUBLIC
   virtual