diff --git a/CHANGELOG.rst b/CHANGELOG.rst index fd438c605..baf2288b2 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,7 +4,7 @@ Changelog for package behaviortree_cpp Forthcoming ----------- -* Add Finally control node: runs a cleanup child after the main child finishes, fails, or throws (then rethrows), and runs it synchronously when halted. Registers the built-in ID ``Finally`` +* Add Finally control node: runs a cleanup child after the main child succeeds, fails, or throws (then rethrows). A main child that is skipped before it starts skips cleanup too. When halted from the thread that ticks the tree, keeps ticking cleanup until it finishes or ``halt_timeout_msec`` (default 10000) runs out; a halt from another thread, or ``halt_timeout_msec`` of 0, gives cleanup one tick. Registers the built-in ID ``Finally`` 4.9.0 (2026-02-11) ------------------ diff --git a/include/behaviortree_cpp/controls/finally_node.h b/include/behaviortree_cpp/controls/finally_node.h index 88b799d9f..16336cf13 100644 --- a/include/behaviortree_cpp/controls/finally_node.h +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -2,7 +2,9 @@ #include "behaviortree_cpp/control_node.h" +#include #include +#include namespace BT { @@ -10,18 +12,30 @@ namespace BT * @brief The Finally node ticks its first child ("main") and then ticks its * second child ("cleanup"), like try/finally. * - * - Cleanup runs after main returns SUCCESS, FAILURE or SKIPPED. + * - Cleanup runs after main returns SUCCESS or FAILURE. If main returns SKIPPED on + * its first tick it did nothing, so cleanup is not run and the node returns + * SKIPPED. If main is skipped after it returned RUNNING, for example by _while, + * cleanup runs and the node returns SKIPPED. * - If main throws (any type), main is halted, cleanup runs, and then the * exception is rethrown. * - The node returns main's status, or FAILURE if cleanup fails. - * - If this node is halted while main or cleanup is RUNNING, main is halted - * and cleanup is ticked again every 10 ms on the thread calling halt(), so - * asynchronous cleanup can finish. This stops when cleanup finishes, fails - * or throws, or when "halt_timeout_msec" runs out, in which case cleanup is - * halted unfinished. Cleanup always gets at least one tick, no later tick - * starts after the timeout, and a tick that blocks is not cut short. halt() blocks its caller, and any lock the - * caller holds, until then, so a slow cleanup also delays a parent such as - * a ReactiveSequence. Call halt() from the thread that ticks the tree. + * - If this node is halted while main or cleanup is RUNNING, main is halted and + * cleanup is ticked inside halt(), on the calling thread and under any lock the + * caller holds, so a cleanup action that blocks also blocks the caller. + * - With "halt_timeout_msec" at 0, cleanup gets one tick, like TryCatch with + * catch_on_halt="true", and a cleanup that needs several ticks is cut short. + * - Above 0 (the default, 10000), halt() ticks cleanup once, and again every + * 10 ms until it finishes, fails or throws, or the timeout runs out, in which + * case cleanup is halted unfinished. No tick starts after the timeout, and a + * tick that blocks is not cut short. halt() blocks its caller, and any lock the + * caller holds, so a slow cleanup also delays a parent such as a + * ReactiveSequence. Upstream BehaviorTree.CPP defaults to 0. This fork keeps + * 10000 because MoveIt Pro 10.2 shipped with it. + * - The loop runs only when halt() is called from the thread that last ticked this + * node. A halt from another thread, such as a Timeout decorator's timer or a + * service callback calling Tree::haltTree(), gives cleanup the single tick and + * prints a note to stderr. A Finally nested inside that halt also gets a single + * tick. halt() is not synchronized with a concurrent tick(). * - An exception thrown by cleanup in tick() halts cleanup, leaves the node * IDLE and propagates. It replaces main's status, or main's exception, * which is dropped. The next tick starts with main. @@ -47,7 +61,9 @@ class FinallyNode : public ControlNode { return { InputPort("halt_timeout_msec", kDefaultHaltTimeoutMsec, "When halted, how long to keep ticking cleanup " - "before halting it unfinished, in milliseconds") }; + "before halting it unfinished, in milliseconds. " + "Below 10 (the tick period) cleanup gets a single " + "tick") }; } void halt() override; @@ -55,12 +71,15 @@ class FinallyNode : public ControlNode private: static constexpr unsigned kDefaultHaltTimeoutMsec = 10000; + // Written in tick(), read in halt(), which may run on another thread. + std::atomic tick_thread_{}; bool in_cleanup_ = false; + bool main_started_ = false; NodeStatus main_status_ = NodeStatus::IDLE; std::exception_ptr main_exception_; - void haltChildNoThrow(size_t i); - void finishCleanupDuringHalt(); + void haltChildNoThrow(size_t i) noexcept; + void finishCleanupDuringHalt(bool foreign_thread); BT::NodeStatus tick() override; }; diff --git a/src/controls/finally_node.cpp b/src/controls/finally_node.cpp index a828b6e55..0e5d8b332 100644 --- a/src/controls/finally_node.cpp +++ b/src/controls/finally_node.cpp @@ -13,8 +13,31 @@ namespace { constexpr std::chrono::milliseconds kHaltTickPeriod{ 10 }; +// True while this thread runs a halt() that came from a thread other than the one ticking the +// node. A Finally ticked or halted inside it, e.g. as cleanup, must not run its blocking loop. +thread_local bool tls_in_foreign_halt = false; + +class ForeignHaltScope +{ +public: + explicit ForeignHaltScope(bool foreign) + : previous_(std::exchange(tls_in_foreign_halt, tls_in_foreign_halt || foreign)) + {} + ~ForeignHaltScope() + { + tls_in_foreign_halt = previous_; + } + ForeignHaltScope(const ForeignHaltScope&) = delete; + ForeignHaltScope& operator=(const ForeignHaltScope&) = delete; + ForeignHaltScope(ForeignHaltScope&&) = delete; + ForeignHaltScope& operator=(ForeignHaltScope&&) = delete; + +private: + bool previous_; +}; + void printException(std::string_view node_name, const char* context, - const std::exception_ptr& exception) + const std::exception_ptr& exception) noexcept { std::cerr << "[" << node_name << "]: " << context << ": "; try @@ -41,25 +64,32 @@ FinallyNode::FinallyNode(const std::string& name, const NodeConfig& config) void FinallyNode::halt() { // halt() also runs from ~Tree(), where a propagating exception terminates, so nothing here throws. - if(status() == NodeStatus::RUNNING && children_nodes_.size() == 2) + const bool foreign = + tls_in_foreign_halt || std::this_thread::get_id() != tick_thread_.load(); { - if(!in_cleanup_) + const ForeignHaltScope scope(foreign); + if(status() == NodeStatus::RUNNING && children_nodes_.size() == 2) { - haltChildNoThrow(0); + if(!in_cleanup_) + { + haltChildNoThrow(0); + } + finishCleanupDuringHalt(foreign); + } + for(size_t i = 0; i < children_nodes_.size(); i++) + { + haltChildNoThrow(i); } - finishCleanupDuringHalt(); - } - for(size_t i = 0; i < children_nodes_.size(); i++) - { - haltChildNoThrow(i); } + tick_thread_.store(std::thread::id{}); in_cleanup_ = false; + main_started_ = false; main_status_ = NodeStatus::IDLE; main_exception_ = nullptr; resetStatus(); } -void FinallyNode::finishCleanupDuringHalt() +void FinallyNode::finishCleanupDuringHalt(bool foreign_thread) { unsigned timeout_msec = kDefaultHaltTimeoutMsec; if(auto input = getInput("halt_timeout_msec")) @@ -75,10 +105,18 @@ void FinallyNode::finishCleanupDuringHalt() std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_msec); try { - // Cleanup is commonly asynchronous, so keep ticking it until it finishes rather than - // halting it after its first tick. NodeStatus cleanup_status = children_nodes_[1]->executeTick(); - while(cleanup_status == NodeStatus::RUNNING) + // Cleanup gets one tick. Keep ticking only if halt_timeout_msec > 0 and this is the thread + // that ticks the tree, so a halt from a timer or service thread does not block its caller. + if(timeout_msec > 0 && foreign_thread && cleanup_status == NodeStatus::RUNNING) + { + std::cerr << "[" << name() + << "]: halted from a thread that does not tick this node, cleanup gets a " + "single tick instead of halt_timeout_msec" + << std::endl; + } + const bool keep_ticking = timeout_msec > 0 && !foreign_thread; + while(keep_ticking && cleanup_status == NodeStatus::RUNNING) { // Start no tick after the deadline, so a tick that blocks cannot extend the wait. const auto next_tick = std::chrono::steady_clock::now() + kHaltTickPeriod; @@ -104,7 +142,7 @@ void FinallyNode::finishCleanupDuringHalt() } } -void FinallyNode::haltChildNoThrow(size_t i) +void FinallyNode::haltChildNoThrow(size_t i) noexcept { try { @@ -126,10 +164,12 @@ NodeStatus FinallyNode::tick() if(!isStatusActive(status())) { in_cleanup_ = false; + main_started_ = false; main_exception_ = nullptr; } setStatus(NodeStatus::RUNNING); + tick_thread_.store(std::this_thread::get_id()); if(!in_cleanup_) { @@ -146,13 +186,23 @@ NodeStatus FinallyNode::tick() if(main_status_ == NodeStatus::RUNNING) { + main_started_ = true; return NodeStatus::RUNNING; } if(main_status_ == NodeStatus::IDLE) { throw LogicError("[", name(), "]: A child should not return IDLE"); } + if(main_status_ == NodeStatus::SKIPPED && !main_started_) + { + // Main did nothing, so there is nothing to clean up. executeTick() keeps our RUNNING + // status on SKIPPED, and halt() would run cleanup. + resetChildren(); + resetStatus(); + return NodeStatus::SKIPPED; + } in_cleanup_ = true; + main_started_ = false; } NodeStatus cleanup_status = NodeStatus::IDLE; @@ -194,7 +244,8 @@ NodeStatus FinallyNode::tick() } if(main_status_ == NodeStatus::SKIPPED) { - // executeTick() keeps our RUNNING status on SKIPPED, and halt() would rerun cleanup. + // Main was skipped after it started, e.g. by _while. executeTick() keeps our RUNNING status + // on SKIPPED, and halt() would rerun cleanup. resetStatus(); } return main_status_; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7ab82a7f5..3fde2d42e 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -48,6 +48,7 @@ set(BT_TESTS gtest_tree.cpp gtest_try_catch.cpp gtest_finally.cpp + gtest_halt_child.cpp gtest_exception_tracking.cpp gtest_updates.cpp gtest_wakeup.cpp diff --git a/tests/gtest_finally.cpp b/tests/gtest_finally.cpp index 74b17eb76..e9b891e6e 100644 --- a/tests/gtest_finally.cpp +++ b/tests/gtest_finally.cpp @@ -2,6 +2,7 @@ #include #include +#include #include @@ -250,7 +251,19 @@ TEST_F(FinallyTest, WrongChildCount_RejectedAtLoad) BT::RuntimeError); } -TEST_F(FinallyTest, MainSkipped_CleanupRunsAndReturnsSkipped) +TEST_F(FinallyTest, HaltByDefault_AsyncCleanupFinishes) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(cleanup_ticks, 2); +} + +TEST_F(FinallyTest, MainSkipped_CleanupDoesNotRun) { auto tree = factory.createTreeFromText(R"( @@ -258,10 +271,10 @@ TEST_F(FinallyTest, MainSkipped_CleanupRunsAndReturnsSkipped) )"); EXPECT_EQ(tree.tickWhileRunning(), NodeStatus::SKIPPED); - EXPECT_EQ(cleanup_count, 1); - // The node must not be left RUNNING, or this halt would run cleanup again. + EXPECT_EQ(cleanup_count, 0); + // The node must not be left RUNNING, or this halt would run cleanup. tree.haltTree(); - EXPECT_EQ(cleanup_count, 1); + EXPECT_EQ(cleanup_count, 0); } TEST_F(FinallyTest, AsyncCleanup_ReturnsMainStatusWhenCleanupCompletes) @@ -294,7 +307,7 @@ TEST_F(FinallyTest, HaltWhileCleanupRunning_CleanupFinishes) { auto tree = factory.createTreeFromText(R"( - + )"); EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); @@ -307,7 +320,7 @@ TEST_F(FinallyTest, HaltWhileMainRunning_AsyncCleanupFinishes) { auto tree = factory.createTreeFromText(R"( - + )"); EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); @@ -320,7 +333,7 @@ TEST_F(FinallyTest, HaltWhileMainRunning_EveryStepOfAsyncCleanupRuns) { auto tree = factory.createTreeFromText(R"( - + @@ -550,7 +563,7 @@ TEST_F(FinallyTest, CleanupThrowsAfterRunningDuringHalt_DoesNotPropagate) { auto tree = factory.createTreeFromText(R"( - + )"); EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); @@ -563,7 +576,7 @@ TEST_F(FinallyTest, CleanupFailsAfterRunningDuringHalt_IsReported) { auto tree = factory.createTreeFromText(R"( - + )"); EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); @@ -605,3 +618,124 @@ TEST_F(FinallyTest, CleanupThrows_StatefulMainRestartsOnNextTick) EXPECT_EQ(two_tick_starts, 2); EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS); } + +TEST_F(FinallyTest, HaltWithZeroTimeout_CleanupGetsOneTick) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(cleanup_ticks, 1); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, HaltFromAnotherThread_CleanupGetsOneTick) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + testing::internal::CaptureStderr(); + std::thread([&tree] { tree.haltTree(); }).join(); + EXPECT_NE(testing::internal::GetCapturedStderr().find("does not tick this node"), + std::string::npos); + EXPECT_EQ(cleanup_ticks, 1); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, HaltedByTimeout_CleanupDoesNotLoopOnTimerThread) +{ + auto tree = factory.createTreeFromText(R"( + + + + + )"); + + EXPECT_EQ(tree.tickWhileRunning(), NodeStatus::FAILURE); + EXPECT_EQ(cleanup_ticks, 1); +} + +TEST_F(FinallyTest, MainSkippedAfterRunning_CleanupRuns) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + tree.rootBlackboard()->set("go", true); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.rootBlackboard()->set("go", false); + EXPECT_EQ(tree.tickOnce(), NodeStatus::SKIPPED); + EXPECT_EQ(cleanup_count, 1); + // The node must not be left RUNNING, or this halt would run cleanup again. + tree.haltTree(); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, HaltOnThreadThatDidNotTick_CleanupGetsOneTick) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + std::thread([&tree] { EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); }).join(); + tree.haltTree(); + EXPECT_EQ(cleanup_ticks, 1); +} + +TEST_F(FinallyTest, HaltTimeoutBelowTickPeriod_CleanupGetsOneTick) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(cleanup_ticks, 1); +} + +TEST_F(FinallyTest, HaltedByTimeout_NestedFinallyDoesNotLoopOnTimerThread) +{ + auto tree = factory.createTreeFromText(R"( + + + + + + + + )"); + + const auto start = std::chrono::steady_clock::now(); + EXPECT_EQ(tree.tickWhileRunning(std::chrono::milliseconds(1)), NodeStatus::FAILURE); + EXPECT_LT(std::chrono::steady_clock::now() - start, std::chrono::milliseconds(500)); +} + +TEST_F(FinallyTest, TickedAgainAfterFinishing_SkippedMainDoesNotRunCleanup) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + tree.rootBlackboard()->set("skip", false); + // Tick the node directly, so it is not reset between runs, as a parent that re-ticks a + // finished child would do. + auto* node = tree.rootNode(); + + EXPECT_EQ(node->executeTick(), NodeStatus::RUNNING); + EXPECT_EQ(node->executeTick(), NodeStatus::RUNNING); + EXPECT_EQ(node->executeTick(), NodeStatus::SUCCESS); + EXPECT_EQ(cleanup_count, 1); + ASSERT_EQ(node->status(), NodeStatus::SUCCESS); + tree.rootBlackboard()->set("skip", true); + EXPECT_EQ(node->executeTick(), NodeStatus::SKIPPED); + EXPECT_EQ(cleanup_count, 1); +} diff --git a/tests/gtest_halt_child.cpp b/tests/gtest_halt_child.cpp new file mode 100644 index 000000000..5f4aff13a --- /dev/null +++ b/tests/gtest_halt_child.cpp @@ -0,0 +1,85 @@ +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/control_node.h" + +#include + +#include + +using BT::NodeStatus; + +namespace +{ +// RUNNING until halted, and its onHalted() throws. Counts the halts. +class ThrowOnHalt : public BT::StatefulActionNode +{ +public: + ThrowOnHalt(const std::string& name, const BT::NodeConfig& config, int* halts) + : StatefulActionNode(name, config), halts_(halts) + {} + static BT::PortsList providedPorts() + { + return {}; + } + NodeStatus onStart() override + { + return NodeStatus::RUNNING; + } + NodeStatus onRunning() override + { + return NodeStatus::RUNNING; + } + void onHalted() override + { + (*halts_)++; + throw std::runtime_error("halt failed"); + } + +private: + int* halts_; +}; + +// Ticks its only child and halts it on request through haltChild(). +class HaltsChild : public BT::ControlNode +{ +public: + HaltsChild(const std::string& name, const BT::NodeConfig& config) + : ControlNode(name, config) + {} + static BT::PortsList providedPorts() + { + return {}; + } + NodeStatus tick() override + { + setStatus(NodeStatus::RUNNING); + return children_nodes_[0]->executeTick(); + } + void halt() override + { + haltChild(0); + resetStatus(); + } +}; +} // namespace + +// A child whose halt() throws must not stay RUNNING, or the next haltChild() +// halts it again and throws a second time. +TEST(ControlNodeHaltChild, ThrowingHaltIsNotRepeated) +{ + int halts = 0; + BT::BehaviorTreeFactory factory; + factory.registerNodeType("ThrowOnHalt", &halts); + factory.registerNodeType("HaltsChild"); + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + auto* node = static_cast(tree.rootNode()); + EXPECT_THROW(node->halt(), std::runtime_error); + EXPECT_EQ(halts, 1); + EXPECT_NO_THROW(node->halt()); + EXPECT_EQ(halts, 1); + EXPECT_EQ(tree.subtrees[0]->nodes[1]->status(), NodeStatus::IDLE); +}