Add in the APIs to enable service introspection.

This PR adds in the rcl implementation of service introspection.
In particular, what it adds in are the implementations of enabling
and disabling service introspection, as well as creating the publisher
when the introspection is enabled.

Signed-off-by: Brian Chen <brian.chen@openrobotics.org>
Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

rcl/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
 find_package(rosidl_runtime_c REQUIRED)
+find_package(service_msgs REQUIRED)
 find_package(tracetools REQUIRED)
 
 include(cmake/rcl_set_symbol_visibility_hidden.cmake)
@@ -59,6 +60,7 @@ set(${PROJECT_NAME}_sources
   src/rcl/rmw_implementation_identifier_check.c
   src/rcl/security.c
   src/rcl/service.c
+  src/rcl/service_event_publisher.c
   src/rcl/subscription.c
   src/rcl/time.c
   src/rcl/timer.c
@@ -70,6 +72,7 @@ set(${PROJECT_NAME}_sources
 add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources})
 target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
 # specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
@@ -81,6 +84,7 @@ ament_target_dependencies(${PROJECT_NAME}
   "rmw_implementation"
   ${RCL_LOGGING_IMPL}
   "rosidl_runtime_c"
+  "service_msgs"
   "tracetools"
 )
 
@@ -121,6 +125,7 @@ ament_export_dependencies(rmw)
 ament_export_dependencies(rcutils)
 ament_export_dependencies(${RCL_LOGGING_IMPL})
 ament_export_dependencies(rosidl_runtime_c)
+ament_export_dependencies(service_msgs)
 ament_export_dependencies(tracetools)
 
 if(BUILD_TESTING)

rcl/include/rcl/client.h

@@ -24,11 +24,16 @@ extern "C"
 
 #include "rosidl_runtime_c/service_type_support_struct.h"
 
+#include "rcl/allocator.h"
 #include "rcl/event_callback.h"
 #include "rcl/macros.h"
 #include "rcl/node.h"
+#include "rcl/publisher.h"
+#include "rcl/time.h"
 #include "rcl/visibility_control.h"
 
+#include "rmw/types.h"
+
 /// Internal rcl client implementation struct.
 typedef struct rcl_client_impl_s rcl_client_impl_t;
 
@@ -44,9 +49,15 @@ typedef struct rcl_client_options_s
 {
   /// Middleware quality of service settings for the client.
   rmw_qos_profile_t qos;
+  /// Publisher options for the service event publisher
+  rcl_publisher_options_t event_publisher_options;
   /// Custom allocator for the client, used for incidental allocations.
   /** For default behavior (malloc/free), use: rcl_get_default_allocator() */
   rcl_allocator_t allocator;
+  /// Enable/Disable service introspection features
+  bool enable_service_introspection;
+  /// The clock to use for service introspection message timestampes
+  rcl_clock_t * clock;
 } rcl_client_options_t;
 
 /// Return a rcl_client_t struct with members set to `NULL`.
@@ -194,7 +205,10 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node);
  * The defaults are:
  *
  * - qos = rmw_qos_profile_services_default
+ * - event_publisher_options = rcl_publisher_get_default_options()
  * - allocator = rcl_get_default_allocator()
+ * - enable_service_introspection = False
+ * - clock = NULL
  */
 RCL_PUBLIC
 RCL_WARN_UNUSED
@@ -493,6 +507,61 @@ rcl_client_set_on_new_response_callback(
   rcl_event_callback_t callback,
   const void * user_data);
 
