Service introspection (ros2#997)

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

The idea here is that by default, clients and services are
created just like they were before.  However, we add an
additional API where the user can choose to configure
service introspection, either to turn it OFF, send out
METADATA, or send out CONTENTS (and METADATA).

There are 4 different events that can get sent out if
introspection is configured for METADATA or CONTENTS;
REQUEST_SENT (from the client), REQUEST_RECEIVED (from
the service), RESPONSE_SENT (from the service), or
RESPONSE_RECEIVED (from the client).  For each of these,
a separate message is sent out a topic called
<service_name>_service_event , so an outside observer can
listen.

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,17 @@ 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/service_introspection.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;
 
@@ -493,6 +499,47 @@ 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.
+ * If the introspection state is RCL_SERVICE_INTROSPECTION_OFF, then introspection will
+ * be disabled.  If the state is RCL_SERVICE_INTROSPECTION_METADATA, the client metadata
+ * will be published.  If the state is RCL_SERVICE_INTROSPECTION_CONTENTS, then the client
+ * metadata and service request and response contents will be published.
+ *
+ * <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 client on which to configure service introspection
+ * \param[in] node valid rcl_node_t to use to create the introspection publisher
+ * \param[in] clock valid rcl_clock_t to use to generate the introspection timestamps
+ * \param[in] type_support type support library associated with this client
+ * \param[in] publisher_options options to use when creating the introspection publisher
+ * \param[in] introspection_state rcl_service_introspection_state_t describing whether
+ *            introspection should be OFF, METADATA, or CONTENTS
+ * \return #RCL_RET_OK if the call was successful, or
+ * \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
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_client_configure_service_introspection(
+  rcl_client_t * client,
+  rcl_node_t * node,
+  rcl_clock_t * clock,
+  const rosidl_service_type_support_t * type_support,
+  const rcl_publisher_options_t publisher_options,
+  rcl_service_introspection_state_t introspection_state);
+
 #ifdef __cplusplus
 }
 #endif

rcl/include/rcl/service.h

@@ -24,11 +24,17 @@ 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/service_introspection.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;
 
@@ -105,7 +111,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):
  *
@@ -524,6 +530,47 @@ 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.
+ * If the introspection state is RCL_SERVICE_INTROSPECTION_OFF, then introspection will
+ * be disabled.  If the state is RCL_SERVICE_INTROSPECTION_METADATA, the client metadata
+ * will be published.  If the state is RCL_SERVICE_INTROSPECTION_CONTENTS, then the client
+ * metadata and service request and response contents will be published.
+ *
+ * <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] service service on which to configure service introspection
+ * \param[in] node valid rcl_node_t to use to create the introspection publisher
+ * \param[in] clock valid rcl_clock_t to use to generate the introspection timestamps
+ * \param[in] type_support type support library associated with this service
+ * \param[in] publisher_options options to use when creating the introspection publisher
+ * \param[in] introspection_state rcl_service_introspection_state_t describing whether
+ *            introspection should be OFF, METADATA, or CONTENTS
+ * \return #RCL_RET_OK if the call was successful, or
+ * \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
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_configure_service_introspection(
+  rcl_service_t * service,
+  rcl_node_t * node,
+  rcl_clock_t * clock,
+  const rosidl_service_type_support_t * type_support,
+  const rcl_publisher_options_t publisher_options,
+  rcl_service_introspection_state_t introspection_state);
+
 #ifdef __cplusplus
 }
 #endif

rcl/include/rcl/service_introspection.h

@@ -0,0 +1,31 @@
+// 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"
+
+/// The introspection state for a client or service.
+typedef enum rcl_service_introspection_state_e
+{
+  /// Introspection disabled
+  RCL_SERVICE_INTROSPECTION_OFF,
+  /// Introspect metadata only
+  RCL_SERVICE_INTROSPECTION_METADATA,
+  /// Introspection metadata and contents
+  RCL_SERVICE_INTROSPECTION_CONTENTS,
+} rcl_service_introspection_state_t;
+
+#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>