diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a406cb6e8e..2a402de819 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -298,16 +298,18 @@ Executor::remove_callback_group_from_map( } else { throw std::runtime_error("Callback group needs to be associated with executor."); } + // The group is no longer handled by this executor, so stop waiting on its guard condition. + // This must not depend on the node having other groups left in the executor: the guard + // condition is owned by the group, and the memory strategy only keeps a raw pointer to it. + auto guard_condition_iter = weak_groups_to_guard_conditions_.find(weak_group_ptr); + if (guard_condition_iter != weak_groups_to_guard_conditions_.end()) { + memory_strategy_->remove_guard_condition(guard_condition_iter->second); + weak_groups_to_guard_conditions_.erase(guard_condition_iter); + } // If the node was matched and removed, interrupt waiting. if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_)) { - auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr); - if (iter != weak_groups_to_guard_conditions_.end()) { - memory_strategy_->remove_guard_condition(iter->second); - } - weak_groups_to_guard_conditions_.erase(weak_group_ptr); - if (notify) { try { interrupt_guard_condition_.trigger(); diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index bdbb0a1079..731c406de6 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -577,3 +577,50 @@ TEST_F(TestExecutor, is_spinning) { ASSERT_TRUE(timer_called); } + +TEST_F(TestExecutor, remove_node_removes_guard_conditions_of_all_callback_groups) { + DummyExecutor dummy; + const size_t guard_conditions_before = dummy.memory_strategy_ptr()->number_of_guard_conditions(); + + auto node = std::make_shared("node", "ns"); + // In addition to the default callback group, this one is added automatically with the node. + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + dummy.add_node(node->get_node_base_interface(), false); + EXPECT_EQ( + guard_conditions_before + 2u, dummy.memory_strategy_ptr()->number_of_guard_conditions()); + + dummy.remove_node(node->get_node_base_interface(), false); + // The guard conditions are owned by the callback groups, so none of them may be left behind. + EXPECT_EQ(guard_conditions_before, dummy.memory_strategy_ptr()->number_of_guard_conditions()); + + cb_group.reset(); + node.reset(); + // The executor must not wait on anything that belonged to the destroyed node. + EXPECT_NO_THROW(dummy.spin_some(std::chrono::milliseconds(1))); +} + +TEST_F(TestExecutor, remove_callback_group_removes_guard_condition) { + DummyExecutor dummy; + const size_t guard_conditions_before = dummy.memory_strategy_ptr()->number_of_guard_conditions(); + + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group1 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::CallbackGroup::SharedPtr cb_group2 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + dummy.add_callback_group(cb_group1, node->get_node_base_interface(), false); + dummy.add_callback_group(cb_group2, node->get_node_base_interface(), false); + EXPECT_EQ( + guard_conditions_before + 2u, dummy.memory_strategy_ptr()->number_of_guard_conditions()); + + // The node still has another callback group in the executor. + dummy.remove_callback_group(cb_group1, false); + EXPECT_EQ( + guard_conditions_before + 1u, dummy.memory_strategy_ptr()->number_of_guard_conditions()); + + dummy.remove_callback_group(cb_group2, false); + EXPECT_EQ(guard_conditions_before, dummy.memory_strategy_ptr()->number_of_guard_conditions()); +}