From 9c0b42edace34ff9783274e4eb18ceeaacc87015 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Wed, 29 Jun 2022 16:10:33 -0700 Subject: [PATCH] service introspection hooked into all four service events Signed-off-by: Brian Chen --- rcl/CMakeLists.txt | 4 + rcl/include/rcl/service.h | 10 ++ rcl/package.xml | 1 + rcl/src/rcl/client.c | 40 +++++ rcl/src/rcl/introspection.c | 305 +++++++++++++++++------------------- rcl/src/rcl/introspection.h | 38 +++-- rcl/src/rcl/service.c | 60 +++++-- 7 files changed, 267 insertions(+), 191 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index a6c7a7cd7..4293a1730 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) @@ -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) diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index 2461bd551..f30187358 100644 --- a/rcl/include/rcl/service.h +++ b/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. diff --git a/rcl/package.xml b/rcl/package.xml index 564f0d66c..18da61d6e 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -25,6 +25,7 @@ tracetools rosidl_typesupport_introspection_c + rosidl_typesupport_c ament_cmake_gtest ament_lint_auto diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 432948d59..a5fbb7399 100644 --- a/rcl/src/rcl/client.c +++ b/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; } diff --git a/rcl/src/rcl/introspection.c b/rcl/src/rcl/introspection.c index 11c5cfe33..93cdbfbd3 100644 --- a/rcl/src/rcl/introspection.c +++ b/rcl/src/rcl/introspection.c @@ -12,11 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef RCL_INTROSPECTION_H_ #define RCL_INTROSPECTION_H_ - #include "./service_impl.h" #include "rcl/error_handling.h" @@ -24,124 +22,145 @@ #include "dlfcn.h" -#include "rcutils/macros.h" #include "rcutils/logging_macros.h" +#include "rcutils/macros.h" #include "rcutils/shared_library.h" -#include "rosidl_runtime_c/string_functions.h" #include "rosidl_runtime_c/primitives_sequence_functions.h" +#include "rosidl_runtime_c/string_functions.h" -#include "unique_identifier_msgs/msg/uuid.h" #include "builtin_interfaces/msg/time.h" #include "rcl_interfaces/msg/service_event.h" #include "rcl_interfaces/msg/service_event_type.h" +#include "rcl_interfaces/msg/service_event_info.h" +#include "unique_identifier_msgs/msg/uuid.h" #include "rosidl_typesupport_introspection_c/service_introspection.h" - +#include "rosidl_typesupport_c/type_support_map.h" #include "rosidl_typesupport_introspection_c/identifier.h" - - +#include rcl_service_introspection_utils_t -rcl_get_zero_initialized_introspection_utils () +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_name = NULL, + .service_type_name = NULL, }; return null_introspection_utils; } + 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_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) { - - // the reason why we can't use a macro and concatenate the Type to get what we want is - // because there we had always {goal status result} but here we can have any service type name... - // TODO(ihasdapie): Need to get the package and message name somehow - // probably hook into rosidl - rcl_ret_t ret; - - introspection_utils->service_name = allocator->allocate( - strlen(service_name) + 1, allocator->state); - strcpy(introspection_utils->service_name, service_name); - + rcutils_ret_t ret; + type_support_map_t * map = (type_support_map_t *)service_typesupport->data; + 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"; - - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Typesupport -----------------"); - - printf("Service typesupport identifier: %s\n", service_type_support->typesupport_identifier); - - const rosidl_service_type_support_t * introspection_type_support = get_service_typesupport_handle( - service_type_support, - // "rosidl_typesupport_introspection_cpp" // this will "work" and then segfault when trying to serialize - rosidl_typesupport_introspection_c__identifier); + const char * service_type_name = rcl_service_get_service_type_name(service_typesupport); - - if (!introspection_type_support) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get introspection typesupport handle"); - return RCL_RET_ERROR; - } - - // printf("Typesupport identifier: %s\n", introspection_type_support->typesupport_identifier); - - - - rosidl_typesupport_introspection_c__ServiceMembers * members; - - if (strcmp(introspection_type_support->typesupport_identifier, "rosidl_typesupport_introspection_c") == 0) { - members = (rosidl_typesupport_introspection_c__ServiceMembers *) introspection_type_support->data; - - } else if (strcmp(introspection_type_support->typesupport_identifier, "rosidl_typesupport_introspection_cpp") == 0) { - // TODO(ihasdapie): Grabbing cpp members (?) - /* ServiceMembers * members = - (ServiceMembers *) introspection_type_support->data; */ - members = (rosidl_typesupport_introspection_c__ServiceMembers *) introspection_type_support->data; - } - else { - printf("Unknown typesupport identifier: %s\n", introspection_type_support->typesupport_identifier); + // 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; - // unknown ts } - const rosidl_message_type_support_t * request_ts = members->request_members_->members_->members_; - const rosidl_message_type_support_t * response_ts = members->response_members_->members_->members_; - introspection_utils->request_type_support = request_ts; - introspection_utils->response_type_support = response_ts; + 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); - RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Typesupport DONE -----------------"); + *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, + 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); - char * service_event_name = (char*) allocator->zero_allocate( - strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, - sizeof(char), allocator->state); + // Make a publisher + char * service_event_name = (char *)allocator->zero_allocate( + strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1, + sizeof(char), allocator->state); strcpy(service_event_name, service_name); strcat(service_event_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); - // Make a publisher - introspection_utils->publisher = allocator->allocate(sizeof(rcl_publisher_t), allocator->state); + 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); // ROSIDL_GET_MSG_TYPE_SUPPORT gives an int?? And complier warns that it is being converted to a ptr - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - ret = rcl_publisher_init(introspection_utils->publisher, node, - service_event_typesupport, service_event_name, &publisher_options); - - - // there has to be a macro for this, right? + 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_name, + &publisher_options); if (RCL_RET_OK != ret) { RCL_SET_ERROR_MSG(rcl_get_error_string().str); return RCL_RET_ERROR; @@ -154,19 +173,16 @@ rcl_service_introspection_init( RCL_SET_ERROR_MSG(rcl_get_error_string().str); return RCL_RET_ERROR; } - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Clock initialized"); allocator->deallocate(service_event_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_allocator_t * allocator, rcl_node_t * node) { rcl_ret_t ret; ret = rcl_publisher_fini(introspection_utils->publisher, node); @@ -184,69 +200,29 @@ rcl_service_introspection_fini( allocator->deallocate(introspection_utils->clock, allocator->state); allocator->deallocate(introspection_utils->service_name, allocator->state); - return RCL_RET_OK; - } - - -rcl_ret_t +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 rmw_request_id_t * header, - const rcl_service_options_t * options) + const int64_t sequence_number, + const uint8_t uuid[16], + const rcl_allocator_t * allocator) { - // need a copy of this function for request/response (?) and also in client.c unless there's - // a utils.c or something... - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Sending introspection message -----------------"); - rcl_ret_t ret; - rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_serialized_message(); - - ret = rmw_serialized_message_init(&serialized_message, 0u, &options->allocator); // why is this 0u? - if (RMW_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return RCL_RET_ERROR; - } - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message -----------------"); + rcl_interfaces__msg__ServiceEvent msg; rcl_interfaces__msg__ServiceEvent__init(&msg); - msg.event_type = event_type; - msg.sequence_number = header->sequence_number; - rosidl_runtime_c__String__assign(&msg.service_name, introspection_utils->service_name); - - /* printf("sequence_number: %ld\n", msg.sequence_number); - printf("service_name: %s\n", msg.service_name.data); - printf("event_type: %d\n", msg.event_type); */ - - rosidl_runtime_c__String__assign(&msg.event_name, "AddTwoInts"); // TODO(ihasdapie): get this properly, the type name is hardcoded throughout right now - - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building time message -----------------"); - 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; - } - // is there a rcl method for getting the seconds too? - timestamp.sec = RCL_NS_TO_S(now); - timestamp.nanosec = now % (1000LL * 1000LL * 1000LL); - msg.stamp = timestamp; - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Building UUID message -----------------"); - unique_identifier_msgs__msg__UUID uuid; - unique_identifier_msgs__msg__UUID__init(&uuid); - memcpy(uuid.uuid, header->writer_guid, 16 * sizeof(uint8_t)); - msg.client_id = uuid; - - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: Serializing introspected message -----------------"); + rcl_interfaces__msg__ServiceEventInfo msg_info; + rcl_interfaces__msg__ServiceEventInfo__init(&msg_info); + // build serialized message + rcl_serialized_message_t serialized_message = rmw_get_zero_initialized_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_RECEIVED: @@ -267,57 +243,56 @@ rcl_introspection_send_message( } ret = rmw_serialize(ros_response_request, serialized_message_ts, &serialized_message); - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Building ServiceEvent Message: memcpying introspected message -----------------"); - // printf("serialized_message.buffer_length: %zu, serialized_message size: %zu\n", serialized_message.buffer_length, serialized_message.buffer_capacity); + 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); - // This segfaults, but thought this should be ok? since serialized_message.buffer is a uint8_t* which _should_ be the same as a byte array - // rosidl_runtime_c__octet__Sequence__copy((rosidl_runtime_c__octet__Sequence *) serialized_message.buffer, &msg.serialized_event); + // populate info message - memcpy(msg.serialized_event.data, serialized_message.buffer, serialized_message.buffer_length); + 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); - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Publishing ServiceEvent Message -----------------"); - ret = rcl_publish(introspection_utils->publisher, &msg, NULL); + 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; - // RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "--------------- Send Introspection Message: Cleanup -----------------"); + 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; + + // 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; + } - unique_identifier_msgs__msg__UUID__fini(&uuid); + // clean up + unique_identifier_msgs__msg__UUID__fini(&uuid_msg); builtin_interfaces__msg__Time__fini(×tamp); 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; } - // have to fini the request/response message here as well + rcl_interfaces__msg__ServiceEventInfo__fini(&msg_info); rcl_interfaces__msg__ServiceEvent__fini(&msg); return RCL_RET_OK; } - - - - - - - - - - - - - - - - - - - - - - #endif // RCL_INTROSPECTION_H_ diff --git a/rcl/src/rcl/introspection.h b/rcl/src/rcl/introspection.h index ba4a5c812..05bec1d26 100644 --- a/rcl/src/rcl/introspection.h +++ b/rcl/src/rcl/introspection.h @@ -12,17 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef RCL__SERVICE_INTROSPECTION_H_ #define RCL__SERVICE_INTROSPECTION_H_ -#include "rcl/time.h" #include "rcl/publisher.h" #include "rcl/service.h" +#include "rcl/time.h" #include "rmw/rmw.h" - - #define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event" typedef struct rcl_service_introspection_utils_s { @@ -31,11 +28,16 @@ typedef struct rcl_service_introspection_utils_s { rosidl_message_type_support_t * request_type_support; rosidl_message_type_support_t * response_type_support; char * service_name; + char * service_type_name; } 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, @@ -44,21 +46,37 @@ rcl_service_introspection_init( 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_node_t * node); -rcl_ret_t +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 rmw_request_id_t * header, - const rcl_service_options_t * options); + 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); + + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_disable(rcl_service_t * service); + + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_enable(rcl_service_t * service); + #endif // RCL__SERVICE_INTROSPECTION_H_ diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index f3d3b21e5..d8e8685a5 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -17,7 +17,6 @@ extern "C" { #endif -#include #include #include @@ -43,7 +42,8 @@ extern "C" #include "rosidl_runtime_c/message_type_support_struct.h" #include "rcl_interfaces/msg/service_event_type.h" -#include "rcl_interfaces/msg/service_event.h" + +#include "rosidl_typesupport_c/type_support_map.h" @@ -234,7 +234,11 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } - rcl_service_introspection_fini(service->impl->introspection_utils, &allocator, node); + 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); allocator.deallocate(service->impl, allocator.state); @@ -274,6 +278,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) { @@ -307,12 +323,6 @@ 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); - /* send_introspection_message( - service, - rcl_interfaces__msg__ServiceEventType__REQUEST_RECEIVED, - ros_request, - request_header, - options); */ bool taken = false; rmw_ret_t ret = rmw_take_request( @@ -329,6 +339,18 @@ rcl_take_request_with_info( if (!taken) { return RCL_RET_SERVICE_TAKE_FAILED; } + + 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; } @@ -361,13 +383,6 @@ 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); - // publish out the introspected content - rcl_introspection_send_message( - service->impl->introspection_utils, - rcl_interfaces__msg__ServiceEventType__RESPONSE_SENT, - ros_response, - request_header, - options); if (rmw_send_response( service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK) @@ -375,6 +390,19 @@ rcl_send_response( RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } + + // publish out the introspected content + 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; }