diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d469d9e1a..4ee1fcc2e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package behaviortree_cpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add Finally control node: always runs a cleanup child after the main child finishes, fails, throws, or is halted + 4.9.0 (2026-02-11) ------------------ * Fix Blackboard thread-safety: 6 data races fixed, use shared_mutex for storage diff --git a/CMakeLists.txt b/CMakeLists.txt index 7b610929d..e7517d10e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -180,6 +180,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..f90c9392d --- /dev/null +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -0,0 +1,48 @@ +#pragma once + +#include "behaviortree_cpp/control_node.h" + +namespace BT +{ +/** + * @brief The Finally node ticks its first child ("main") and then always ticks + * its second child ("cleanup"), like try/finally. + * + * - Cleanup runs after main returns SUCCESS, FAILURE or SKIPPED. + * - If main throws, the exception is printed to stderr, main is halted, + * cleanup runs, and this node returns FAILURE. + * - If this node is halted while main is RUNNING, main is halted and cleanup + * is ticked once synchronously. If cleanup returns RUNNING, it is halted. + * - The node returns main's status, or FAILURE if cleanup fails. + * - Exceptions thrown by cleanup propagate from tick(). During halt they are + * printed to stderr instead, because halt() also runs from ~Tree(). + * + * Requires exactly 2 children. + */ +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 {}; + } + + void halt() override; + +private: + bool in_cleanup_ = false; + NodeStatus main_status_ = NodeStatus::IDLE; + + BT::NodeStatus tick() override; +}; + +} // namespace BT diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 42cf43307..38d05da16 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -132,6 +132,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..9cbf4debf --- /dev/null +++ b/src/controls/finally_node.cpp @@ -0,0 +1,95 @@ +#include "behaviortree_cpp/controls/finally_node.h" + +#include + +namespace BT +{ +FinallyNode::FinallyNode(const std::string& name, const NodeConfig& config) + : ControlNode::ControlNode(name, config) +{ + setRegistrationID("Finally"); +} + +void FinallyNode::halt() +{ + if(!in_cleanup_ && status() == NodeStatus::RUNNING && children_nodes_.size() == 2) + { + haltChild(0); + // halt() also runs from ~Tree(), where a propagating exception terminates. + try + { + if(children_nodes_[1]->executeTick() == NodeStatus::RUNNING) + { + haltChild(1); + } + } + catch(const std::exception& ex) + { + std::cerr << "[" << name() << "]: Finally cleanup threw during halt: " << ex.what() + << std::endl; + } + } + in_cleanup_ = false; + ControlNode::halt(); +} + +NodeStatus FinallyNode::tick() +{ + if(children_nodes_.size() != 2) + { + throw LogicError("[", name(), "]: Finally requires exactly 2 children"); + } + + if(!isStatusActive(status())) + { + in_cleanup_ = false; + } + + setStatus(NodeStatus::RUNNING); + + if(!in_cleanup_) + { + try + { + main_status_ = children_nodes_[0]->executeTick(); + } + catch(const std::exception& ex) + { + std::cerr << "[" << name() << "]: Finally caught an exception from its main child, " + << "running cleanup and returning FAILURE: " << ex.what() << std::endl; + haltChild(0); + main_status_ = NodeStatus::FAILURE; + } + + if(main_status_ == NodeStatus::RUNNING) + { + return NodeStatus::RUNNING; + } + if(main_status_ == NodeStatus::IDLE) + { + throw LogicError("[", name(), "]: A child should not return IDLE"); + } + in_cleanup_ = true; + } + + const NodeStatus cleanup_status = children_nodes_[1]->executeTick(); + if(cleanup_status == NodeStatus::RUNNING) + { + return NodeStatus::RUNNING; + } + + resetChildren(); + in_cleanup_ = false; + if(cleanup_status == NodeStatus::FAILURE) + { + return NodeStatus::FAILURE; + } + if(main_status_ == NodeStatus::SKIPPED) + { + // 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 d15fd0a26..ec1d593b0 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -681,6 +681,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 30d65ab3a..7ab82a7f5 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -47,6 +47,7 @@ set(BT_TESTS gtest_switch.cpp gtest_tree.cpp gtest_try_catch.cpp + gtest_finally.cpp gtest_exception_tracking.cpp gtest_updates.cpp gtest_wakeup.cpp diff --git a/tests/gtest_finally.cpp b/tests/gtest_finally.cpp new file mode 100644 index 000000000..f85a700ea --- /dev/null +++ b/tests/gtest_finally.cpp @@ -0,0 +1,249 @@ +#include "behaviortree_cpp/bt_factory.h" + +#include + +#include + +using BT::NodeStatus; + +// RUNNING until halted; throws on its second tick if `throw_on_running` is set. +class AsyncMain : public BT::StatefulActionNode +{ +public: + AsyncMain(const std::string& name, const BT::NodeConfig& config, int* halted, + bool throw_on_running) + : StatefulActionNode(name, config), halted_(halted), throw_(throw_on_running) + {} + static BT::PortsList providedPorts() + { + return {}; + } + NodeStatus onStart() override + { + return NodeStatus::RUNNING; + } + NodeStatus onRunning() override + { + if(throw_) + { + throw std::runtime_error("boom"); + } + return NodeStatus::RUNNING; + } + void onHalted() override + { + (*halted_)++; + } + +private: + int* halted_; + bool throw_; +}; + +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; + + void SetUp() override + { + factory.registerSimpleAction("Cleanup", [this](BT::TreeNode&) { + cleanup_count++; + return NodeStatus::SUCCESS; + }); + factory.registerSimpleAction( + "Throw", [](BT::TreeNode&) -> NodeStatus { throw std::runtime_error("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); + factory.registerNodeType("AsyncThrow", &main_halted, true); + // 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_CleanupRunsAndReturnsFailure) +{ + EXPECT_EQ(run(""), NodeStatus::FAILURE); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, NestedMainThrows_CleanupRunsAndReturnsFailure) +{ + EXPECT_EQ(run(""), NodeStatus::FAILURE); + 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_CleanupRunsAndReturnsSkipped) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickWhileRunning(), NodeStatus::SKIPPED); + tree.haltTree(); + EXPECT_EQ(cleanup_count, 1); +} + +TEST_F(FinallyTest, AsyncCleanup_ReturnsMainStatusWhenCleanupCompletes) +{ + EXPECT_EQ(run("", ""), NodeStatus::FAILURE); + EXPECT_EQ(cleanup_ticks, 2); +} + +TEST_F(FinallyTest, AsyncMainThrows_MainHaltedAndCleanupRuns) +{ + EXPECT_EQ(run(""), NodeStatus::FAILURE); + 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_CleanupNotRestarted) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + tree.haltTree(); + EXPECT_EQ(cleanup_ticks, 1); +} + +TEST_F(FinallyTest, HaltCleanupRunning_CleanupHalted) +{ + 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, 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); +}