+/// Configures service introspection features for the client.
+/**
+ * Enables or disables service introspection features for this client.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | Maybe [1]
+ * Lock-Free          | Maybe [1]
+ * <i>[1] rmw implementation defined</i>
+ *
+ * \param[in] client The client on which to configure service introspection
+ * \param[in] node The node for which the service event publisher is to be associated to
+ * \param[in] enable Whether to enable or disable service introspection for the client.
+ * \return `RCL_RET_ERROR` if the event publisher is invalid, or
+ * \return `RCL_RET_NODE_INVALID` if the given node is invalid, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if the client or node structure is invalid,
+ * \return `RCL_RET_BAD_ALLOC` if a memory allocation failed, or
+ * \return `RCL_RET_OK` if the call was successful
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_configure_client_service_events(
+  rcl_client_t * client,
+  rcl_node_t * node,
+  bool enable);
+
+/// Configures whether service introspection messages contain only metadata or content.
+/**
+ * Enables or disables whether service introspection messages contain just the metadata
+ * about the transaction, or also contains the content.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] client The client on which to configure content
+ * \param[in] enable Whether to enable or disable service introspection content
+ * \return `RCL_RET_INVALID_ARGUMENT` if the client structure is invalid,
+ * \return `RCL_RET_OK` if the call was successful
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_configure_client_service_event_message_payload(
+  rcl_client_t * client,
+  bool enable);
+
 #ifdef __cplusplus
 }
 #endif

rcl/include/rcl/node_options.h

@@ -54,6 +54,9 @@ typedef struct rcl_node_options_s
 
   /// Middleware quality of service settings for /rosout.
   rmw_qos_profile_t rosout_qos;
+
+  /// Flag to enable introspection features for services related to this node
+  bool enable_service_introspection;
 } rcl_node_options_t;
 
 /// Return the default node options in a rcl_node_options_t.

rcl/include/rcl/service.h

@@ -24,11 +24,16 @@ extern "C"
 
 #include "rosidl_runtime_c/service_type_support_struct.h"
 
+#include "rcl/allocator.h"
 #include "rcl/event_callback.h"
 #include "rcl/macros.h"
 #include "rcl/node.h"
+#include "rcl/publisher.h"
+#include "rcl/time.h"
 #include "rcl/visibility_control.h"
 
+#include "rmw/types.h"
+
 /// Internal rcl implementation struct.
 typedef struct rcl_service_impl_s rcl_service_impl_t;
 
@@ -44,9 +49,15 @@ typedef struct rcl_service_options_s
 {
   /// Middleware quality of service settings for the service.
   rmw_qos_profile_t qos;
+  /// Publisher options for the service event publisher
+  rcl_publisher_options_t event_publisher_options;
   /// Custom allocator for the service, used for incidental allocations.
   /** For default behavior (malloc/free), see: rcl_get_default_allocator() */
   rcl_allocator_t allocator;
+  /// Enable/Disable service introspection features
+  bool enable_service_introspection;
+  /// The clock to use for service introspection message timestamps
+  rcl_clock_t * clock;
 } rcl_service_options_t;
 
 /// Return a rcl_service_t struct with members set to `NULL`.
@@ -105,7 +116,7 @@ rcl_get_zero_initialized_service(void);
  *
  * The options struct allows the user to set the quality of service settings as
  * well as a custom allocator which is used when initializing/finalizing the
- * client to allocate space for incidentals, e.g. the service name string.
+ * service to allocate space for incidentals, e.g. the service name string.
  *
  * Expected usage (for C services):
  *
@@ -197,7 +208,10 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node);
  * The defaults are:
  *
  * - qos = rmw_qos_profile_services_default
+ * - event_publisher_options = rcl_publisher_get_default_options()
  * - allocator = rcl_get_default_allocator()
+ * - enable_service_introspection = False
+ * - clock = NULL
  */
 RCL_PUBLIC
 RCL_WARN_UNUSED
@@ -524,6 +538,58 @@ rcl_service_set_on_new_request_callback(
   rcl_event_callback_t callback,
   const void * user_data);
 
+/// Configure service introspection features for the service
+/**
+ * Enables or disables service introspection features for this service.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | Maybe [1]
+ * Lock-Free          | Maybe [1]
+ * <i>[1] rmw implementation defined</i>
+ *
+ * \param[in] server The server on which to enable service introspection
+ * \param[in] node The node for which the service event publisher is to be associated to
+ * \param[in] enable Whether to enable or disable service introspection
+ * \return `RCL_RET_ERROR` if the event publisher is invalid, or
+ * \return `RCL_RET_NODE_INVALID` if the given node is invalid, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if the client or node structure is invalid,
+ * \return `RCL_RET_BAD_ALLOC` if a memory allocation failed, or
+ * \return `RCL_RET_OK` if the call was successful
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_configure_server_service_events(
+  rcl_service_t * service,
+  rcl_node_t * node,
+  bool enable);
+
+/// Configure if the payload (server response) is included in service event messages.
+/**
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] server The server on which to enable/disable payload for service event messages.
+ * \param[in] enable Whether to enable or disable including the payload in the event message.
+ * \return `RCL_RET_INVALID_ARGUMENT` if the service structure is invalid,
+ * \return `RCL_RET_OK` if the call was successful
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_configure_server_service_event_message_payload(
+  rcl_service_t * service,
+  bool enable);
+
 #ifdef __cplusplus
 }
 #endif

rcl/include/rcl/service_introspection.h

@@ -0,0 +1,20 @@
+// Copyright 2022 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.
+
+#ifndef RCL__SERVICE_INTROSPECTION_H_
+#define RCL__SERVICE_INTROSPECTION_H_
+
+#define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event"
+
+#endif  // RCL__SERVICE_INTROSPECTION_H_

rcl/package.xml

@@ -26,6 +26,7 @@
   <depend>rcutils</depend>
   <depend>rmw_implementation</depend>
   <depend>rosidl_runtime_c</depend>
+  <depend>service_msgs</depend>
   <depend>tracetools</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>