diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt
index 11462f119..6b48615dd 100644
--- a/rcl/CMakeLists.txt
+++ b/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)
diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h
index 224f74b8c..f7d28651d 100644
--- a/rcl/include/rcl/client.h
+++ b/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`.
diff --git a/rcl/include/rcl/introspection.h b/rcl/include/rcl/introspection.h
new file mode 100644
index 000000000..58cf8b0a1
--- /dev/null
+++ b/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_
diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h
index 2461bd551..0fad89ee9 100644
--- a/rcl/include/rcl/service.h
+++ b/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.
diff --git a/rcl/package.xml b/rcl/package.xml
index 03df8701b..6679c1f25 100644
--- a/rcl/package.xml
+++ b/rcl/package.xml
@@ -24,6 +24,8 @@
rosidl_runtime_c
tracetools
+ rosidl_typesupport_c
+
ament_cmake_gtest
ament_lint_auto
ament_lint_common
diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c
index 432948d59..9a1c3828a 100644
--- a/rcl/src/rcl/client.c
+++ b/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;
}
diff --git a/rcl/src/rcl/client_impl.h b/rcl/src/rcl/client_impl.h
new file mode 100644
index 000000000..8adc64208
--- /dev/null
+++ b/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_
diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c
new file mode 100644
index 000000000..fa98029fd
--- /dev/null
+++ b/rcl/src/rcl/introspection.c
@@ -0,0 +1,383 @@
+// 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.
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "rcl/introspection.h"
+
+#include
+
+#include "./client_impl.h"
+#include "./service_impl.h"
+#include "builtin_interfaces/msg/time.h"
+#include "dlfcn.h"
+#include "rcl/error_handling.h"
+#include "rcl_interfaces/msg/service_event.h"
+#include "rcl_interfaces/msg/service_event_info.h"
+#include "rcl_interfaces/msg/service_event_type.h"
+#include "rcutils/logging_macros.h"
+#include "rcutils/macros.h"
+#include "rcutils/shared_library.h"
+#include "rmw/error_handling.h"
+#include "rosidl_runtime_c/primitives_sequence_functions.h"
+#include "rosidl_runtime_c/string_functions.h"
+#include "rosidl_typesupport_c/type_support_map.h"
+#include "unique_identifier_msgs/msg/uuid.h"
+
+rcl_service_introspection_utils_t rcl_get_zero_initialized_introspection_utils()
+{
+ static rcl_service_introspection_utils_t null_introspection_utils = {
+ .response_type_support = NULL,
+ .request_type_support = NULL,
+ .publisher = NULL,
+ .clock = NULL,
+ .service_name = NULL,
+ .service_type_name = NULL,
+ .service_event_topic_name = NULL,
+ ._enabled = true,
+ ._content_enabled = true,
+ };
+ return null_introspection_utils;
+}
+
+rcl_ret_t rcl_service_typesupport_to_message_typesupport(
+ const rosidl_service_type_support_t * service_typesupport,
+ rosidl_message_type_support_t ** request_typesupport,
+ rosidl_message_type_support_t ** response_typesupport,
+ const rcl_allocator_t * allocator)
+{
+ rcutils_ret_t ret;
+ type_support_map_t * map = (type_support_map_t *)service_typesupport->data;
+ // TODO(ihasdapie): change to #define or some constant
+ const char * typesupport_library_fmt = "lib%s__rosidl_typesupport_c.so";
+ const char * service_message_fmt = // package_name, type name, Request/Response
+ "rosidl_typesupport_c__get_message_type_support_handle__%s__srv__%s_%s";
+
+ const char * service_type_name = rcl_service_get_service_type_name(service_typesupport);
+
+ // build out typesupport library and symbol names
+ char * typesupport_library_name = allocator->allocate(
+ sizeof(char) * ((strlen(typesupport_library_fmt) - 2) + strlen(map->package_name) + 1),
+ allocator->state);
+ char * request_message_symbol = allocator->allocate(
+ sizeof(char) * ((strlen(service_message_fmt) - 6) + strlen(map->package_name) +
+ strlen(service_type_name) + strlen("Request") + 1),
+ allocator->state);
+ char * response_message_symbol = allocator->allocate(
+ sizeof(char) * ((strlen(service_message_fmt) - 6) + strlen(map->package_name) +
+ strlen(service_type_name) + strlen("Request") + 1),
+ allocator->state);
+
+ sprintf(typesupport_library_name, typesupport_library_fmt, map->package_name);
+ sprintf(
+ request_message_symbol, service_message_fmt, map->package_name, service_type_name, "Request");
+ sprintf(
+ response_message_symbol, service_message_fmt, map->package_name, service_type_name, "Response");
+
+ rcutils_shared_library_t typesupport_library = rcutils_get_zero_initialized_shared_library();
+ ret = rcutils_load_shared_library(&typesupport_library, typesupport_library_name, *allocator);
+ if (RCUTILS_RET_OK != ret) {
+ return RCL_RET_ERROR;
+ }
+
+ rosidl_message_type_support_t * (*req_typesupport_func_handle)() =
+ (rosidl_message_type_support_t * (*)())
+ rcutils_get_symbol(&typesupport_library, request_message_symbol);
+ RCL_CHECK_FOR_NULL_WITH_MSG(
+ req_typesupport_func_handle, "Looking up request type support failed", return RCL_RET_ERROR);
+
+ rosidl_message_type_support_t * (*resp_typesupport_func_handle)() =
+ (rosidl_message_type_support_t * (*)())
+ rcutils_get_symbol(&typesupport_library, response_message_symbol);
+ RCL_CHECK_FOR_NULL_WITH_MSG(
+ resp_typesupport_func_handle, "Looking up response type support failed", return RCL_RET_ERROR);
+
+ *request_typesupport = req_typesupport_func_handle();
+ *response_typesupport = resp_typesupport_func_handle();
+ allocator->deallocate(typesupport_library_name, allocator->state);
+ allocator->deallocate(request_message_symbol, allocator->state);
+ allocator->deallocate(response_message_symbol, allocator->state);
+
+ return RCL_RET_OK;
+}
+
+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_ret_t ret;
+
+ introspection_utils->service_name =
+ allocator->allocate(strlen(service_name) + 1, allocator->state);
+ strcpy(introspection_utils->service_name, service_name);
+
+ const char * service_type_name = rcl_service_get_service_type_name(service_type_support);
+ introspection_utils->service_type_name =
+ allocator->allocate(strlen(service_type_name) + 1, allocator->state);
+ strcpy(introspection_utils->service_type_name, service_type_name);
+
+ rcl_service_typesupport_to_message_typesupport(
+ service_type_support, &introspection_utils->request_type_support,
+ &introspection_utils->response_type_support, allocator);
+
+ // Make a publisher
+ char * service_event_topic_name = (char *)allocator->zero_allocate(
+ strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, sizeof(char),
+ allocator->state);
+ strcpy(service_event_topic_name, service_name);
+ strcat(service_event_topic_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX);
+
+ introspection_utils->service_event_topic_name =
+ allocator->allocate(strlen(service_event_topic_name) + 1, allocator->state);
+
+ strcpy(introspection_utils->service_event_topic_name, service_event_topic_name);
+
+ introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state);
+ *introspection_utils->publisher = rcl_get_zero_initialized_publisher();
+ const rosidl_message_type_support_t * service_event_typesupport =
+ ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent);
+
+ rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+ ret = rcl_publisher_init(
+ introspection_utils->publisher, node, service_event_typesupport, service_event_topic_name,
+ &publisher_options);
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+
+ // make a clock
+ introspection_utils->clock = clock;
+ allocator->deallocate(service_event_topic_name, allocator->state);
+
+ return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_service_introspection_fini(
+ rcl_service_introspection_utils_t * introspection_utils, rcl_allocator_t * allocator,
+ rcl_node_t * node)
+{
+ rcl_ret_t ret;
+ ret = rcl_publisher_fini(introspection_utils->publisher, node);
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+
+ allocator->deallocate(introspection_utils->publisher, allocator->state);
+ allocator->deallocate(introspection_utils->service_name, allocator->state);
+ allocator->deallocate(introspection_utils->service_event_topic_name, allocator->state);
+ allocator->deallocate(introspection_utils->service_type_name, allocator->state);
+
+ introspection_utils->publisher = NULL;
+ introspection_utils->service_name = NULL;
+ introspection_utils->service_event_topic_name = NULL;
+ introspection_utils->service_type_name = NULL;
+
+ return RCL_RET_OK;
+}
+
+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],
+ const rcl_allocator_t * allocator)
+{
+ // Early exit of service introspection if it isn't enabled
+ // TODO(ihasdapie): Different return code?
+ if (!introspection_utils->_enabled) return RCL_RET_OK;
+
+ rcl_ret_t ret;
+
+ rcl_interfaces__msg__ServiceEvent msg;
+ rcl_interfaces__msg__ServiceEvent__init(&msg);
+
+ rcl_interfaces__msg__ServiceEventInfo msg_info;
+ rcl_interfaces__msg__ServiceEventInfo__init(&msg_info);
+
+ rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message();
+ if (introspection_utils->_content_enabled) {
+ // build serialized message
+ ret = rmw_serialized_message_init(&serialized_message, 0U, allocator);
+ rosidl_message_type_support_t * serialized_message_ts;
+ switch (event_type) {
+ case rcl_interfaces__msg__ServiceEventType__REQUEST_SENT:
+ case rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED:
+ serialized_message_ts = introspection_utils->request_type_support;
+ break;
+ case rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT:
+ case rcl_interfaces__msg__ServiceEventType__RESPONSE_RECEIVED:
+ serialized_message_ts = introspection_utils->response_type_support;
+ break;
+ default:
+ RCL_SET_ERROR_MSG("Invalid event type");
+ return RCL_RET_ERROR;
+ }
+
+ ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message);
+ if (RMW_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+ rosidl_runtime_c__octet__Sequence__init(
+ &msg.serialized_event, serialized_message.buffer_length);
+ memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length);
+ }
+
+ // populate info message
+ msg_info.event_type = event_type;
+ msg_info.sequence_number = sequence_number;
+ rosidl_runtime_c__String__assign(&msg_info.service_name, introspection_utils->service_name);
+ rosidl_runtime_c__String__assign(&msg_info.event_name, introspection_utils->service_type_name);
+
+ builtin_interfaces__msg__Time timestamp;
+ builtin_interfaces__msg__Time__init(×tamp);
+
+ rcl_time_point_value_t now;
+ ret = rcl_clock_get_now(introspection_utils->clock, &now);
+ if (RMW_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+ timestamp.sec = RCL_NS_TO_S(now);
+ timestamp.nanosec = now % (1000LL * 1000LL * 1000LL);
+ msg_info.stamp = timestamp;
+
+ unique_identifier_msgs__msg__UUID uuid_msg;
+ unique_identifier_msgs__msg__UUID__init(&uuid_msg);
+ memcpy(uuid_msg.uuid, uuid, 16 * sizeof(uint8_t));
+ msg_info.client_id = uuid_msg;
+
+ msg.info = msg_info;
+
+ // and publish it out!
+ ret = rcl_publish(introspection_utils->publisher, &msg, NULL);
+ if (RMW_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+
+ // clean up
+ if (introspection_utils->_content_enabled) {
+ ret = rmw_serialized_message_fini(&serialized_message);
+ if (RMW_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+ }
+
+ rcl_interfaces__msg__ServiceEvent__fini(&msg);
+ return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_service_introspection_enable(
+ rcl_service_introspection_utils_t * introspection_utils, const rcl_node_t * node,
+ rcl_allocator_t * allocator)
+{
+ rcl_ret_t ret;
+
+ introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state);
+ *introspection_utils->publisher = rcl_get_zero_initialized_publisher();
+ const rosidl_message_type_support_t * service_event_typesupport =
+ ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, ServiceEvent);
+
+ rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+ ret = rcl_publisher_init(
+ introspection_utils->publisher, node, service_event_typesupport,
+ introspection_utils->service_event_topic_name, &publisher_options);
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+
+ introspection_utils->_enabled = true;
+ return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_service_introspection_disable(
+ rcl_service_introspection_utils_t * introspection_utils, rcl_node_t * node,
+ const rcl_allocator_t * allocator)
+{
+ rcl_ret_t ret;
+ ret = rcl_publisher_fini(introspection_utils->publisher, node);
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+
+ allocator->deallocate(introspection_utils->publisher, allocator->state);
+ introspection_utils->publisher = NULL;
+
+ introspection_utils->_enabled = false;
+ return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_service_introspection_configure_service_events(
+ rcl_service_t * service, rcl_node_t * node, bool enable)
+{
+ rcl_service_introspection_utils_t * introspection_utils = service->impl->introspection_utils;
+ rcl_ret_t ret;
+
+ if (enable) {
+ ret = rcl_service_introspection_enable(
+ introspection_utils, node, &service->impl->options.allocator);
+ } else {
+ ret = rcl_service_introspection_disable(
+ introspection_utils, node, &service->impl->options.allocator);
+ }
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+ return RCL_RET_OK;
+}
+
+rcl_ret_t rcl_service_introspection_configure_client_events(
+ rcl_client_t * client, rcl_node_t * node, bool enable)
+{
+ rcl_service_introspection_utils_t * introspection_utils = client->impl->introspection_utils;
+ rcl_ret_t ret;
+
+ if (enable) {
+ ret =
+ rcl_service_introspection_enable(introspection_utils, node, &client->impl->options.allocator);
+ } else {
+ ret = rcl_service_introspection_disable(
+ introspection_utils, node, &client->impl->options.allocator);
+ }
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+ return RCL_RET_OK;
+}
+
+void rcl_service_introspection_configure_client_content(rcl_client_t * client, bool enable)
+{
+ client->impl->introspection_utils->_content_enabled = enable;
+}
+
+void rcl_service_introspection_configure_service_content(rcl_service_t * service, bool enable)
+{
+ service->impl->introspection_utils->_content_enabled = enable;
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c
index e1bdb0174..752bf9e98 100644
--- a/rcl/src/rcl/service.c
+++ b/rcl/src/rcl/service.c
@@ -17,26 +17,38 @@ extern "C"
{
#endif
-#include "rcl/service.h"
-
#include
#include
+#include "rcl/publisher.h"
+
+#include "rcl/time.h"
+#include "rcl/types.h"
#include "rcl/error_handling.h"
#include "rcl/node.h"
-#include "rcutils/logging_macros.h"
-#include "rcutils/macros.h"
+#include "rcl/graph.h"
+
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
+
+#include "rcutils/logging_macros.h"
+#include "rcutils/macros.h"
+
#include "tracetools/tracetools.h"
-struct rcl_service_impl_s
-{
- rcl_service_options_t options;
- rmw_qos_profile_t actual_request_subscription_qos;
- rmw_qos_profile_t actual_response_publisher_qos;
- rmw_service_t * rmw_handle;
-};
+#include "rcl/introspection.h"
+
+#include "rosidl_runtime_c/service_type_support_struct.h"
+#include "rosidl_runtime_c/message_type_support_struct.h"
+
+#include "rcl_interfaces/msg/service_event_type.h"
+
+#include "rosidl_typesupport_c/type_support_map.h"
+
+
+
+#include "./service_impl.h"
+
rcl_service_t
rcl_get_zero_initialized_service()
@@ -51,7 +63,8 @@ rcl_service_init(
const rcl_node_t * node,
const rosidl_service_type_support_t * type_support,
const char * service_name,
- const rcl_service_options_t * options)
+ const rcl_service_options_t * options
+ ) // user provide clock
{
RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT);
@@ -82,6 +95,7 @@ rcl_service_init(
// Expand and remap the given service name.
char * remapped_service_name = NULL;
+
rcl_ret_t ret = rcl_node_resolve_name(
node,
service_name,
@@ -97,12 +111,14 @@ rcl_service_init(
}
goto cleanup;
}
+
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name);
// Allocate space for the implementation struct.
service->impl = (rcl_service_impl_t *)allocator->allocate(
sizeof(rcl_service_impl_t), allocator->state);
+
RCL_CHECK_FOR_NULL_WITH_MSG(
service->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup);
@@ -112,6 +128,8 @@ rcl_service_init(
"Warning: Setting QoS durability to 'transient local' for service servers "
"can cause them to receive requests from clients that have since terminated.");
}
+
+
// Fill out implementation struct.
// rmw handle (create rmw service)
// TODO(wjwwood): pass along the allocator to rmw when it supports it
@@ -124,6 +142,28 @@ rcl_service_init(
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
goto fail;
}
+
+ service->impl->introspection_utils = NULL;
+ if (options->enable_service_introspection) {
+ service->impl->introspection_utils = (rcl_service_introspection_utils_t *) allocator->allocate(
+ sizeof(rcl_service_introspection_utils_t), allocator->state);
+ *service->impl->introspection_utils = rcl_get_zero_initialized_introspection_utils();
+ ret = rcl_service_introspection_init(
+ service->impl->introspection_utils,
+ type_support,
+ remapped_service_name,
+ node,
+ options->clock,
+ allocator
+ );
+ }
+
+
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+ goto fail;
+ }
+
// get actual qos, and store it
rmw_ret_t rmw_ret = rmw_service_request_subscription_get_actual_qos(
service->impl->rmw_handle,
@@ -162,6 +202,10 @@ rcl_service_init(
goto cleanup;
fail:
if (service->impl) {
+ if (service->impl->introspection_utils) {
+ allocator->deallocate(service->impl->introspection_utils, allocator->state);
+ service->impl->introspection_utils = NULL;
+ }
allocator->deallocate(service->impl, allocator->state);
service->impl = NULL;
}
@@ -198,9 +242,25 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node)
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
result = RCL_RET_ERROR;
}
+
+ if (service->impl->introspection_utils) {
+ ret = rcl_service_introspection_fini(service->impl->introspection_utils, &allocator, node);
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+ result = RCL_RET_ERROR;
+ }
+ allocator.deallocate(service->impl->introspection_utils, allocator.state);
+ service->impl->introspection_utils = NULL;
+ }
+
allocator.deallocate(service->impl, allocator.state);
service->impl = NULL;
+ if (ret != RCL_RET_OK) {
+ RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+ result = RCL_RET_ERROR;
+ }
}
+
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized");
return result;
}
@@ -213,6 +273,8 @@ rcl_service_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;
}
@@ -229,6 +291,18 @@ rcl_service_get_service_name(const rcl_service_t * service)
#define _service_get_options(service) & service->impl->options
+const char *
+rcl_service_get_service_type_name(const rosidl_service_type_support_t * service_type_support)
+{
+ type_support_map_t * map = (type_support_map_t *)service_type_support->data;
+ // By inspection all of the symbol_name(s) end in the service type name
+ // We can do this because underscores not allowed in service/msg/action names
+ char* service_type_name = strrchr(map->symbol_name[0], '_');
+ service_type_name++;
+ return service_type_name;
+}
+
+
const rcl_service_options_t *
rcl_service_get_options(const rcl_service_t * service)
{
@@ -262,6 +336,7 @@ rcl_take_request_with_info(
const rcl_service_options_t * options = rcl_service_get_options(service);
RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR);
+
bool taken = false;
rmw_ret_t ret = rmw_take_request(
service->impl->rmw_handle, request_header, ros_request, &taken);
@@ -277,6 +352,20 @@ rcl_take_request_with_info(
if (!taken) {
return RCL_RET_SERVICE_TAKE_FAILED;
}
+
+ if (rcl_service_get_options(service)->enable_service_introspection) {
+ rcl_ret_t rclret = rcl_introspection_send_message(
+ service->impl->introspection_utils,
+ rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED,
+ ros_request,
+ request_header->request_id.sequence_number,
+ request_header->request_id.writer_guid,
+ &options->allocator);
+ if (RCL_RET_OK != rclret) {
+ RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+ }
return RCL_RET_OK;
}
@@ -293,6 +382,7 @@ rcl_take_request(
return ret;
}
+
rcl_ret_t
rcl_send_response(
const rcl_service_t * service,
@@ -308,12 +398,29 @@ rcl_send_response(
const rcl_service_options_t * options = rcl_service_get_options(service);
RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR);
+
if (rmw_send_response(
service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK)
{
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
return RCL_RET_ERROR;
}
+
+ // publish out the introspected content
+
+ if (rcl_service_get_options(service)->enable_service_introspection) {
+ rcl_ret_t ret = rcl_introspection_send_message(
+ service->impl->introspection_utils,
+ rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT,
+ ros_response,
+ request_header->sequence_number,
+ request_header->writer_guid,
+ &options->allocator);
+ if (RCL_RET_OK != ret) {
+ RCL_SET_ERROR_MSG(rcl_get_error_string().str);
+ return RCL_RET_ERROR;
+ }
+ }
return RCL_RET_OK;
}
diff --git a/rcl/src/rcl/service_impl.h b/rcl/src/rcl/service_impl.h
new file mode 100644
index 000000000..e27923a3e
--- /dev/null
+++ b/rcl/src/rcl/service_impl.h
@@ -0,0 +1,32 @@
+// 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_IMPL_H_
+#define RCL__SERVICE_IMPL_H_
+
+
+#include "rcl/service.h"
+#include "rcl/introspection.h"
+
+struct rcl_service_impl_s
+{
+ rcl_service_options_t options;
+ rmw_qos_profile_t actual_request_subscription_qos;
+ rmw_qos_profile_t actual_response_publisher_qos;
+ rmw_service_t * rmw_handle;
+ rcl_service_introspection_utils_t * introspection_utils;
+};
+
+#endif // RCL__SERVICE_IMPL_H_