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