Revert "Don't shutdown contained entities. (ros2#313)"

This reverts commit 5366957.
ZettaScaleLabs · Nov 21, 2024 · 2e3ad43 · 2e3ad43
1 parent 1e705c1
commit 2e3ad43
Show file tree

Hide file tree

Showing 2 changed files with 63 additions and 0 deletions.
diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp
@@ -246,6 +246,19 @@ class rmw_context_impl_s::Data final
         return ret;
       }
 
+      // Shutdown all the nodes in this context.
+      for (auto node_it = nodes_.begin(); node_it != nodes_.end(); ++node_it) {
+        ret = node_it->second->shutdown();
+        if (ret != RMW_RET_OK) {
+          RMW_ZENOH_LOG_ERROR_NAMED(
+            "rmw_zenoh_cpp",
+            "Unable to shutdown node with id %zu. rmw_ret_t code: %zu.",
+            node_it->second->id(),
+            ret
+          );
+        }
+      }
+
       z_undeclare_subscriber(z_move(graph_subscriber_));
       if (shm_provider_.has_value()) {
         z_drop(z_move(shm_provider_.value()));

diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp
@@ -401,6 +401,56 @@ rmw_ret_t NodeData::shutdown()
     return ret;
   }
 
+  // Shutdown all the entities within this node.
+  for (auto pub_it = pubs_.begin(); pub_it != pubs_.end(); ++pub_it) {
+    ret = pub_it->second->shutdown();
+    if (ret != RMW_RET_OK) {
+      RMW_ZENOH_LOG_ERROR_NAMED(
+        "rmw_zenoh_cpp",
+        "Unable to shutdown publisher %s within id %zu. rmw_ret_t code: %zu.",
+        pub_it->second->topic_info().name_.c_str(),
+        id_,
+        ret
+      );
+    }
+  }
+  for (auto sub_it = subs_.begin(); sub_it != subs_.end(); ++sub_it) {
+    ret = sub_it->second->shutdown();
+    if (ret != RMW_RET_OK) {
+      RMW_ZENOH_LOG_ERROR_NAMED(
+        "rmw_zenoh_cpp",
+        "Unable to shutdown subscription %s within id %zu. rmw_ret_t code: %zu.",
+        sub_it->second->topic_info().name_.c_str(),
+        id_,
+        ret
+      );
+    }
+  }
+  for (auto srv_it = services_.begin(); srv_it != services_.end(); ++srv_it) {
+    ret = srv_it->second->shutdown();
+    if (ret != RMW_RET_OK) {
+      RMW_ZENOH_LOG_ERROR_NAMED(
+        "rmw_zenoh_cpp",
+        "Unable to shutdown service %s within id %zu. rmw_ret_t code: %zu.",
+        srv_it->second->topic_info().name_.c_str(),
+        id_,
+        ret
+      );
+    }
+  }
+  for (auto cli_it = clients_.begin(); cli_it != clients_.end(); ++cli_it) {
+    ret = cli_it->second->shutdown();
+    if (ret != RMW_RET_OK) {
+      RMW_ZENOH_LOG_ERROR_NAMED(
+        "rmw_zenoh_cpp",
+        "Unable to shutdown client %s within id %zu. rmw_ret_t code: %zu.",
+        cli_it->second->topic_info().name_.c_str(),
+        id_,
+        ret
+      );
+    }
+  }
+
   // Unregister this node from the ROS graph.
   zc_liveliness_undeclare_token(z_move(token_));