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,8 @@ 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_introspection_c)
+ament_export_dependencies(rosidl_typesupport_c)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

rcl/include/rcl/introspection.h

@@ -0,0 +1,162 @@
+// 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/time.h"
+#include "rmw/rmw.h"
+#include "rcl/client.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;
+  bool _enabled;
+  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,
+  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,
+  const uint8_t event_type,
+  const void * ros_response_request,
+  const 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

@@ -361,6 +361,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,9 @@
   <depend>rosidl_runtime_c</depend>
   <depend>tracetools</depend>
 
+  <depend>rosidl_typesupport_introspection_c</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()
@@ -114,6 +109,14 @@ rcl_client_init(
     goto fail;
   }
 
+  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, allocator);
+
   // get actual qos, and store it
   rmw_ret_t rmw_ret = rmw_client_request_publisher_get_actual_qos(
     client->impl->rmw_handle,
@@ -252,6 +255,19 @@ 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);
+
+  rcl_ret_t rclret = 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 != rclret) {
+    RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+    return RCL_RET_ERROR;
+  }
   return RCL_RET_OK;
 }
 
@@ -283,6 +299,19 @@ rcl_take_response_with_info(
   if (!taken) {
     return RCL_RET_CLIENT_TAKE_FAILED;
   }
+
+  rcl_ret_t rclret = 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 != rclret) {
+    RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+    return RCL_RET_ERROR;
+  }
   return RCL_RET_OK;
 }
 

rcl/src/rcl/client_impl.h

@@ -0,0 +1,35 @@
+// 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__CLIENT_IMPL_H_
+#define RCL__CLIENT_IMPL_H_
+
+
+#include "rcl/client.h"
+
+#include "rcutils/stdatomic_helper.h"
+#include "rcl/introspection.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;
+  rcl_service_introspection_utils_t * introspection_utils;
+};
+
+#endif  // RCL__CLIENT_IMPL_H_