Fix rmw_destroy_node() implementations. (#458)

Destruction must always run to completion. Signed-off-by: Michel Hidalgo <[email protected]>
ros2 · Oct 1, 2020 · f887e5f · f887e5f
1 parent 65bb626
commit f887e5f
Show file tree

Hide file tree

Showing 3 changed files with 53 additions and 12 deletions.
diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp
@@ -85,12 +85,34 @@ rmw_destroy_node(rmw_node_t * node)
     return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   rmw_context_t * context = node->context;
-  rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node(
+
+  rmw_ret_t ret = RMW_RET_OK;
+  rmw_error_state_t error_state;
+  rmw_ret_t inner_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node(
     eprosima_fastrtps_identifier, node);
   if (RMW_RET_OK != ret) {
-    return ret;
+    error_state = *rmw_get_error_state();
+    ret = inner_ret;
+    rmw_reset_error();
+  }
+
+  inner_ret = rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context);
+  if (RMW_RET_OK != inner_ret) {
+    if (RMW_RET_OK != ret) {
+      RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str);
+      RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n");
+    } else {
+      error_state = *rmw_get_error_state();
+      ret = inner_ret;
+    }
+    rmw_reset_error();
+  }
+
+  if (RMW_RET_OK != ret) {
+    rmw_set_error_state(error_state.message, error_state.file, error_state.line_number);
   }
-  return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context);
+
+  return ret;
 }
 
 const rmw_guard_condition_t *

diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp
@@ -84,12 +84,34 @@ rmw_destroy_node(rmw_node_t * node)
     eprosima_fastrtps_identifier,
     return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   rmw_context_t * context = node->context;
-  rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node(
+
+  rmw_ret_t ret = RMW_RET_OK;
+  rmw_error_state_t error_state;
+  rmw_ret_t inner_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node(
     eprosima_fastrtps_identifier, node);
   if (RMW_RET_OK != ret) {
-    return ret;
+    error_state = *rmw_get_error_state();
+    ret = inner_ret;
+    rmw_reset_error();
+  }
+
+  inner_ret = rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context);
+  if (RMW_RET_OK != inner_ret) {
+    if (RMW_RET_OK != ret) {
+      RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str);
+      RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n");
+    } else {
+      error_state = *rmw_get_error_state();
+      ret = inner_ret;
+    }
+    rmw_reset_error();
+  }
+
+  if (RMW_RET_OK != ret) {
+    rmw_set_error_state(error_state.message, error_state.file, error_state.line_number);
   }
-  return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context);
+
+  return ret;
 }
 
 const rmw_guard_condition_t *

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp
@@ -131,26 +131,23 @@ __rmw_destroy_node(
   rmw_node_t * node)
 {
   assert(node->implementation_identifier == identifier);
-
+  rmw_ret_t ret = RMW_RET_OK;
   auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
   rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache;
   {
     std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
     rmw_dds_common::msg::ParticipantEntitiesInfo participant_msg =
       graph_cache.remove_node(common_context->gid, node->name, node->namespace_);
-    rmw_ret_t ret = __rmw_publish(
+    ret = __rmw_publish(
       identifier,
       common_context->pub,
       static_cast<void *>(&participant_msg),
       nullptr);
-    if (RMW_RET_OK != ret) {
-      return ret;
-    }
   }
   rmw_free(const_cast<char *>(node->name));
   rmw_free(const_cast<char *>(node->namespace_));
   rmw_node_free(node);
-  return RMW_RET_OK;
+  return ret;
 }
 
 const rmw_guard_condition_t *