diff --git a/rcl/include/rcl/node_type_cache.h b/rcl/include/rcl/node_type_cache.h
index 4522134dd..01a6d6616 100644
--- a/rcl/include/rcl/node_type_cache.h
+++ b/rcl/include/rcl/node_type_cache.h
@@ -33,6 +33,50 @@ typedef struct rcl_type_info_t
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.
+ *
+ *
+ * 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,
diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c
index 21f1c753f..4fa9351aa 100644
--- a/rcl/src/rcl/node.c
+++ b/rcl/src/rcl/node.c
@@ -322,6 +322,11 @@ rcl_node_init(
(ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), ROS_PACKAGE_NAME,
"Failed to fini get_type_description service for node: %i", ret);
+ ret = rcl_node_type_cache_fini(node);
+ RCUTILS_LOG_ERROR_EXPRESSION_NAMED(
+ (ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), ROS_PACKAGE_NAME,
+ "Failed to fini node_type_cache for node: %i", ret);
+
if (rcl_logging_rosout_enabled() &&
node->impl->options.enable_rosout &&
node->impl->logger_name)
@@ -605,6 +610,9 @@ static void rcl_node_get_type_description_service_callback(
RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME, "Failed to parse type hash '%s'",
request.type_hash.data);
+ response.successful = false;
+ rosidl_runtime_c__String__assign(&response.failure_reason, "Failed to parse type hash");
+ goto send_response;
}
rcl_ret_t ret =
@@ -631,6 +639,7 @@ static void rcl_node_get_type_description_service_callback(
"Type not currently used by this node");
}
+send_response:
if (RCL_RET_OK != rcl_send_response(
&node->impl->get_type_description_service,
&request_header, &response))
@@ -654,7 +663,7 @@ rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node)
}
rcl_reset_error(); // Reset the error message set by rcl_service_is_valid()
- char * serviceName = NULL;
+ char * service_name = NULL;
const rosidl_service_type_support_t * type_support =
ROSIDL_GET_SRV_TYPE_SUPPORT(
type_description_interfaces, srv,
@@ -665,7 +674,7 @@ rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node)
// Construct service name
rcl_ret_t ret = rcl_node_resolve_name(
node, "~/get_type_description",
- allocator, true, true, &serviceName);
+ allocator, true, true, &service_name);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG(
"Failed to construct ~/get_type_description service name");
@@ -675,8 +684,8 @@ rcl_ret_t rcl_node_type_description_service_init(rcl_node_t * node)
// Initialize service
ret = rcl_service_init(
&node->impl->get_type_description_service, node,
- type_support, serviceName, &service_ops);
- allocator.deallocate(serviceName, allocator.state);
+ type_support, service_name, &service_ops);
+ allocator.deallocate(service_name, allocator.state);
if (RCL_RET_OK != ret) {
return ret;
}
diff --git a/rcl/src/rcl/node_impl.h b/rcl/src/rcl/node_impl.h
index c4a00faae..8d4f5847e 100644
--- a/rcl/src/rcl/node_impl.h
+++ b/rcl/src/rcl/node_impl.h
@@ -34,48 +34,4 @@ struct rcl_node_impl_s
rcl_service_t get_type_description_service;
};
-/// 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.
- *
- *
- * 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);
-
#endif // RCL__NODE_IMPL_H_
diff --git a/rcl/src/rcl/node_type_cache.c b/rcl/src/rcl/node_type_cache.c
index 7258151d2..83bd14318 100644
--- a/rcl/src/rcl/node_type_cache.c
+++ b/rcl/src/rcl/node_type_cache.c
@@ -25,7 +25,7 @@
typedef struct rcl_type_info_with_registration_count_t
{
/// Counter to keep track of registrations
- size_t numRegistrations;
+ size_t num_registrations;
/// The actual type info.
rcl_type_info_t type_info;
@@ -133,8 +133,7 @@ rcl_ret_t rcl_node_type_cache_get_type_info(
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__TypeDescription * type_description,
const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources)
{
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
@@ -145,25 +144,17 @@ rcl_ret_t rcl_node_type_cache_register_type(
rcl_type_info_with_registration_count_t type_info_with_registrations;
- // If the type already exists, we only have to increment the registration
- // count.
- if (rcutils_hash_map_key_exists(
- &node->impl->registered_types_by_type_hash,
- type_hash))
- {
- if (RCUTILS_RET_OK !=
- rcutils_hash_map_get(
- &node->impl->registered_types_by_type_hash,
- type_hash, &type_info_with_registrations))
- {
- RCL_SET_ERROR_MSG("Failed to retrieve type info");
- return RCL_RET_ERROR;
- }
+ const rcutils_ret_t rcutils_ret = rcutils_hash_map_get(
+ &node->impl->registered_types_by_type_hash,
+ type_hash, &type_info_with_registrations);
- type_info_with_registrations.numRegistrations++;
- } else {
+ 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.numRegistrations = 1;
+ type_info_with_registrations.num_registrations = 1;
// Convert type description struct to type description message struct.
type_info_with_registrations.type_info.type_description =
@@ -177,7 +168,12 @@ rcl_ret_t rcl_node_type_cache_register_type(
rcl_convert_type_source_sequence_runtime_to_msg(type_description_sources);
RCL_CHECK_FOR_NULL_WITH_MSG(
type_info_with_registrations.type_info.type_description,
- "converting type sources struct failed", return RCL_RET_ERROR);
+ "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.
@@ -187,6 +183,10 @@ rcl_ret_t rcl_node_type_cache_register_type(
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;
}
@@ -211,7 +211,7 @@ rcl_ret_t rcl_node_type_cache_unregister_type(
return RCL_RET_ERROR;
}
- if (--type_info.numRegistrations > 0) {
+ if (--type_info.num_registrations > 0) {
if (RCUTILS_RET_OK !=
rcutils_hash_map_set(
&node->impl->registered_types_by_type_hash,
diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c
index 10b2ff5f9..7d6c94a42 100644
--- a/rcl/src/rcl/publisher.c
+++ b/rcl/src/rcl/publisher.c
@@ -116,17 +116,6 @@ rcl_publisher_init(
&(options->rmw_publisher_options));
RCL_CHECK_FOR_NULL_WITH_MSG(
publisher->impl->rmw_handle, rmw_get_error_string().str, goto fail);
- // Register type.
- 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;
- }
// get actual qos, and store it
rmw_ret_t rmw_ret = rmw_publisher_get_actual_qos(
publisher->impl->rmw_handle,
@@ -151,6 +140,18 @@ rcl_publisher_init(
(const void *)publisher->impl->rmw_handle,
remapped_topic_name,
options->qos.depth);
+ // Register type.
+ 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;
+ }
+
goto cleanup;
fail:
if (publisher->impl) {
diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c
index 616f087b7..7ca2e9901 100644
--- a/rcl/src/rcl/service.c
+++ b/rcl/src/rcl/service.c
@@ -120,6 +120,7 @@ rcl_service_init(
RCL_CHECK_FOR_NULL_WITH_MSG(
service->impl, "allocating memory failed",
return RCL_RET_BAD_ALLOC;);
+
// Expand and remap the given service name.
rcl_ret_t ret = rcl_node_resolve_name(
node,
@@ -139,19 +140,6 @@ rcl_service_init(
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'",
service->impl->remapped_service_name);
- // Register type.
- 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 free_remapped_service_name;
- }
-
if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) {
RCUTILS_LOG_WARN_NAMED(
ROS_PACKAGE_NAME,
@@ -208,6 +196,18 @@ rcl_service_init(
(const void *)node,
(const void *)service->impl->rmw_handle,
service->impl->remapped_service_name);
+ // Register type.
+ 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;
+ }
return RCL_RET_OK;
diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c
index f8523a782..682d8373e 100644
--- a/rcl/src/rcl/subscription.c
+++ b/rcl/src/rcl/subscription.c
@@ -111,17 +111,6 @@ rcl_subscription_init(
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
goto fail;
}
- // Register type.
- 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;
- }
// get actual qos, and store it
rmw_ret_t rmw_ret = rmw_subscription_get_actual_qos(
subscription->impl->rmw_handle,
@@ -145,6 +134,18 @@ rcl_subscription_init(
(const void *)subscription->impl->rmw_handle,
remapped_topic_name,
options->qos.depth);
+ // Register type.
+ 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;
+ }
+
goto cleanup;
fail:
if (subscription->impl) {
diff --git a/rcl/src/rcl/type_description_conversions.c b/rcl/src/rcl/type_description_conversions.c
index e49db71c8..977269168 100644
--- a/rcl/src/rcl/type_description_conversions.c
+++ b/rcl/src/rcl/type_description_conversions.c
@@ -12,6 +12,7 @@
// 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"
@@ -26,9 +27,8 @@ static bool individual_type_description_runtime_to_msg(
const rosidl_runtime_c__type_description__IndividualTypeDescription * in,
type_description_interfaces__msg__IndividualTypeDescription * out)
{
- if (NULL == in) {
- return false;
- }
+ 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) &&
@@ -86,9 +86,8 @@ static bool individual_type_description_msg_to_runtime(
const type_description_interfaces__msg__IndividualTypeDescription * in,
rosidl_runtime_c__type_description__IndividualTypeDescription * out)
{
- if (NULL == in) {
- return false;
- }
+ 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) &&
@@ -145,16 +144,12 @@ type_description_interfaces__msg__TypeDescription *
rcl_convert_type_description_runtime_to_msg(
const rosidl_runtime_c__type_description__TypeDescription * runtime_description)
{
- if (NULL == runtime_description) {
- return NULL;
- }
+ RCL_CHECK_ARGUMENT_FOR_NULL(runtime_description, NULL);
// Create the object
type_description_interfaces__msg__TypeDescription * out =
type_description_interfaces__msg__TypeDescription__create();
- if (NULL == out) {
- return NULL;
- }
+ RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL);
// init referenced_type_descriptions with the correct size
if (!type_description_interfaces__msg__IndividualTypeDescription__Sequence__init(
@@ -193,16 +188,12 @@ rosidl_runtime_c__type_description__TypeDescription *
rcl_convert_type_description_msg_to_runtime(
const type_description_interfaces__msg__TypeDescription * description_msg)
{
- if (NULL == description_msg) {
- return NULL;
- }
+ 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();
- if (NULL == out) {
- return NULL;
- }
+ RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL);
// init referenced_type_descriptions with the correct size
if (!rosidl_runtime_c__type_description__IndividualTypeDescription__Sequence__init(
@@ -239,16 +230,12 @@ 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)
{
- if (NULL == runtime_type_sources) {
- return NULL;
- }
+ 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);
- if (NULL == out) {
- return NULL;
- }
+ RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL);
// Copy type sources
for (size_t i = 0; i < runtime_type_sources->size; ++i) {
@@ -283,16 +270,12 @@ 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)
{
- if (NULL == type_sources_msg) {
- return NULL;
- }
+ 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);
- if (NULL == out) {
- return NULL;
- }
+ RCL_CHECK_ARGUMENT_FOR_NULL(out, NULL);
// Copy type sources
for (size_t i = 0; i < type_sources_msg->size; ++i) {
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
index d75e71012..510239fe7 100644
--- a/rcl_action/src/rcl_action/action_server.c
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -142,17 +142,6 @@ rcl_action_server_init(
return RCL_RET_ALREADY_INIT;
}
- // Register type.
- 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)))
- {
- RCL_SET_ERROR_MSG("Failed to register type for action");
- return RCL_RET_ERROR;
- }
-
// Allocate for action server
action_server->impl = (rcl_action_server_impl_t *)allocator.allocate(
sizeof(rcl_action_server_impl_t), allocator.state);
@@ -206,6 +195,18 @@ rcl_action_server_init(
ret = RCL_RET_BAD_ALLOC;
goto fail;
}
+
+ // Register type.
+ ret = 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));
+ if (RCL_RET_OK != ret) {
+ rcutils_reset_error();
+ RCL_SET_ERROR_MSG("Failed to register type for action");
+ goto fail;
+ }
+
return ret;
fail:
{