diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 36fb0d63cf..32713a2719 100644 --- 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 timeout = std::chrono::duration(-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 index 73b7b8d80d..934c927579 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -389,7 +389,12 @@ 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); } @@ -397,7 +402,12 @@ 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 index 0a900c07da..1d0f9b8718 100644 --- 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); }