From a4633b83fa45eead06a43e83c7bce34224c70faf Mon Sep 17 00:00:00 2001 From: Hans-Joachim Krauch Date: Thu, 6 Jul 2023 16:43:31 -0300 Subject: [PATCH] Add `~/get_type_description` service (rep2011) (#1052) * Add ~/get_type_description service API * Add node type cache * Register subscription, publication, service and action types with node type cache * Add functions to convert between rosidl_runtime_c / type_description_interfaces structs RCL does not initialize the get_type_description service itself, instead providing a full enough API for full language clients to initialize it and register its callback within their threading/execution framework Signed-off-by: Hans-Joachim Krauch Signed-off-by: Emerson Knapp --- rcl/CMakeLists.txt | 2 + rcl/include/rcl/node.h | 112 ++++++ rcl/include/rcl/node_options.h | 3 - rcl/include/rcl/node_type_cache.h | 174 +++++++++ .../rcl/type_description_conversions.h | 133 +++++++ rcl/src/rcl/client.c | 23 ++ rcl/src/rcl/node.c | 184 ++++++++- rcl/src/rcl/node_impl.h | 37 ++ rcl/src/rcl/node_options.c | 2 - rcl/src/rcl/node_type_cache.c | 245 ++++++++++++ rcl/src/rcl/publisher.c | 25 +- rcl/src/rcl/publisher_impl.h | 1 + rcl/src/rcl/service.c | 23 ++ rcl/src/rcl/subscription.c | 23 ++ rcl/src/rcl/subscription_impl.h | 1 + rcl/src/rcl/type_description_conversions.c | 309 +++++++++++++++ rcl/test/CMakeLists.txt | 26 ++ .../rcl/test_get_type_description_service.cpp | 360 ++++++++++++++++++ rcl/test/rcl/test_node_type_cache.cpp | 186 +++++++++ .../rcl/test_type_description_conversions.cpp | 112 ++++++ rcl_action/src/rcl_action/action_client.c | 21 +- .../src/rcl_action/action_client_impl.h | 1 + rcl_action/src/rcl_action/action_server.c | 21 + .../src/rcl_action/action_server_impl.h | 1 + 24 files changed, 2001 insertions(+), 24 deletions(-) create mode 100644 rcl/include/rcl/node_type_cache.h create mode 100644 rcl/include/rcl/type_description_conversions.h create mode 100644 rcl/src/rcl/node_impl.h create mode 100644 rcl/src/rcl/node_type_cache.c create mode 100644 rcl/src/rcl/type_description_conversions.c create mode 100644 rcl/test/rcl/test_get_type_description_service.cpp create mode 100644 rcl/test/rcl/test_node_type_cache.cpp create mode 100644 rcl/test/rcl/test_type_description_conversions.cpp diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index eef888492..c5092071e 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -59,6 +59,7 @@ set(${PROJECT_NAME}_sources src/rcl/network_flow_endpoints.c src/rcl/node.c src/rcl/node_options.c + src/rcl/node_type_cache.c src/rcl/publisher.c src/rcl/remap.c src/rcl/node_resolve_name.c @@ -70,6 +71,7 @@ set(${PROJECT_NAME}_sources src/rcl/time.c src/rcl/timer.c src/rcl/type_hash.c + src/rcl/type_description_conversions.c src/rcl/validate_enclave_name.c src/rcl/validate_topic_name.c src/rcl/wait.c diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index ce41d0d9b..e0213b7de 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -33,9 +33,12 @@ extern "C" #include "rcl/types.h" #include "rcl/visibility_control.h" +#include "type_description_interfaces/srv/get_type_description.h" + extern const char * const RCL_DISABLE_LOANED_MESSAGES_ENV_VAR; typedef struct rcl_node_impl_s rcl_node_impl_t; +typedef struct rcl_service_s rcl_service_t; /// Structure which encapsulates a ROS Node. typedef struct rcl_node_s @@ -549,6 +552,115 @@ RCL_PUBLIC rcl_ret_t rcl_get_disable_loaned_message(bool * disable_loaned_message); +/// Initialize the node's ~/get_type_description service. +/** + * This function initializes the node's ~/get_type_description service + * which can be used to retrieve information about types used by the node's + * publishers, subscribers, services or actions. + * + * Note that this will not register any callback for the service, client-level code + * must register rcl_node_type_description_service_handle_request or a custom callback + * to handle incoming requests, via that client's executor/waitset capabilities. + * + * This will initialize the node's type cache, if it has not been initialized already. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node handle to the node for which to initialize the service + * \return #RCL_RET_OK if the service was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ALREADY_INIT if the service is already initialized, or + * \return #RCL_RET_BAD_ALLOC if memory allocation for the service failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node); + +/// Finalizes the node's ~/get_type_description service. +/** + * This function finalizes the node's private ~/get_type_description service. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be initialized + * \return #RCL_RET_OK if service was deinitialized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SERVICE_INVALID if the service is invalid, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_description_service_fini(rcl_node_t * node); + + +/// Returns a pointer to the node's ~/get_type_description service. +/** + * On success, sets service_out to the initialized service. + * rcl_node_type_description_service_init must be called before this. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node + * \param[out] service_out Handle to pointer that will be set + * \return #RCL_RET_OK if valid service was returned successfully, or + * \return #RCL_RET_NODE_INVALID if node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NOT_INIT if the service hasn't yet been initialized, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_get_type_description_service( + const rcl_node_t * node, + rcl_service_t ** service_out); + + +/// Process a single pending request to the GetTypeDescription service. +/** + * This function may be called to handle incoming requests by any client starting the service. + * It is not intended to be called directly by users. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node + * \param[in] request_header ID of the incoming request + * \param[in] request Request that came in to the service + * \param[out] response Allocated, uninitialized response to the request + * \return void + */ +RCL_PUBLIC +void rcl_node_type_description_service_handle_request( + rcl_node_t * node, + const rmw_request_id_t * request_header, + const type_description_interfaces__srv__GetTypeDescription_Request * request, + type_description_interfaces__srv__GetTypeDescription_Response * response); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/node_options.h b/rcl/include/rcl/node_options.h index a74fa4bf5..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; - - /// Register the ~/get_type_description service. Defaults to false. - bool enable_type_description_service; } rcl_node_options_t; /// Return the default node options in a rcl_node_options_t. diff --git a/rcl/include/rcl/node_type_cache.h b/rcl/include/rcl/node_type_cache.h new file mode 100644 index 000000000..c9f21b117 --- /dev/null +++ b/rcl/include/rcl/node_type_cache.h @@ -0,0 +1,174 @@ +// Copyright 2023 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__NODE_TYPE_CACHE_H_ +#define RCL__NODE_TYPE_CACHE_H_ + +#include "rcl/node.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rosidl_runtime_c/type_description/type_description__struct.h" +#include "rosidl_runtime_c/type_description/type_source__struct.h" +#include "type_description_interfaces/msg/type_description.h" +#include "type_description_interfaces/msg/type_source.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct rcl_type_info_t +{ + type_description_interfaces__msg__TypeDescription * type_description; + type_description_interfaces__msg__TypeSource__Sequence * type_sources; +} rcl_type_info_t; + +/// Initialize the node's type cache. +/** + * This function initializes hash map of the node's type cache such that types + * can be registered and retrieved. + * Note that to correctly capture all types used by a node, this needs to be called + * before any "builtin" publishers or services are created. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be initialized + * \return #RCL_RET_OK if the node's type cache was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node); + +/// Finalize the node's type cache. +/** + * This function clears the hash map of the node's type cache and deallocates + * used memory. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be finalized + * \return #RCL_RET_OK if the node's type cache was successfully finalized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_fini(rcl_node_t * node); + +/// Register a type with the node's type cache. +/** + * This function registers the given type, uniquely identified by the type_hash, + * with the node with the node's type cache. Multiple registrations of the same + * type will increment its registration count. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be finalized + * \param[in] type_hash hash of the type + * \param[in] type_description type description struct + * \param[in] type_description_sources type description sources struct + * \return #RCL_RET_OK if the type was successfully registered, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_register_type( + const rcl_node_t * node, const rosidl_type_hash_t * type_hash, + const rosidl_runtime_c__type_description__TypeDescription * type_description, + const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources +); + +/// Unregister a message type from the node's type cache. +/** + * This function uses the given `type_hash` to unregister the associated type in + * the node's type cache. If the type has been registered multiple times, the + * type will only be removed if its registration count reaches 0. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be finalized + * \param[in] type_hash type hash + * \return #RCL_RET_OK if the type was successfully registered, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_unregister_type( + const rcl_node_t * node, const rosidl_type_hash_t * type_hash); + +/// Retrieve type information from the node's type cache. +/** + * This function returns the desired type information from the node's type cache + * + * The `type_info` field must point to an allocated `rcl_type_info_t` object to + * which the type information will be written. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node whose type cache should be queried + * \param[in] type_hash type hash + * \param[out] type_info pointer to the type info struct that will be populated + * \return #RCL_RET_OK if type information was retrieved successfully + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the given `node` is invalid, or + * \return #RCL_RET_NOT_INIT if node's type cache has not been initialized, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_node_type_cache_get_type_info( + const rcl_node_t * node, + const rosidl_type_hash_t * type_hash, + rcl_type_info_t * type_info); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__NODE_TYPE_CACHE_H_ diff --git a/rcl/include/rcl/type_description_conversions.h b/rcl/include/rcl/type_description_conversions.h new file mode 100644 index 000000000..25b604767 --- /dev/null +++ b/rcl/include/rcl/type_description_conversions.h @@ -0,0 +1,133 @@ +// Copyright 2023 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__TYPE_DESCRIPTION_CONVERSIONS_H_ +#define RCL__TYPE_DESCRIPTION_CONVERSIONS_H_ + +#include "rcl/macros.h" +#include "rcl/visibility_control.h" +#include "rosidl_runtime_c/type_description/type_description__struct.h" +#include "rosidl_runtime_c/type_description/type_source__struct.h" +#include "type_description_interfaces/msg/type_description.h" +#include "type_description_interfaces/msg/type_source.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/// Convert type description runtime struct to msg struct. +/** + * This function converts a rosidl_runtime_c__type_description__TypeDescription + * to the corresponding type_description_interfaces/msg/TypeDescription struct. + * The retrieved pointer shall be destroyed with + * `type_description_interfaces__msg__TypeDescription__destroy()` after use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] runtime_description the pointer to the runtime type description struct + * \return a valid type_description_interfaces/msg/TypeDescription pointer, or + * \return NULL if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +type_description_interfaces__msg__TypeDescription * +rcl_convert_type_description_runtime_to_msg( + const rosidl_runtime_c__type_description__TypeDescription * runtime_description); + +/// Convert type description msg struct to a rosidl runtime struct. +/** + * This function converts a type_description_interfaces/msg/TypeDescription + * to the corresponding rosidl_runtime_c__type_description__TypeDescription + * struct. The retrieved pointer shall be destroyed with + * `rosidl_runtime_c__type_description__TypeDescription__destroy()` after use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] description_msg the pointer to the msg type description struct + * \return a valid pointer to the runtime struct, or + * \return NULL if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rosidl_runtime_c__type_description__TypeDescription * +rcl_convert_type_description_msg_to_runtime( + const type_description_interfaces__msg__TypeDescription * description_msg); + +/// Convert type sources sequence runtime struct to msg struct. +/** + * This function converts a rosidl_runtime_c__type_description__TypeSource__Sequence + * struct to the corresponding type_description_interfaces__msg__TypeSource__Sequence + * struct. The retrieved pointer shall be destroyed with + * `type_description_interfaces__msg__TypeSource__Sequence__destroy()` after use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] runtime_type_sources the pointer to the type sources sequence struct + * \return a valid pointer to the msg struct, or + * \return NULL if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +type_description_interfaces__msg__TypeSource__Sequence * +rcl_convert_type_source_sequence_runtime_to_msg( + const rosidl_runtime_c__type_description__TypeSource__Sequence * runtime_type_sources); + +/// Convert type sources sequece msg struct to a rosidl runtime struct. +/** + * This function converts a rosidl_runtime_c__type_description__TypeSource__Sequence + * struct to the corresponding type_description_interfaces__msg__TypeSource__Sequence + * struct. The retrieved pointer shall be destroyed with + * `type_description_interfaces__msg__TypeSource__Sequence__destroy()` after use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] type_sources_msg the pointer to the type sources sequence struct + * \return a valid pointer to the msg struct, or + * \return NULL if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rosidl_runtime_c__type_description__TypeSource__Sequence * +rcl_convert_type_source_sequence_msg_to_runtime( + const type_description_interfaces__msg__TypeSource__Sequence * type_sources_msg); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__TYPE_DESCRIPTION_CONVERSIONS_H_ diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 8dca90ab7..0a30427ed 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -24,6 +24,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/node_type_cache.h" #include "rcl/publisher.h" #include "rcl/time.h" #include "rcutils/logging_macros.h" @@ -48,6 +49,7 @@ struct rcl_client_impl_s atomic_int_least64_t sequence_number; rcl_service_event_publisher_t * service_event_publisher; char * remapped_service_name; + rosidl_type_hash_t type_hash; }; rcl_client_t @@ -175,6 +177,19 @@ rcl_client_init( // options client->impl->options = *options; atomic_init(&client->impl->sequence_number, 0); + + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for client"); + ret = RCL_RET_ERROR; + goto destroy_client; + } + client->impl->type_hash = *type_support->get_type_hash_func(type_support); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized"); TRACETOOLS_TRACEPOINT( rcl_client_init, @@ -236,6 +251,14 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node) result = RCL_RET_ERROR; } + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != client->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &client->impl->type_hash)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(client->impl->remapped_service_name, allocator.state); client->impl->remapped_service_name = NULL; diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index fed56a746..b0fe0cfc2 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -30,6 +30,7 @@ extern "C" #include "rcl/localhost.h" #include "rcl/logging.h" #include "rcl/logging_rosout.h" +#include "rcl/node_type_cache.h" #include "rcl/rcl.h" #include "rcl/remap.h" #include "rcl/security.h" @@ -43,28 +44,24 @@ extern "C" #include "rcutils/repl_str.h" #include "rcutils/snprintf.h" #include "rcutils/strdup.h" +#include "rcutils/types/hash_map.h" #include "rmw/error_handling.h" #include "rmw/security_options.h" #include "rmw/rmw.h" #include "rmw/validate_namespace.h" #include "rmw/validate_node_name.h" +#include "rosidl_runtime_c/string_functions.h" +#include "rosidl_runtime_c/type_description/type_description__functions.h" +#include "rosidl_runtime_c/type_description/type_source__functions.h" #include "tracetools/tracetools.h" +#include "type_description_interfaces/srv/get_type_description.h" #include "./context_impl.h" +#include "./node_impl.h" const char * const RCL_DISABLE_LOANED_MESSAGES_ENV_VAR = "ROS_DISABLE_LOANED_MESSAGES"; -struct rcl_node_impl_s -{ - rcl_node_options_t options; - rmw_node_t * rmw_node_handle; - rcl_guard_condition_t * graph_guard_condition; - const char * logger_name; - const char * fq_name; -}; - - /// Return the logger name associated with a node given the validated node name and namespace. /** * E.g. for a node named "c" in namespace "/a/b", the logger name will be @@ -207,6 +204,8 @@ rcl_node_init( node->impl->logger_name = NULL; node->impl->fq_name = NULL; node->impl->options = rcl_node_get_default_options(); + node->impl->registered_types_by_type_hash = rcutils_get_zero_initialized_hash_map(); + node->impl->get_type_description_service = rcl_get_zero_initialized_service(); node->context = context; // Initialize node impl. ret = rcl_node_options_copy(options, &(node->impl->options)); @@ -284,6 +283,14 @@ rcl_node_init( // error message already set goto fail; } + + // To capture all types from builtin topics and services, the type cache needs to be initialized + // before any publishers/subscriptions/services/etc can be created + ret = rcl_node_type_cache_init(node); + if (ret != RCL_RET_OK) { + goto fail; + } + // The initialization for the rosout publisher requires the node to be in initialized to a point // that it can create new topic publishers if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) { @@ -293,12 +300,6 @@ rcl_node_init( goto fail; } } - if (node->impl->options.enable_type_description_service) { - RCUTILS_LOG_WARN_NAMED( - ROS_PACKAGE_NAME, - "Requested ~/get_type_description service enabled, but feature is not yet implemented. " - "Service is not created."); - } RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized"); ret = RCL_RET_OK; TRACETOOLS_TRACEPOINT( @@ -320,6 +321,12 @@ rcl_node_init( ROS_PACKAGE_NAME, "Failed to fini publisher for node: %i", ret); allocator->deallocate((char *)node->impl->logger_name, allocator->state); } + if (node->impl->registered_types_by_type_hash.impl) { + ret = rcl_node_type_cache_fini(node); + RCUTILS_LOG_ERROR_EXPRESSION_NAMED( + (ret != RCL_RET_OK), + ROS_PACKAGE_NAME, "Failed to fini type cache for node: %i", ret); + } if (node->impl->fq_name) { allocator->deallocate((char *)node->impl->fq_name, allocator->state); } @@ -387,6 +394,11 @@ rcl_node_fini(rcl_node_t * node) result = RCL_RET_ERROR; } } + rcl_ret = rcl_node_type_cache_fini(node); + if (rcl_ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG("Unable to fini type cache for node."); + result = RCL_RET_ERROR; + } rmw_ret_t rmw_ret = rmw_destroy_node(node->impl->rmw_node_handle); if (rmw_ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); @@ -542,6 +554,146 @@ rcl_get_disable_loaned_message(bool * disable_loaned_message) *disable_loaned_message = (strcmp(env_val, "1") == 0); return RCL_RET_OK; } + +void rcl_node_type_description_service_handle_request( + rcl_node_t * node, + const rmw_request_id_t * request_header, + const type_description_interfaces__srv__GetTypeDescription_Request * request, + type_description_interfaces__srv__GetTypeDescription_Response * response) +{ + rcl_type_info_t type_info; + RCL_CHECK_FOR_NULL_WITH_MSG(node, "invalid node handle", return;); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "invalid node", return;); + RCL_CHECK_FOR_NULL_WITH_MSG(request_header, "invalid request header", return;); + RCL_CHECK_FOR_NULL_WITH_MSG(request, "null request pointer", return;); + RCL_CHECK_FOR_NULL_WITH_MSG(response, "null response pointer", return;); + + if (!type_description_interfaces__srv__GetTypeDescription_Response__init(response)) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Failed to initialize service response."); + return; + } + response->successful = false; + + rosidl_type_hash_t type_hash; + if (RCUTILS_RET_OK != + rosidl_parse_type_hash_string(request->type_hash.data, &type_hash)) + { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to parse type hash '%s'", + request->type_hash.data); + rosidl_runtime_c__String__assign( + &response->failure_reason, + "Failed to parse type hash"); + return; + } + + if (RCUTILS_RET_OK != + rcl_node_type_cache_get_type_info(node, &type_hash, &type_info)) + { + rosidl_runtime_c__String__assign( + &response->failure_reason, + "Type not currently in use by this node"); + return; + } + + if (!type_description_interfaces__msg__TypeDescription__copy( + type_info.type_description, &response->type_description)) + { + rosidl_runtime_c__String__assign( + &response->failure_reason, + "Failed to populate TypeDescription to response."); + return; + } + + if (request->include_type_sources) { + if (!type_description_interfaces__msg__TypeSource__Sequence__copy( + type_info.type_sources, &response->type_sources)) + { + rosidl_runtime_c__String__assign( + &response->failure_reason, + "Failed to populate TypeSource_Sequence to response."); + return; + } + } + + response->successful = true; +} + +rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + + rcl_ret_t ret; + + if (rcl_service_is_valid(&node->impl->get_type_description_service)) { + return RCL_RET_ALREADY_INIT; + } + rcl_reset_error(); // Reset the error message set by rcl_service_is_valid() + + char * service_name = NULL; + const rosidl_service_type_support_t * type_support = + ROSIDL_GET_SRV_TYPE_SUPPORT( + type_description_interfaces, srv, + GetTypeDescription); + rcl_service_options_t service_ops = rcl_service_get_default_options(); + rcl_allocator_t allocator = node->context->impl->allocator; + + // Construct service name + ret = rcl_node_resolve_name( + node, "~/get_type_description", + allocator, true, true, &service_name); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG( + "Failed to construct ~/get_type_description service name"); + return ret; + } + + // Initialize service + ret = rcl_service_init( + &node->impl->get_type_description_service, node, + type_support, service_name, &service_ops); + allocator.deallocate(service_name, allocator.state); + + return ret; +} + +rcl_ret_t rcl_node_type_description_service_fini(rcl_node_t * node) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + if (!rcl_service_is_valid(&node->impl->get_type_description_service)) { + rcl_reset_error(); + return RCL_RET_NOT_INIT; + } + + const rcl_ret_t ret = + rcl_service_fini(&node->impl->get_type_description_service, node); + if (RCL_RET_OK == ret) { + node->impl->get_type_description_service = rcl_get_zero_initialized_service(); + } + + return ret; +} + +rcl_ret_t rcl_node_get_type_description_service( + const rcl_node_t * node, + rcl_service_t ** service_out) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(service_out, RCL_RET_SERVICE_INVALID); + + if (!rcl_service_is_valid(&node->impl->get_type_description_service)) { + return RCL_RET_NOT_INIT; + } + + *service_out = &node->impl->get_type_description_service; + return RCL_RET_OK; +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/node_impl.h b/rcl/src/rcl/node_impl.h new file mode 100644 index 000000000..8d4f5847e --- /dev/null +++ b/rcl/src/rcl/node_impl.h @@ -0,0 +1,37 @@ +// Copyright 2023 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__NODE_IMPL_H_ +#define RCL__NODE_IMPL_H_ + +#include "rcl/guard_condition.h" +#include "rcl/node_options.h" +#include "rcl/node.h" +#include "rcl/service.h" +#include "rcl/types.h" +#include "rcutils/types/hash_map.h" +#include "rmw/types.h" + +struct rcl_node_impl_s +{ + rcl_node_options_t options; + rmw_node_t * rmw_node_handle; + rcl_guard_condition_t * graph_guard_condition; + const char * logger_name; + const char * fq_name; + rcutils_hash_map_t registered_types_by_type_hash; + rcl_service_t get_type_description_service; +}; + +#endif // RCL__NODE_IMPL_H_ diff --git a/rcl/src/rcl/node_options.c b/rcl/src/rcl/node_options.c index 2d97799f1..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_type_description_service = 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_type_description_service = options->enable_type_description_service; if (NULL != options->arguments.impl) { return rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); } diff --git a/rcl/src/rcl/node_type_cache.c b/rcl/src/rcl/node_type_cache.c new file mode 100644 index 000000000..0d3734a53 --- /dev/null +++ b/rcl/src/rcl/node_type_cache.c @@ -0,0 +1,245 @@ +// Copyright 2023 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/node_type_cache.h" +#include "rcl/type_description_conversions.h" + +#include "rcl/error_handling.h" +#include "rcutils/logging_macros.h" +#include "rcutils/types/hash_map.h" + +#include "./context_impl.h" +#include "./node_impl.h" + +typedef struct rcl_type_info_with_registration_count_t +{ + /// Counter to keep track of registrations + size_t num_registrations; + + /// The actual type info. + rcl_type_info_t type_info; +} rcl_type_info_with_registration_count_t; + +static size_t get_type_hash_hashmap_key(const void * key) +{ + // Reinterpret-cast the first sizeof(size_t) bytes of the hash value + const rosidl_type_hash_t * type_hash = key; + return *(size_t *)type_hash->value; +} + +static int cmp_type_hash(const void * val1, const void * val2) +{ + return memcmp(val1, val2, sizeof(rosidl_type_hash_t)); +} + +rcl_ret_t rcl_node_type_cache_init(rcl_node_t * node) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + if (NULL != node->impl->registered_types_by_type_hash.impl) { + // already initialized + return RCL_RET_OK; + } + + rcutils_ret_t ret = rcutils_hash_map_init( + &node->impl->registered_types_by_type_hash, 2, sizeof(rosidl_type_hash_t), + sizeof(rcl_type_info_with_registration_count_t), + get_type_hash_hashmap_key, cmp_type_hash, + &node->context->impl->allocator); + + if (RCUTILS_RET_OK != ret) { + RCL_SET_ERROR_MSG("Failed to initialize type cache hash map"); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +rcl_ret_t rcl_node_type_cache_fini(rcl_node_t * node) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + + // Clean up any remaining types. + rosidl_type_hash_t key; + rcl_type_info_with_registration_count_t type_info_with_registrations; + rcutils_ret_t hash_map_ret = rcutils_hash_map_get_next_key_and_data( + &node->impl->registered_types_by_type_hash, NULL, &key, + &type_info_with_registrations); + + if (RCUTILS_RET_NOT_INITIALIZED == hash_map_ret) { + return RCL_RET_NOT_INIT; + } + + while (RCUTILS_RET_OK == hash_map_ret) { + hash_map_ret = rcutils_hash_map_unset( + &node->impl->registered_types_by_type_hash, &key); + if (hash_map_ret != RCUTILS_RET_OK) { + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Failed to clear out type informations [%s] during shutdown; memory " + "will be leaked.", + rcutils_get_error_string().str); + break; + } + + type_description_interfaces__msg__TypeDescription__destroy( + type_info_with_registrations.type_info.type_description); + type_description_interfaces__msg__TypeSource__Sequence__destroy( + type_info_with_registrations.type_info.type_sources); + + hash_map_ret = rcutils_hash_map_get_next_key_and_data( + &node->impl->registered_types_by_type_hash, NULL, &key, + &type_info_with_registrations); + } + + rcutils_ret_t rcutils_ret = + rcutils_hash_map_fini(&node->impl->registered_types_by_type_hash); + + return RCUTILS_RET_OK == rcutils_ret ? RCL_RET_OK : RCL_RET_ERROR; +} + +rcl_ret_t rcl_node_type_cache_get_type_info( + const rcl_node_t * node, + const rosidl_type_hash_t * type_hash, + rcl_type_info_t * type_info) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(type_hash, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_info, RCL_RET_INVALID_ARGUMENT); + + rcl_type_info_with_registration_count_t type_info_with_registrations; + + rcutils_ret_t ret = + rcutils_hash_map_get( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info_with_registrations); + if (RCUTILS_RET_OK == ret) { + *type_info = type_info_with_registrations.type_info; + return RCL_RET_OK; + } else if (RCUTILS_RET_NOT_INITIALIZED == ret) { + return RCL_RET_NOT_INIT; + } + + return RCL_RET_ERROR; +} + +rcl_ret_t rcl_node_type_cache_register_type( + const rcl_node_t * node, const rosidl_type_hash_t * type_hash, + const rosidl_runtime_c__type_description__TypeDescription * type_description, + const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(type_hash, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_description, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(type_description_sources, RCL_RET_INVALID_ARGUMENT); + + rcl_type_info_with_registration_count_t type_info_with_registrations; + + const rcutils_ret_t rcutils_ret = rcutils_hash_map_get( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info_with_registrations); + + if (RCUTILS_RET_OK == rcutils_ret) { + // If the type already exists, we only have to increment the registration + // count. + type_info_with_registrations.num_registrations++; + } else if (RCUTILS_RET_NOT_FOUND == rcutils_ret) { + // First registration of this type + type_info_with_registrations.num_registrations = 1; + + // Convert type description struct to type description message struct. + type_info_with_registrations.type_info.type_description = + rcl_convert_type_description_runtime_to_msg(type_description); + RCL_CHECK_FOR_NULL_WITH_MSG( + type_info_with_registrations.type_info.type_description, + "converting type description struct failed", return RCL_RET_ERROR); + + // Convert type sources struct to type sources message struct. + type_info_with_registrations.type_info.type_sources = + rcl_convert_type_source_sequence_runtime_to_msg(type_description_sources); + RCL_CHECK_FOR_NULL_WITH_MSG( + type_info_with_registrations.type_info.type_sources, + "converting type sources struct failed", + type_description_interfaces__msg__TypeDescription__destroy( + type_info_with_registrations.type_info.type_description); + return RCL_RET_ERROR); + } else { + return RCL_RET_ERROR; + } + + // Update the hash map entry. + if (RCUTILS_RET_OK != + rcutils_hash_map_set( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info_with_registrations)) + { + RCL_SET_ERROR_MSG("Failed to update type info"); + type_description_interfaces__msg__TypeDescription__destroy( + type_info_with_registrations.type_info.type_description); + type_description_interfaces__msg__TypeSource__Sequence__destroy( + type_info_with_registrations.type_info.type_sources); + return RCL_RET_ERROR; + } + + return RCL_RET_OK; +} + +rcl_ret_t rcl_node_type_cache_unregister_type( + const rcl_node_t * node, + const rosidl_type_hash_t * type_hash) +{ + rcl_type_info_with_registration_count_t type_info; + + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node->impl, RCL_RET_NODE_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(type_hash, RCL_RET_INVALID_ARGUMENT); + + if (RCUTILS_RET_OK != + rcutils_hash_map_get( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info)) + { + RCL_SET_ERROR_MSG("Failed to unregister type, hash not present in map."); + return RCL_RET_ERROR; + } + + if (--type_info.num_registrations > 0) { + if (RCUTILS_RET_OK != + rcutils_hash_map_set( + &node->impl->registered_types_by_type_hash, + type_hash, &type_info)) + { + RCL_SET_ERROR_MSG("Failed to update type info"); + return RCL_RET_ERROR; + } + } else { + if (RCUTILS_RET_OK != + rcutils_hash_map_unset( + &node->impl->registered_types_by_type_hash, + type_hash)) + { + RCL_SET_ERROR_MSG("Failed to unregister type info"); + return RCL_RET_ERROR; + } + + type_description_interfaces__msg__TypeDescription__destroy( + type_info.type_info.type_description); + type_description_interfaces__msg__TypeSource__Sequence__destroy( + type_info.type_info.type_sources); + } + + return RCL_RET_OK; +} diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 5a132becd..4e1fee3f0 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -25,6 +25,7 @@ extern "C" #include "rcl/allocator.h" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/node_type_cache.h" #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rcl/time.h" @@ -99,8 +100,8 @@ rcl_publisher_init( ROS_PACKAGE_NAME, "Expanded and remapped topic name '%s'", remapped_topic_name); // Allocate space for the implementation struct. - publisher->impl = (rcl_publisher_impl_t *)allocator->allocate( - sizeof(rcl_publisher_impl_t), allocator->state); + publisher->impl = (rcl_publisher_impl_t *)allocator->zero_allocate( + 1, sizeof(rcl_publisher_impl_t), allocator->state); RCL_CHECK_FOR_NULL_WITH_MSG( publisher->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); @@ -127,6 +128,18 @@ rcl_publisher_init( options->qos.avoid_ros_namespace_conventions; // options publisher->impl->options = *options; + + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for subscription"); + goto fail; + } + publisher->impl->type_hash = *type_support->get_type_hash_func(type_support); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized"); // context publisher->impl->context = node->context; @@ -137,6 +150,7 @@ rcl_publisher_init( (const void *)publisher->impl->rmw_handle, remapped_topic_name, options->qos.depth); + goto cleanup; fail: if (publisher->impl) { @@ -187,6 +201,13 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != publisher->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &publisher->impl->type_hash)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } allocator.deallocate(publisher->impl, allocator.state); publisher->impl = NULL; } diff --git a/rcl/src/rcl/publisher_impl.h b/rcl/src/rcl/publisher_impl.h index cd5ff2324..26bbb43da 100644 --- a/rcl/src/rcl/publisher_impl.h +++ b/rcl/src/rcl/publisher_impl.h @@ -25,6 +25,7 @@ struct rcl_publisher_impl_s rmw_qos_profile_t actual_qos; rcl_context_t * context; rmw_publisher_t * rmw_handle; + rosidl_type_hash_t type_hash; }; #endif // RCL__PUBLISHER_IMPL_H_ diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 0d49289f4..1cca57acc 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -24,6 +24,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/node_type_cache.h" #include "rcl/publisher.h" #include "rcl/time.h" #include "rcl/types.h" @@ -47,6 +48,7 @@ struct rcl_service_impl_s rmw_service_t * rmw_handle; rcl_service_event_publisher_t * service_event_publisher; char * remapped_service_name; + rosidl_type_hash_t type_hash; }; rcl_service_t @@ -186,6 +188,19 @@ rcl_service_init( // options service->impl->options = *options; + + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for service"); + ret = RCL_RET_ERROR; + goto destroy_service; + } + service->impl->type_hash = *type_support->get_type_hash_func(type_support); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); TRACETOOLS_TRACEPOINT( rcl_service_init, @@ -248,6 +263,14 @@ rcl_service_fini(rcl_service_t * service, rcl_node_t * node) result = RCL_RET_ERROR; } + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != service->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &service->impl->type_hash)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(service->impl->remapped_service_name, allocator.state); service->impl->remapped_service_name = NULL; diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 41a7d641b..1ca6edd8c 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -23,6 +23,7 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" +#include "rcl/node_type_cache.h" #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" #include "rcutils/types/string_array.h" @@ -122,6 +123,18 @@ rcl_subscription_init( options->qos.avoid_ros_namespace_conventions; // options subscription->impl->options = *options; + + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for subscription"); + goto fail; + } + subscription->impl->type_hash = *type_support->get_type_hash_func(type_support); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; TRACETOOLS_TRACEPOINT( @@ -131,6 +144,7 @@ rcl_subscription_init( (const void *)subscription->impl->rmw_handle, remapped_topic_name, options->qos.depth); + goto cleanup; fail: if (subscription->impl) { @@ -192,6 +206,15 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) result = RCL_RET_ERROR; } + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != subscription->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &subscription->impl->type_hash)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + result = RCL_RET_ERROR; + } + allocator.deallocate(subscription->impl, allocator.state); subscription->impl = NULL; } diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 0fe962ab4..8193421cf 100644 --- a/rcl/src/rcl/subscription_impl.h +++ b/rcl/src/rcl/subscription_impl.h @@ -24,6 +24,7 @@ struct rcl_subscription_impl_s rcl_subscription_options_t options; rmw_qos_profile_t actual_qos; rmw_subscription_t * rmw_handle; + rosidl_type_hash_t type_hash; }; #endif // RCL__SUBSCRIPTION_IMPL_H_ diff --git a/rcl/src/rcl/type_description_conversions.c b/rcl/src/rcl/type_description_conversions.c new file mode 100644 index 000000000..f3a041868 --- /dev/null +++ b/rcl/src/rcl/type_description_conversions.c @@ -0,0 +1,309 @@ +// Copyright 2023 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/error_handling.h" +#include "rcl/type_description_conversions.h" + +#include "rosidl_runtime_c/string_functions.h" +#include "rosidl_runtime_c/type_description/field__functions.h" +#include "rosidl_runtime_c/type_description/individual_type_description__functions.h" +#include "rosidl_runtime_c/type_description/type_description__functions.h" +#include "rosidl_runtime_c/type_description/type_source__functions.h" +#include "type_description_interfaces/msg/detail/field__functions.h" +#include "type_description_interfaces/msg/individual_type_description.h" + +static bool individual_type_description_runtime_to_msg( + const rosidl_runtime_c__type_description__IndividualTypeDescription * in, + type_description_interfaces__msg__IndividualTypeDescription * out) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(in, false); + RCL_CHECK_ARGUMENT_FOR_NULL(out, false); + + const bool success = + rosidl_runtime_c__String__copy(&in->type_name, &out->type_name) && + type_description_interfaces__msg__Field__Sequence__init( + &out->fields, + in->fields.size); + if (!success) { + goto error; + } + + for (size_t i = 0; i < in->fields.size; ++i) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].name), + &(out->fields.data[i].name))) + { + goto error; + } + + if (in->fields.data[i].default_value.size) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].default_value), + &(out->fields.data[i].default_value))) + { + goto error; + } + } + + // type_id + out->fields.data[i].type.type_id = in->fields.data[i].type.type_id; + // capacity + out->fields.data[i].type.capacity = in->fields.data[i].type.capacity; + // string_capacity + out->fields.data[i].type.string_capacity = + in->fields.data[i].type.string_capacity; + + // nested_type_name + if (in->fields.data[i].type.nested_type_name.size) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].type.nested_type_name), + &(out->fields.data[i].type.nested_type_name))) + { + goto error; + } + } + } + + return true; + +error: + type_description_interfaces__msg__IndividualTypeDescription__fini(out); + return false; +} + +static bool individual_type_description_msg_to_runtime( + const type_description_interfaces__msg__IndividualTypeDescription * in, + rosidl_runtime_c__type_description__IndividualTypeDescription * out) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(in, false); + RCL_CHECK_ARGUMENT_FOR_NULL(out, false); + + const bool success = + rosidl_runtime_c__String__copy(&in->type_name, &out->type_name) && + rosidl_runtime_c__type_description__Field__Sequence__init( + &out->fields, in->fields.size); + if (!success) { + goto error; + } + + for (size_t i = 0; i < in->fields.size; ++i) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].name), + &(out->fields.data[i].name))) + { + goto error; + } + + if (in->fields.data[i].default_value.size) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].default_value), + &(out->fields.data[i].default_value))) + { + goto error; + } + } + + // type_id + out->fields.data[i].type.type_id = in->fields.data[i].type.type_id; + // capacity + out->fields.data[i].type.capacity = in->fields.data[i].type.capacity; + // string_capacity + out->fields.data[i].type.string_capacity = + in->fields.data[i].type.string_capacity; + + // nested_type_name + if (in->fields.data[i].type.nested_type_name.size) { + if (!rosidl_runtime_c__String__copy( + &(in->fields.data[i].type.nested_type_name), + &(out->fields.data[i].type.nested_type_name))) + { + goto error; + } + } + } + + return true; + +error: + rosidl_runtime_c__type_description__IndividualTypeDescription__init(out); + return false; +} + +type_description_interfaces__msg__TypeDescription * +rcl_convert_type_description_runtime_to_msg( + const rosidl_runtime_c__type_description__TypeDescription * runtime_description) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(runtime_description, NULL); + + // Create the object + type_description_interfaces__msg__TypeDescription * out = + type_description_interfaces__msg__TypeDescription__create(); + RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL); + + // init referenced_type_descriptions with the correct size + if (!type_description_interfaces__msg__IndividualTypeDescription__Sequence__init( + &out->referenced_type_descriptions, + runtime_description->referenced_type_descriptions.size)) + { + goto fail; + } + + // Convert individual type description + if (!individual_type_description_runtime_to_msg( + &runtime_description->type_description, + &out->type_description)) + { + goto fail; + } + + // Convert referenced type descriptions + for (size_t i = 0; i < runtime_description->referenced_type_descriptions.size; ++i) { + if (!individual_type_description_runtime_to_msg( + &runtime_description->referenced_type_descriptions.data[i], + &out->referenced_type_descriptions.data[i])) + { + goto fail; + } + } + + return out; + +fail: + type_description_interfaces__msg__TypeDescription__destroy(out); + return NULL; +} + +rosidl_runtime_c__type_description__TypeDescription * +rcl_convert_type_description_msg_to_runtime( + const type_description_interfaces__msg__TypeDescription * description_msg) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(description_msg, NULL); + + // Create the object + rosidl_runtime_c__type_description__TypeDescription * out = + rosidl_runtime_c__type_description__TypeDescription__create(); + RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL); + + // init referenced_type_descriptions with the correct size + if (!rosidl_runtime_c__type_description__IndividualTypeDescription__Sequence__init( + &out->referenced_type_descriptions, + description_msg->referenced_type_descriptions.size)) + { + goto fail; + } + + if (!individual_type_description_msg_to_runtime( + &description_msg->type_description, + &out->type_description)) + { + goto fail; + } + + for (size_t i = 0; i < description_msg->referenced_type_descriptions.size; ++i) { + if (!individual_type_description_msg_to_runtime( + &description_msg->referenced_type_descriptions.data[i], + &out->referenced_type_descriptions.data[i])) + { + goto fail; + } + } + + return out; + +fail: + rosidl_runtime_c__type_description__TypeDescription__destroy(out); + return NULL; +} + +type_description_interfaces__msg__TypeSource__Sequence * +rcl_convert_type_source_sequence_runtime_to_msg( + const rosidl_runtime_c__type_description__TypeSource__Sequence * runtime_type_sources) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(runtime_type_sources, NULL); + + // Create the object + type_description_interfaces__msg__TypeSource__Sequence * out = + type_description_interfaces__msg__TypeSource__Sequence__create(runtime_type_sources->size); + RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL); + + // Copy type sources + for (size_t i = 0; i < runtime_type_sources->size; ++i) { + // type_name + if (!rosidl_runtime_c__String__copy( + &(runtime_type_sources->data[i].type_name), &(out->data[i].type_name))) + { + goto fail; + } + // encoding + if (!rosidl_runtime_c__String__copy( + &(runtime_type_sources->data[i].encoding), &(out->data[i].encoding))) + { + goto fail; + } + // raw_file_contents + if (runtime_type_sources->data[i].raw_file_contents.size > 0) { + if (!rosidl_runtime_c__String__copy( + &(runtime_type_sources->data[i].raw_file_contents), &(out->data[i].raw_file_contents))) + { + goto fail; + } + } + } + + return out; + +fail: + type_description_interfaces__msg__TypeSource__Sequence__destroy(out); + return NULL; +} + +rosidl_runtime_c__type_description__TypeSource__Sequence * +rcl_convert_type_source_sequence_msg_to_runtime( + const type_description_interfaces__msg__TypeSource__Sequence * type_sources_msg) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(type_sources_msg, NULL); + + // Create the object + rosidl_runtime_c__type_description__TypeSource__Sequence * out = + rosidl_runtime_c__type_description__TypeSource__Sequence__create(type_sources_msg->size); + RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL); + + // Copy type sources + for (size_t i = 0; i < type_sources_msg->size; ++i) { + // type_name + if (!rosidl_runtime_c__String__copy( + &(type_sources_msg->data[i].type_name), &(out->data[i].type_name))) + { + goto fail; + } + // encoding + if (!rosidl_runtime_c__String__copy( + &(type_sources_msg->data[i].encoding), &(out->data[i].encoding))) + { + goto fail; + } + // raw_file_contents + if (!rosidl_runtime_c__String__copy( + &(type_sources_msg->data[i].raw_file_contents), &(out->data[i].raw_file_contents))) + { + goto fail; + } + } + + return out; + +fail: + rosidl_runtime_c__type_description__TypeSource__Sequence__destroy(out); + return NULL; +} diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index d106c0ae5..1931d0cb2 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -328,6 +328,32 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) + rcl_add_custom_gtest(test_type_description_conversions${target_suffix} + SRCS rcl/test_type_description_conversions.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES "test_msgs" + ) + + rcl_add_custom_gtest(test_node_type_cache${target_suffix} + SRCS rcl/test_node_type_cache.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_get_type_description_service${target_suffix} + SRCS rcl/test_get_type_description_service.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" "type_description_interfaces" + ) + # Launch tests rcl_add_custom_executable(service_fixture${target_suffix} diff --git a/rcl/test/rcl/test_get_type_description_service.cpp b/rcl/test/rcl/test_get_type_description_service.cpp new file mode 100644 index 000000000..e2571e75c --- /dev/null +++ b/rcl/test/rcl/test_get_type_description_service.cpp @@ -0,0 +1,360 @@ +// Copyright 2023 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 "rcl/error_handling.h" +#include "rcl/graph.h" +#include "rcl/service.h" +#include "rcl/rcl.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcutils/types/string_array.h" +#include "rosidl_runtime_c/string_functions.h" +#include "type_description_interfaces/srv/get_type_description.h" + +#include "node_impl.h" // NOLINT +#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 + +constexpr char GET_TYPE_DESCRIPTION_SRV_TYPE_NAME[] = + "type_description_interfaces/srv/GetTypeDescription"; + +static bool string_in_array(rcutils_string_array_t * array, const char * pattern) +{ + for (size_t i = 0; i < array->size; ++i) { + if (strcmp(array->data[i], pattern) == 0) { + return true; + } + } + return false; +} + +static bool service_exists( + const rcl_node_t * node_ptr, const char * service_name, + const char * service_type) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + + rcl_names_and_types_t * srv_names_and_types = + static_cast(allocator.allocate( + sizeof(rcl_names_and_types_t), + allocator.state)); + if (nullptr == srv_names_and_types) { + return false; + } + EXPECT_EQ(RCL_RET_OK, rcl_names_and_types_init(srv_names_and_types, 0, &allocator)); + srv_names_and_types->names.data = NULL; + srv_names_and_types->names.size = 0; + srv_names_and_types->types = NULL; + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_names_and_types_fini(srv_names_and_types)); + allocator.deallocate(srv_names_and_types, allocator.state); + }); + + if ( + RCL_RET_OK != rcl_get_service_names_and_types( + node_ptr, + &allocator, srv_names_and_types)) + { + return false; + } + + if (srv_names_and_types->names.size < 1) { + return false; + } + + const bool srv_name_found = string_in_array( + &srv_names_and_types->names, + service_name); + + if (!srv_name_found) {return false;} + + bool type_name_found = false; + for (size_t i = 0; i < srv_names_and_types->names.size; ++i) { + type_name_found = string_in_array( + &srv_names_and_types->types[i], + service_type); + if (type_name_found) { + break; + } + } + + return type_name_found; +} + +class CLASSNAME (TestGetTypeDescSrvFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + char get_type_description_service_name[256]; + + virtual bool get_type_description_service_enabled() const + { + return true; + } + + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_service_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_type_description_service_init(node_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + const char * node_fqn = rcl_node_get_fully_qualified_name(this->node_ptr); + snprintf( + get_type_description_service_name, sizeof(get_type_description_service_name), + "%s/get_type_description", node_fqn); + } + + void TearDown() + { + 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; + } +}; + + +/* Test init and fini functions. */ +TEST_F( + CLASSNAME(TestGetTypeDescSrvFixture, RMW_IMPLEMENTATION), + test_service_init_and_fini_functions) { + EXPECT_TRUE( + service_exists( + this->node_ptr, this->get_type_description_service_name, + GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); + EXPECT_EQ(RCL_RET_OK, rcl_node_type_description_service_fini(this->node_ptr)); + EXPECT_FALSE( + service_exists( + this->node_ptr, this->get_type_description_service_name, + GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); + EXPECT_EQ(RCL_RET_NOT_INIT, rcl_node_type_description_service_fini(this->node_ptr)); + + EXPECT_EQ(RCL_RET_OK, rcl_node_type_description_service_init(this->node_ptr)); + EXPECT_TRUE( + service_exists( + this->node_ptr, this->get_type_description_service_name, + GET_TYPE_DESCRIPTION_SRV_TYPE_NAME)); + EXPECT_EQ(RCL_RET_ALREADY_INIT, rcl_node_type_description_service_init(this->node_ptr)); +} + +/* Basic nominal test of the ~/get_type_description service. */ +TEST_F(CLASSNAME(TestGetTypeDescSrvFixture, RMW_IMPLEMENTATION), test_service_nominal) { + rcl_ret_t ret; + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + type_description_interfaces, srv, GetTypeDescription); + + // Create client. + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init( + &client, this->node_ptr, ts, this->get_type_description_service_name, + &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); + + // Initialize a request. + type_description_interfaces__srv__GetTypeDescription_Request client_request; + type_description_interfaces__srv__GetTypeDescription_Request__init(&client_request); + + // Fill the request. We use the GetTypeDescription hash because we know that that type + // is registered. + const rosidl_type_hash_t * type_hash = ts->get_type_hash_func(ts); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * type_hash_str; + EXPECT_EQ(RCUTILS_RET_OK, rosidl_stringify_type_hash(type_hash, allocator, &type_hash_str)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(type_hash_str, allocator.state); + }); + rosidl_runtime_c__String__assign(&client_request.type_hash, type_hash_str); + rosidl_runtime_c__String__assign(&client_request.type_name, GET_TYPE_DESCRIPTION_SRV_TYPE_NAME); + client_request.include_type_sources = false; + + // Send the request. + int64_t sequence_number; + ret = rcl_send_request(&client, &client_request, &sequence_number); + type_description_interfaces__srv__GetTypeDescription_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // This scope simulates handling request in a different context + { + auto service = &node_ptr->impl->get_type_description_service; + ASSERT_TRUE(wait_for_service_to_be_ready(service, context_ptr, 10, 100)); + + type_description_interfaces__srv__GetTypeDescription_Response service_response; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + type_description_interfaces__srv__GetTypeDescription_Response__fini(&service_response); + }); + + type_description_interfaces__srv__GetTypeDescription_Request service_request; + type_description_interfaces__srv__GetTypeDescription_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + type_description_interfaces__srv__GetTypeDescription_Request__fini(&service_request); + }); + rmw_service_info_t header; + ret = rcl_take_request_with_info(service, &header, &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_node_type_description_service_handle_request( + node_ptr, + &header.request_id, + &service_request, + &service_response); + + ret = rcl_send_response(service, &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)); + // Initialize the response owned by the client and take the response. + type_description_interfaces__srv__GetTypeDescription_Response client_response; + type_description_interfaces__srv__GetTypeDescription_Response__init(&client_response); + + // Retrieve the response. + rmw_service_info_t header; + ret = rcl_take_response_with_info(&client, &header, &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(client_response.successful, true); + EXPECT_EQ(sequence_number, header.request_id.sequence_number); + + type_description_interfaces__srv__GetTypeDescription_Response__fini(&client_response); +} + +/* Test calling ~/get_type_description service with invalid hash. */ +TEST_F( + CLASSNAME( + TestGetTypeDescSrvFixture, + RMW_IMPLEMENTATION), test_service_invalid_hash) { + rcl_ret_t ret; + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + type_description_interfaces, srv, GetTypeDescription); + + // Create client. + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init( + &client, this->node_ptr, ts, this->get_type_description_service_name, + &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); + + // Initialize a request. + type_description_interfaces__srv__GetTypeDescription_Request client_request; + type_description_interfaces__srv__GetTypeDescription_Request__init(&client_request); + + // Fill the request. + rosidl_runtime_c__String__assign(&client_request.type_hash, "foo"); + rosidl_runtime_c__String__assign(&client_request.type_name, "bar"); + client_request.include_type_sources = false; + + // Send the request. + int64_t sequence_number; + ret = rcl_send_request(&client, &client_request, &sequence_number); + type_description_interfaces__srv__GetTypeDescription_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // This scope simulates handling request in a different context + { + auto service = &node_ptr->impl->get_type_description_service; + ASSERT_TRUE(wait_for_service_to_be_ready(service, context_ptr, 10, 100)); + + type_description_interfaces__srv__GetTypeDescription_Response service_response; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + type_description_interfaces__srv__GetTypeDescription_Response__fini(&service_response); + }); + + type_description_interfaces__srv__GetTypeDescription_Request service_request; + type_description_interfaces__srv__GetTypeDescription_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + type_description_interfaces__srv__GetTypeDescription_Request__fini(&service_request); + }); + rmw_service_info_t header; + ret = rcl_take_request_with_info(service, &header, &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_node_type_description_service_handle_request( + node_ptr, + &header.request_id, + &service_request, + &service_response); + + ret = rcl_send_response(service, &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)); + // Initialize the response owned by the client and take the response. + type_description_interfaces__srv__GetTypeDescription_Response client_response; + type_description_interfaces__srv__GetTypeDescription_Response__init(&client_response); + + // Retrieve the response. + rmw_service_info_t header; + ret = rcl_take_response_with_info(&client, &header, &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(client_response.successful, false); + EXPECT_GT(strlen(client_response.failure_reason.data), 0); + EXPECT_EQ(sequence_number, header.request_id.sequence_number); + + type_description_interfaces__srv__GetTypeDescription_Response__fini(&client_response); +} diff --git a/rcl/test/rcl/test_node_type_cache.cpp b/rcl/test/rcl/test_node_type_cache.cpp new file mode 100644 index 000000000..378f4c4d5 --- /dev/null +++ b/rcl/test/rcl/test_node_type_cache.cpp @@ -0,0 +1,186 @@ +// Copyright 2023 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 "rcl/node_type_cache.h" +#include "rcl/rcl.h" +#include "rmw/rmw.h" + +#include "test_msgs/msg/basic_types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcutils/env.h" + +#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 (TestNodeTypeCacheFixture, RMW_IMPLEMENTATION) + : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) + << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_type_cache_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; + } + + void TearDown() + { + 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; + } +}; + +TEST_F( + CLASSNAME(TestNodeTypeCacheFixture, RMW_IMPLEMENTATION), + test_type_cache_invalid_args) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_type_info_t type_info; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_register_type( + NULL, ts->get_type_hash_func(ts), + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_register_type( + this->node_ptr, NULL, + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), NULL, ts->get_type_description_sources_func(ts))); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), ts->get_type_description_func(ts), NULL)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_unregister_type(NULL, ts->get_type_hash_func(ts))); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_unregister_type(this->node_ptr, NULL)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_type_cache_get_type_info(NULL, ts->get_type_hash_func(ts), &type_info)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_node_type_cache_get_type_info( + this->node_ptr, NULL, &type_info)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_node_type_cache_get_type_info( + this->node_ptr, ts->get_type_hash_func(ts), NULL)); + rcl_reset_error(); +} + +TEST_F( + CLASSNAME(TestNodeTypeCacheFixture, RMW_IMPLEMENTATION), + test_type_registration_count) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_type_info_t type_info; + + // Register once + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + EXPECT_EQ( + RCL_RET_OK, rcl_node_type_cache_get_type_info( + this->node_ptr, ts->get_type_hash_func(ts), &type_info)); + + // Unregister once and confirm that it got removed from the type cache + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_unregister_type(this->node_ptr, ts->get_type_hash_func(ts))); + EXPECT_EQ( + RCL_RET_ERROR, rcl_node_type_cache_get_type_info( + this->node_ptr, ts->get_type_hash_func(ts), &type_info)); + rcl_reset_error(); + + // Register twice and unregister once. Type info should still be available + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_register_type( + this->node_ptr, ts->get_type_hash_func(ts), + ts->get_type_description_func(ts), ts->get_type_description_sources_func(ts))); + EXPECT_EQ( + RCL_RET_OK, + rcl_node_type_cache_unregister_type(this->node_ptr, ts->get_type_hash_func(ts))); + EXPECT_EQ( + RCL_RET_OK, rcl_node_type_cache_get_type_info( + this->node_ptr, ts->get_type_hash_func(ts), &type_info)); +} + +TEST_F( + CLASSNAME(TestNodeTypeCacheFixture, RMW_IMPLEMENTATION), + test_invalid_unregistration) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + EXPECT_EQ( + RCL_RET_ERROR, + rcl_node_type_cache_unregister_type(this->node_ptr, ts->get_type_hash_func(ts))); + rcl_reset_error(); +} diff --git a/rcl/test/rcl/test_type_description_conversions.cpp b/rcl/test/rcl/test_type_description_conversions.cpp new file mode 100644 index 000000000..71a698f91 --- /dev/null +++ b/rcl/test/rcl/test_type_description_conversions.cpp @@ -0,0 +1,112 @@ +// Copyright 2023 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 "rcl/type_description_conversions.h" +#include "rosidl_runtime_c/message_type_support_struct.h" +#include "rosidl_runtime_c/type_description/type_description__functions.h" +#include "rosidl_runtime_c/type_description/type_source__functions.h" +#include "test_msgs/msg/constants.h" +#include "test_msgs/srv/basic_types.h" + +TEST(TestTypeDescriptionConversions, type_description_conversion_round_trip) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Constants); + + type_description_interfaces__msg__TypeDescription * type_description_msg = + rcl_convert_type_description_runtime_to_msg(ts->get_type_description_func(ts)); + EXPECT_TRUE(NULL != type_description_msg); + + rosidl_runtime_c__type_description__TypeDescription * type_description_rt = + rcl_convert_type_description_msg_to_runtime(type_description_msg); + EXPECT_TRUE(NULL != type_description_rt); + + EXPECT_TRUE( + rosidl_runtime_c__type_description__TypeDescription__are_equal( + type_description_rt, ts->get_type_description_func(ts))); + + type_description_interfaces__msg__TypeDescription__destroy( + type_description_msg); + rosidl_runtime_c__type_description__TypeDescription__destroy( + type_description_rt); +} + +TEST(TestTypeDescriptionConversions, type_description_invalid_input) { + EXPECT_TRUE(NULL == rcl_convert_type_description_runtime_to_msg(NULL)); + EXPECT_TRUE(NULL == rcl_convert_type_description_msg_to_runtime(NULL)); +} + +TEST(TestTypeDescriptionConversions, type_source_sequence_conversion_round_trip) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Constants); + + const rosidl_runtime_c__type_description__TypeSource__Sequence * original_sources = + ts->get_type_description_sources_func(ts); + type_description_interfaces__msg__TypeSource__Sequence * type_sources_msg = + rcl_convert_type_source_sequence_runtime_to_msg(original_sources); + EXPECT_TRUE(NULL != type_sources_msg); + ASSERT_EQ(1, type_sources_msg->size); + { + auto source = type_sources_msg->data[0]; + std::string type_name = source.type_name.data; + EXPECT_GT(source.raw_file_contents.size, 0); + std::string encoding = source.encoding.data; + EXPECT_EQ(encoding, "msg"); + } + + rosidl_runtime_c__type_description__TypeSource__Sequence * type_sources_rt = + rcl_convert_type_source_sequence_msg_to_runtime(type_sources_msg); + EXPECT_TRUE(NULL != type_sources_rt); + + ASSERT_EQ(1, type_sources_rt->size); + { + auto source = type_sources_rt->data[0]; + std::string type_name = source.type_name.data; + EXPECT_GT(source.raw_file_contents.size, 0); + std::string encoding = source.encoding.data; + EXPECT_EQ(encoding, "msg"); + } + + EXPECT_TRUE( + rosidl_runtime_c__type_description__TypeSource__Sequence__are_equal( + type_sources_rt, ts->get_type_description_sources_func(ts))); + + type_description_interfaces__msg__TypeSource__Sequence__destroy( + type_sources_msg); + rosidl_runtime_c__type_description__TypeSource__Sequence__destroy( + type_sources_rt); +} + +TEST(TestTypeDescriptionConversions, actually_empty_sources_ok) { + // Implicit definition will always be empty + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, srv, BasicTypes_Request); + + const auto * sources = ts->get_type_description_sources_func(ts); + auto * msg = rcl_convert_type_source_sequence_runtime_to_msg(sources); + ASSERT_NE(nullptr, msg); +} + +TEST(TestTypeDescriptionConversions, type_source_sequence_invalid_input) { + EXPECT_TRUE(NULL == rcl_convert_type_source_sequence_msg_to_runtime(NULL)); + EXPECT_TRUE(NULL == rcl_convert_type_source_sequence_runtime_to_msg(NULL)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index d999f1e01..a173200cc 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -28,6 +28,7 @@ extern "C" #include "rcl/client.h" #include "rcl/error_handling.h" #include "rcl/graph.h" +#include "rcl/node_type_cache.h" #include "rcl/subscription.h" #include "rcl/types.h" #include "rcl/wait.h" @@ -63,7 +64,8 @@ _rcl_action_get_zero_initialized_client_impl(void) 0, 0, 0, - 0 + 0, + rosidl_get_zero_initialized_type_hash() }; return null_action_client; } @@ -91,6 +93,12 @@ _rcl_action_client_fini_impl( if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->status_subscription, node)) { ret = RCL_RET_ERROR; } + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != action_client->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_client->impl->type_hash)) + { + ret = RCL_RET_ERROR; + } allocator.deallocate(action_client->impl->action_name, allocator.state); allocator.deallocate(action_client->impl, allocator.state); action_client->impl = NULL; @@ -222,6 +230,17 @@ rcl_action_client_init( SUBSCRIPTION_INIT(feedback); SUBSCRIPTION_INIT(status); + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for action"); + goto fail; + } + action_client->impl->type_hash = *type_support->get_type_hash_func(type_support); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); return ret; fail: diff --git a/rcl_action/src/rcl_action/action_client_impl.h b/rcl_action/src/rcl_action/action_client_impl.h index 777416bfd..f1d430a8d 100644 --- a/rcl_action/src/rcl_action/action_client_impl.h +++ b/rcl_action/src/rcl_action/action_client_impl.h @@ -33,6 +33,7 @@ typedef struct rcl_action_client_impl_s size_t wait_set_result_client_index; size_t wait_set_feedback_subscription_index; size_t wait_set_status_subscription_index; + rosidl_type_hash_t type_hash; } rcl_action_client_impl_t; diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 2320c9286..fbba7b9ec 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -27,6 +27,7 @@ extern "C" #include "rcl_action/wait.h" #include "rcl/error_handling.h" +#include "rcl/node_type_cache.h" #include "rcl/rcl.h" #include "rcl/time.h" @@ -159,6 +160,7 @@ rcl_action_server_init( action_server->impl->goal_handles = NULL; action_server->impl->num_goal_handles = 0u; action_server->impl->clock = NULL; + action_server->impl->type_hash = rosidl_get_zero_initialized_type_hash(); rcl_ret_t ret = RCL_RET_OK; // Initialize services @@ -192,6 +194,19 @@ rcl_action_server_init( ret = RCL_RET_BAD_ALLOC; goto fail; } + + // Store type hash + if (RCL_RET_OK != rcl_node_type_cache_register_type( + node, type_support->get_type_hash_func(type_support), + type_support->get_type_description_func(type_support), + type_support->get_type_description_sources_func(type_support))) + { + rcutils_reset_error(); + RCL_SET_ERROR_MSG("Failed to register type for action"); + goto fail; + } + action_server->impl->type_hash = *type_support->get_type_hash_func(type_support); + return ret; fail: { @@ -249,6 +264,12 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node) } allocator.deallocate(action_server->impl->goal_handles, allocator.state); action_server->impl->goal_handles = NULL; + if ( + ROSIDL_TYPE_HASH_VERSION_UNSET != action_server->impl->type_hash.version && + RCL_RET_OK != rcl_node_type_cache_unregister_type(node, &action_server->impl->type_hash)) + { + ret = RCL_RET_ERROR; + } // Deallocate struct allocator.deallocate(action_server->impl, allocator.state); action_server->impl = NULL; diff --git a/rcl_action/src/rcl_action/action_server_impl.h b/rcl_action/src/rcl_action/action_server_impl.h index 0002e376c..572f799f6 100644 --- a/rcl_action/src/rcl_action/action_server_impl.h +++ b/rcl_action/src/rcl_action/action_server_impl.h @@ -39,6 +39,7 @@ typedef struct rcl_action_server_impl_s size_t wait_set_cancel_service_index; size_t wait_set_result_service_index; size_t wait_set_expire_timer_index; + rosidl_type_hash_t type_hash; } rcl_action_server_impl_t;