Remove node from executor in case of an exception in spin_*() impleme…

…ntations
ros2 · Sep 21, 2022 · 78df041 · 78df041
1 parent 6a8c61c
commit 78df041
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 4 deletions.
diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp
@@ -74,10 +74,16 @@ spin_node_until_future_complete(
   const FutureT & future,
   std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
+  rclcpp::FutureReturnCode retcode;
   // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
   // inside a callback executed by an executor.
   executor.add_node(node_ptr);
-  auto retcode = executor.spin_until_future_complete(future, timeout);
+  try {
+    retcode = executor.spin_until_future_complete(future, timeout);
+  } catch (...) {
+    executor.remove_node(node_ptr);
+    throw;
+  }
   executor.remove_node(node_ptr);
   return retcode;
 }

diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
@@ -389,15 +389,25 @@ Executor::spin_node_once_nanoseconds(
 {
   this->add_node(node, false);
   // non-blocking = true
-  spin_once(timeout);
+  try {
+    spin_once(timeout);
+  } catch (...) {
+    this->remove_node(node, false);
+    throw;
+  }
   this->remove_node(node, false);
 }
 
 void
 Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
 {
   this->add_node(node, false);
-  spin_some();
+  try {
+    spin_some();
+  } catch (...) {
+    this->remove_node(node, false);
+    throw;
+  }
   this->remove_node(node, false);
 }
 

diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp
@@ -32,7 +32,12 @@ rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
 {
   rclcpp::executors::SingleThreadedExecutor exec;
   exec.add_node(node_ptr);
-  exec.spin();
+  try {
+    exec.spin();
+  } catch (...) {
+    exec.remove_node(node_ptr);
+    throw;
+  }
   exec.remove_node(node_ptr);
 }