service introspection hooked into all four service events

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

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)
@@ -83,6 +85,7 @@ ament_target_dependencies(${PROJECT_NAME}
   "rosidl_runtime_c"
   "tracetools"
   "rosidl_typesupport_introspection_c"
+  "rosidl_typesupport_c"
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,
@@ -124,6 +127,7 @@ 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/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

@@ -25,6 +25,7 @@
   <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>

rcl/src/rcl/client.c

@@ -30,7 +30,9 @@ extern "C"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 #include "tracetools/tracetools.h"
+#include "rcl_interfaces/msg/service_event_type.h"
 
+#include "./introspection.h"
 #include "./common.h"
 
 struct rcl_client_impl_s
@@ -40,6 +42,7 @@ struct rcl_client_impl_s
   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;
 };
 
 rcl_client_t
@@ -114,6 +117,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 +263,22 @@ 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);
+
+  // TODO(ihasdapie): Writ
+  uint8_t tmp_writer_guid[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+  rcl_ret_t rclret = rcl_introspection_send_message(
+    client->impl->introspection_utils,
+    rcl_interfaces__msg__ServiceEventType__REQUEST_SENT,
+    ros_request,
+    *sequence_number,
+    tmp_writer_guid,
+    // request_header->request_id.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 +310,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,
+    request_header->request_id.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;
 }