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..71d5a4294 100644 --- a/rcl/include/rcl/client.h +++ b/rcl/include/rcl/client.h @@ -24,11 +24,16 @@ 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/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; @@ -44,9 +49,15 @@ typedef struct rcl_client_options_s { /// Middleware quality of service settings for the client. rmw_qos_profile_t qos; + /// Publisher options for the service event publisher + rcl_publisher_options_t event_publisher_options; /// Custom allocator for the client, used for incidental allocations. /** For default behavior (malloc/free), use: rcl_get_default_allocator() */ rcl_allocator_t allocator; + /// Enable/Disable service introspection features + bool enable_service_introspection; + /// The clock to use for service introspection message timestampes + rcl_clock_t * clock; } rcl_client_options_t; /// Return a rcl_client_t struct with members set to `NULL`. @@ -194,7 +205,10 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node); * The defaults are: * * - qos = rmw_qos_profile_services_default + * - event_publisher_options = rcl_publisher_get_default_options() * - allocator = rcl_get_default_allocator() + * - enable_service_introspection = False + * - clock = NULL */ RCL_PUBLIC RCL_WARN_UNUSED @@ -493,6 +507,61 @@ 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. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] client The client on which to configure service introspection + * \param[in] node The node for which the service event publisher is to be associated to + * \param[in] enable Whether to enable or disable service introspection for the client. + * \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, or + * \return `RCL_RET_OK` if the call was successful + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_configure_client_service_events( + rcl_client_t * client, + rcl_node_t * node, + bool enable); + +/// Configures whether service introspection messages contain only metadata or content. +/** + * Enables or disables whether service introspection messages contain just the metadata + * about the transaction, or also contains the content. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] client The client on which to configure content + * \param[in] enable Whether to enable or disable service introspection content + * \return `RCL_RET_INVALID_ARGUMENT` if the client structure is invalid, + * \return `RCL_RET_OK` if the call was successful + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_configure_client_service_event_message_payload( + rcl_client_t * client, + bool enable); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/node_options.h b/rcl/include/rcl/node_options.h index 551d79437..de2915f96 100644 --- a/rcl/include/rcl/node_options.h +++ b/rcl/include/rcl/node_options.h @@ -54,6 +54,9 @@ typedef struct rcl_node_options_s /// Middleware quality of service settings for /rosout. rmw_qos_profile_t rosout_qos; + + /// Flag to enable introspection features for services related to this node + bool enable_service_introspection; } rcl_node_options_t; /// Return the default node options in a rcl_node_options_t. diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index 2461bd551..877da6ff3 100644 --- a/rcl/include/rcl/service.h +++ b/rcl/include/rcl/service.h @@ -24,11 +24,16 @@ 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/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; @@ -44,9 +49,15 @@ typedef struct rcl_service_options_s { /// Middleware quality of service settings for the service. rmw_qos_profile_t qos; + /// Publisher options for the service event publisher + rcl_publisher_options_t event_publisher_options; /// Custom allocator for the service, used for incidental allocations. /** For default behavior (malloc/free), see: rcl_get_default_allocator() */ rcl_allocator_t allocator; + /// Enable/Disable service introspection features + bool enable_service_introspection; + /// The clock to use for service introspection message timestamps + rcl_clock_t * clock; } rcl_service_options_t; /// Return a rcl_service_t struct with members set to `NULL`. @@ -105,7 +116,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): * @@ -197,7 +208,10 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node); * The defaults are: * * - qos = rmw_qos_profile_services_default + * - event_publisher_options = rcl_publisher_get_default_options() * - allocator = rcl_get_default_allocator() + * - enable_service_introspection = False + * - clock = NULL */ RCL_PUBLIC RCL_WARN_UNUSED @@ -524,6 +538,58 @@ 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. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] server The server on which to enable service introspection + * \param[in] node The node for which the service event publisher is to be associated to + * \param[in] enable Whether to enable or disable service introspection + * \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, or + * \return `RCL_RET_OK` if the call was successful + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_configure_server_service_events( + rcl_service_t * service, + rcl_node_t * node, + bool enable); + +/// Configure if the payload (server response) is included in service event messages. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] server The server on which to enable/disable payload for service event messages. + * \param[in] enable Whether to enable or disable including the payload in the event message. + * \return `RCL_RET_INVALID_ARGUMENT` if the service structure is invalid, + * \return `RCL_RET_OK` if the call was successful + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_configure_server_service_event_message_payload( + rcl_service_t * service, + bool enable); + #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..884f37ea4 --- /dev/null +++ b/rcl/include/rcl/service_introspection.h @@ -0,0 +1,20 @@ +// 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" + +#endif // RCL__SERVICE_INTROSPECTION_H_ diff --git a/rcl/package.xml b/rcl/package.xml index 401155e43..85c61651f 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..6b36c73c6 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -24,23 +24,18 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/publisher.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 "./common.h" - -struct rcl_client_impl_s -{ - rcl_client_options_t options; - rmw_qos_profile_t actual_request_publisher_qos; - rmw_qos_profile_t actual_response_subscription_qos; - rmw_client_t * rmw_handle; - atomic_int_least64_t sequence_number; -}; +#include "./client_impl.h" +#include "./service_event_publisher.h" rcl_client_t rcl_get_zero_initialized_client() @@ -49,6 +44,63 @@ 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->service_event_publisher) { + 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; +} + +static +rcl_ret_t +configure_service_introspection( + const rcl_node_t * node, + struct rcl_client_impl_s * client_impl, + rcl_allocator_t * allocator, + const rcl_client_options_t * options, + const rosidl_service_type_support_t * type_support, + const char * remapped_service_name) +{ + if (!rcl_node_get_options(node)->enable_service_introspection) { + return RCL_RET_OK; + } + + client_impl->service_event_publisher = allocator->zero_allocate( + 1, 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;); + + rcl_service_event_publisher_options_t service_event_options = + rcl_service_event_publisher_get_default_options(); + service_event_options.publisher_options = options->event_publisher_options; + service_event_options.clock = options->clock; + + *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, &service_event_options, + remapped_service_name, type_support); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + allocator->deallocate(client_impl->service_event_publisher, allocator->state); + client_impl->service_event_publisher = NULL; + return ret; + } + + return RCL_RET_OK; +} + rcl_ret_t rcl_client_init( rcl_client_t * client, @@ -91,16 +143,17 @@ rcl_client_init( } else if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; } - goto cleanup; + return ret; } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name); // Allocate space for the implementation struct. - client->impl = (rcl_client_impl_t *)allocator->allocate( - sizeof(rcl_client_impl_t), allocator->state); + 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", ret = RCL_RET_BAD_ALLOC; goto cleanup); + client->impl, "allocating memory failed", + ret = RCL_RET_BAD_ALLOC; goto free_remapped_service_name); // Fill out implementation struct. // rmw handle (create rmw client) // TODO(wjwwood): pass along the allocator to rmw when it supports it @@ -111,26 +164,33 @@ rcl_client_init( &options->qos); if (!client->impl->rmw_handle) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - goto fail; + ret = RCL_RET_ERROR; + goto free_client_impl; + } + + ret = configure_service_introspection( + node, client->impl, allocator, options, type_support, remapped_service_name); + if (RCL_RET_OK != ret) { + goto destroy_client; } // 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 unconfigure_introspection; } 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 unconfigure_introspection; } // ROS specific namespacing conventions avoidance @@ -151,15 +211,29 @@ rcl_client_init( (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; + + goto free_remapped_service_name; + +unconfigure_introspection: + // TODO(clalancette): I don't love casting away the const from node here, + // but the cleanup path goes deep and I didn't want to change 6 or so + // different signatures. + fail_ret = unconfigure_service_introspection((rcl_node_t *)node, client->impl, allocator); + if (RCL_RET_OK != fail_ret) { + // TODO(clalancette): print the error message here + } + +destroy_client: + rmw_ret = rmw_destroy_client(rcl_node_get_rmw_handle(node), client->impl->rmw_handle); + if (RMW_RET_OK != rmw_ret) { + // TODO(clalancette): print the error message here } - ret = fail_ret; - // Fall through to cleanup -cleanup: + +free_client_impl: + allocator->deallocate(client->impl, allocator->state); + client->impl = NULL; + +free_remapped_service_name: allocator->deallocate(remapped_service_name, allocator->state); return ret; } @@ -177,17 +251,26 @@ 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, allocator.state); client->impl = NULL; } @@ -202,7 +285,10 @@ rcl_client_get_default_options() static rcl_client_options_t default_options; // Must set the allocator and qos after because they are not a compile time constant. default_options.qos = rmw_qos_profile_services_default; + default_options.event_publisher_options = rcl_publisher_get_default_options(); default_options.allocator = rcl_get_default_allocator(); + default_options.enable_service_introspection = false; + default_options.clock = NULL; return default_options; } @@ -252,6 +338,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 (rcl_client_get_options(client)->enable_service_introspection) { + 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 +388,25 @@ rcl_take_response_with_info( if (!taken) { return RCL_RET_CLIENT_TAKE_FAILED; } + + if (rcl_client_get_options(client)->enable_service_introspection) { + 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 +469,35 @@ rcl_client_set_on_new_response_callback( user_data); } +rcl_ret_t +rcl_service_introspection_configure_client_service_events( + rcl_client_t * client, + rcl_node_t * node, + bool enable) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(client->impl, RCL_RET_INVALID_ARGUMENT); + + if (enable) { + return rcl_service_introspection_enable(client->impl->service_event_publisher, node); + } + return rcl_service_introspection_disable(client->impl->service_event_publisher, node); +} + +rcl_ret_t +rcl_service_introspection_configure_client_service_event_message_payload( + rcl_client_t * client, + bool enable) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(client->impl, RCL_RET_INVALID_ARGUMENT); + + client->impl->service_event_publisher->impl->options._content_enabled = enable; + + return RCL_RET_OK; +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/client_impl.h b/rcl/src/rcl/client_impl.h new file mode 100644 index 000000000..cf8963c99 --- /dev/null +++ b/rcl/src/rcl/client_impl.h @@ -0,0 +1,35 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef RCL__CLIENT_IMPL_H_ +#define RCL__CLIENT_IMPL_H_ + +#include "rcl/client.h" +#include "rcutils/stdatomic_helper.h" +#include "rmw/types.h" + +#include "./service_event_publisher.h" + +struct rcl_client_impl_s +{ + rcl_client_options_t options; + rmw_qos_profile_t actual_request_publisher_qos; + rmw_qos_profile_t actual_response_subscription_qos; + rmw_client_t * rmw_handle; + atomic_int_least64_t sequence_number; + rcl_service_event_publisher_t * service_event_publisher; +}; + +#endif // RCL__CLIENT_IMPL_H_ diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index 34408f18b..f3c295983 100644 --- a/rcl/src/rcl/node_options.c +++ b/rcl/src/rcl/node_options.c @@ -36,6 +36,7 @@ rcl_node_get_default_options() .arguments = rcl_get_zero_initialized_arguments(), .enable_rosout = true, .rosout_qos = rcl_qos_profile_rosout_default, + .enable_service_introspection = false, }; return default_options; } @@ -61,6 +62,7 @@ rcl_node_options_copy( options_out->use_global_arguments = options->use_global_arguments; options_out->enable_rosout = options->enable_rosout; options_out->rosout_qos = options->rosout_qos; + options_out->enable_service_introspection = options->enable_service_introspection; if (NULL != options->arguments.impl) { return rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); } diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index e1bdb0174..756ee0336 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -17,26 +17,29 @@ extern "C" { #endif -#include "rcl/service.h" - #include #include #include "rcl/error_handling.h" +#include "rcl/graph.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" -struct rcl_service_impl_s -{ - rcl_service_options_t options; - rmw_qos_profile_t actual_request_subscription_qos; - rmw_qos_profile_t actual_response_publisher_qos; - rmw_service_t * rmw_handle; -}; +#include "rosidl_runtime_c/service_type_support_struct.h" +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "./common.h" +#include "./service_event_publisher.h" +#include "./service_impl.h" + rcl_service_t rcl_get_zero_initialized_service() @@ -45,6 +48,63 @@ 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->service_event_publisher) { + 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; +} + +static +rcl_ret_t +configure_service_introspection( + const rcl_node_t * node, + struct rcl_service_impl_s * service_impl, + rcl_allocator_t * allocator, + const rcl_service_options_t * options, + const rosidl_service_type_support_t * type_support, + const char * remapped_service_name) +{ + if (!rcl_node_get_options(node)->enable_service_introspection) { + return RCL_RET_OK; + } + + service_impl->service_event_publisher = allocator->zero_allocate( + 1, 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;); + + rcl_service_event_publisher_options_t service_event_options = + rcl_service_event_publisher_get_default_options(); + service_event_options.publisher_options = options->event_publisher_options; + service_event_options.clock = options->clock; + + *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, &service_event_options, + remapped_service_name, type_support); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + allocator->deallocate(service_impl->service_event_publisher, allocator->state); + service_impl->service_event_publisher = NULL; + return ret; + } + + return RCL_RET_OK; +} + rcl_ret_t rcl_service_init( rcl_service_t * service, @@ -95,16 +155,17 @@ rcl_service_init( } else if (ret != RCL_RET_BAD_ALLOC) { ret = RCL_RET_ERROR; } - goto cleanup; + return ret; } 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); + 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", ret = RCL_RET_BAD_ALLOC; goto cleanup); + service->impl, "allocating memory failed", + ret = RCL_RET_BAD_ALLOC; goto free_remapped_service_name); if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) { RCUTILS_LOG_WARN_NAMED( @@ -122,25 +183,33 @@ rcl_service_init( &options->qos); if (!service->impl->rmw_handle) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - goto fail; + ret = RCL_RET_ERROR; + goto free_service_impl; } + + ret = configure_service_introspection( + node, service->impl, allocator, options, type_support, remapped_service_name); + if (RCL_RET_OK != ret) { + goto destroy_service; + } + // 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 unconfigure_introspection; } 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 unconfigure_introspection; } // ROS specific namespacing conventions is not retrieved by get_actual_qos @@ -159,15 +228,29 @@ rcl_service_init( (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; + + goto free_remapped_service_name; + +unconfigure_introspection: + // TODO(clalancette): I don't love casting away the const from node here, + // but the cleanup path goes deep and I didn't want to change 6 or so + // different signatures. + fail_ret = unconfigure_service_introspection((rcl_node_t *)node, service->impl, allocator); + if (RCL_RET_OK != fail_ret) { + // TODO(clalancette): print the error message here + } + +destroy_service: + rmw_ret = rmw_destroy_service(rcl_node_get_rmw_handle(node), service->impl->rmw_handle); + if (RMW_RET_OK != rmw_ret) { + // TODO(clalancette): print the error message here } - ret = fail_ret; - // Fall through to clean up -cleanup: + +free_service_impl: + allocator->deallocate(service->impl, allocator->state); + service->impl = NULL; + +free_remapped_service_name: allocator->deallocate(remapped_service_name, allocator->state); return ret; } @@ -193,11 +276,19 @@ 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, allocator.state); service->impl = NULL; } @@ -212,7 +303,10 @@ rcl_service_get_default_options() static rcl_service_options_t default_options; // Must set the allocator and qos after because they are not a compile time constant. default_options.qos = rmw_qos_profile_services_default; + default_options.event_publisher_options = rcl_publisher_get_default_options(); default_options.allocator = rcl_get_default_allocator(); + default_options.enable_service_introspection = false; + default_options.clock = NULL; return default_options; } @@ -277,6 +371,18 @@ rcl_take_request_with_info( if (!taken) { return RCL_RET_SERVICE_TAKE_FAILED; } + if (rcl_service_get_options(service)->enable_service_introspection) { + rcl_ret_t rclret = rcl_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 +420,20 @@ rcl_send_response( RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } + + // publish out the introspected content + if (rcl_service_get_options(service)->enable_service_introspection) { + rcl_ret_t ret = rcl_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 +483,35 @@ rcl_service_set_on_new_request_callback( user_data); } +rcl_ret_t +rcl_service_introspection_configure_server_service_events( + rcl_service_t * service, + rcl_node_t * node, + bool enable) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service->impl, RCL_RET_INVALID_ARGUMENT); + + if (enable) { + return rcl_service_introspection_enable(service->impl->service_event_publisher, node); + } + return rcl_service_introspection_disable(service->impl->service_event_publisher, node); +} + +rcl_ret_t +rcl_service_introspection_configure_server_service_event_message_payload( + rcl_service_t * service, + bool enable) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service->impl, RCL_RET_INVALID_ARGUMENT); + + service->impl->service_event_publisher->impl->options._content_enabled = enable; + + return RCL_RET_OK; +} + #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..d7cebf038 --- /dev/null +++ b/rcl/src/rcl/service_event_publisher.c @@ -0,0 +1,373 @@ +// 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/service_introspection.h" + +#include "./client_impl.h" +#include "./service_impl.h" + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/error_handling.h" +#include "rcl/publisher.h" +#include "rcl/node.h" +#include "rcl/time.h" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" +#include "rcutils/shared_library.h" +#include "rmw/error_handling.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" +#include "rosidl_runtime_c/string_functions.h" +#include "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; +} + +rcl_service_event_publisher_options_t +rcl_service_event_publisher_get_default_options() +{ + static rcl_service_event_publisher_options_t default_options; + // Must set the options after because they are not a compile time constant. + default_options._enabled = true; + default_options._content_enabled = true; + default_options.publisher_options = rcl_publisher_get_default_options(); + default_options.clock = NULL; + return default_options; +} + +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->impl, "service_event_publisher's implementation is invalid", + return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + service_event_publisher->impl->service_type_support, + "service_event_publisher's service type support is invalid", return false); + if (!rcl_clock_valid(service_event_publisher->impl->options.clock)) { + RCL_SET_ERROR_MSG("service_event_publisher's clock is invalid"); + return false; + } + return true; +} + +rcl_ret_t rcl_service_event_publisher_init( + rcl_service_event_publisher_t * service_event_publisher, + const rcl_node_t * node, + const rcl_service_event_publisher_options_t * 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(options, 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( + &options->publisher_options.allocator, + "allocator is invalid", return RCL_RET_ERROR); + rcl_allocator_t allocator = options->publisher_options.allocator; + + rcl_ret_t ret = RCL_RET_OK; + + if (service_event_publisher->impl) { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("service event publisher already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + + if (!rcl_clock_valid(options->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); + service_event_publisher->impl = (rcl_service_event_publisher_impl_t *) allocator.allocate( + sizeof(rcl_service_event_publisher_impl_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service_event_publisher->impl, "allocating memory for rcl_service_event_publisher failed", + return RCL_RET_BAD_ALLOC;); + + // Typesupports have static lifetimes + service_event_publisher->impl->service_type_support = service_type_support; + service_event_publisher->impl->options = *options; + + size_t topic_length = strlen(service_name) + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + 1; + service_event_publisher->impl->service_event_topic_name = (char *) allocator.allocate( + topic_length, allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service_event_publisher->impl->service_event_topic_name, + "allocating memory for service introspection topic name failed", + ret = RCL_RET_BAD_ALLOC; goto free_impl;); + + snprintf( + service_event_publisher->impl->service_event_topic_name, + topic_length, + "%s%s", service_name, RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); + + service_event_publisher->impl->options._enabled = false; + service_event_publisher->impl->publisher = NULL; + ret = rcl_service_introspection_enable(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->impl->service_event_topic_name, allocator.state); + +free_impl: + allocator.deallocate(service_event_publisher->impl, allocator.state); + service_event_publisher->impl = NULL; + + 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); + + // Skip checking service_event_publisher and node as they will + // be checked by rcl_service_introspection_disable + rcl_ret_t ret = rcl_service_introspection_disable(service_event_publisher, node); + if (RCL_RET_OK != ret && RCL_RET_ALREADY_SHUTDOWN != ret) { + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + rcutils_reset_error(); + return ret; + } + rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator; + + allocator.deallocate(service_event_publisher->impl->service_event_topic_name, allocator.state); + service_event_publisher->impl->service_event_topic_name = NULL; + + allocator.deallocate(service_event_publisher->impl, allocator.state); + service_event_publisher->impl = 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; + } + + // early exit if service introspection disabled during runtime + if (!service_event_publisher->impl->options._enabled) { + return RCL_RET_OK; + } + + rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + if (!rcl_publisher_is_valid(service_event_publisher->impl->publisher)) { + return RCL_RET_PUBLISHER_INVALID; + } + + rcl_ret_t ret; + + rcl_time_point_value_t now; + ret = rcl_clock_get_now(service_event_publisher->impl->options.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 = 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->impl->options._content_enabled) { + 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->impl->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->impl->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! + // TODO(ihasdapie): Publisher context can become invalidated on shutdown + ret = rcl_publish(service_event_publisher->impl->publisher, service_introspection_message, NULL); + // clean up before error checking + service_event_publisher->impl->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_introspection_enable( + rcl_service_event_publisher_t * service_event_publisher, + const rcl_node_t * node) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_TOPIC_NAME_INVALID); + + if (!rcl_service_event_publisher_is_valid(service_event_publisher)) { + return RCL_RET_ERROR; + } + + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + // need to check if node_opt is disabled + + // Early exit if already enabled + if (service_event_publisher->impl->options._enabled) { + return RCL_RET_OK; + } + + rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_ERROR); + + service_event_publisher->impl->publisher = allocator.allocate( + sizeof(rcl_publisher_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service_event_publisher->impl->publisher, + "allocate service_event_publisher failed in enable", return RCL_RET_BAD_ALLOC); + *service_event_publisher->impl->publisher = rcl_get_zero_initialized_publisher(); + rcl_ret_t ret = rcl_publisher_init( + service_event_publisher->impl->publisher, node, + service_event_publisher->impl->service_type_support->event_typesupport, + service_event_publisher->impl->service_event_topic_name, + &service_event_publisher->impl->options.publisher_options); + if (RCL_RET_OK != ret) { + allocator.deallocate(service_event_publisher->impl->publisher, allocator.state); + service_event_publisher->impl->publisher = NULL; + rcutils_reset_error(); + RCL_SET_ERROR_MSG(rcl_get_error_string().str); + return ret; + } + + service_event_publisher->impl->options._enabled = true; + return RCL_RET_OK; +} + +rcl_ret_t rcl_service_introspection_disable( + rcl_service_event_publisher_t * service_event_publisher, + rcl_node_t * node) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_SHUTDOWN); + + if (!rcl_service_event_publisher_is_valid(service_event_publisher)) { + return RCL_RET_ERROR; + } + + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + + // Early exit if already disabled + if (!service_event_publisher->impl->options._enabled) { + return RCL_RET_OK; + } + + rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_ERROR); + + if (service_event_publisher->impl->publisher) { + rcl_ret_t ret = rcl_publisher_fini(service_event_publisher->impl->publisher, node); + allocator.deallocate(service_event_publisher->impl->publisher, allocator.state); + service_event_publisher->impl->publisher = NULL; + if (RCL_RET_OK != ret) { + rcutils_reset_error(); + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return ret; + } + } + + service_event_publisher->impl->options._enabled = false; + 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..d546b8caa --- /dev/null +++ b/rcl/src/rcl/service_event_publisher.h @@ -0,0 +1,129 @@ +// 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/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_options_s +{ + // Enable/disable service introspection during runtime + bool _enabled; + // Enable/disable including request/response payload in service event message during runtime + bool _content_enabled; + /// Handle to clock for timestamping service events + rcl_clock_t * clock; + /// publisher options for service event publisher + rcl_publisher_options_t publisher_options; +} rcl_service_event_publisher_options_t; + +typedef struct rcl_service_event_publisher_impl_s +{ + /// Handle to publisher for publishing service events + rcl_publisher_t * publisher; + /// Name of service introspection topic: / + char * service_event_topic_name; + /// rcl_service_event_publisher options + rcl_service_event_publisher_options_t options; + /// Handle to service typesupport + const rosidl_service_type_support_t * service_type_support; +} rcl_service_event_publisher_impl_t; + +typedef struct rcl_service_event_publisher_s +{ + /// Pointer to implementation struct + rcl_service_event_publisher_impl_t * impl; +} rcl_service_event_publisher_t; + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_service_event_publisher_options_t +rcl_service_event_publisher_get_default_options(); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_service_event_publisher_t +rcl_get_zero_initialized_service_event_publisher(); + +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, + const rcl_service_event_publisher_options_t * options, + const char * service_name, + const rosidl_service_type_support_t * service_type_support); + +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); + +RCL_PUBLIC +bool +rcl_service_event_publisher_is_valid(const rcl_service_event_publisher_t * service_event_publisher); + +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]); + +/* Enables service introspection by reconstructing the introspection clock and publisher + * + * Does nothing and returns RCL_RET_OK if already enabled + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_enable( + rcl_service_event_publisher_t * service_event_publisher, + const rcl_node_t * node); + +/* Disables service introspection by fini-ing and freeing the introspection clock and publisher + * + * Does nothing and returns RCL_RET_OK if already disabled + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_introspection_disable( + rcl_service_event_publisher_t * service_event_publisher, + rcl_node_t * node); + +#ifdef __cplusplus +} +#endif +#endif // RCL__SERVICE_EVENT_PUBLISHER_H_ diff --git a/rcl/src/rcl/service_impl.h b/rcl/src/rcl/service_impl.h new file mode 100644 index 000000000..8f0e31171 --- /dev/null +++ b/rcl/src/rcl/service_impl.h @@ -0,0 +1,32 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef RCL__SERVICE_IMPL_H_ +#define RCL__SERVICE_IMPL_H_ + + +#include "rcl/service.h" +#include "./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; +}; + +#endif // RCL__SERVICE_IMPL_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..da93a1d92 --- /dev/null +++ b/rcl/test/rcl/test_service_event_publisher.cpp @@ -0,0 +1,668 @@ +// 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: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + rcl_allocator_t allocator; + rcl_ret_t ret; + const rosidl_service_type_support_t * srv_ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + 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(); + const char * name = "test_service_event_publisher_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + node_options.enable_service_introspection = true; + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t 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; + } +}; + +/* 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_service_event_publisher_options_t service_event_publisher_options = + rcl_service_event_publisher_get_default_options(); + rcl_clock_t clock; + service_event_publisher_options.clock = &clock; + + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_init( + &service_event_publisher, this->node_ptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_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_service_event_publisher_options_t service_event_publisher_options = + rcl_service_event_publisher_get_default_options(); + rcl_clock_t clock; + + ret = rcl_service_event_publisher_init( + &service_event_publisher, nullptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_init( + &service_event_publisher, this->node_ptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + service_event_publisher_options.clock = &clock; + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_ALREADY_INIT, 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; + + 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, &service_event_publisher_options, + "123test_service_event_publisher<>h", srv_ts); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, 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, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_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", rcl_publisher_init, "patch rcl_publisher_init to fail", RCL_RET_ERROR); + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; +} + +/* 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_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + rcl_service_event_publisher_options_t service_event_publisher_options = + rcl_service_event_publisher_get_default_options(); + rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + service_event_publisher_options.clock = &clock; + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, &service_event_publisher_options, topic.c_str(), srv_ts); + EXPECT_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); + 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; + }); + + 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); + EXPECT_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_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + rcl_service_event_publisher_options_t service_event_publisher_options = + rcl_service_event_publisher_get_default_options(); + + 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"; + rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + service_event_publisher_options.clock = &clock; + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, &service_event_publisher_options, topic, srv_ts); + EXPECT_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; + + 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; + + ret = rcl_send_service_event_message( + &service_event_publisher, service_msgs__msg__ServiceEventInfo__RESPONSE_RECEIVED, &test_req, 0, + guid); + EXPECT_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; + + 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_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + rcl_service_event_publisher_options_t service_event_publisher_options = + rcl_service_event_publisher_get_default_options(); + rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + service_event_publisher_options.clock = &clock; + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_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.impl->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, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + service_event_publisher.impl->options.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; + + service_event_publisher.impl->options.clock = &clock; + 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_service_event_publisher_t service_event_publisher = + rcl_get_zero_initialized_service_event_publisher(); + rcl_service_event_publisher_options_t service_event_publisher_options = + rcl_service_event_publisher_get_default_options(); + rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + service_event_publisher_options.clock = &clock; + ret = rcl_service_event_publisher_init( + &service_event_publisher, node_ptr, &service_event_publisher_options, + "test_service_event_publisher", srv_ts); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // ok to enable twice, starts enabled + EXPECT_EQ( + RCL_RET_OK, rcl_service_introspection_enable(&service_event_publisher, node_ptr)); + + EXPECT_EQ(RCL_RET_OK, rcl_service_introspection_disable(&service_event_publisher, node_ptr)); + + EXPECT_EQ( + RCL_RET_OK, + rcl_service_introspection_disable(&service_event_publisher, node_ptr)); + + EXPECT_EQ(RCL_RET_OK, rcl_service_introspection_enable(&service_event_publisher, node_ptr)); + + EXPECT_EQ( + RCL_RET_OK, rcl_service_introspection_enable(&service_event_publisher, node_ptr)); + + 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: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + rcl_service_t service; + rcl_client_t client; + rcl_clock_t clock; + rcl_subscription_t subscription; + rosidl_service_type_support_t * srv_ts; + 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(); + node_options.enable_service_introspection = true; + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + const rosidl_service_type_support_t * srv_ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + + // rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &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; + + service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.enable_service_introspection = true; + service_options.clock = &clock; + ret = rcl_service_init(&service, this->node_ptr, srv_ts, srv_name.c_str(), &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + client_options.enable_service_introspection = true; + client_options.clock = &clock; + ret = rcl_client_init(&client, this->node_ptr, srv_ts, srv_name.c_str(), &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init( + &subscription, 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; + } + + void TearDown() + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // not ok, error + ret = rcl_client_fini(&client, this->node_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; + } +}; + +/* 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); + + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); + + 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); + client_request.bool_value = false; + client_request.uint8_value = 1; + client_request.uint32_value = 2; + int64_t sequence_number; + ret = rcl_send_request(&client, &client_request, &sequence_number); + EXPECT_NE(sequence_number, 0); + test_msgs__srv__BasicTypes_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100)); + + { // expect a REQUEST_SENT event + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ret = rcl_take(&subscription, &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); + + rmw_service_info_t header; + ret = rcl_take_request( + &service, &(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 + ret = rcl_take(&subscription, &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(&service, &header.request_id, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { // expect a RESPONSE_SEND event + 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(service_msgs__msg__ServiceEventInfo__RESPONSE_SENT, event_msg.info.event_type); + } + test_msgs__srv__BasicTypes_Request__fini(&service_request); + } + + ASSERT_TRUE( + wait_for_client_to_be_ready(&client, context_ptr, 10, 100)); + + 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); + + rmw_service_info_t header; + ret = rcl_take_response(&client, &(header.request_id), &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + test_msgs__srv__BasicTypes_Response__fini(&client_response); + + { // expect a RESPONSE_RECEIVED event + ret = rcl_take(&subscription, &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); + } + + test_msgs__srv__BasicTypes_Event__fini(&event_msg); +} + +/* 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); + + ret = rcl_service_introspection_configure_server_service_events(&service, node_ptr, false); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); + + 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); + client_request.bool_value = false; + client_request.uint8_value = 1; + client_request.uint32_value = 2; + int64_t sequence_number; + ret = rcl_send_request(&client, &client_request, &sequence_number); + EXPECT_NE(sequence_number, 0); + test_msgs__srv__BasicTypes_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100)); + + { // expect a REQUEST_SENT event + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ret = rcl_take(&subscription, &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); + + rmw_service_info_t header; + ret = rcl_take_request( + &service, &(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(&subscription, &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(&service, &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 + 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_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; + } + test_msgs__srv__BasicTypes_Request__fini(&service_request); + } + + ASSERT_TRUE( + wait_for_client_to_be_ready(&client, context_ptr, 10, 100)); + + 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); + + rmw_service_info_t header; + ret = rcl_take_response(&client, &(header.request_id), &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + test_msgs__srv__BasicTypes_Response__fini(&client_response); + + { // expect a RESPONSE_RECEIVED event + ret = rcl_take(&subscription, &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); + } + + test_msgs__srv__BasicTypes_Event__fini(&event_msg); +}