diff --git a/include/behaviortree_cpp/bt_factory.h b/include/behaviortree_cpp/bt_factory.h index e5640df25..c7d8e0cec 100644 --- a/include/behaviortree_cpp/bt_factory.h +++ b/include/behaviortree_cpp/bt_factory.h @@ -122,6 +122,8 @@ class Tree void initialize(); + /// Halt the tree and wake tickWhileRunning(). Concurrent or reentrant calls + /// return immediately while the first call is still halting the tree. void haltTree(); [[nodiscard]] TreeNode* rootNode() const; diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index dca440acc..0a2c4de08 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -20,6 +20,7 @@ #include "behaviortree_cpp/utils/strcat.hpp" #include "behaviortree_cpp/utils/wakeup_signal.hpp" +#include #include #include #include @@ -470,6 +471,8 @@ class TreeNode Expected checkPreConditions(); void checkPostConditions(NodeStatus status); + std::atomic_bool& treeHaltFlag(); + /// The method used to interrupt the execution of a RUNNING node. /// Only Async nodes that may return RUNNING should implement it. virtual void halt() = 0; diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 2d7d67d27..66438a0bd 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -671,19 +671,46 @@ void Tree::initialize() // NOLINTNEXTLINE(readability-make-member-function-const) void Tree::haltTree() { - if(rootNode() == nullptr) + auto* root = rootNode(); + if(root == nullptr) + { + return; + } + + auto& halting = root->treeHaltFlag(); + if(halting.exchange(true)) { return; } + // Release the guard even if a node's halt callback throws. + class ResetHaltFlag + { + public: + explicit ResetHaltFlag(std::atomic_bool& flag) : flag_(flag) + {} + ~ResetHaltFlag() + { + flag_.store(false); + } + ResetHaltFlag(const ResetHaltFlag&) = delete; + ResetHaltFlag& operator=(const ResetHaltFlag&) = delete; + ResetHaltFlag(ResetHaltFlag&&) = delete; + ResetHaltFlag& operator=(ResetHaltFlag&&) = delete; + + private: + std::atomic_bool& flag_; + }; + const ResetHaltFlag reset_halting(halting); + // the halt should propagate to all the node if the nodes // have been implemented correctly - rootNode()->haltNode(); + root->haltNode(); //but, just in case.... this should be no-op auto visitor = [](BT::TreeNode* node) { node->haltNode(); }; - BT::applyRecursiveVisitor(rootNode(), visitor); + BT::applyRecursiveVisitor(root, visitor); - rootNode()->resetStatus(); + root->resetStatus(); // interrupt the sleep of tickWhileRunning(), if called from another thread if(wake_up_) diff --git a/src/tree_node.cpp b/src/tree_node.cpp index b1f1b93ae..4fb6126e7 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -30,6 +30,8 @@ struct TreeNode::PImpl const std::string name; NodeStatus status = NodeStatus::IDLE; + // Tree::haltTree() uses this only on the root node. + std::atomic_bool tree_halting{ false }; std::condition_variable state_condition_variable; @@ -178,6 +180,11 @@ void TreeNode::haltNode() } } +std::atomic_bool& TreeNode::treeHaltFlag() +{ + return _p->tree_halting; +} + void TreeNode::setStatus(NodeStatus new_status) { if(new_status == NodeStatus::IDLE) diff --git a/tests/gtest_finally.cpp b/tests/gtest_finally.cpp index 5a92ea65d..59c1fcb8b 100644 --- a/tests/gtest_finally.cpp +++ b/tests/gtest_finally.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -788,3 +789,82 @@ TEST_F(FinallyTest, ConcurrentHaltDuringHaltLoop_DoesNotTickCleanupConcurrently) EXPECT_EQ(ticks, 3); EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); } + +namespace +{ +class CallbackCleanup : public AsyncMain +{ +public: + CallbackCleanup(const std::string& name, const BT::NodeConfig& config, int* halts, + std::function on_running) + : AsyncMain(name, config, halts, false, false), on_running_(std::move(on_running)) + {} + + NodeStatus onRunning() override + { + return on_running_(); + } + +private: + std::function on_running_; +}; + +constexpr auto halt_test_tree = R"( + + + )"; +} // namespace + +TEST_F(FinallyTest, ConcurrentHaltDuringHaltLoop_DoesNotHaltRunningCleanup) +{ + int cleanup_halts = 0; + std::promise entered; + std::promise release; + auto entered_future = entered.get_future(); + auto release_future = release.get_future(); + factory.registerNodeType("CallbackCleanup", &cleanup_halts, [&] { + entered.set_value(); + release_future.wait(); + return NodeStatus::SUCCESS; + }); + auto tree = factory.createTreeFromText(halt_test_tree); + + auto first_halt = std::async(std::launch::async, [&] { + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + }); + + const auto timeout = std::chrono::seconds(2); + if(entered_future.wait_for(timeout) != std::future_status::ready) + { + release.set_value(); + first_halt.get(); + FAIL() << "The first halt did not enter cleanup"; + } + auto second_halt = std::async(std::launch::async, [&] { tree.haltTree(); }); + const bool returned_during_cleanup = + second_halt.wait_for(timeout) == std::future_status::ready; + release.set_value(); + second_halt.get(); + first_halt.get(); + + EXPECT_TRUE(returned_during_cleanup); + EXPECT_EQ(cleanup_halts, 0); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, ReentrantHaltDuringHaltLoop_DoesNotHaltRunningCleanup) +{ + int cleanup_halts = 0; + BT::Tree tree; + factory.registerNodeType("CallbackCleanup", &cleanup_halts, [&] { + tree.haltTree(); + return NodeStatus::SUCCESS; + }); + tree = factory.createTreeFromText(halt_test_tree); + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + + EXPECT_EQ(cleanup_halts, 0); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} diff --git a/tests/gtest_halt_child.cpp b/tests/gtest_halt_child.cpp index 2e9f4ff1c..84b891a26 100644 --- a/tests/gtest_halt_child.cpp +++ b/tests/gtest_halt_child.cpp @@ -106,6 +106,9 @@ void checkBuiltinParentThrowingHalt(const char* body) } EXPECT_NO_THROW(tree.haltTree()); EXPECT_EQ(halts, 1); + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_THROW(tree.haltTree(), std::runtime_error); + EXPECT_EQ(halts, 2); // ~Tree() calls haltTree() again: it must not throw (-> terminate). } } // namespace