diff --git a/CMakeLists.txt b/CMakeLists.txt index f82c6bf52..95f22c581 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -187,6 +187,7 @@ list(APPEND BT_SOURCE src/controls/sequence_node.cpp src/controls/sequence_with_memory_node.cpp src/controls/switch_node.cpp + src/controls/finally_node.cpp src/controls/try_catch_node.cpp src/controls/while_do_else_node.cpp diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index e7373d4dc..86e0d4812 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -25,6 +25,7 @@ #include "behaviortree_cpp/actions/updated_action.h" #include "behaviortree_cpp/condition_node.h" #include "behaviortree_cpp/controls/fallback_node.h" +#include "behaviortree_cpp/controls/finally_node.h" #include "behaviortree_cpp/controls/if_then_else_node.h" #include "behaviortree_cpp/controls/parallel_all_node.h" #include "behaviortree_cpp/controls/parallel_node.h" diff --git a/include/behaviortree_cpp/controls/finally_node.h b/include/behaviortree_cpp/controls/finally_node.h new file mode 100644 index 000000000..a4f7e64a9 --- /dev/null +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -0,0 +1,101 @@ +#pragma once + +#include "behaviortree_cpp/control_node.h" + +#include +#include +#include + +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 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 gets one tick, like TryCatch with catch_on_halt="true". A cleanup that + * needs several ticks is cut short by the final halt of its subtree. That tick + * runs inside halt(), on the calling thread and under any lock the caller holds, + * so a cleanup action that blocks also blocks the caller. + * - The single tick suits synchronous cleanup. An asynchronous cleanup child (e.g. a + * StatefulActionNode) gets onStart() and then onHalted() right away. Use + * "halt_timeout_msec" for asynchronous cleanup on halt. + * - To let a multi-step cleanup finish, set "halt_timeout_msec" above 10 (the tick + * period). Then 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. With 10 or less, cleanup gets a single tick. + * - That loop BLOCKS the halting parent, e.g. a ReactiveSequence, for up to + * halt_timeout_msec, and nothing else in the tree is ticked meanwhile. An + * enclosing Timeout that holds its mutex during the child's tick is delayed by up + * to the same amount, and nested Finally nodes with the option set add up their + * timeouts. Keep the value short. + * - 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. 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 printed and dropped by a halt. + * + * Requires exactly 2 children, checked when the XML is loaded and on tick. + */ +class FinallyNode : public ControlNode +{ +public: + FinallyNode(const std::string& name, const NodeConfig& config); + + ~FinallyNode() override = default; + + FinallyNode(const FinallyNode&) = delete; + FinallyNode& operator=(const FinallyNode&) = delete; + FinallyNode(FinallyNode&&) = delete; + FinallyNode& operator=(FinallyNode&&) = delete; + + static PortsList providedPorts() + { + return { InputPort( + "halt_timeout_msec", kDefaultHaltTimeoutMsec, + "When halted, how long to keep ticking cleanup 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() noexcept override; + +private: + static constexpr unsigned kDefaultHaltTimeoutMsec = 0; + + // Written in tick(), read in halt(), which may run on another thread. + std::atomic tick_thread_{}; + // Set while a halt() runs, so a concurrent halt() from another thread does not tick + // cleanup at the same time. + std::atomic halting_{ false }; + bool in_cleanup_ = false; + bool main_started_ = false; + NodeStatus main_status_ = NodeStatus::IDLE; + std::exception_ptr main_exception_; + + void haltChildNoThrow(size_t i) noexcept; + void finishCleanupDuringHalt(bool foreign_thread); + + BT::NodeStatus tick() override; +}; + +} // namespace BT diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 8dba3ff02..e3e376c50 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -134,6 +134,7 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl) registerNodeType("IfThenElse"); registerNodeType("WhileDoElse"); registerNodeType("TryCatch"); + registerNodeType("Finally"); registerNodeType("Inverter"); diff --git a/src/controls/finally_node.cpp b/src/controls/finally_node.cpp new file mode 100644 index 000000000..d848029d0 --- /dev/null +++ b/src/controls/finally_node.cpp @@ -0,0 +1,306 @@ +#include "behaviortree_cpp/controls/finally_node.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace BT +{ +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_; +}; + +// Clears the flag on every exit from halt(). +class ClearOnExit +{ +public: + explicit ClearOnExit(std::atomic* flag) : flag_(flag) + {} + ~ClearOnExit() + { + flag_->store(false); + } + 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 +{ + 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; + } +} +} // namespace + +FinallyNode::FinallyNode(const std::string& name, const NodeConfig& config) + : ControlNode::ControlNode(name, config) +{ + setRegistrationID("Finally"); +} + +void FinallyNode::halt() noexcept +{ + // 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(!halting_.compare_exchange_strong(expected, true)) + { + return; + } + const ClearOnExit clear_halting(&halting_); + + // halt() also runs from ~Tree(), where a propagating exception terminates, so nothing here throws. + try + { + const bool foreign = + tls_in_foreign_halt || std::this_thread::get_id() != tick_thread_.load(); + const ForeignHaltScope scope(foreign); + if(status() == NodeStatus::RUNNING && children_nodes_.size() == 2) + { + if(!in_cleanup_) + { + haltChildNoThrow(0); + } + finishCleanupDuringHalt(foreign); + } + for(size_t i = 0; i < children_nodes_.size(); i++) + { + haltChildNoThrow(i); + } + } + catch(...) + { + printException(name(), "halt() threw", std::current_exception()); + } + if(main_exception_) + { + printException(name(), + "halted while cleanup ran after main threw, dropping main's exception", + main_exception_); + } + tick_thread_.store(std::thread::id{}); + in_cleanup_ = false; + main_started_ = false; + main_status_ = NodeStatus::IDLE; + main_exception_ = nullptr; + try + { + // Notifies status observers, such as loggers, which may throw. + resetStatus(); + } + catch(...) + { + printException(name(), "resetStatus() threw during halt", std::current_exception()); + } +} + +void FinallyNode::finishCleanupDuringHalt(bool foreign_thread) +{ + unsigned timeout_msec = kDefaultHaltTimeoutMsec; + if(auto input = getInput("halt_timeout_msec")) + { + timeout_msec = input.value(); + } + else + { + std::cerr << "[" << name() << "]: cannot read halt_timeout_msec (" << input.error() + << "), using " << kDefaultHaltTimeoutMsec << " ms" << std::endl; + } + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_msec); + try + { + NodeStatus cleanup_status = children_nodes_[1]->executeTick(); + // 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; + if(next_tick > deadline) + { + std::cerr << "[" << name() + << "]: cleanup did not finish within halt_timeout_msec (" + << timeout_msec << " ms), halting it unfinished" << std::endl; + return; + } + std::this_thread::sleep_until(next_tick); + cleanup_status = children_nodes_[1]->executeTick(); + } + if(cleanup_status == NodeStatus::FAILURE) + { + std::cerr << "[" << name() << "]: cleanup returned FAILURE during halt" + << std::endl; + } + } + catch(...) + { + printException(name(), "cleanup threw during halt", std::current_exception()); + } +} + +void FinallyNode::haltChildNoThrow(size_t i) noexcept +{ + try + { + haltChild(i); + } + catch(...) + { + printException(name(), "a child threw while being halted", std::current_exception()); + } +} + +NodeStatus FinallyNode::tick() +{ + if(children_nodes_.size() != 2) + { + throw LogicError("[", name(), "]: Finally requires exactly 2 children"); + } + + 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_) + { + try + { + main_status_ = children_nodes_[0]->executeTick(); + } + catch(...) + { + main_exception_ = std::current_exception(); + haltChildNoThrow(0); + main_status_ = NodeStatus::FAILURE; + } + + 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; + try + { + cleanup_status = children_nodes_[1]->executeTick(); + } + catch(...) + { + // As in a finally block, the exception replaces main's outcome and ends this run, so a later + // halt, such as the one ~Tree() runs, does not retry cleanup. + // Main finished but was not reset yet. haltChildNoThrow() resets both without throwing, so the + // cleanup exception is the one that propagates. + for(size_t i = 0; i < children_nodes_.size(); i++) + { + haltChildNoThrow(i); + } + in_cleanup_ = false; + main_exception_ = nullptr; + resetStatus(); + throw; + } + if(cleanup_status == NodeStatus::RUNNING) + { + return NodeStatus::RUNNING; + } + + resetChildren(); + in_cleanup_ = false; + if(main_exception_) + { + // executeTick() keeps our RUNNING status when tick() throws, and halt() would rerun cleanup. + resetStatus(); + std::rethrow_exception(std::exchange(main_exception_, nullptr)); + } + if(cleanup_status == NodeStatus::FAILURE) + { + return NodeStatus::FAILURE; + } + if(main_status_ == NodeStatus::SKIPPED) + { + // 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_; +} + +} // namespace BT diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index 5663c7889..e86dd4886 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -702,6 +702,11 @@ void VerifyXML(const std::string& xml_text, ThrowError(line_number, std::string("The node 'TryCatch' must have " "at least 2 children")); } + if(registered_name == "Finally" && children_count != 2) + { + ThrowError(line_number, std::string("The node 'Finally' must have " + "exactly 2 children")); + } if(registered_name == "ReactiveSequence") { size_t async_count = 0; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 103715ee6..5d4e643c9 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -49,6 +49,7 @@ set(BT_TESTS gtest_switch.cpp gtest_tree.cpp gtest_try_catch.cpp + gtest_finally.cpp gtest_halt_child.cpp gtest_exception_tracking.cpp gtest_updates.cpp diff --git a/tests/gtest_finally.cpp b/tests/gtest_finally.cpp new file mode 100644 index 000000000..5a92ea65d --- /dev/null +++ b/tests/gtest_finally.cpp @@ -0,0 +1,790 @@ +#include "behaviortree_cpp/bt_factory.h" + +#include +#include +#include +#include +#include +#include + +#include + +using BT::NodeStatus; + +struct TestError : std::runtime_error +{ + using std::runtime_error::runtime_error; +}; + +// RUNNING until halted. Optionally throws on its second tick, or from onHalted(). +class AsyncMain : public BT::StatefulActionNode +{ +public: + AsyncMain(const std::string& name, const BT::NodeConfig& config, int* halted, + bool throw_on_running, bool throw_on_halt) + : StatefulActionNode(name, config) + , halted_(halted) + , throw_(throw_on_running) + , throw_on_halt_(throw_on_halt) + {} + static BT::PortsList providedPorts() + { + return {}; + } + NodeStatus onStart() override + { + return NodeStatus::RUNNING; + } + NodeStatus onRunning() override + { + if(throw_) + { + throw TestError("boom"); + } + return NodeStatus::RUNNING; + } + void onHalted() override + { + (*halted_)++; + if(throw_on_halt_) + { + throw TestError("halt boom"); + } + } + +private: + int* halted_; + bool throw_; + bool throw_on_halt_; +}; + +// RUNNING on start, SUCCESS on the next tick. Counts how often it starts. +class TwoTickMain : public BT::StatefulActionNode +{ +public: + TwoTickMain(const std::string& name, const BT::NodeConfig& config, int* starts) + : StatefulActionNode(name, config), starts_(starts) + {} + static BT::PortsList providedPorts() + { + return {}; + } + NodeStatus onStart() override + { + (*starts_)++; + return NodeStatus::RUNNING; + } + NodeStatus onRunning() override + { + return NodeStatus::SUCCESS; + } + void onHalted() override + {} + +private: + int* starts_; +}; + +class FinallyTest : public testing::Test +{ +protected: + BT::BehaviorTreeFactory factory; + int cleanup_count = 0; + int main_ticks = 0; + int main_halted = 0; + int cleanup_ticks = 0; + int throw_once_ticks = 0; + int spin_ticks = 0; + int two_tick_starts = 0; + int second_tick_ticks = 0; + bool gate = true; + + void SetUp() override + { + factory.registerSimpleAction("Cleanup", [this](BT::TreeNode&) { + cleanup_count++; + return NodeStatus::SUCCESS; + }); + factory.registerSimpleAction( + "Throw", [](BT::TreeNode&) -> NodeStatus { throw TestError("boom"); }); + // RUNNING twice, then SUCCESS + factory.registerSimpleCondition("RunTwice", [this](BT::TreeNode&) { + return ++main_ticks < 3 ? NodeStatus::RUNNING : NodeStatus::SUCCESS; + }); + factory.registerNodeType("AsyncMain", &main_halted, false, false); + factory.registerNodeType("AsyncThrow", &main_halted, true, false); + factory.registerNodeType("AsyncHaltThrows", &main_halted, false, true); + factory.registerNodeType("AsyncThrowHaltThrows", &main_halted, true, true); + factory.registerSimpleAction("ThrowInt", + [](BT::TreeNode&) -> NodeStatus { throw 1; }); + factory.registerSimpleAction("ThrowOnce", [this](BT::TreeNode&) { + if(++throw_once_ticks == 1) + { + throw TestError("once"); + } + return NodeStatus::SUCCESS; + }); + factory.registerNodeType("TwoTickMain", &two_tick_starts); + factory.registerSimpleCondition("Spin", [this](BT::TreeNode&) { + spin_ticks++; + return NodeStatus::RUNNING; + }); + // RUNNING on its first tick, then throws on its second + factory.registerSimpleCondition("RunThenThrow", [this](BT::TreeNode&) -> NodeStatus { + if(++second_tick_ticks < 2) + { + return NodeStatus::RUNNING; + } + throw TestError("late boom"); + }); + // RUNNING on its first tick, then FAILURE + factory.registerSimpleCondition("RunThenFail", [this](BT::TreeNode&) { + return ++second_tick_ticks < 2 ? NodeStatus::RUNNING : NodeStatus::FAILURE; + }); + factory.registerSimpleCondition("Gate", [this](BT::TreeNode&) { + return gate ? NodeStatus::SUCCESS : NodeStatus::FAILURE; + }); + // Cleanup that is RUNNING on its first tick, then SUCCESS + factory.registerSimpleCondition("AsyncCleanup", [this](BT::TreeNode&) { + return ++cleanup_ticks < 2 ? NodeStatus::RUNNING : NodeStatus::SUCCESS; + }); + factory.registerSimpleCondition("AlwaysRunning", [this](BT::TreeNode&) { + main_ticks++; + return NodeStatus::RUNNING; + }); + } + + NodeStatus run(const std::string& main, const std::string& cleanup = "") + { + auto tree = + factory.createTreeFromText(R"()" + + main + cleanup + ""); + return tree.tickWhileRunning(); + } +}; + +TEST_F(FinallyTest, MainSucceeds_CleanupRuns) +{ + EXPECT_EQ(run(""), NodeStatus::SUCCESS); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, MainFails_CleanupRuns) +{ + EXPECT_EQ(run(""), NodeStatus::FAILURE); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, CleanupFails_ReturnsFailure) +{ + EXPECT_EQ(run("", ""), NodeStatus::FAILURE); +} + +TEST_F(FinallyTest, MainThrows_CleanupRunsThenRethrows) +{ + EXPECT_THROW(run(""), BT::RuntimeError); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, NestedMainThrows_CleanupRunsThenRethrows) +{ + EXPECT_THROW(run(""), BT::RuntimeError); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, CleanupThrows_Propagates) +{ + EXPECT_THROW(run("", ""), BT::RuntimeError); +} + +TEST_F(FinallyTest, AsyncMain_CleanupRunsOnceAfterMainCompletes) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_EQ(cleanup_count, 0); + EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, HaltWhileMainRunning_CleanupRuns) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(cleanup_count, 1); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); + + // A second halt on an idle node does not run cleanup again. + tree.haltTree(); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, TickedAgain_CleanupRunsEachTime) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickWhileRunning(), NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickWhileRunning(), NodeStatus::SUCCESS); + EXPECT_EQ(cleanup_count, 2); +} + +TEST_F(FinallyTest, WrongChildCount_RejectedAtLoad) +{ + EXPECT_THROW((void)factory.createTreeFromText(R"( + + + )"), + BT::RuntimeError); + EXPECT_THROW((void)factory.createTreeFromText(R"( + + + )"), + BT::RuntimeError); +} + +TEST_F(FinallyTest, MainSkipped_CleanupDoesNotRun) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickWhileRunning(), NodeStatus::SKIPPED); + EXPECT_EQ(cleanup_count, 0); + // The node must not be left RUNNING, or this halt would run cleanup. + tree.haltTree(); + EXPECT_EQ(cleanup_count, 0); +} + +TEST_F(FinallyTest, AsyncCleanup_ReturnsMainStatusWhenCleanupCompletes) +{ + EXPECT_EQ(run("", ""), NodeStatus::FAILURE); + EXPECT_EQ(cleanup_ticks, 2); +} + +TEST_F(FinallyTest, AsyncMainThrows_MainHaltedAndCleanupRuns) +{ + EXPECT_THROW(run(""), BT::RuntimeError); + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, HaltWhileMainRunning_MainHalted) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, HaltWhileCleanupRunning_CleanupFinishes) +{ + 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, HaltWhileMainRunning_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, HaltWhileMainRunning_EveryStepOfAsyncCleanupRuns) +{ + auto tree = factory.createTreeFromText(R"( + + + + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(cleanup_ticks, 2); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, HaltWithCleanupThatNeverFinishes_HaltsItAfterTimeout) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + const auto start = std::chrono::steady_clock::now(); + tree.haltTree(); + const auto elapsed = std::chrono::steady_clock::now() - start; + // The wait stops before a tick that would start after the timeout, up to one 10 ms period early. + EXPECT_GE(elapsed, std::chrono::milliseconds(40)); + EXPECT_LT(elapsed, std::chrono::seconds(5)); + // At least one tick from the run and one more during the halt. + EXPECT_GE(spin_ticks, 2); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, CleanupThrowsDuringHalt_DoesNotPropagate) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_NO_THROW(tree.haltTree()); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, MainThrowsAndItsHaltThrows_CleanupStillRuns) +{ + EXPECT_THROW(run(""), BT::RuntimeError); + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, HaltWhereMainHaltThrows_CleanupRunsAndNothingPropagates) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_NO_THROW(tree.haltTree()); + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, MainThrowsNonStdException_CleanupRunsThenRethrows) +{ + EXPECT_THROW(run(""), int); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, CleanupThrowsNonStdExceptionDuringHalt_DoesNotPropagate) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_NO_THROW(tree.haltTree()); +} + +TEST_F(FinallyTest, CleanupThrows_EndsTheRunAndHaltDoesNotRetry) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_THROW(tree.tickOnce(), BT::RuntimeError); + tree.haltTree(); + EXPECT_EQ(throw_once_ticks, 1); + + // The next tick starts again with main. + EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS); + EXPECT_EQ(main_ticks, 4); + EXPECT_EQ(throw_once_ticks, 2); +} + +TEST_F(FinallyTest, CleanupSkipped_ReturnsMainStatus) +{ + EXPECT_EQ(run("", R"()"), + NodeStatus::FAILURE); +} + +TEST_F(FinallyTest, HaltedByReactiveSequence_CleanupRunsOnce) +{ + auto tree = factory.createTreeFromText(R"( + + + + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_EQ(cleanup_count, 0); + gate = false; + EXPECT_EQ(tree.tickOnce(), NodeStatus::FAILURE); + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup_count, 1); + tree.haltTree(); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, InsideHaltedSubTree_CleanupRuns) +{ + auto tree = factory.createTreeFromText(R"( + + + + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, NestedFinallyAsCleanup_RunsDuringOuterHalt) +{ + auto tree = factory.createTreeFromText(R"( + + + + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup_count, 2); +} + +TEST_F(FinallyTest, NestedFinallyAsMain_BothCleanupsRunOnHalt) +{ + auto tree = factory.createTreeFromText(R"( + + + + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(main_halted, 1); + EXPECT_EQ(cleanup_count, 2); +} + +TEST_F(FinallyTest, CleanupFailsDuringHalt_IsReported) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + testing::internal::CaptureStderr(); + tree.haltTree(); + EXPECT_NE(testing::internal::GetCapturedStderr().find("cleanup returned FAILURE during " + "halt"), + std::string::npos); +} + +TEST_F(FinallyTest, MainThrows_HaltAfterRethrowDoesNotRerunCleanup) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_THROW(tree.tickOnce(), BT::RuntimeError); + tree.haltTree(); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, MainThrowsWithAsyncCleanup_RethrowsWhenCleanupCompletes) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_THROW(tree.tickOnce(), BT::RuntimeError); + EXPECT_EQ(cleanup_ticks, 2); +} + +TEST_F(FinallyTest, MainThrows_NextTickStartsFresh) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_THROW(tree.tickOnce(), BT::RuntimeError); + EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS); + EXPECT_EQ(throw_once_ticks, 2); + EXPECT_EQ(cleanup_count, 2); +} + +TEST_F(FinallyTest, CleanupThrowsAfterRunningDuringHalt_DoesNotPropagate) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_NO_THROW(tree.haltTree()); + EXPECT_EQ(second_tick_ticks, 2); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +} + +TEST_F(FinallyTest, CleanupFailsAfterRunningDuringHalt_IsReported) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + testing::internal::CaptureStderr(); + tree.haltTree(); + EXPECT_NE(testing::internal::GetCapturedStderr().find("cleanup returned FAILURE during " + "halt"), + std::string::npos); + EXPECT_EQ(second_tick_ticks, 2); +} + +TEST_F(FinallyTest, UnreadableHaltTimeout_FallsBackToDefault) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + 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); + EXPECT_EQ(cleanup_ticks, 1); +} + +TEST_F(FinallyTest, CleanupThrows_StatefulMainRestartsOnNextTick) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_THROW(tree.tickOnce(), BT::RuntimeError); + + // Main was reset with the node, so it starts again rather than reporting its old result. + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_EQ(two_tick_starts, 2); + EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS); +} + +TEST_F(FinallyTest, HaltByDefault_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); +} + +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(); + }); + entered.get_future().wait(); + tree.haltTree(); + release.set_value(); + tick_thread.join(); + + EXPECT_EQ(max_in_tick, 1); + EXPECT_EQ(ticks, 3); + EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); +}