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_