From db4ebe7f4d225b786b5adea0723c57cd7439fe60 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 21 Feb 2023 19:05:25 +0000 Subject: [PATCH] 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));