From 9a481a1b1fd630efadebc178e8ce8b10f56494b4 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Fri, 14 Mar 2025 16:46:36 +0100 Subject: [PATCH] fix: Fixed possible memory violation after remove_node Up until this change, after the call to remove node, entities of a node might still be in the use by the executor. Therefore direct deletion of the node after the call might result in a memory violation if an entity would do a callback into the node itself. From now on, by default the remove_node will be blocking until all entities are released by the executor, which should be the expected behavior for most users. Signed-off-by: Janosch Machowinski --- rclcpp/include/rclcpp/executor.hpp | 12 ++++- rclcpp/src/rclcpp/executor.cpp | 20 ++++++-- rclcpp/test/rclcpp/test_executor.cpp | 68 ++++++++++++++++++++++++++++ 3 files changed, 95 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 4b75cc8711..da33ed0868 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -220,12 +220,18 @@ class Executor * \param[in] notify True to trigger the interrupt guard condition and wake up the executor. * This is useful if the last node was removed from the executor while the executor was blocked * waiting for work in another thread, because otherwise the executor would never be notified. + * \param[in] wait_until_removed If true and the executor is spinning, the method will block until + * all entities from the node have been removed from the executor. Note, if the remove_node call + * was triggered from a callback of an entity of the node itself, and this value is true, this is + * likely to result in a deadlock. * \throw std::runtime_error if the node is not associated with an executor. * \throw std::runtime_error if the node is not associated with this executor. */ RCLCPP_PUBLIC virtual void - remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); + remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true, + bool wait_until_removed = true); /// Convenience function which takes Node and forwards NodeBaseInterface. /** @@ -233,7 +239,9 @@ class Executor */ RCLCPP_PUBLIC virtual void - remove_node(std::shared_ptr node_ptr, bool notify = true); + remove_node( + std::shared_ptr node_ptr, bool notify = true, + bool wait_until_removed = true); /// Add a node to executor, execute the next available unit of work, and remove the node. /** diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 70e2f69989..e6c71dc3dd 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -220,7 +220,9 @@ Executor::add_node(std::shared_ptr node_ptr, bool notify) } void -Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +Executor::remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify, + bool wait_until_removed) { this->collector_.remove_node(node_ptr); @@ -231,12 +233,24 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node std::string( "Failed to handle entities update on node remove: ") + ex.what()); } + + if(wait_until_removed) { + // some other thread is spinning, wait until the executor + // picked up the remove + while(spinning && collector_.has_pending()) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + // if we are not spinning, we can directly update the collection + if(!spinning) { + this->collector_.update_collections(); + } + } } void -Executor::remove_node(std::shared_ptr node_ptr, bool notify) +Executor::remove_node(std::shared_ptr node_ptr, bool notify, bool wait_until_removed) { - this->remove_node(node_ptr->get_node_base_interface(), notify); + this->remove_node(node_ptr->get_node_base_interface(), notify, wait_until_removed); } void diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index e0669fc647..00c9086108 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -528,3 +528,71 @@ TEST_F(TestExecutor, is_spinning) { ASSERT_TRUE(timer_called); } + +TEST_F(TestExecutor, remove_node) { + using namespace std::chrono_literals; + + // Create an Executor + rclcpp::executors::SingleThreadedExecutor executor; + + auto future = std::async(std::launch::async, [&executor] {executor.spin();}); + + auto node = std::make_shared("remove_node_test"); + std::vector timers; + + std::atomic_bool timer_running = false; + auto timer = node->create_timer(std::chrono::milliseconds(1), [&timer_running] () { + timer_running = true; + std::this_thread::sleep_for(std::chrono::milliseconds(400)); + timer_running = false; + }); + timer->reset(); + + executor.add_node(node); + + while(!timer_running) { + // let the executor pick up the nodes + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + ASSERT_GT(timer.use_count(), 1); + + executor.remove_node(node, true, true); + + ASSERT_EQ(timer.use_count(), 1); + + std::future_status future_status = std::future_status::timeout; + do { + executor.cancel(); + future_status = future.wait_for(1s); + } while (future_status == std::future_status::timeout); + EXPECT_EQ(future_status, std::future_status::ready); + future.get(); +} + +TEST_F(TestExecutor, remove_node_not_spinning) { + using namespace std::chrono_literals; + + // Create an Executor + rclcpp::executors::SingleThreadedExecutor executor; + + auto node = std::make_shared("remove_node_test"); + std::vector timers; + + bool executed = false; + auto timer = node->create_timer(std::chrono::milliseconds(1), [&executed] () { + executed = true; + }); + timer->reset(); + + executor.add_node(node); + + while(!executed) { + // let the executor pick up the nodes + executor.spin_some(std::chrono::milliseconds(2)); + } + + executor.remove_node(node, true, true); + + ASSERT_EQ(timer.use_count(), 1); + +}