address review comments

Signed-off-by: Hans-Joachim Krauch <achim.krauch@gmail.com>

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.
+ *
+ * <hr>
+ * 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.
+ *
+ * <hr>
+ * 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,

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;
   }

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.
- *
- * <hr>
- * 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.
- *
- * <hr>
- * 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_

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,

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) {

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;
 

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) {