Service Introspection

rcl/CMakeLists.txt

@@ -12,6 +12,8 @@ find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
 find_package(rosidl_runtime_c REQUIRED)
 find_package(tracetools REQUIRED)
+find_package(rosidl_typesupport_introspection_c)
+find_package(rosidl_typesupport_c)
 
 include(cmake/rcl_set_symbol_visibility_hidden.cmake)
 include(cmake/get_default_rcl_logging_implementation.cmake)
@@ -64,6 +66,7 @@ set(${PROJECT_NAME}_sources
   src/rcl/validate_enclave_name.c
   src/rcl/validate_topic_name.c
   src/rcl/wait.c
+  src/rcl/introspection.c
 )
 
 add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources})
@@ -81,6 +84,8 @@ ament_target_dependencies(${PROJECT_NAME}
   ${RCL_LOGGING_IMPL}
   "rosidl_runtime_c"
   "tracetools"
+  "rosidl_typesupport_introspection_c"
+  "rosidl_typesupport_c"
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,
@@ -121,6 +126,7 @@ ament_export_dependencies(rcutils)
 ament_export_dependencies(${RCL_LOGGING_IMPL})
 ament_export_dependencies(rosidl_runtime_c)
 ament_export_dependencies(tracetools)
+ament_export_dependencies(rosidl_typesupport_c)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

rcl/include/rcl/client.h

@@ -27,6 +27,7 @@ extern "C"
 #include "rcl/event_callback.h"
 #include "rcl/macros.h"
 #include "rcl/node.h"
+#include "rcl/time.h"
 #include "rcl/visibility_control.h"
 
 /// Internal rcl client implementation struct.
@@ -47,6 +48,10 @@ typedef struct rcl_client_options_s
   /// 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`.

rcl/include/rcl/introspection.h

@@ -0,0 +1,170 @@
+// 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_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "rcl/publisher.h"
+#include "rcl/service.h"
+#include "rcl/client.h"
+#include "rcl/time.h"
+#include "rmw/rmw.h"
+#include "stdbool.h"
+
+#define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event"
+
+typedef struct rcl_service_introspection_utils_s {
+  rcl_clock_t * clock;
+  rcl_publisher_t * publisher;
+  rosidl_message_type_support_t * request_type_support;
+  rosidl_message_type_support_t * response_type_support;
+  char * service_name;
+  char * service_type_name;
+  char * service_event_topic_name;
+
+
+  // enable/disable service introspection during runtime
+  bool _enabled;
+  // enable/disable passing along service introspection content during runtime
+  bool _content_enabled;
+} rcl_service_introspection_utils_t;
+
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_service_introspection_utils_t
+rcl_get_zero_initialized_introspection_utils();
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_init(
+  rcl_service_introspection_utils_t * introspection_utils,
+  const rosidl_service_type_support_t * service_type_support,
+  const char * service_name,
+  const rcl_node_t * node,
+  const rcl_clock_t * clock,
+  rcl_allocator_t * allocator);
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_fini(
+  rcl_service_introspection_utils_t * introspection_utils,
+  rcl_allocator_t * allocator,
+  rcl_node_t *  node);
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_introspection_send_message(
+  const rcl_service_introspection_utils_t * introspection_utils,
+  uint8_t event_type,
+  const void * ros_response_request,
+  int64_t sequence_number,
+  const uint8_t uuid[16], // uuid is uint8_t but the guid is int8_t
+  const rcl_allocator_t * allocator);
+
+
+
+
+
+/*  Enables service introspection by reconstructing the introspection clock and publisher
+ *  
+ *  Does nothing and returns RCL_RET_OK if already enabled
+ */
+RCL_LOCAL
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_enable(
+    rcl_service_introspection_utils_t * introspection_utils,
+    const rcl_node_t * node,
+    rcl_allocator_t * allocator);
+
+/*  Disabled service introspection by fini-ing and freeing the introspection clock and publisher
+ *  
+ *  Does nothing and returns RCL_RET_OK if already disabled
+ *
+ *
+ *
+ */
+
+RCL_LOCAL
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_disable(
+    rcl_service_introspection_utils_t * introspection_utils,
+    rcl_node_t * node,
+    const rcl_allocator_t * allocator);
+
+
+
+
+/*
+ * Enables/disables service introspection for client/service
+ * These functions are thin wrappers around rcl_service_introspection_{enable, disable}
+ *
+ *
+ *
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_configure_service_events(
+    rcl_service_t * service,
+    rcl_node_t * node,
+    bool enable);
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_service_introspection_configure_client_events(
+    rcl_client_t * client,
+    rcl_node_t * node,
+    bool enable);
+
+
+/*
+ *
+ */
+RCL_PUBLIC
+void
+rcl_service_introspection_configure_client_content(rcl_client_t * client, bool enable);
+
+RCL_PUBLIC
+void
+rcl_service_introspection_configure_service_content(rcl_service_t * service, bool enable);
+
+
+
+// TODO(ihasdapie): Do we want some getters for if content and/or introspection enabled/disabled?
+
+
+
+
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif // RCL__SERVICE_INTROSPECTION_H_

rcl/include/rcl/service.h

@@ -17,6 +17,7 @@
 #ifndef RCL__SERVICE_H_
 #define RCL__SERVICE_H_
 
+#include "rcl/time.h"
 #ifdef __cplusplus
 extern "C"
 {
@@ -28,6 +29,7 @@ extern "C"
 #include "rcl/macros.h"
 #include "rcl/node.h"
 #include "rcl/visibility_control.h"
+// #include "rcl/introspection.h" // TODO(ihasdapie): Dependency cycle. Instead let's just put the clock into service_options.
 
 /// Internal rcl implementation struct.
 typedef struct rcl_service_impl_s rcl_service_impl_t;
@@ -47,6 +49,11 @@ typedef struct rcl_service_options_s
   /// 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 timestampes
+  rcl_clock_t * clock;
 } rcl_service_options_t;
 
 /// Return a rcl_service_t struct with members set to `NULL`.
@@ -198,6 +205,7 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node);
  *
  * - qos = rmw_qos_profile_services_default
  * - allocator = rcl_get_default_allocator()
