From cbc03e8a6062d85ed7b3c171c3d10e353e211e05 Mon Sep 17 00:00:00 2001 From: Brian Chen Date: Thu, 23 Jun 2022 10:11:06 -0700 Subject: [PATCH 1/6] Add in the APIs to enable service introspection. This PR adds in the rcl implementation of service introspection. In particular, what it adds in are the implementations of enabling and disabling service introspection, as well as creating the publisher when the introspection is enabled. Signed-off-by: Brian Chen Signed-off-by: Ivan Santiago Paunovic Signed-off-by: Chris Lalancette --- rcl/CMakeLists.txt | 5 + rcl/include/rcl/client.h | 69 ++ rcl/include/rcl/node_options.h | 3 + rcl/include/rcl/service.h | 68 +- rcl/include/rcl/service_introspection.h | 20 + rcl/package.xml | 1 + rcl/src/rcl/client.c | 205 +++++- rcl/src/rcl/client_impl.h | 35 + rcl/src/rcl/node_options.c | 2 + rcl/src/rcl/service.c | 201 +++++- rcl/src/rcl/service_event_publisher.c | 373 ++++++++++ rcl/src/rcl/service_event_publisher.h | 129 ++++ rcl/src/rcl/service_impl.h | 32 + rcl/test/CMakeLists.txt | 9 + rcl/test/rcl/test_service_event_publisher.cpp | 668 ++++++++++++++++++ 15 files changed, 1767 insertions(+), 53 deletions(-) create mode 100644 rcl/include/rcl/service_introspection.h create mode 100644 rcl/src/rcl/client_impl.h create mode 100644 rcl/src/rcl/service_event_publisher.c create mode 100644 rcl/src/rcl/service_event_publisher.h create mode 100644 rcl/src/rcl/service_impl.h create mode 100644 rcl/test/rcl/test_service_event_publisher.cpp diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 778e09084..a2aa1d65a 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rosidl_runtime_c REQUIRED) +find_package(service_msgs REQUIRED) find_package(tracetools REQUIRED) include(cmake/rcl_set_symbol_visibility_hidden.cmake) @@ -59,6 +60,7 @@ set(${PROJECT_NAME}_sources src/rcl/rmw_implementation_identifier_check.c src/rcl/security.c src/rcl/service.c + src/rcl/service_event_publisher.c src/rcl/subscription.c src/rcl/time.c src/rcl/timer.c @@ -70,6 +72,7 @@ set(${PROJECT_NAME}_sources add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources}) target_include_directories(${PROJECT_NAME} PUBLIC "$" + "$" "$") # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} @@ -81,6 +84,7 @@ ament_target_dependencies(${PROJECT_NAME} "rmw_implementation" ${RCL_LOGGING_IMPL} "rosidl_runtime_c" + "service_msgs" "tracetools" ) @@ -121,6 +125,7 @@ ament_export_dependencies(rmw) ament_export_dependencies(rcutils) ament_export_dependencies(${RCL_LOGGING_IMPL}) ament_export_dependencies(rosidl_runtime_c) +ament_export_dependencies(service_msgs) ament_export_dependencies(tracetools) if(BUILD_TESTING) diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h index 224f74b8c..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 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..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); +} From 296e51813c349d36261ce4af6b32df71a6393f20 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 21 Feb 2023 19:05:25 +0000 Subject: [PATCH 2/6] Implement service introspection. The idea here is that by default, clients and services are created just like they were before. However, we add an additional API where the user can choose to configure service introspection, either to turn it OFF, send out METADATA, or send out CONTENTS (and METADATA). There are 4 different events that can get sent out if introspection is configured for METADATA or CONTENTS; REQUEST_SENT (from the client), REQUEST_RECEIVED (from the service), RESPONSE_SENT (from the service), or RESPONSE_RECEIVED (from the client). For each of these, a separate message is sent out a topic called _service_event , so an outside observer can listen. Signed-off-by: Chris Lalancette --- rcl/include/rcl/client.h | 66 ++-- rcl/include/rcl/node_options.h | 3 - rcl/include/rcl/service.h | 65 ++-- rcl/include/rcl/service_introspection.h | 11 + rcl/src/rcl/client.c | 179 +++++------ rcl/src/rcl/client_impl.h | 35 --- rcl/src/rcl/node_options.c | 2 - rcl/src/rcl/service.c | 180 +++++------ rcl/src/rcl/service_event_publisher.c | 241 +++++--------- rcl/src/rcl/service_event_publisher.h | 175 ++++++++--- rcl/src/rcl/service_impl.h | 32 -- rcl/test/rcl/test_service_event_publisher.cpp | 296 +++++++++--------- 12 files changed, 572 insertions(+), 713 deletions(-) delete mode 100644 rcl/src/rcl/client_impl.h delete mode 100644 rcl/src/rcl/service_impl.h diff --git a/rcl/include/rcl/client.h b/rcl/include/rcl/client.h index 71d5a4294..f7995fabc 100644 --- a/rcl/include/rcl/client.h +++ b/rcl/include/rcl/client.h @@ -29,6 +29,7 @@ extern "C" #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" @@ -49,15 +50,9 @@ 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`. @@ -205,10 +200,7 @@ 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 @@ -510,6 +502,10 @@ rcl_client_set_on_new_response_callback( /// 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 @@ -520,47 +516,29 @@ rcl_client_set_on_new_response_callback( * 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 + * \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_service_introspection_configure_client_service_events( +rcl_client_configure_service_introspection( 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); + 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 } diff --git a/rcl/include/rcl/node_options.h b/rcl/include/rcl/node_options.h index de2915f96..551d79437 100644 --- a/rcl/include/rcl/node_options.h +++ b/rcl/include/rcl/node_options.h @@ -54,9 +54,6 @@ 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 877da6ff3..dec7dc913 100644 --- a/rcl/include/rcl/service.h +++ b/rcl/include/rcl/service.h @@ -29,6 +29,7 @@ extern "C" #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" @@ -49,15 +50,9 @@ 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`. @@ -208,10 +203,7 @@ 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 @@ -538,9 +530,13 @@ rcl_service_set_on_new_request_callback( rcl_event_callback_t callback, const void * user_data); -/// Configure service introspection features for the service +/// 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 @@ -551,44 +547,29 @@ rcl_service_set_on_new_request_callback( * 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 + * \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_introspection_configure_server_service_events( +rcl_service_configure_service_introspection( 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); + 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 } diff --git a/rcl/include/rcl/service_introspection.h b/rcl/include/rcl/service_introspection.h index 884f37ea4..1055d6b9d 100644 --- a/rcl/include/rcl/service_introspection.h +++ b/rcl/include/rcl/service_introspection.h @@ -17,4 +17,15 @@ #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/src/rcl/client.c b/rcl/src/rcl/client.c index 6b36c73c6..63d905855 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -25,6 +25,7 @@ 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" @@ -33,10 +34,22 @@ extern "C" #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 "./client_impl.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; + char * remapped_service_name; +}; + rcl_client_t rcl_get_zero_initialized_client() { @@ -51,7 +64,11 @@ unconfigure_service_introspection( struct rcl_client_impl_s * client_impl, rcl_allocator_t * allocator) { - if (!client_impl->service_event_publisher) { + if (client_impl == NULL) { + return RCL_RET_ERROR; + } + + if (client_impl->service_event_publisher == NULL) { return RCL_RET_OK; } @@ -63,44 +80,6 @@ unconfigure_service_introspection( 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, @@ -109,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; @@ -128,50 +105,45 @@ 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; } - return ret; + 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->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 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 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); 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; + goto free_remapped_service_name; } // get actual qos, and store it @@ -181,7 +153,7 @@ rcl_client_init( if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); - goto unconfigure_introspection; + goto destroy_client; } rmw_ret = rmw_client_response_subscription_get_actual_qos( @@ -190,7 +162,7 @@ rcl_client_init( if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); - goto unconfigure_introspection; + goto destroy_client; } // ROS specific namespacing conventions avoidance @@ -204,37 +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 free_remapped_service_name; + client->impl->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 - } + 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) { - // TODO(clalancette): print the error message here + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); } +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; -free_remapped_service_name: - allocator->deallocate(remapped_service_name, allocator->state); return ret; } @@ -271,6 +236,9 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node) 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; } @@ -285,10 +253,7 @@ 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; } @@ -339,7 +304,7 @@ rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t } rcutils_atomic_exchange_int64_t(&client->impl->sequence_number, *sequence_number); - if (rcl_client_get_options(client)->enable_service_introspection) { + 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) { @@ -389,7 +354,7 @@ rcl_take_response_with_info( return RCL_RET_CLIENT_TAKE_FAILED; } - if (rcl_client_get_options(client)->enable_service_introspection) { + 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) { @@ -470,32 +435,48 @@ rcl_client_set_on_new_response_callback( } rcl_ret_t -rcl_service_introspection_configure_client_service_events( +rcl_client_configure_service_introspection( rcl_client_t * client, rcl_node_t * node, - bool enable) + 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) { - RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + 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(client->impl, 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); - if (enable) { - return rcl_service_introspection_enable(client->impl->service_event_publisher, node); + rcl_allocator_t allocator = client->impl->options.allocator; + + if (introspection_state == RCL_SERVICE_INTROSPECTION_OFF) { + return unconfigure_service_introspection(node, client->impl, &allocator); } - 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); + 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->impl->options._content_enabled = enable; + 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;); - return RCL_RET_OK; + *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 diff --git a/rcl/src/rcl/client_impl.h b/rcl/src/rcl/client_impl.h deleted file mode 100644 index cf8963c99..000000000 --- a/rcl/src/rcl/client_impl.h +++ /dev/null @@ -1,35 +0,0 @@ -// 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 f3c295983..34408f18b 100644 --- a/rcl/src/rcl/node_options.c +++ b/rcl/src/rcl/node_options.c @@ -36,7 +36,6 @@ 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; } @@ -62,7 +61,6 @@ 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 756ee0336..78eb70d0f 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -17,11 +17,12 @@ 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" @@ -34,12 +35,19 @@ extern "C" #include "tracetools/tracetools.h" #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" +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 rcl_get_zero_initialized_service() @@ -55,7 +63,11 @@ unconfigure_service_introspection( struct rcl_service_impl_s * service_impl, rcl_allocator_t * allocator) { - if (!service_impl->service_event_publisher) { + if (service_impl == NULL) { + return RCL_RET_ERROR; + } + + if (service_impl->service_event_publisher == NULL) { return RCL_RET_OK; } @@ -67,44 +79,6 @@ unconfigure_service_introspection( 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, @@ -120,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; @@ -140,32 +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; } - return ret; + 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->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 free_remapped_service_name); + 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( @@ -179,18 +151,12 @@ 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); 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; + goto free_remapped_service_name; } // get actual qos, and store it @@ -200,7 +166,7 @@ rcl_service_init( if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); - goto unconfigure_introspection; + goto destroy_service; } rmw_ret = rmw_service_response_publisher_get_actual_qos( @@ -209,7 +175,7 @@ rcl_service_init( if (RMW_RET_OK != rmw_ret) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); - goto unconfigure_introspection; + goto destroy_service; } // ROS specific namespacing conventions is not retrieved by get_actual_qos @@ -221,37 +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 free_remapped_service_name; + service->impl->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 - } + 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) { - // TODO(clalancette): print the error message here + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); } +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; -free_remapped_service_name: - allocator->deallocate(remapped_service_name, allocator->state); return ret; } @@ -289,6 +248,9 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) 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; } @@ -303,10 +265,7 @@ 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; } @@ -371,7 +330,7 @@ rcl_take_request_with_info( if (!taken) { return RCL_RET_SERVICE_TAKE_FAILED; } - if (rcl_service_get_options(service)->enable_service_introspection) { + 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, @@ -422,7 +381,7 @@ rcl_send_response( } // publish out the introspected content - if (rcl_service_get_options(service)->enable_service_introspection) { + 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, @@ -484,32 +443,49 @@ rcl_service_set_on_new_request_callback( } rcl_ret_t -rcl_service_introspection_configure_server_service_events( +rcl_service_configure_service_introspection( rcl_service_t * service, rcl_node_t * node, - bool enable) + 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) { - RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); + 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(service->impl, 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); - if (enable) { - return rcl_service_introspection_enable(service->impl->service_event_publisher, node); + rcl_allocator_t allocator = service->impl->options.allocator; + + if (introspection_state == RCL_SERVICE_INTROSPECTION_OFF) { + return unconfigure_service_introspection(node, service->impl, &allocator); } - 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); + 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->impl->options._content_enabled = enable; + 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;); - return RCL_RET_OK; + *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 diff --git a/rcl/src/rcl/service_event_publisher.c b/rcl/src/rcl/service_event_publisher.c index d7cebf038..8b3e8c16c 100644 --- a/rcl/src/rcl/service_event_publisher.c +++ b/rcl/src/rcl/service_event_publisher.c @@ -16,24 +16,17 @@ #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/service_introspection.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() @@ -42,40 +35,55 @@ rcl_service_event_publisher_t rcl_get_zero_initialized_service_event_publisher() 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->service_type_support, "service_event_publisher's service type support is invalid", return false); - if (!rcl_clock_valid(service_event_publisher->impl->options.clock)) { + 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, - const rcl_service_event_publisher_options_t * options, + rcl_clock_t * clock, + const rcl_publisher_options_t publisher_options, const char * service_name, const rosidl_service_type_support_t * service_type_support) { @@ -89,28 +97,22 @@ rcl_ret_t rcl_service_event_publisher_init( 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, + &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; + rcl_allocator_t allocator = publisher_options.allocator; - 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; - } + rcl_ret_t ret = RCL_RET_OK; if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; } - if (!rcl_clock_valid(options->clock)) { + if (!rcl_clock_valid(clock)) { rcutils_reset_error(); RCL_SET_ERROR_MSG("clock is invalid"); return RCL_RET_ERROR; @@ -118,46 +120,37 @@ rcl_ret_t rcl_service_event_publisher_init( 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; + 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->impl->service_event_topic_name = (char *) allocator.allocate( + service_event_publisher->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, + service_event_publisher->service_event_topic_name, "allocating memory for service introspection topic name failed", - ret = RCL_RET_BAD_ALLOC; goto free_impl;); + return RCL_RET_BAD_ALLOC;); snprintf( - service_event_publisher->impl->service_event_topic_name, + service_event_publisher->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); + 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->impl->service_event_topic_name, allocator.state); - -free_impl: - allocator.deallocate(service_event_publisher->impl, allocator.state); - service_event_publisher->impl = NULL; + allocator.deallocate(service_event_publisher->service_event_topic_name, allocator.state); return ret; } @@ -173,21 +166,28 @@ rcl_ret_t rcl_service_event_publisher_fini( 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; + if (!rcl_service_event_publisher_is_valid(service_event_publisher)) { + return RCL_RET_ERROR; } - 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; + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } - allocator.deallocate(service_event_publisher->impl, allocator.state); - service_event_publisher->impl = NULL; + 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; } @@ -211,22 +211,21 @@ rcl_ret_t rcl_send_service_event_message( return RCL_RET_ERROR; } - // early exit if service introspection disabled during runtime - if (!service_event_publisher->impl->options._enabled) { - return RCL_RET_OK; + if (service_event_publisher->introspection_state == RCL_SERVICE_INTROSPECTION_OFF) { + return RCL_RET_ERROR; } - rcl_allocator_t allocator = service_event_publisher->impl->options.publisher_options.allocator; + 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->impl->publisher)) { + 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->impl->options.clock, &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); @@ -243,20 +242,20 @@ rcl_ret_t rcl_send_service_event_message( memcpy(info.client_gid, guid, 16); void * service_introspection_message; - if (!service_event_publisher->impl->options._content_enabled) { + 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->impl->service_type_support->event_message_create_handle_function( + 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->impl->service_type_support->event_message_create_handle_function( + service_event_publisher->service_type_support->event_message_create_handle_function( &info, &allocator, NULL, ros_response_request); break; default: @@ -268,10 +267,9 @@ rcl_ret_t rcl_send_service_event_message( 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); + ret = rcl_publish(service_event_publisher->publisher, service_introspection_message, NULL); // clean up before error checking - service_event_publisher->impl->service_type_support->event_message_destroy_handle_function( + service_event_publisher->service_type_support->event_message_destroy_handle_function( service_introspection_message, &allocator); if (RCL_RET_OK != ret) { rcutils_reset_error(); @@ -281,93 +279,16 @@ rcl_ret_t rcl_send_service_event_message( return ret; } -rcl_ret_t rcl_service_introspection_enable( +rcl_ret_t +rcl_service_event_publisher_change_state( rcl_service_event_publisher_t * service_event_publisher, - const rcl_node_t * node) + rcl_service_introspection_state_t introspection_state) { - 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->introspection_state = introspection_state; - 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 index d546b8caa..05f9af481 100644 --- a/rcl/src/rcl/service_event_publisher.h +++ b/rcl/src/rcl/service_event_publisher.h @@ -23,63 +23,109 @@ extern "C" #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_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 +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; - /// rcl_service_event_publisher options - rcl_service_event_publisher_options_t options; + /// 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_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(); - +/// 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, - const rcl_service_event_publisher_options_t * options, + 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 @@ -87,10 +133,58 @@ 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 @@ -101,27 +195,18 @@ rcl_send_service_event_message( 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 +/// 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_WARN_UNUSED rcl_ret_t -rcl_service_introspection_enable( +rcl_service_event_publisher_change_state( 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); + rcl_service_introspection_state_t introspection_state); #ifdef __cplusplus } diff --git a/rcl/src/rcl/service_impl.h b/rcl/src/rcl/service_impl.h deleted file mode 100644 index 8f0e31171..000000000 --- a/rcl/src/rcl/service_impl.h +++ /dev/null @@ -1,32 +0,0 @@ -// 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/rcl/test_service_event_publisher.cpp b/rcl/test/rcl/test_service_event_publisher.cpp index da93a1d92..bab3e0c03 100644 --- a/rcl/test/rcl/test_service_event_publisher.cpp +++ b/rcl/test/rcl/test_service_event_publisher.cpp @@ -53,6 +53,7 @@ class CLASSNAME (TestServiceEventPublisherFixture, RMW_IMPLEMENTATION) : public 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; @@ -74,7 +75,6 @@ class CLASSNAME (TestServiceEventPublisherFixture, RMW_IMPLEMENTATION) : public *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; } @@ -100,18 +100,19 @@ TEST_F( { 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; + ASSERT_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, + &service_event_publisher, this->node_ptr, &clock, rcl_publisher_get_default_options(), "test_service_event_publisher", srv_ts); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + 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); @@ -125,7 +126,6 @@ TEST_F( 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); @@ -138,55 +138,46 @@ TEST_F( { 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, + &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, &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, + &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_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; - + ASSERT_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, + &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), "test_service_event_publisher", srv_ts); - EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + 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, &service_event_publisher_options, + &service_event_publisher, node_ptr, &clock, 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, &service_event_publisher_options, + &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), "test_service_event_publisher", srv_ts); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + 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; @@ -196,9 +187,10 @@ TEST_F( 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, + &service_event_publisher, node_ptr, &clock, 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 @@ -214,21 +206,24 @@ TEST_F( 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; + ASSERT_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, topic.c_str(), srv_ts); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + &service_event_publisher, node_ptr, &clock, 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); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + 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); @@ -247,29 +242,26 @@ TEST_F( 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; - } + 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); - } + + 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( @@ -278,21 +270,23 @@ TEST_F( { 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; + ASSERT_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, topic, srv_ts); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + &service_event_publisher, node_ptr, &clock, 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); @@ -305,14 +299,16 @@ TEST_F( &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); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + 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; @@ -324,20 +320,17 @@ TEST_F( { 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; + ASSERT_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, + &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), "test_service_event_publisher", srv_ts); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + 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.impl->publisher, node_ptr); + 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); @@ -345,17 +338,18 @@ TEST_F( 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, + &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), "test_service_event_publisher", srv_ts); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - service_event_publisher.impl->options.clock = nullptr; + 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.impl->options.clock = &clock; + service_event_publisher.clock = &clock; ret = rcl_service_event_publisher_fini(&service_event_publisher, node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } @@ -366,31 +360,32 @@ TEST_F( { 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; + ASSERT_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, + &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), "test_service_event_publisher", srv_ts); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - // ok to enable twice, starts enabled + // ok to enable twice 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)); - + RCL_RET_OK, + rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_METADATA)); 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)); + rcl_service_event_publisher_change_state( + &service_event_publisher, RCL_SERVICE_INTROSPECTION_METADATA)); EXPECT_EQ( - RCL_RET_OK, rcl_service_introspection_enable(&service_event_publisher, node_ptr)); + 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; @@ -400,13 +395,6 @@ class CLASSNAME (TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMP : 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(); @@ -428,10 +416,9 @@ class CLASSNAME (TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMP *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 = + srv_ts = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); // rcl_clock_t clock; @@ -443,18 +430,24 @@ class CLASSNAME (TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMP 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; + ret = rcl_service_configure_service_introspection( + &service, this->node_ptr, &clock, srv_ts, rcl_publisher_get_default_options(), + RCL_SERVICE_INTROSPECTION_CONTENTS); + 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; + ret = rcl_client_configure_service_introspection( + &client, this->node_ptr, &clock, srv_ts, rcl_publisher_get_default_options(), + RCL_SERVICE_INTROSPECTION_CONTENTS); + 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( @@ -478,6 +471,15 @@ class CLASSNAME (TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMP 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; + rcl_client_t client; + rcl_clock_t clock; + rcl_subscription_t subscription; + const rosidl_service_type_support_t * srv_ts; }; /* Whole test of service event publisher with service, client, and subscription @@ -507,49 +509,44 @@ TEST_F( 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); - } + // 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_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); + 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); + 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); - } + // 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; + 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); - } + // 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)); @@ -558,17 +555,16 @@ TEST_F( 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); - } + // 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(1U, event_msg.response.size); + ASSERT_EQ(2U, event_msg.response.data[0].uint32_value); test_msgs__srv__BasicTypes_Event__fini(&event_msg); } @@ -584,7 +580,9 @@ TEST_F( 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); + ret = rcl_service_configure_service_introspection( + &service, node_ptr, &clock, srv_ts, rcl_publisher_get_default_options(), + RCL_SERVICE_INTROSPECTION_OFF); 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)); From f4a62630ed7b5e185580b1cc425747da5cc0dfa6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 23 Feb 2023 20:32:05 +0000 Subject: [PATCH 3/6] Minor bug fix when shutting down. Signed-off-by: Chris Lalancette --- rcl/src/rcl/service_event_publisher.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/src/rcl/service_event_publisher.c b/rcl/src/rcl/service_event_publisher.c index 8b3e8c16c..ddf2c8be9 100644 --- a/rcl/src/rcl/service_event_publisher.c +++ b/rcl/src/rcl/service_event_publisher.c @@ -170,7 +170,7 @@ rcl_ret_t rcl_service_event_publisher_fini( return RCL_RET_ERROR; } - if (!rcl_node_is_valid(node)) { + if (!rcl_node_is_valid_except_context(node)) { return RCL_RET_NODE_INVALID; } From fe72a3542c2dbe941034a76f0db075aed857b37e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 24 Feb 2023 13:47:59 +0000 Subject: [PATCH 4/6] Fix tests to work with Connext. Along the way, I noticed quite a few potential bugs, so this ends up mostly being a rewrite of the tests. With this in place, tests pass on all RMWs. Signed-off-by: Chris Lalancette --- rcl/test/rcl/test_service_event_publisher.cpp | 352 ++++++++++-------- rcl/test/rcl/wait_for_entity_helpers.cpp | 27 ++ rcl/test/rcl/wait_for_entity_helpers.hpp | 8 + 3 files changed, 228 insertions(+), 159 deletions(-) diff --git a/rcl/test/rcl/test_service_event_publisher.cpp b/rcl/test/rcl/test_service_event_publisher.cpp index bab3e0c03..bcb319917 100644 --- a/rcl/test/rcl/test_service_event_publisher.cpp +++ b/rcl/test/rcl/test_service_event_publisher.cpp @@ -47,49 +47,63 @@ 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( { - 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; - } + 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"; + 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 = rcl_node_fini(this->node_ptr); + 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 @@ -100,13 +114,10 @@ TEST_F( { rcl_service_event_publisher_t service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); - rcl_clock_t clock; - - ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_ret_t ret; ret = rcl_service_event_publisher_init( - &service_event_publisher, this->node_ptr, &clock, rcl_publisher_get_default_options(), + &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; @@ -139,6 +150,7 @@ TEST_F( 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(), @@ -152,10 +164,8 @@ TEST_F( EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; rcutils_reset_error(); - ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_service_event_publisher_init( - &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), + &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; @@ -168,14 +178,14 @@ TEST_F( service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); ret = rcl_service_event_publisher_init( - &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), + &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, rcl_publisher_get_default_options(), + &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; @@ -187,7 +197,7 @@ TEST_F( 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, &clock, rcl_publisher_get_default_options(), + &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(); @@ -203,15 +213,13 @@ TEST_F( 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(); - rcl_clock_t clock; - ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_service_event_publisher_init( - &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), + &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( @@ -234,6 +242,8 @@ TEST_F( 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); @@ -268,16 +278,17 @@ 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"; - rcl_clock_t clock; - ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_service_event_publisher_init( - &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), topic, srv_ts); + &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( @@ -318,13 +329,13 @@ 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(); - rcl_clock_t clock; - ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_service_event_publisher_init( - &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), + &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; @@ -338,7 +349,7 @@ TEST_F( service_event_publisher = rcl_get_zero_initialized_service_event_publisher(); ret = rcl_service_event_publisher_init( - &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), + &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; @@ -349,7 +360,7 @@ TEST_F( EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; rcutils_reset_error(); - service_event_publisher.clock = &clock; + 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; } @@ -358,13 +369,13 @@ 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(); - rcl_clock_t clock; - ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_service_event_publisher_init( - &service_event_publisher, node_ptr, &clock, rcl_publisher_get_default_options(), + &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; @@ -399,74 +410,97 @@ class CLASSNAME (TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMP { 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( { - 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; - } + 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; - srv_ts = - ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); - // rcl_clock_t clock; - ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + 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; - service = rcl_get_zero_initialized_service(); + 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(&service, this->node_ptr, srv_ts, srv_name.c_str(), &service_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( - &service, this->node_ptr, &clock, srv_ts, rcl_publisher_get_default_options(), + 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; - client = rcl_get_zero_initialized_client(); + 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(&client, this->node_ptr, srv_ts, srv_name.c_str(), &client_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( - &client, this->node_ptr, &clock, srv_ts, rcl_publisher_get_default_options(), + 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; - subscription = rcl_get_zero_initialized_subscription(); + 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( - &subscription, this->node_ptr, srv_ts->event_typesupport, service_event_topic.c_str(), - &subscription_options); + 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 = 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); + 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; @@ -475,11 +509,12 @@ class CLASSNAME (TestServiceEventPublisherWithServicesAndClientsFixture, RMW_IMP protected: 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; - const rosidl_service_type_support_t * srv_ts; + 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 @@ -492,26 +527,27 @@ TEST_F( 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)); + 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(&client, &client_request, &sequence_number); + ret = rcl_send_request(this->client_ptr, &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)); + 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(&subscription, context_ptr, 10, 100)); - ret = rcl_take(&subscription, &event_msg, &message_info, nullptr); + 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); @@ -524,49 +560,53 @@ TEST_F( 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( - &service, &(header.request_id), &service_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 - ret = rcl_take(&subscription, &event_msg, &message_info, nullptr); + 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(&service, &header.request_id, &service_response); + 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 - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({test_msgs__srv__BasicTypes_Event__fini(&event_msg);}); - ret = rcl_take(&subscription, &event_msg, &message_info, nullptr); + 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_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); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + {test_msgs__srv__BasicTypes_Response__fini(&client_response);}); - ret = rcl_take_response(&client, &(header.request_id), &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; - test_msgs__srv__BasicTypes_Response__fini(&client_response); // expect a RESPONSE_RECEIVED event - ret = rcl_take(&subscription, &event_msg, &message_info, nullptr); + 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); - - test_msgs__srv__BasicTypes_Event__fini(&event_msg); } /* Integration level test with disabling service events @@ -579,88 +619,82 @@ TEST_F( 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( - &service, node_ptr, &clock, srv_ts, rcl_publisher_get_default_options(), + 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; - 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); + 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(&client, &client_request, &sequence_number); + ret = rcl_send_request(this->client_ptr, &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); - } + ASSERT_TRUE(wait_for_service_to_be_ready(this->service_ptr, this->context_ptr, 10, 100)); - { - 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);}); + // 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_Request service_request; - memset(&service_request, 0, sizeof(test_msgs__srv__BasicTypes_Request)); - test_msgs__srv__BasicTypes_Request__init(&service_request); + 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);}); - 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); + 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);}); - { // 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; - } + 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); - 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 + ret = rcl_take(this->subscription_ptr, &event_msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, 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); - } + 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; - ASSERT_TRUE( - wait_for_client_to_be_ready(&client, context_ptr, 10, 100)); + // 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);}); - rmw_service_info_t header; - ret = rcl_take_response(&client, &(header.request_id), &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; - 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); + // 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 From 8b60900622a1732c84ce2f39e35a978176589fde Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 24 Feb 2023 21:11:29 +0000 Subject: [PATCH 5/6] Scope patch_to_fail. Signed-off-by: Chris Lalancette --- rcl/test/rcl/test_service_event_publisher.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/rcl/test/rcl/test_service_event_publisher.cpp b/rcl/test/rcl/test_service_event_publisher.cpp index bcb319917..f6ee979c4 100644 --- a/rcl/test/rcl/test_service_event_publisher.cpp +++ b/rcl/test/rcl/test_service_event_publisher.cpp @@ -194,13 +194,15 @@ TEST_F( 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, 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(); + { + 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 From 6fcbd03d5c21fc05360e67a524f05737b46e34b4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Sat, 25 Feb 2023 01:38:40 +0000 Subject: [PATCH 6/6] Fix for warning on Windows. Signed-off-by: Chris Lalancette --- rcl/src/rcl/service_event_publisher.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/src/rcl/service_event_publisher.c b/rcl/src/rcl/service_event_publisher.c index ddf2c8be9..f262dba70 100644 --- a/rcl/src/rcl/service_event_publisher.c +++ b/rcl/src/rcl/service_event_publisher.c @@ -234,7 +234,7 @@ rcl_ret_t rcl_send_service_event_message( rosidl_service_introspection_info_t info = { .event_type = event_type, - .stamp_sec = RCL_NS_TO_S(now), + .stamp_sec = (int32_t)RCL_NS_TO_S(now), .stamp_nanosec = now % (1000LL * 1000LL * 1000LL), .sequence_number = sequence_number, };