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); +}