+ * - 
  */
 RCL_PUBLIC
 RCL_WARN_UNUSED
@@ -361,6 +369,16 @@ RCL_WARN_UNUSED
 const char *
 rcl_service_get_service_name(const rcl_service_t * service);
 
+/// Get the service type name for the service
+/**
+
+ * TODO(ihasdapie): document this function
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+const char *
+rcl_service_get_service_type_name(const rosidl_service_type_support_t * service_type_support);
+
 /// Return the rcl service options.
 /**
  * This function returns the service's internal options struct.

rcl/package.xml

@@ -24,6 +24,8 @@
   <depend>rosidl_runtime_c</depend>
   <depend>tracetools</depend>
 
+  <depend>rosidl_typesupport_c</depend>
+
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rcl/src/rcl/client.c

@@ -26,21 +26,16 @@ extern "C"
 #include "rcl/node.h"
 #include "rcutils/logging_macros.h"
 #include "rcutils/macros.h"
-#include "rcutils/stdatomic_helper.h"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 #include "tracetools/tracetools.h"
+#include "rcl_interfaces/msg/service_event_type.h"
 
-#include "./common.h"
 
-struct rcl_client_impl_s
-{
-  rcl_client_options_t options;
-  rmw_qos_profile_t actual_request_publisher_qos;
-  rmw_qos_profile_t actual_response_subscription_qos;
-  rmw_client_t * rmw_handle;
-  atomic_int_least64_t sequence_number;
-};
+#include "rcl/introspection.h"
+
+#include "./common.h"
+#include "./client_impl.h"
 
 rcl_client_t
 rcl_get_zero_initialized_client()
@@ -113,6 +108,17 @@ rcl_client_init(
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
     goto fail;
   }
+
+  client->impl->introspection_utils = NULL;
+  if (options->enable_service_introspection) {
+    client->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate(
+        sizeof(rcl_service_introspection_utils_t), allocator->state);
+    *client->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils();
+    ret = rcl_service_introspection_init(
+        client->impl->introspection_utils, type_support,
+        remapped_service_name, node, options->clock, allocator);
+  }
+
 
   // get actual qos, and store it
   rmw_ret_t rmw_ret = rmw_client_request_publisher_get_actual_qos(
@@ -153,6 +159,10 @@ rcl_client_init(
     remapped_service_name);
   goto cleanup;
 fail:
+  if (client->impl->introspection_utils) {
+    allocator->deallocate(client->impl->introspection_utils, allocator->state);
+    client->impl->introspection_utils = NULL;
+  }
   if (client->impl) {
     allocator->deallocate(client->impl, allocator->state);
     client->impl = NULL;
@@ -203,6 +213,8 @@ rcl_client_get_default_options()
   // Must set the allocator and qos after because they are not a compile time constant.
   default_options.qos = rmw_qos_profile_services_default;
   default_options.allocator = rcl_get_default_allocator();
+  default_options.clock = NULL;
+  default_options.enable_service_introspection = false;
   return default_options;
 }
 
@@ -252,6 +264,23 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t
     return RCL_RET_ERROR;
   }
   rcutils_atomic_exchange_int64_t(&client->impl->sequence_number, *sequence_number);
+
+
+  if (rcl_client_get_options(client)->enable_service_introspection) {
+    rcl_ret_t ret = rcl_introspection_send_message(
+        client->impl->introspection_utils,
+        rcl_interfaces__msg__ServiceEventType__REQUEST_SENT,
+        ros_request,
+        *sequence_number,
+        client->impl->rmw_handle->writer_guid,
+        &client->impl->options.allocator);
+
+    if (RCL_RET_OK != ret) {
+      RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+      return RCL_RET_ERROR;
+    }
+  }
+
   return RCL_RET_OK;
 }
 
@@ -283,6 +312,22 @@ rcl_take_response_with_info(
   if (!taken) {
     return RCL_RET_CLIENT_TAKE_FAILED;
   }
+
+  if (rcl_client_get_options(client)->enable_service_introspection) {
+    rcl_ret_t ret = rcl_introspection_send_message(
+      client->impl->introspection_utils,
+      rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED,
+      ros_response,
+      request_header->request_id.sequence_number,
+      client->impl->rmw_handle->writer_guid,
+      &client->impl->options.allocator);
+
+    if (RCL_RET_OK != ret) {
+      RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+      return RCL_RET_ERROR;
+    }
+  }
+
   return RCL_RET_OK;
 }