From d1661316d741f68cb389504530d209ba75e7744f Mon Sep 17 00:00:00 2001 From: Brian Date: Tue, 28 Feb 2023 13:41:57 -0500 Subject: [PATCH] Service introspection (#997) * Add in the APIs to enable service introspection. This PR adds in the rcl implementation of service introspection. In particular, what it adds in are the implementations of enabling and disabling service introspection, as well as creating the publisher when the introspection is enabled. The idea here is that by default, clients and services are created just like they were before. However, we add an additional API where the user can choose to configure service introspection, either to turn it OFF, send out METADATA, or send out CONTENTS (and METADATA). There are 4 different events that can get sent out if introspection is configured for METADATA or CONTENTS; REQUEST_SENT (from the client), REQUEST_RECEIVED (from the service), RESPONSE_SENT (from the service), or RESPONSE_RECEIVED (from the client). For each of these, a separate message is sent out a topic called _service_event , so an outside observer can listen. Signed-off-by: Brian Chen Signed-off-by: Ivan Santiago Paunovic Signed-off-by: Chris Lalancette --- rcl/CMakeLists.txt | 5 + rcl/include/rcl/client.h | 47 ++ rcl/include/rcl/service.h | 49 +- rcl/include/rcl/service_introspection.h | 31 + rcl/package.xml | 1 + rcl/src/rcl/client.c | 190 ++++- rcl/src/rcl/service.c | 183 ++++- rcl/src/rcl/service_event_publisher.c | 294 ++++++++ rcl/src/rcl/service_event_publisher.h | 214 ++++++ rcl/test/CMakeLists.txt | 9 + rcl/test/rcl/test_service_event_publisher.cpp | 702 ++++++++++++++++++ rcl/test/rcl/wait_for_entity_helpers.cpp | 27 + rcl/test/rcl/wait_for_entity_helpers.hpp | 8 + 13 files changed, 1702 insertions(+), 58 deletions(-) create mode 100644 rcl/include/rcl/service_introspection.h create mode 100644 rcl/src/rcl/service_event_publisher.c create mode 100644 rcl/src/rcl/service_event_publisher.h create mode 100644 rcl/test/rcl/test_service_event_publisher.cpp diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 778e09084..a2aa1d65a 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rosidl_runtime_c REQUIRED) +find_package(service_msgs REQUIRED) find_package(tracetools REQUIRED) include(cmake/rcl_set_symbol_visibility_hidden.cmake) @@ -59,6 +60,7 @@ set(${PROJECT_NAME}_sources src/rcl/rmw_implementation_identifier_check.c src/rcl/security.c src/rcl/service.c + src/rcl/service_event_publisher.c src/rcl/subscription.c src/rcl/time.c src/rcl/timer.c @@ -70,6 +72,7 @@ set(${PROJECT_NAME}_sources add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources}) target_include_directories(${PROJECT_NAME} PUBLIC "$" + "$" "$") # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} @@ -81,6 +84,7 @@ ament_target_dependencies(${PROJECT_NAME} "rmw_implementation" ${RCL_LOGGING_IMPL} "rosidl_runtime_c" + "service_msgs" "tracetools" ) @@ -121,6 +125,7 @@ ament_export_dependencies(rmw) ament_export_dependencies(rcutils) ament_export_dependencies(${RCL_LOGGING_IMPL}) ament_export_dependencies(rosidl_runtime_c) +ament_export_dependencies(service_msgs) ament_export_dependencies(tracetools) if(BUILD_TESTING) diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h index 224f74b8c..f7995fabc 100644 --- a/rcl/include/rcl/client.h +++ b/rcl/include/rcl/client.h @@ -24,11 +24,17 @@ extern "C" #include "rosidl_runtime_c/service_type_support_struct.h" +#include "rcl/allocator.h" #include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/node.h" +#include "rcl/publisher.h" +#include "rcl/service_introspection.h" +#include "rcl/time.h" #include "rcl/visibility_control.h" +#include "rmw/types.h" + /// Internal rcl client implementation struct. typedef struct rcl_client_impl_s rcl_client_impl_t; @@ -493,6 +499,47 @@ rcl_client_set_on_new_response_callback( rcl_event_callback_t callback, const void * user_data); +/// Configures service introspection features for the client. +/** + * Enables or disables service introspection features for this client. + * If the introspection state is RCL_SERVICE_INTROSPECTION_OFF, then introspection will + * be disabled. If the state is RCL_SERVICE_INTROSPECTION_METADATA, the client metadata + * will be published. If the state is RCL_SERVICE_INTROSPECTION_CONTENTS, then the client + * metadata and service request and response contents will be published. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] client client on which to configure service introspection + * \param[in] node valid rcl_node_t to use to create the introspection publisher + * \param[in] clock valid rcl_clock_t to use to generate the introspection timestamps + * \param[in] type_support type support library associated with this client + * \param[in] publisher_options options to use when creating the introspection publisher + * \param[in] introspection_state rcl_service_introspection_state_t describing whether + * introspection should be OFF, METADATA, or CONTENTS + * \return #RCL_RET_OK if the call was successful, or + * \return #RCL_RET_ERROR if the event publisher is invalid, or + * \return #RCL_RET_NODE_INVALID if the given node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if the client or node structure is invalid, + * \return #RCL_RET_BAD_ALLOC if a memory allocation failed + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_client_configure_service_introspection( + rcl_client_t * client, + rcl_node_t * node, + rcl_clock_t * clock, + const rosidl_service_type_support_t * type_support, + const rcl_publisher_options_t publisher_options, + rcl_service_introspection_state_t introspection_state); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index 2461bd551..dec7dc913 100644 --- a/rcl/include/rcl/service.h +++ b/rcl/include/rcl/service.h @@ -24,11 +24,17 @@ extern "C" #include "rosidl_runtime_c/service_type_support_struct.h" +#include "rcl/allocator.h" #include "rcl/event_callback.h" #include "rcl/macros.h" #include "rcl/node.h" +#include "rcl/publisher.h" +#include "rcl/service_introspection.h" +#include "rcl/time.h" #include "rcl/visibility_control.h" +#include "rmw/types.h" + /// Internal rcl implementation struct. typedef struct rcl_service_impl_s rcl_service_impl_t; @@ -105,7 +111,7 @@ rcl_get_zero_initialized_service(void); * * The options struct allows the user to set the quality of service settings as * well as a custom allocator which is used when initializing/finalizing the - * client to allocate space for incidentals, e.g. the service name string. + * service to allocate space for incidentals, e.g. the service name string. * * Expected usage (for C services): * @@ -524,6 +530,47 @@ rcl_service_set_on_new_request_callback( rcl_event_callback_t callback, const void * user_data); +/// Configure service introspection features for the service. +/** + * Enables or disables service introspection features for this service. + * If the introspection state is RCL_SERVICE_INTROSPECTION_OFF, then introspection will + * be disabled. If the state is RCL_SERVICE_INTROSPECTION_METADATA, the client metadata + * will be published. If the state is RCL_SERVICE_INTROSPECTION_CONTENTS, then the client + * metadata and service request and response contents will be published. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] service service on which to configure service introspection + * \param[in] node valid rcl_node_t to use to create the introspection publisher + * \param[in] clock valid rcl_clock_t to use to generate the introspection timestamps + * \param[in] type_support type support library associated with this service + * \param[in] publisher_options options to use when creating the introspection publisher + * \param[in] introspection_state rcl_service_introspection_state_t describing whether + * introspection should be OFF, METADATA, or CONTENTS + * \return #RCL_RET_OK if the call was successful, or + * \return #RCL_RET_ERROR if the event publisher is invalid, or + * \return #RCL_RET_NODE_INVALID if the given node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if the client or node structure is invalid, + * \return #RCL_RET_BAD_ALLOC if a memory allocation failed + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_configure_service_introspection( + rcl_service_t * service, + rcl_node_t * node, + rcl_clock_t * clock, + const rosidl_service_type_support_t * type_support, + const rcl_publisher_options_t publisher_options, + rcl_service_introspection_state_t introspection_state); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/service_introspection.h b/rcl/include/rcl/service_introspection.h new file mode 100644 index 000000000..1055d6b9d --- /dev/null +++ b/rcl/include/rcl/service_introspection.h @@ -0,0 +1,31 @@ +// 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_ + +#define RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX "/_service_event" + +/// The introspection state for a client or service. +typedef enum rcl_service_introspection_state_e +{ + /// Introspection disabled + RCL_SERVICE_INTROSPECTION_OFF, + /// Introspect metadata only + RCL_SERVICE_INTROSPECTION_METADATA, + /// Introspection metadata and contents + RCL_SERVICE_INTROSPECTION_CONTENTS, +} rcl_service_introspection_state_t; + +#endif // RCL__SERVICE_INTROSPECTION_H_ diff --git a/rcl/package.xml b/rcl/package.xml index 8bbe62883..a80c42ac5 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -26,6 +26,7 @@ rcutils rmw_implementation rosidl_runtime_c + service_msgs tracetools ament_cmake_gtest diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 432948d59..63d905855 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -24,14 +24,20 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/publisher.h" +#include "rcl/time.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 "service_msgs/msg/service_event_info.h" #include "tracetools/tracetools.h" +#include "rosidl_runtime_c/service_type_support_struct.h" + #include "./common.h" +#include "./service_event_publisher.h" struct rcl_client_impl_s { @@ -40,6 +46,8 @@ 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_event_publisher_t * service_event_publisher; + char * remapped_service_name; }; rcl_client_t @@ -49,6 +57,29 @@ rcl_get_zero_initialized_client() return null_client; } +static +rcl_ret_t +unconfigure_service_introspection( + rcl_node_t * node, + struct rcl_client_impl_s * client_impl, + rcl_allocator_t * allocator) +{ + if (client_impl == NULL) { + return RCL_RET_ERROR; + } + + if (client_impl->service_event_publisher == NULL) { + return RCL_RET_OK; + } + + rcl_ret_t ret = rcl_service_event_publisher_fini(client_impl->service_event_publisher, node); + + allocator->deallocate(client_impl->service_event_publisher, allocator->state); + client_impl->service_event_publisher = NULL; + + return ret; +} + rcl_ret_t rcl_client_init( rcl_client_t * client, @@ -57,8 +88,6 @@ rcl_client_init( const char * service_name, const rcl_client_options_t * options) { - rcl_ret_t fail_ret = RCL_RET_ERROR; - // check the options and allocator first, so the allocator can be passed to errors RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); rcl_allocator_t * allocator = (rcl_allocator_t *)&options->allocator; @@ -76,61 +105,64 @@ rcl_client_init( return RCL_RET_ALREADY_INIT; } + // Allocate space for the implementation struct. + client->impl = (rcl_client_impl_t *)allocator->zero_allocate( + 1, sizeof(rcl_client_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "allocating memory failed", + return RCL_RET_BAD_ALLOC;); + // Expand the given service name. - char * remapped_service_name = NULL; rcl_ret_t ret = rcl_node_resolve_name( node, service_name, *allocator, true, false, - &remapped_service_name); + &client->impl->remapped_service_name); if (ret != RCL_RET_OK) { if (ret == RCL_RET_SERVICE_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { ret = RCL_RET_SERVICE_NAME_INVALID; } else if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; } - goto cleanup; + goto free_client_impl; } RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name); + ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", + client->impl->remapped_service_name); - // Allocate space for the implementation struct. - client->impl = (rcl_client_impl_t *)allocator->allocate( - sizeof(rcl_client_impl_t), allocator->state); - RCL_CHECK_FOR_NULL_WITH_MSG( - client->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); // Fill out implementation struct. // rmw handle (create rmw client) // TODO(wjwwood): pass along the allocator to rmw when it supports it client->impl->rmw_handle = rmw_create_client( rcl_node_get_rmw_handle(node), type_support, - remapped_service_name, + client->impl->remapped_service_name, &options->qos); if (!client->impl->rmw_handle) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - goto fail; + ret = RCL_RET_ERROR; + goto free_remapped_service_name; } // get actual qos, and store it rmw_ret_t rmw_ret = rmw_client_request_publisher_get_actual_qos( client->impl->rmw_handle, &client->impl->actual_request_publisher_qos); - if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - goto fail; + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto destroy_client; } rmw_ret = rmw_client_response_subscription_get_actual_qos( client->impl->rmw_handle, &client->impl->actual_response_subscription_qos); - if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - goto fail; + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto destroy_client; } // ROS specific namespacing conventions avoidance @@ -144,23 +176,30 @@ rcl_client_init( client->impl->options = *options; atomic_init(&client->impl->sequence_number, 0); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized"); - ret = RCL_RET_OK; TRACEPOINT( rcl_client_init, (const void *)client, (const void *)node, (const void *)client->impl->rmw_handle, - remapped_service_name); - goto cleanup; -fail: - if (client->impl) { - allocator->deallocate(client->impl, allocator->state); - client->impl = NULL; + client->impl->remapped_service_name); + + return RCL_RET_OK; + +destroy_client: + rmw_ret = rmw_destroy_client(rcl_node_get_rmw_handle(node), client->impl->rmw_handle); + if (RMW_RET_OK != rmw_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); } - ret = fail_ret; - // Fall through to cleanup -cleanup: - allocator->deallocate(remapped_service_name, allocator->state); + +free_remapped_service_name: + allocator->deallocate(client->impl->remapped_service_name, allocator->state); + client->impl->remapped_service_name = NULL; + +free_client_impl: + allocator->deallocate(client->impl, allocator->state); + client->impl = NULL; + return ret; } @@ -177,17 +216,29 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node) if (!rcl_node_is_valid_except_context(node)) { return RCL_RET_NODE_INVALID; // error already set } + if (client->impl) { rcl_allocator_t allocator = client->impl->options.allocator; rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); if (!rmw_node) { return RCL_RET_INVALID_ARGUMENT; } + + rcl_ret_t rcl_ret = unconfigure_service_introspection(node, client->impl, &allocator); + if (RCL_RET_OK != rcl_ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + result = rcl_ret; + } + rmw_ret_t ret = rmw_destroy_client(rmw_node, client->impl->rmw_handle); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } + + allocator.deallocate(client->impl->remapped_service_name, allocator.state); + client->impl->remapped_service_name = NULL; + allocator.deallocate(client->impl, allocator.state); client->impl = NULL; } @@ -252,6 +303,25 @@ 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 (client->impl->service_event_publisher != NULL) { + rmw_gid_t gid; + rmw_ret_t rmw_ret = rmw_get_gid_for_client(client->impl->rmw_handle, &gid); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcl_ret_t ret = rcl_send_service_event_message( + client->impl->service_event_publisher, + service_msgs__msg__ServiceEventInfo__REQUEST_SENT, + ros_request, + *sequence_number, + gid.data); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return ret; + } + } return RCL_RET_OK; } @@ -283,6 +353,25 @@ rcl_take_response_with_info( if (!taken) { return RCL_RET_CLIENT_TAKE_FAILED; } + + if (client->impl->service_event_publisher != NULL) { + rmw_gid_t gid; + rmw_ret_t rmw_ret = rmw_get_gid_for_client(client->impl->rmw_handle, &gid); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcl_ret_t ret = rcl_send_service_event_message( + client->impl->service_event_publisher, + service_msgs__msg__ServiceEventInfo__RESPONSE_RECEIVED, + ros_response, + request_header->request_id.sequence_number, + gid.data); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return ret; + } + } return RCL_RET_OK; } @@ -345,6 +434,51 @@ rcl_client_set_on_new_response_callback( user_data); } +rcl_ret_t +rcl_client_configure_service_introspection( + rcl_client_t * client, + rcl_node_t * node, + rcl_clock_t * clock, + const rosidl_service_type_support_t * type_support, + const rcl_publisher_options_t publisher_options, + rcl_service_introspection_state_t introspection_state) +{ + if (!rcl_client_is_valid(client)) { + return RCL_RET_CLIENT_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + + rcl_allocator_t allocator = client->impl->options.allocator; + + if (introspection_state == RCL_SERVICE_INTROSPECTION_OFF) { + return unconfigure_service_introspection(node, client->impl, &allocator); + } + + if (client->impl->service_event_publisher == NULL) { + // We haven't been introspecting, so we need to allocate the service event publisher + + client->impl->service_event_publisher = allocator.allocate( + sizeof(rcl_service_event_publisher_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl->service_event_publisher, "allocating memory failed", return RCL_RET_BAD_ALLOC;); + + *client->impl->service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); + rcl_ret_t ret = rcl_service_event_publisher_init( + client->impl->service_event_publisher, node, clock, publisher_options, + client->impl->remapped_service_name, type_support); + if (RCL_RET_OK != ret) { + allocator.deallocate(client->impl->service_event_publisher, allocator.state); + client->impl->service_event_publisher = NULL; + return ret; + } + } + + return rcl_service_event_publisher_change_state( + client->impl->service_event_publisher, introspection_state); +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index e1bdb0174..78eb70d0f 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -24,18 +24,29 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/publisher.h" +#include "rcl/time.h" +#include "rcl/types.h" #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "service_msgs/msg/service_event_info.h" #include "tracetools/tracetools.h" +#include "rosidl_runtime_c/service_type_support_struct.h" + +#include "./common.h" +#include "./service_event_publisher.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_event_publisher_t * service_event_publisher; + char * remapped_service_name; }; rcl_service_t @@ -45,6 +56,29 @@ rcl_get_zero_initialized_service() return null_service; } +static +rcl_ret_t +unconfigure_service_introspection( + rcl_node_t * node, + struct rcl_service_impl_s * service_impl, + rcl_allocator_t * allocator) +{ + if (service_impl == NULL) { + return RCL_RET_ERROR; + } + + if (service_impl->service_event_publisher == NULL) { + return RCL_RET_OK; + } + + rcl_ret_t ret = rcl_service_event_publisher_fini(service_impl->service_event_publisher, node); + + allocator->deallocate(service_impl->service_event_publisher, allocator->state); + service_impl->service_event_publisher = NULL; + + return ret; +} + rcl_ret_t rcl_service_init( rcl_service_t * service, @@ -60,8 +94,6 @@ rcl_service_init( RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SERVICE_NAME_INVALID); - rcl_ret_t fail_ret = RCL_RET_ERROR; - // Check options and allocator first, so the allocator can be used in errors. RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); rcl_allocator_t * allocator = (rcl_allocator_t *)&options->allocator; @@ -80,31 +112,32 @@ rcl_service_init( return RCL_RET_ALREADY_INIT; } + // Allocate space for the implementation struct. + service->impl = (rcl_service_impl_t *)allocator->zero_allocate( + 1, sizeof(rcl_service_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "allocating memory failed", + return RCL_RET_BAD_ALLOC;); + // Expand and remap the given service name. - char * remapped_service_name = NULL; rcl_ret_t ret = rcl_node_resolve_name( node, service_name, *allocator, true, false, - &remapped_service_name); + &service->impl->remapped_service_name); if (ret != RCL_RET_OK) { if (ret == RCL_RET_SERVICE_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { ret = RCL_RET_SERVICE_NAME_INVALID; } else if (ret != RCL_RET_BAD_ALLOC) { ret = RCL_RET_ERROR; } - goto cleanup; + goto free_service_impl; } 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); + ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", + service->impl->remapped_service_name); if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) { RCUTILS_LOG_WARN_NAMED( @@ -118,29 +151,31 @@ rcl_service_init( service->impl->rmw_handle = rmw_create_service( rcl_node_get_rmw_handle(node), type_support, - remapped_service_name, + service->impl->remapped_service_name, &options->qos); if (!service->impl->rmw_handle) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - goto fail; + ret = RCL_RET_ERROR; + goto free_remapped_service_name; } + // get actual qos, and store it rmw_ret_t rmw_ret = rmw_service_request_subscription_get_actual_qos( service->impl->rmw_handle, &service->impl->actual_request_subscription_qos); - if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - goto fail; + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto destroy_service; } rmw_ret = rmw_service_response_publisher_get_actual_qos( service->impl->rmw_handle, &service->impl->actual_response_publisher_qos); - if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - goto fail; + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto destroy_service; } // ROS specific namespacing conventions is not retrieved by get_actual_qos @@ -152,23 +187,30 @@ rcl_service_init( // options service->impl->options = *options; RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); - ret = RCL_RET_OK; TRACEPOINT( rcl_service_init, (const void *)service, (const void *)node, (const void *)service->impl->rmw_handle, - remapped_service_name); - goto cleanup; -fail: - if (service->impl) { - allocator->deallocate(service->impl, allocator->state); - service->impl = NULL; + service->impl->remapped_service_name); + + return RCL_RET_OK; + +destroy_service: + rmw_ret = rmw_destroy_service(rcl_node_get_rmw_handle(node), service->impl->rmw_handle); + if (RMW_RET_OK != rmw_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); } - ret = fail_ret; - // Fall through to clean up -cleanup: - allocator->deallocate(remapped_service_name, allocator->state); + +free_remapped_service_name: + allocator->deallocate(service->impl->remapped_service_name, allocator->state); + service->impl->remapped_service_name = NULL; + +free_service_impl: + allocator->deallocate(service->impl, allocator->state); + service->impl = NULL; + return ret; } @@ -193,11 +235,22 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) if (!rmw_node) { return RCL_RET_INVALID_ARGUMENT; } + + rcl_ret_t rcl_ret = unconfigure_service_introspection(node, service->impl, &allocator); + if (RCL_RET_OK != rcl_ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + result = rcl_ret; + } + rmw_ret_t ret = rmw_destroy_service(rmw_node, service->impl->rmw_handle); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } + + allocator.deallocate(service->impl->remapped_service_name, allocator.state); + service->impl->remapped_service_name = NULL; + allocator.deallocate(service->impl, allocator.state); service->impl = NULL; } @@ -277,6 +330,18 @@ rcl_take_request_with_info( if (!taken) { return RCL_RET_SERVICE_TAKE_FAILED; } + if (service->impl->service_event_publisher != NULL) { + rcl_ret_t rclret = rcl_send_service_event_message( + service->impl->service_event_publisher, + service_msgs__msg__ServiceEventInfo__REQUEST_RECEIVED, + ros_request, + request_header->request_id.sequence_number, + request_header->request_id.writer_guid); + if (RCL_RET_OK != rclret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return rclret; + } + } return RCL_RET_OK; } @@ -314,6 +379,20 @@ rcl_send_response( RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } + + // publish out the introspected content + if (service->impl->service_event_publisher != NULL) { + rcl_ret_t ret = rcl_send_service_event_message( + service->impl->service_event_publisher, + service_msgs__msg__ServiceEventInfo__RESPONSE_SENT, + ros_response, + request_header->sequence_number, + request_header->writer_guid); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return ret; + } + } return RCL_RET_OK; } @@ -363,6 +442,52 @@ rcl_service_set_on_new_request_callback( user_data); } +rcl_ret_t +rcl_service_configure_service_introspection( + rcl_service_t * service, + rcl_node_t * node, + rcl_clock_t * clock, + const rosidl_service_type_support_t * type_support, + const rcl_publisher_options_t publisher_options, + rcl_service_introspection_state_t introspection_state) +{ + if (!rcl_service_is_valid(service)) { + return RCL_RET_SERVICE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + + rcl_allocator_t allocator = service->impl->options.allocator; + + if (introspection_state == RCL_SERVICE_INTROSPECTION_OFF) { + return unconfigure_service_introspection(node, service->impl, &allocator); + } + + if (service->impl->service_event_publisher == NULL) { + // We haven't been introspecting, so we need to allocate the service event publisher + + service->impl->service_event_publisher = allocator.allocate( + sizeof(rcl_service_event_publisher_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl->service_event_publisher, "allocating memory failed", + return RCL_RET_BAD_ALLOC;); + + *service->impl->service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); + rcl_ret_t ret = rcl_service_event_publisher_init( + service->impl->service_event_publisher, node, clock, publisher_options, + service->impl->remapped_service_name, type_support); + if (RCL_RET_OK != ret) { + allocator.deallocate(service->impl->service_event_publisher, allocator.state); + service->impl->service_event_publisher = NULL; + return ret; + } + } + + return rcl_service_event_publisher_change_state( + service->impl->service_event_publisher, introspection_state); +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/service_event_publisher.c b/rcl/src/rcl/service_event_publisher.c new file mode 100644 index 000000000..f262dba70 --- /dev/null +++ b/rcl/src/rcl/service_event_publisher.c @@ -0,0 +1,294 @@ +// 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. + +#include "rcl/service_event_publisher.h" + +#include + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/error_handling.h" +#include "rcl/publisher.h" +#include "rcl/node.h" +#include "rcl/service_introspection.h" +#include "rcl/time.h" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" +#include "rmw/error_handling.h" +#include "service_msgs/msg/service_event_info.h" + +rcl_service_event_publisher_t rcl_get_zero_initialized_service_event_publisher() +{ + static rcl_service_event_publisher_t zero_service_event_publisher = {0}; + return zero_service_event_publisher; +} + +bool +rcl_service_event_publisher_is_valid(const rcl_service_event_publisher_t * service_event_publisher) +{ + RCL_CHECK_FOR_NULL_WITH_MSG( + service_event_publisher, "service_event_publisher is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + service_event_publisher->service_type_support, + "service_event_publisher's service type support is invalid", return false); + if (!rcl_clock_valid(service_event_publisher->clock)) { + RCL_SET_ERROR_MSG("service_event_publisher's clock is invalid"); + return false; + } + return true; +} + +static rcl_ret_t introspection_create_publisher( + rcl_service_event_publisher_t * service_event_publisher, + const rcl_node_t * node) +{ + rcl_allocator_t allocator = service_event_publisher->publisher_options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_ERROR); + + service_event_publisher->publisher = allocator.allocate( + sizeof(rcl_publisher_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service_event_publisher->publisher, + "allocate service_event_publisher failed in enable", return RCL_RET_BAD_ALLOC); + *service_event_publisher->publisher = rcl_get_zero_initialized_publisher(); + rcl_ret_t ret = rcl_publisher_init( + service_event_publisher->publisher, node, + service_event_publisher->service_type_support->event_typesupport, + service_event_publisher->service_event_topic_name, + &service_event_publisher->publisher_options); + if (RCL_RET_OK != ret) { + allocator.deallocate(service_event_publisher->publisher, allocator.state); + service_event_publisher->publisher = NULL; + rcutils_reset_error(); + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return ret; + } + + return RCL_RET_OK; +} + +rcl_ret_t rcl_service_event_publisher_init( + rcl_service_event_publisher_t * service_event_publisher, + const rcl_node_t * node, + rcl_clock_t * clock, + const rcl_publisher_options_t publisher_options, + const char * service_name, + const rosidl_service_type_support_t * service_type_support) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_TOPIC_NAME_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID); + + RCL_CHECK_ARGUMENT_FOR_NULL(service_event_publisher, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_type_support, RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ALLOCATOR_WITH_MSG( + &publisher_options.allocator, + "allocator is invalid", return RCL_RET_ERROR); + + rcl_allocator_t allocator = publisher_options.allocator; + + rcl_ret_t ret = RCL_RET_OK; + + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + + if (!rcl_clock_valid(clock)) { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("clock is invalid"); + return RCL_RET_ERROR; + } + + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing service introspection for service name '%s'", service_name); + + // Typesupports have static lifetimes + service_event_publisher->service_type_support = service_type_support; + service_event_publisher->clock = clock; + service_event_publisher->publisher_options = publisher_options; + + size_t topic_length = strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1; + service_event_publisher->service_event_topic_name = (char *) allocator.allocate( + topic_length, allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service_event_publisher->service_event_topic_name, + "allocating memory for service introspection topic name failed", + return RCL_RET_BAD_ALLOC;); + + snprintf( + service_event_publisher->service_event_topic_name, + topic_length, + "%s%s", service_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); + + ret = introspection_create_publisher(service_event_publisher, node); + if (ret != RCL_RET_OK) { + goto free_topic_name; + } + + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Service introspection for service '%s' initialized", service_name); + + return RCL_RET_OK; + +free_topic_name: + allocator.deallocate(service_event_publisher->service_event_topic_name, allocator.state); + + return ret; +} + +rcl_ret_t rcl_service_event_publisher_fini( + rcl_service_event_publisher_t * service_event_publisher, + rcl_node_t * node) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_SHUTDOWN); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + + if (!rcl_service_event_publisher_is_valid(service_event_publisher)) { + return RCL_RET_ERROR; + } + + if (!rcl_node_is_valid_except_context(node)) { + return RCL_RET_NODE_INVALID; + } + + rcl_allocator_t allocator = service_event_publisher->publisher_options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_ERROR); + + if (service_event_publisher->publisher) { + rcl_ret_t ret = rcl_publisher_fini(service_event_publisher->publisher, node); + allocator.deallocate(service_event_publisher->publisher, allocator.state); + service_event_publisher->publisher = NULL; + if (RCL_RET_OK != ret) { + return ret; + } + } + + allocator.deallocate(service_event_publisher->service_event_topic_name, allocator.state); + service_event_publisher->service_event_topic_name = NULL; + + return RCL_RET_OK; +} + +rcl_ret_t rcl_send_service_event_message( + const rcl_service_event_publisher_t * service_event_publisher, + const uint8_t event_type, + const void * ros_response_request, + const int64_t sequence_number, + const uint8_t guid[16]) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + + RCL_CHECK_ARGUMENT_FOR_NULL(ros_response_request, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(guid, "guid is NULL", return RCL_RET_INVALID_ARGUMENT); + + if (!rcl_service_event_publisher_is_valid(service_event_publisher)) { + return RCL_RET_ERROR; + } + + if (service_event_publisher->introspection_state == RCL_SERVICE_INTROSPECTION_OFF) { + return RCL_RET_ERROR; + } + + rcl_allocator_t allocator = service_event_publisher->publisher_options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + if (!rcl_publisher_is_valid(service_event_publisher->publisher)) { + return RCL_RET_PUBLISHER_INVALID; + } + + rcl_ret_t ret; + + rcl_time_point_value_t now; + ret = rcl_clock_get_now(service_event_publisher->clock, &now); + if (RMW_RET_OK != ret) { + rcutils_reset_error(); + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + + rosidl_service_introspection_info_t info = { + .event_type = event_type, + .stamp_sec = (int32_t)RCL_NS_TO_S(now), + .stamp_nanosec = now % (1000LL * 1000LL * 1000LL), + .sequence_number = sequence_number, + }; + + memcpy(info.client_gid, guid, 16); + + void * service_introspection_message; + if (service_event_publisher->introspection_state == RCL_SERVICE_INTROSPECTION_METADATA) { + ros_response_request = NULL; + } + switch (event_type) { + case service_msgs__msg__ServiceEventInfo__REQUEST_RECEIVED: + case service_msgs__msg__ServiceEventInfo__REQUEST_SENT: + service_introspection_message = + service_event_publisher->service_type_support->event_message_create_handle_function( + &info, &allocator, ros_response_request, NULL); + break; + case service_msgs__msg__ServiceEventInfo__RESPONSE_RECEIVED: + case service_msgs__msg__ServiceEventInfo__RESPONSE_SENT: + service_introspection_message = + service_event_publisher->service_type_support->event_message_create_handle_function( + &info, &allocator, NULL, ros_response_request); + break; + default: + rcutils_reset_error(); + RCL_SET_ERROR_MSG("unsupported event type"); + return RCL_RET_ERROR; + } + RCL_CHECK_FOR_NULL_WITH_MSG( + service_introspection_message, "service_introspection_message is NULL", return RCL_RET_ERROR); + + // and publish it out! + ret = rcl_publish(service_event_publisher->publisher, service_introspection_message, NULL); + // clean up before error checking + service_event_publisher->service_type_support->event_message_destroy_handle_function( + service_introspection_message, &allocator); + if (RCL_RET_OK != ret) { + rcutils_reset_error(); + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + } + + return ret; +} + +rcl_ret_t +rcl_service_event_publisher_change_state( + rcl_service_event_publisher_t * service_event_publisher, + rcl_service_introspection_state_t introspection_state) +{ + if (!rcl_service_event_publisher_is_valid(service_event_publisher)) { + return RCL_RET_ERROR; + } + + service_event_publisher->introspection_state = introspection_state; + + return RCL_RET_OK; +} diff --git a/rcl/src/rcl/service_event_publisher.h b/rcl/src/rcl/service_event_publisher.h new file mode 100644 index 000000000..05f9af481 --- /dev/null +++ b/rcl/src/rcl/service_event_publisher.h @@ -0,0 +1,214 @@ +// 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_EVENT_PUBLISHER_H_ +#define RCL__SERVICE_EVENT_PUBLISHER_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/macros.h" +#include "rcl/node.h" +#include "rcl/publisher.h" +#include "rcl/service_introspection.h" +#include "rcl/time.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#include "rosidl_runtime_c/service_type_support_struct.h" + +typedef struct rcl_service_event_publisher_s +{ + /// Handle to publisher for publishing service events + rcl_publisher_t * publisher; + /// Name of service introspection topic: / + char * service_event_topic_name; + /// Current state of introspection; off, metadata, or contents + rcl_service_introspection_state_t introspection_state; + /// Handle to clock for timestamping service events + rcl_clock_t * clock; + /// Publisher options for service event publisher + rcl_publisher_options_t publisher_options; + /// Handle to service typesupport + const rosidl_service_type_support_t * service_type_support; +} rcl_service_event_publisher_t; + +/// Return a rcl_service_event_publisher_t struct with members set to `NULL`. +/** + * Should be called to get a null rcl_service_event_publisher_t before passing to + * rcl_service_event_publisher_init(). + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_service_event_publisher_t +rcl_get_zero_initialized_service_event_publisher(); + +/// Initialize a service event publisher. +/** + * After calling this function on a rcl_service_event_publisher_t, it can be used to + * send service introspection messages by calling rcl_send_service_event_message(). + * + * The given rcl_node_t must be valid and the resulting rcl_service_event_publisher_t is + * only valid as long as the given rcl_node_t remains valid. + * + * Similarly, the given rcl_clock_t must be valid and the resulting rcl_service_event_publisher_t + * is only valid as long as the given rcl_clock_t remains valid. + * + * The passed in service_name should be the fully-qualified, remapped service name. + * The service event publisher will add a custom suffix as the topic name. + * + * The rosidl_service_type_support_t is obtained on a per `.srv` type basis. + * When the user defines a ROS service, code is generated which provides the + * required rosidl_service_type_support_t object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[inout] service_event_publisher preallocated rcl_service_event_publisher_t + * \param[in] node valid rcl_node_t to use to create the introspection publisher + * \param[in] clock valid rcl_clock_t to use to generate the introspection timestamps + * \param[in] publisher_options options to use when creating the introspection publisher + * \param[in] service_name fully-qualified and remapped service name + * \param[in] service_type_support type support library associated with this service + * \return #RCL_RET_OK if the call was successful + * \return #RCL_RET_INVALID_ARGUMENT if the event publisher, client, or node is invalid, + * \return #RCL_RET_NODE_INVALID if the given node is invalid, or + * \return #RCL_RET_BAD_ALLOC if a memory allocation failed, or + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_event_publisher_init( + rcl_service_event_publisher_t * service_event_publisher, + const rcl_node_t * node, + rcl_clock_t * clock, + const rcl_publisher_options_t publisher_options, + const char * service_name, + const rosidl_service_type_support_t * service_type_support); + +/// Finalize a rcl_service_event_publisher_t. +/** + * After calling this function, calls to any of the other functions here + * (except for rcl_service_event_publisher_init()) will fail. + * However, the given node handle is still valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] service_event_publisher handle to the event publisher to be finalized + * \param[in] node a valid (not finalized) handle to the node used to create the client + * \return #RCL_RET_OK if client was finalized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_event_publisher_fini( + rcl_service_event_publisher_t * service_event_publisher, + rcl_node_t * node); + +/// Check that the service event publisher is valid. +/** + * The bool returned is `false` if the service event publisher is invalid. + * The bool returned is `true` otherwise. + * In the case where `false` is returned, an error message is set. + * This function cannot fail. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] service_event_publisher pointer to the service event publisher + * \return `true` if `service_event_publisher` is valid, otherwise `false` + */ +RCL_PUBLIC +bool +rcl_service_event_publisher_is_valid(const rcl_service_event_publisher_t * service_event_publisher); + +/// Send a service event message. +/** + * It is the job of the caller to ensure that the type of the `ros_request` + * parameter and the type associated with the event publisher (via the type support) + * match. + * Passing a different type to publish produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * rcl_send_service_event_message() is a potentially blocking call. + * + * The ROS request message given by the `ros_response_request` void pointer is always + * owned by the calling code, but should remain constant during rcl_send_service_event_message(). + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] service_event_publisher pointer to the service event publisher + * \param[in] event_type introspection event type from service_msgs::msg::ServiceEventInfo + * \param[in] ros_response_request type-erased pointer to the ROS response request + * \param[in] sequence_number sequence number of the event + * \param[in] guid GUID associated with this event + * \return #RCL_RET_OK if the event was published successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_send_service_event_message( + const rcl_service_event_publisher_t * service_event_publisher, + uint8_t event_type, + const void * ros_response_request, + int64_t sequence_number, + const uint8_t guid[16]); + +/// Change the operating state of this service event publisher. +/** + * \param[in] service_event_publisher pointer to the service event publisher + * \param[in] introspection_state new introspection state + * \return #RCL_RET_OK if the event was published successfully, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +rcl_ret_t +rcl_service_event_publisher_change_state( + rcl_service_event_publisher_t * service_event_publisher, + rcl_service_introspection_state_t introspection_state); + +#ifdef __cplusplus +} +#endif +#endif // RCL__SERVICE_EVENT_PUBLISHER_H_ diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 0292affa9..743270fda 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -315,6 +315,15 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) + rcl_add_custom_gtest(test_service_event_publisher${target_suffix} + SRCS rcl/test_service_event_publisher.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} mimick wait_for_entity_helpers + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + # Launch tests rcl_add_custom_executable(service_fixture${target_suffix} diff --git a/rcl/test/rcl/test_service_event_publisher.cpp b/rcl/test/rcl/test_service_event_publisher.cpp new file mode 100644 index 000000000..f6ee979c4 --- /dev/null +++ b/rcl/test/rcl/test_service_event_publisher.cpp @@ -0,0 +1,702 @@ +// 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. + +#include +#include +#include + +#include +#include + +#include "../mocking_utils/patch.hpp" +#include "./service_event_publisher.h" +#include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/allocator.h" +#include "rcl/client.h" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/service.h" +#include "rcl/service_introspection.h" +#include "rcl/subscription.h" +#include "rcl/time.h" +#include "rcl/types.h" +#include "rmw/rmw.h" +#include "service_msgs/msg/service_event_info.h" +#include "test_msgs/srv/basic_types.h" +#include "wait_for_entity_helpers.hpp" + +#ifdef RMW_IMPLEMENTATION +#define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +#define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +#define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestServiceEventPublisherFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() override + { + rcl_ret_t ret; + allocator = rcl_get_default_allocator(); + + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_service_event_publisher_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->clock_ptr = new rcl_clock_t; + ret = rcl_clock_init(RCL_STEADY_TIME, clock_ptr, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t ret; + + ret = rcl_clock_fini(this->clock_ptr); + delete this->clock_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + +protected: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + rcl_clock_t * clock_ptr; + rcl_allocator_t allocator; + const rosidl_service_type_support_t * srv_ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); +}; + +/* Basic nominal test of service introspection features covering init, fini, and sending a message + */ +TEST_F( + CLASSNAME(TestServiceEventPublisherFixture, RMW_IMPLEMENTATION), + test_service_event_publisher_nominal) +{ + rcl_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + rcl_ret_t ret; + + ret = rcl_service_event_publisher_init( + &service_event_publisher, this->node_ptr, clock_ptr, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_METADATA); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + test_msgs__srv__BasicTypes_Request client_request; + test_msgs__srv__BasicTypes_Request__init(&client_request); + + client_request.bool_value = false; + client_request.uint8_value = 1; + client_request.uint32_value = 2; + int64_t sequence_number = 1; + uint8_t guid[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; + + ret = rcl_send_service_event_message( + &service_event_publisher, service_msgs__msg__ServiceEventInfo__REQUEST_SENT, &client_request, + sequence_number, guid); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_fini(&service_event_publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F( + CLASSNAME(TestServiceEventPublisherFixture, RMW_IMPLEMENTATION), + test_service_event_publisher_init_and_fini) +{ + rcl_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + rcl_clock_t clock; + rcl_ret_t ret; + + ret = rcl_service_event_publisher_init( + &service_event_publisher, nullptr, &clock, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + + ret = rcl_service_event_publisher_init( + &service_event_publisher, this->node_ptr, nullptr, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_fini(&service_event_publisher, nullptr); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + + ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + "123test_service_event_publisher<>h", srv_ts); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + + service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_create_publisher, "patch rmw_create_publisher to fail", nullptr); + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + } +} + +/* Test sending service introspection message via service_event_publisher.h + */ +TEST_F( + CLASSNAME(TestServiceEventPublisherFixture, RMW_IMPLEMENTATION), + test_service_event_publisher_send_message_nominal) +{ + uint8_t guid[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; + auto sub_opts = rcl_subscription_get_default_options(); + std::string topic = "test_service_event_publisher"; + std::string service_event_topic = topic + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX; + rcl_ret_t ret; + + rcl_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + topic.c_str(), srv_ts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + ret = rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_CONTENTS); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init( + &subscription, node_ptr, srv_ts->event_typesupport, service_event_topic.c_str(), &sub_opts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + ASSERT_TRUE(wait_for_established_subscription(service_event_publisher.publisher, 10, 100)); + + test_msgs__srv__BasicTypes_Request test_req; + memset(&test_req, 0, sizeof(test_msgs__srv__BasicTypes_Request)); + test_msgs__srv__BasicTypes_Request__init(&test_req); + test_req.bool_value = true; + test_req.uint16_value = 42; + test_req.uint32_value = 123; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({test_msgs__srv__BasicTypes_Request__fini(&test_req);}); + + ret = rcl_send_service_event_message( + &service_event_publisher, service_msgs__msg__ServiceEventInfo__REQUEST_RECEIVED, &test_req, 1, + guid); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + rmw_message_info_t message_info = rmw_get_zero_initialized_message_info(); + test_msgs__srv__BasicTypes_Event event_msg; + test_msgs__srv__BasicTypes_Event__init(&event_msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({test_msgs__srv__BasicTypes_Event__fini(&event_msg);}); + ret = rcl_take(&subscription, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(1, event_msg.info.sequence_number); + ASSERT_EQ(0, memcmp(guid, event_msg.info.client_gid, sizeof(guid))); + ASSERT_EQ(0U, event_msg.response.size); + ASSERT_EQ(1U, event_msg.request.size); + ASSERT_EQ(test_req.bool_value, event_msg.request.data[0].bool_value); + ASSERT_EQ(test_req.uint16_value, event_msg.request.data[0].uint16_value); + ASSERT_EQ(test_req.uint32_value, event_msg.request.data[0].uint32_value); +} + +TEST_F( + CLASSNAME(TestServiceEventPublisherFixture, RMW_IMPLEMENTATION), + test_service_event_publisher_send_message_return_codes) +{ + rcl_ret_t ret; + + rcl_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + + uint8_t guid[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; + char topic[] = "test_service_event_publisher"; + + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + topic, srv_ts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_METADATA); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_send_service_event_message(nullptr, 0, nullptr, 0, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + + test_msgs__srv__BasicTypes_Request test_req; + test_msgs__srv__BasicTypes_Request__init(&test_req); + test_req.bool_value = true; + test_req.uint16_value = 42; + test_req.uint32_value = 123; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({test_msgs__srv__BasicTypes_Request__fini(&test_req);}); + + ret = rcl_send_service_event_message( + &service_event_publisher, service_msgs__msg__ServiceEventInfo__REQUEST_SENT, &test_req, 0, + nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + + ret = rcl_send_service_event_message( + &service_event_publisher, service_msgs__msg__ServiceEventInfo__RESPONSE_RECEIVED, &test_req, 0, + guid); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_send_service_event_message(&service_event_publisher, 5, &test_req, 0, guid); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + + ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F( + CLASSNAME(TestServiceEventPublisherFixture, RMW_IMPLEMENTATION), + test_service_event_publisher_utils) +{ + rcl_ret_t ret; + + rcl_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_TRUE(rcl_service_event_publisher_is_valid(&service_event_publisher)); + + rcl_publisher_fini(service_event_publisher.publisher, node_ptr); + EXPECT_TRUE(rcl_service_event_publisher_is_valid(&service_event_publisher)); + + ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + service_event_publisher.clock = nullptr; + EXPECT_FALSE(rcl_service_event_publisher_is_valid(&service_event_publisher)); + + ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcutils_reset_error(); + + service_event_publisher.clock = clock_ptr; + ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F( + CLASSNAME(TestServiceEventPublisherFixture, RMW_IMPLEMENTATION), + test_service_event_publisher_enable_and_disable_return_codes) +{ + rcl_ret_t ret; + + rcl_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, clock_ptr, rcl_publisher_get_default_options(), + "test_service_event_publisher", srv_ts); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // ok to enable twice + EXPECT_EQ( + RCL_RET_OK, + rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_METADATA)); + EXPECT_EQ( + RCL_RET_OK, + rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_METADATA)); + + EXPECT_EQ( + RCL_RET_OK, + rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_OFF)); + EXPECT_EQ( + RCL_RET_OK, + rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_OFF)); + + ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +class CLASSNAME (TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMPLEMENTATION) + : public ::testing::Test +{ +public: + void SetUp() + { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_service_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->clock_ptr = new rcl_clock_t; + ret = rcl_clock_init(RCL_STEADY_TIME, clock_ptr, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + std::string srv_name = "test_service_introspection_service"; + std::string service_event_topic = srv_name + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX; + + this->service_ptr = new rcl_service_t; + *this->service_ptr = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init( + this->service_ptr, this->node_ptr, srv_ts, srv_name.c_str(), &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_configure_service_introspection( + this->service_ptr, this->node_ptr, clock_ptr, srv_ts, rcl_publisher_get_default_options(), + RCL_SERVICE_INTROSPECTION_CONTENTS); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->client_ptr = new rcl_client_t; + *this->client_ptr = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init( + this->client_ptr, this->node_ptr, srv_ts, srv_name.c_str(), &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_client_configure_service_introspection( + this->client_ptr, this->node_ptr, clock_ptr, srv_ts, rcl_publisher_get_default_options(), + RCL_SERVICE_INTROSPECTION_CONTENTS); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->subscription_ptr = new rcl_subscription_t; + *this->subscription_ptr = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init( + this->subscription_ptr, this->node_ptr, srv_ts->event_typesupport, + service_event_topic.c_str(), &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_established_publisher(this->subscription_ptr, 10, 100)); + + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, this->client_ptr, 10, 1000)); + } + + void TearDown() + { + rcl_ret_t ret; + + ret = rcl_subscription_fini(this->subscription_ptr, this->node_ptr); + delete this->subscription_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_client_fini(this->client_ptr, this->node_ptr); + delete this->client_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_fini(this->service_ptr, this->node_ptr); + delete this->service_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_clock_fini(this->clock_ptr); + delete this->clock_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + +protected: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + rcl_service_t * service_ptr; + rcl_client_t * client_ptr; + rcl_clock_t * clock_ptr; + rcl_subscription_t * subscription_ptr; + const rosidl_service_type_support_t * srv_ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); +}; + +/* Whole test of service event publisher with service, client, and subscription + */ +TEST_F( + CLASSNAME(TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMPLEMENTATION), + test_service_event_publisher_with_subscriber) +{ + rcl_ret_t ret; + rmw_message_info_t message_info = rmw_get_zero_initialized_message_info(); + test_msgs__srv__BasicTypes_Event event_msg; + test_msgs__srv__BasicTypes_Event__init(&event_msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({test_msgs__srv__BasicTypes_Event__fini(&event_msg);}); + + test_msgs__srv__BasicTypes_Request client_request; + memset(&client_request, 0, sizeof(test_msgs__srv__BasicTypes_Request)); + test_msgs__srv__BasicTypes_Request__init(&client_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({test_msgs__srv__BasicTypes_Request__fini(&client_request);}); + client_request.bool_value = false; + client_request.uint8_value = 1; + client_request.uint32_value = 2; + + int64_t sequence_number; + ret = rcl_send_request(this->client_ptr, &client_request, &sequence_number); + EXPECT_NE(sequence_number, 0); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_service_to_be_ready(this->service_ptr, this->context_ptr, 10, 100)); + + // expect a REQUEST_SENT event + ASSERT_TRUE( + wait_for_subscription_to_be_ready(this->subscription_ptr, this->context_ptr, 10, 100)); + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_SENT, event_msg.info.event_type); + + test_msgs__srv__BasicTypes_Response service_response; + memset(&service_response, 0, sizeof(test_msgs__srv__BasicTypes_Response)); + test_msgs__srv__BasicTypes_Response__init(&service_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + {test_msgs__srv__BasicTypes_Response__fini(&service_response);}); + + test_msgs__srv__BasicTypes_Request service_request; + memset(&service_request, 0, sizeof(test_msgs__srv__BasicTypes_Request)); + test_msgs__srv__BasicTypes_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + {test_msgs__srv__BasicTypes_Request__fini(&service_request);}); + + rmw_service_info_t header; + ret = rcl_take_request( + this->service_ptr, &(header.request_id), &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(2U, service_request.uint32_value); + + // expect a REQUEST_RECEIVED event + ASSERT_TRUE( + wait_for_subscription_to_be_ready(this->subscription_ptr, this->context_ptr, 10, 100)); + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_RECEIVED, event_msg.info.event_type); + + service_response.uint32_value = 2; + service_response.uint8_value = 3; + ret = rcl_send_response(this->service_ptr, &header.request_id, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // expect a RESPONSE_SEND event + ASSERT_TRUE( + wait_for_subscription_to_be_ready(this->subscription_ptr, this->context_ptr, 10, 100)); + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(service_msgs__msg__ServiceEventInfo__RESPONSE_SENT, event_msg.info.event_type); + + test_msgs__srv__BasicTypes_Response client_response; + memset(&client_response, 0, sizeof(test_msgs__srv__BasicTypes_Response)); + test_msgs__srv__BasicTypes_Response__init(&client_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + {test_msgs__srv__BasicTypes_Response__fini(&client_response);}); + + ASSERT_TRUE( + wait_for_client_to_be_ready(this->client_ptr, this->context_ptr, 10, 100)); + ret = rcl_take_response(this->client_ptr, &(header.request_id), &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // expect a RESPONSE_RECEIVED event + ASSERT_TRUE( + wait_for_subscription_to_be_ready(this->subscription_ptr, this->context_ptr, 10, 100)); + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(service_msgs__msg__ServiceEventInfo__RESPONSE_RECEIVED, event_msg.info.event_type); + ASSERT_EQ(1U, event_msg.response.size); + ASSERT_EQ(2U, event_msg.response.data[0].uint32_value); +} + +/* Integration level test with disabling service events + */ +TEST_F( + CLASSNAME(TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMPLEMENTATION), + test_service_event_publisher_with_subscriber_disable_service_events) +{ + rcl_ret_t ret; + rmw_message_info_t message_info = rmw_get_zero_initialized_message_info(); + test_msgs__srv__BasicTypes_Event event_msg; + test_msgs__srv__BasicTypes_Event__init(&event_msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({test_msgs__srv__BasicTypes_Event__fini(&event_msg);}); + + ret = rcl_service_configure_service_introspection( + this->service_ptr, this->node_ptr, this->clock_ptr, srv_ts, rcl_publisher_get_default_options(), + RCL_SERVICE_INTROSPECTION_OFF); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + test_msgs__srv__BasicTypes_Request client_request; + memset(&client_request, 0, sizeof(test_msgs__srv__BasicTypes_Request)); + test_msgs__srv__BasicTypes_Request__init(&client_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({test_msgs__srv__BasicTypes_Request__fini(&client_request);}); + client_request.bool_value = false; + client_request.uint8_value = 1; + client_request.uint32_value = 2; + + int64_t sequence_number; + ret = rcl_send_request(this->client_ptr, &client_request, &sequence_number); + EXPECT_NE(sequence_number, 0); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_service_to_be_ready(this->service_ptr, this->context_ptr, 10, 100)); + + // expect a REQUEST_SENT event + ASSERT_TRUE( + wait_for_subscription_to_be_ready(this->subscription_ptr, this->context_ptr, 10, 100)); + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(service_msgs__msg__ServiceEventInfo__REQUEST_SENT, event_msg.info.event_type); + + test_msgs__srv__BasicTypes_Response service_response; + memset(&service_response, 0, sizeof(test_msgs__srv__BasicTypes_Response)); + test_msgs__srv__BasicTypes_Response__init(&service_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + {test_msgs__srv__BasicTypes_Response__fini(&service_response);}); + + test_msgs__srv__BasicTypes_Request service_request; + memset(&service_request, 0, sizeof(test_msgs__srv__BasicTypes_Request)); + test_msgs__srv__BasicTypes_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + {test_msgs__srv__BasicTypes_Request__fini(&service_request);}); + + rmw_service_info_t header; + ret = rcl_take_request( + this->service_ptr, &(header.request_id), &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(2U, service_request.uint32_value); + + // expect take to fail since no introspection message should be published + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; + + service_response.uint32_value = 2; + service_response.uint8_value = 3; + ret = rcl_send_response(this->service_ptr, &header.request_id, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // expect take to fail since no introspection message should be published + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; + + test_msgs__srv__BasicTypes_Response client_response; + memset(&client_response, 0, sizeof(test_msgs__srv__BasicTypes_Response)); + test_msgs__srv__BasicTypes_Response__init(&client_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + {test_msgs__srv__BasicTypes_Response__fini(&client_response);}); + + ASSERT_TRUE( + wait_for_client_to_be_ready(this->client_ptr, this->context_ptr, 10, 100)); + ret = rcl_take_response(this->client_ptr, &(header.request_id), &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // expect a RESPONSE_RECEIVED event + ASSERT_TRUE( + wait_for_subscription_to_be_ready(this->subscription_ptr, this->context_ptr, 10, 100)); + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(service_msgs__msg__ServiceEventInfo__RESPONSE_RECEIVED, event_msg.info.event_type); + ASSERT_EQ(2U, event_msg.response.data[0].uint32_value); +} diff --git a/rcl/test/rcl/wait_for_entity_helpers.cpp b/rcl/test/rcl/wait_for_entity_helpers.cpp index 625e1ea4a..3b9fecddd 100644 --- a/rcl/test/rcl/wait_for_entity_helpers.cpp +++ b/rcl/test/rcl/wait_for_entity_helpers.cpp @@ -184,6 +184,33 @@ wait_for_established_subscription( return false; } +bool +wait_for_established_publisher( + const rcl_subscription_t * subscription, + size_t max_tries, + int64_t period_ms) +{ + size_t iteration = 0; + rcl_ret_t ret = RCL_RET_OK; + size_t publisher_count = 0; + while (iteration < max_tries) { + ++iteration; + ret = rcl_subscription_get_publisher_count(subscription, &publisher_count); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Error in rcl_subscription_get_publisher_count: %s", + rcl_get_error_string().str); + return false; + } + if (publisher_count > 0) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + return false; +} + bool wait_for_subscription_to_be_ready( rcl_subscription_t * subscription, diff --git a/rcl/test/rcl/wait_for_entity_helpers.hpp b/rcl/test/rcl/wait_for_entity_helpers.hpp index 35baa645b..9bfec9538 100644 --- a/rcl/test/rcl/wait_for_entity_helpers.hpp +++ b/rcl/test/rcl/wait_for_entity_helpers.hpp @@ -54,6 +54,14 @@ wait_for_established_subscription( size_t max_tries, int64_t period_ms); +/// Wait for a subscription to get one or more established publishers +/// by trying at most `max_tries` times with a `period_ms` period. +bool +wait_for_established_publisher( + const rcl_subscription_t * subscription, + size_t max_tries, + int64_t period_ms); + /// Wait a subscription to be ready, i.e. a message is ready to be handled, /// by trying at least `max_tries` times with a `period_ms` period. bool