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;