From 5b878e9a0137f80c9880394d2b257d61739ed1dc Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Fri, 13 Nov 2026 12:02:08 -0700 Subject: [PATCH] fix(finally): sync upstream halt handling without changing default Sync `Finally` with the implementation merged in https://github.com/BehaviorTree/BehaviorTree.CPP/pull/1229. A halt from another thread gives cleanup one tick, a main skipped before it starts skips cleanup, concurrent halts do not tick cleanup together, and `halt()` does not throw. Keep `halt_timeout_msec` at 10000 by default because MoveIt Pro 10.2 shipped with that behavior. Upstream defaults to 0. Explicit values of 10 or less still give cleanup one tick. Add coverage for the timeout boundary, the recommended `Timeout`-inside-main pattern, concurrent halts, exception reporting, and the `ControlNode::haltChild` fix from https://github.com/BehaviorTree/BehaviorTree.CPP/pull/1273. Co-Authored-By: Claude --- CHANGELOG.rst | 2 +- .../behaviortree_cpp/controls/finally_node.h | 44 ++- include/behaviortree_cpp/tree_node.h | 4 + src/controls/finally_node.cpp | 157 +++++++- src/finally_node_state.h | 18 + src/tree_node.cpp | 9 + tests/CMakeLists.txt | 1 + tests/gtest_finally.cpp | 343 +++++++++++++++++- tests/gtest_halt_child.cpp | 85 +++++ 9 files changed, 620 insertions(+), 43 deletions(-) create mode 100644 src/finally_node_state.h create mode 100644 tests/gtest_halt_child.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst index fd438c605..3da3a6922 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 a ``halt_timeout_msec`` of 10 or less, gives cleanup one tick. A halt that arrives while another halt runs returns at once. 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..280311422 100644 --- a/include/behaviortree_cpp/controls/finally_node.h +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -10,24 +10,40 @@ 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 10 or less, 0 included, cleanup gets one tick, + * like TryCatch with catch_on_halt="true", and a cleanup that needs several + * ticks is cut short. + * - Above 10 (the tick period; the default is 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 if "halt_timeout_msec" is above 0 and cleanup is + * still RUNNING after that tick. A Finally nested inside that halt also gets a + * single tick. If halt() is called while another halt() of this node is still running, + * it returns at once and leaves the cleanup to the first one. 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. * - halt() never throws, because it also runs from ~Tree(). It prints to * stderr when cleanup throws, fails or times out, and when halting a child - * throws. A pending exception from main is dropped by a halt. + * throws. A pending exception from main is printed and dropped by a halt. * * Requires exactly 2 children, checked when the XML is loaded and on tick. */ @@ -47,7 +63,13 @@ 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. " + "10 (the tick period) or less: a single tick, " + "which cuts an asynchronous cleanup short. Above " + "10, halt() blocks the halting parent, and any " + "enclosing Timeout, for up to this long, and " + "nested Finally nodes add up; halts from another " + "thread get a single tick. Keep it short") }; } void halt() override; diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index bd7f22425..aca7c87a9 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -445,6 +445,7 @@ class TreeNode friend class BehaviorTreeFactory; friend class DecoratorNode; friend class ControlNode; + friend class FinallyNode; friend class Tree; [[nodiscard]] NodeConfig& config(); @@ -479,6 +480,9 @@ class TreeNode T parseString(const std::string& str) const; private: + struct FinallyState; + [[nodiscard]] FinallyState& finallyState() noexcept; + struct PImpl; std::unique_ptr _p; diff --git a/src/controls/finally_node.cpp b/src/controls/finally_node.cpp index a828b6e55..ef827fed3 100644 --- a/src/controls/finally_node.cpp +++ b/src/controls/finally_node.cpp @@ -1,5 +1,6 @@ #include "behaviortree_cpp/controls/finally_node.h" +#include #include #include #include @@ -7,28 +8,77 @@ #include #include +#include "../finally_node_state.h" + namespace BT { namespace { constexpr std::chrono::milliseconds kHaltTickPeriod{ 10 }; -void printException(std::string_view node_name, const char* context, - const std::exception_ptr& exception) +// 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 { - std::cerr << "[" << node_name << "]: " << context << ": "; - try +public: + explicit ForeignHaltScope(bool foreign) + : previous_(std::exchange(tls_in_foreign_halt, tls_in_foreign_halt || foreign)) + {} + ~ForeignHaltScope() { - std::rethrow_exception(exception); + tls_in_foreign_halt = previous_; } - catch(const std::exception& ex) + ForeignHaltScope(const ForeignHaltScope&) = delete; + ForeignHaltScope& operator=(const ForeignHaltScope&) = delete; + ForeignHaltScope(ForeignHaltScope&&) = delete; + ForeignHaltScope& operator=(ForeignHaltScope&&) = delete; + +private: + bool previous_; +}; + +// Clears the flag on every exit from halt(). +class ClearOnExit +{ +public: + explicit ClearOnExit(std::atomic* flag) : flag_(flag) + {} + ~ClearOnExit() { - std::cerr << ex.what() << std::endl; + flag_->store(false); } - catch(...) + ClearOnExit(const ClearOnExit&) = delete; + ClearOnExit& operator=(const ClearOnExit&) = delete; + ClearOnExit(ClearOnExit&&) = delete; + ClearOnExit& operator=(ClearOnExit&&) = delete; + +private: + std::atomic* flag_; +}; + +void printException(std::string_view node_name, const char* context, + const std::exception_ptr& exception) noexcept +{ + try { - std::cerr << "non-std exception" << std::endl; + std::cerr << "[" << node_name << "]: " << context << ": "; + try + { + std::rethrow_exception(exception); + } + catch(const std::exception& ex) + { + std::cerr << ex.what() << std::endl; + } + catch(...) + { + std::cerr << "non-std exception" << std::endl; + } } + catch(...) // NOLINT(bugprone-empty-catch): diagnostics must not escape halt(). + {} } } // namespace @@ -40,27 +90,72 @@ FinallyNode::FinallyNode(const std::string& name, const NodeConfig& config) void FinallyNode::halt() { + FinallyState& state = finallyState(); + // A halt() that arrives while another one runs, e.g. Tree::haltTree() from a service thread + // while the tick thread is in the halt_timeout_msec loop, returns at once without touching + // children or state, so cleanup is never ticked concurrently. The first halt() still halts + // both children and resets this node when it is done; until then the caller's haltChild() + // already shows this node IDLE. Waiting for the first halt() instead would block a timer or + // service thread for up to halt_timeout_msec, and deadlock on a re-entrant halt(). + bool expected = false; + if(!state.halting.compare_exchange_strong(expected, true)) + { + return; + } + const ClearOnExit clear_halting(&state.halting); + // halt() also runs from ~Tree(), where a propagating exception terminates, so nothing here throws. - if(status() == NodeStatus::RUNNING && children_nodes_.size() == 2) + try { - if(!in_cleanup_) + const bool foreign = + tls_in_foreign_halt || std::this_thread::get_id() != state.tick_thread.load(); + const ForeignHaltScope scope(foreign); + if(status() == NodeStatus::RUNNING && children_nodes_.size() == 2) { - haltChildNoThrow(0); + if(!in_cleanup_) + { + haltChildNoThrow(0); + } + finishCleanupDuringHalt(); + } + for(size_t i = 0; i < children_nodes_.size(); i++) + { + haltChildNoThrow(i); + } + } + catch(...) + { + printException(name(), "halt() threw", std::current_exception()); + for(size_t i = 0; i < children_nodes_.size(); i++) + { + haltChildNoThrow(i); } - finishCleanupDuringHalt(); } - for(size_t i = 0; i < children_nodes_.size(); i++) + if(main_exception_) { - haltChildNoThrow(i); + printException(name(), + "halted while cleanup ran after main threw, dropping main's exception", + main_exception_); } + state.tick_thread.store(std::thread::id{}); in_cleanup_ = false; + state.main_started = false; main_status_ = NodeStatus::IDLE; main_exception_ = nullptr; - resetStatus(); + try + { + // Notifies status observers, such as loggers, which may throw. + resetStatus(); + } + catch(...) + { + printException(name(), "resetStatus() threw during halt", std::current_exception()); + } } void FinallyNode::finishCleanupDuringHalt() { + const bool foreign_thread = tls_in_foreign_halt; unsigned timeout_msec = kDefaultHaltTimeoutMsec; if(auto input = getInput("halt_timeout_msec")) { @@ -75,10 +170,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; @@ -123,13 +226,16 @@ NodeStatus FinallyNode::tick() throw LogicError("[", name(), "]: Finally requires exactly 2 children"); } + FinallyState& state = finallyState(); if(!isStatusActive(status())) { in_cleanup_ = false; + state.main_started = false; main_exception_ = nullptr; } setStatus(NodeStatus::RUNNING); + state.tick_thread.store(std::this_thread::get_id()); if(!in_cleanup_) { @@ -146,13 +252,23 @@ NodeStatus FinallyNode::tick() if(main_status_ == NodeStatus::RUNNING) { + state.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 && !state.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; + state.main_started = false; } NodeStatus cleanup_status = NodeStatus::IDLE; @@ -194,7 +310,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/src/finally_node_state.h b/src/finally_node_state.h new file mode 100644 index 000000000..5c3d28fa7 --- /dev/null +++ b/src/finally_node_state.h @@ -0,0 +1,18 @@ +#pragma once + +#include "behaviortree_cpp/tree_node.h" + +#include +#include + +namespace BT +{ + +struct TreeNode::FinallyState +{ + std::atomic tick_thread{}; + std::atomic halting{ false }; + bool main_started = false; +}; + +} // namespace BT diff --git a/src/tree_node.cpp b/src/tree_node.cpp index 0a24dd606..4217369ee 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -13,6 +13,8 @@ #include "behaviortree_cpp/tree_node.h" +#include "finally_node_state.h" + #include #include #include @@ -51,6 +53,8 @@ struct TreeNode::PImpl std::array pre_parsed; std::array post_parsed; + + FinallyState finally_state; }; TreeNode::TreeNode(std::string name, NodeConfig config) @@ -68,6 +72,11 @@ TreeNode& TreeNode::operator=(TreeNode&& other) noexcept TreeNode::~TreeNode() = default; +TreeNode::FinallyState& TreeNode::finallyState() noexcept +{ + return _p->finally_state; +} + NodeStatus TreeNode::executeTick() { auto new_status = _p->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..41c44a221 100644 --- a/tests/gtest_finally.cpp +++ b/tests/gtest_finally.cpp @@ -1,7 +1,12 @@ #include "behaviortree_cpp/bt_factory.h" +#include +#include #include +#include #include +#include +#include #include @@ -81,6 +86,15 @@ class TwoTickMain : public BT::StatefulActionNode int* starts_; }; +class FailingStreamBuffer : public std::streambuf +{ +protected: + int overflow(int) override + { + return traits_type::eof(); + } +}; + class FinallyTest : public testing::Test { protected: @@ -250,7 +264,7 @@ TEST_F(FinallyTest, WrongChildCount_RejectedAtLoad) BT::RuntimeError); } -TEST_F(FinallyTest, MainSkipped_CleanupRunsAndReturnsSkipped) +TEST_F(FinallyTest, MainSkipped_CleanupDoesNotRun) { auto tree = factory.createTreeFromText(R"( @@ -258,10 +272,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 +308,7 @@ TEST_F(FinallyTest, HaltWhileCleanupRunning_CleanupFinishes) { auto tree = factory.createTreeFromText(R"( - + )"); EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); @@ -307,7 +321,7 @@ TEST_F(FinallyTest, HaltWhileMainRunning_AsyncCleanupFinishes) { auto tree = factory.createTreeFromText(R"( - + )"); EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); @@ -320,7 +334,7 @@ TEST_F(FinallyTest, HaltWhileMainRunning_EveryStepOfAsyncCleanupRuns) { auto tree = factory.createTreeFromText(R"( - + @@ -550,7 +564,7 @@ TEST_F(FinallyTest, CleanupThrowsAfterRunningDuringHalt_DoesNotPropagate) { auto tree = factory.createTreeFromText(R"( - + )"); EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); @@ -563,7 +577,7 @@ TEST_F(FinallyTest, CleanupFailsAfterRunningDuringHalt_IsReported) { auto tree = factory.createTreeFromText(R"( - + )"); EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); @@ -585,8 +599,9 @@ TEST_F(FinallyTest, UnreadableHaltTimeout_FallsBackToDefault) EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); testing::internal::CaptureStderr(); tree.haltTree(); - EXPECT_NE(testing::internal::GetCapturedStderr().find("cannot read halt_timeout_msec"), - std::string::npos); + const std::string err = testing::internal::GetCapturedStderr(); + EXPECT_NE(err.find("cannot read halt_timeout_msec"), std::string::npos); + EXPECT_NE(err.find("using 10000 ms"), std::string::npos); EXPECT_EQ(cleanup_ticks, 2); } @@ -605,3 +620,309 @@ TEST_F(FinallyTest, CleanupThrows_StatefulMainRestartsOnNextTick) EXPECT_EQ(two_tick_starts, 2); EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS); } + +TEST_F(FinallyTest, DefaultHaltTimeoutIsTenSeconds) +{ + const BT::PortsList ports = BT::FinallyNode::providedPorts(); + const auto port = ports.find("halt_timeout_msec"); + ASSERT_NE(port, ports.end()); + EXPECT_EQ(port->second.defaultValue().cast(), 10000u); +} + +TEST_F(FinallyTest, HaltByDefault_AsyncCleanupFinishes) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(cleanup_ticks, 2); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +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, HaltTimeoutAtTickPeriod_CleanupGetsOneTick) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + testing::internal::CaptureStderr(); + tree.haltTree(); + EXPECT_NE(testing::internal::GetCapturedStderr().find("did not finish within"), + std::string::npos); + EXPECT_EQ(cleanup_ticks, 1); +} + +TEST_F(FinallyTest, TimeoutInsideMain_AsyncCleanupFinishes) +{ + // The pattern to use instead of a Timeout above Finally: the timer halts only main, and + // tick() then runs cleanup over several ticks. halt_timeout_msec="0" proves no halt of + // Finally is involved, since a halt would give cleanup a single tick. + auto tree = factory.createTreeFromText(R"( + + + + + + )"); + + EXPECT_EQ(tree.tickWhileRunning(), NodeStatus::FAILURE); + EXPECT_EQ(cleanup_ticks, 2); +} + +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); +} + +TEST_F(FinallyTest, HaltContainsStatusObserverException) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + auto subscriber = tree.rootNode()->subscribeToStatusChange( + [](BT::TimePoint, const BT::TreeNode&, NodeStatus, NodeStatus status) { + if(status == NodeStatus::IDLE) + { + throw TestError("observer boom"); + } + }); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_NO_THROW(tree.haltTree()); + EXPECT_EQ(cleanup_count, 1); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, HaltThatThrowsBeforeCleanupTick_StillHaltsRunningCleanup) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + ASSERT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + auto* node = tree.rootNode(); + auto* cleanup = tree.subtrees[0]->nodes[2].get(); + ASSERT_EQ(cleanup->status(), NodeStatus::RUNNING); + + // The unreadable timeout makes halt() write to stderr before ticking cleanup, and that + // write throws here. Halt the node directly so Tree::haltTree()'s sweep cannot help. + FailingStreamBuffer buffer; + std::streambuf* old_buffer = std::cerr.rdbuf(&buffer); + const std::ios_base::iostate old_exceptions = std::cerr.exceptions(); + std::cerr.exceptions(std::ios::badbit); + EXPECT_NO_THROW(node->haltNode()); + std::cerr.exceptions(std::ios::goodbit); + std::cerr.clear(); + std::cerr.rdbuf(old_buffer); + std::cerr.exceptions(old_exceptions); + + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup->status(), NodeStatus::IDLE); + EXPECT_EQ(node->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, HaltContainsDiagnosticStreamException) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + FailingStreamBuffer buffer; + std::streambuf* old_buffer = std::cerr.rdbuf(&buffer); + const std::ios_base::iostate old_exceptions = std::cerr.exceptions(); + std::cerr.exceptions(std::ios::badbit); + EXPECT_NO_THROW(tree.haltTree()); + std::cerr.exceptions(std::ios::goodbit); + std::cerr.clear(); + std::cerr.rdbuf(old_buffer); + std::cerr.exceptions(old_exceptions); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, HaltWhileMainThrewAndCleanupRunning_ExceptionIsReported) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + testing::internal::CaptureStderr(); + EXPECT_NO_THROW(tree.haltTree()); + const std::string err = testing::internal::GetCapturedStderr(); + EXPECT_NE(err.find("dropping main's exception"), std::string::npos) << err; + EXPECT_NE(err.find("boom"), std::string::npos) << err; + EXPECT_EQ(cleanup_ticks, 2); +} + +TEST_F(FinallyTest, ConcurrentHaltDuringHaltLoop_DoesNotTickCleanupConcurrently) +{ + std::atomic in_tick{ 0 }; + std::atomic max_in_tick{ 0 }; + std::atomic ticks{ 0 }; + std::promise entered; + std::promise release; + auto release_future = release.get_future(); + // RUNNING twice, then SUCCESS. The second tick, which runs inside the halt loop, blocks + // until released, so the concurrent halt below provably overlaps the loop. + factory.registerSimpleCondition("BlockingCleanup", [&](BT::TreeNode&) { + const int now = ++in_tick; + max_in_tick = std::max(max_in_tick.load(), now); + const int tick = ++ticks; + if(tick == 2) + { + entered.set_value(); + release_future.wait(); + } + --in_tick; + return tick < 3 ? NodeStatus::RUNNING : NodeStatus::SUCCESS; + }); + auto tree = factory.createTreeFromText(R"( + + + )"); + + // The tick thread ticks and then halts, so its halt() runs the loop. + std::thread tick_thread([&tree] { + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + }); + const std::future_status entered_status = + entered.get_future().wait_for(std::chrono::seconds(5)); + if(entered_status == std::future_status::ready) + { + tree.haltTree(); + } + release.set_value(); + tick_thread.join(); + ASSERT_EQ(entered_status, std::future_status::ready); + + EXPECT_EQ(max_in_tick, 1); + EXPECT_EQ(ticks, 3); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} 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); +}