From 605e2a8e219a2577baacb6bf256818313fca8d9c Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Mon, 5 Oct 2026 12:48:10 -0600 Subject: [PATCH 1/6] add Finally control node for guaranteed cleanup Finally ticks its first child (main), then always ticks its second child (cleanup): after main returns SUCCESS, FAILURE or SKIPPED, after main throws (printed to stderr, returned as FAILURE), and synchronously when the node is halted while main is RUNNING. It returns main's status, or FAILURE if cleanup fails. A cleanup exception propagates from tick() and is printed during halt(), because halt() also runs from ~Tree(). Co-Authored-By: Claude Opus 5.5 (cherry picked from commit 8094d46c8bd440274dff2f99ac3abc2007fc63f5) --- CMakeLists.txt | 1 + include/behaviortree_cpp/behavior_tree.h | 1 + .../behaviortree_cpp/controls/finally_node.h | 48 ++++ src/bt_factory.cpp | 1 + src/controls/finally_node.cpp | 95 +++++++ src/xml_parsing.cpp | 5 + tests/CMakeLists.txt | 1 + tests/gtest_finally.cpp | 249 ++++++++++++++++++ 8 files changed, 401 insertions(+) create mode 100644 include/behaviortree_cpp/controls/finally_node.h create mode 100644 src/controls/finally_node.cpp create mode 100644 tests/gtest_finally.cpp 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 f8a2edf66..8612f028b 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -542,6 +542,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); +} From e36fce8f551e297809048b6a07123bc2e528e1f5 Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Mon, 5 Oct 2026 12:50:07 -0600 Subject: [PATCH 2/6] docs: Note the Finally node in the changelog Co-Authored-By: Claude Opus 5.5 --- CHANGELOG.rst | 4 ++++ 1 file changed, 4 insertions(+) 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 From 25b88828d67a2ff7f40b61ee57062ca24343e573 Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Mon, 5 Oct 2026 13:03:40 -0600 Subject: [PATCH 3/6] fix: Keep Finally cleanup running when halting a child throws Halting main from the exception path, and every halt inside halt(), now prints instead of propagating, so cleanup still runs and ~Tree() cannot terminate. Non-std exceptions are caught too. FinallyNode joins TreeNode's friends to reset a child whose halt() threw, since ControlNode::resetChildren() would otherwise halt it again. Adds tests for these paths, a ReactiveSequence parent, a cleanup retry after a throw, and a SKIPPED cleanup. Co-Authored-By: Claude Opus 5.5 --- .../behaviortree_cpp/controls/finally_node.h | 11 +- include/behaviortree_cpp/tree_node.h | 1 + src/controls/finally_node.cpp | 62 +++++++--- tests/gtest_finally.cpp | 115 +++++++++++++++++- 4 files changed, 167 insertions(+), 22 deletions(-) diff --git a/include/behaviortree_cpp/controls/finally_node.h b/include/behaviortree_cpp/controls/finally_node.h index f90c9392d..786655afe 100644 --- a/include/behaviortree_cpp/controls/finally_node.h +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -9,13 +9,16 @@ namespace BT * 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, + * - If main throws (any type), 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(). + * - Exceptions thrown by cleanup propagate from tick(), and the next tick + * retries cleanup. halt() never throws, because it also runs from ~Tree(): + * it prints exceptions from cleanup or from halting a child to stderr. + * - Halt-time cleanup runs inside halt(), so a slow cleanup delays the parent, + * for example a ReactiveSequence whose condition changed. * * Requires exactly 2 children. */ @@ -42,6 +45,8 @@ class FinallyNode : public ControlNode bool in_cleanup_ = false; NodeStatus main_status_ = NodeStatus::IDLE; + void haltChildNoThrow(size_t i); + BT::NodeStatus tick() override; }; diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index bd7f22425..3d7fc013c 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; // resets a child whose halt() threw friend class Tree; [[nodiscard]] NodeConfig& config(); diff --git a/src/controls/finally_node.cpp b/src/controls/finally_node.cpp index 9cbf4debf..0b37ec189 100644 --- a/src/controls/finally_node.cpp +++ b/src/controls/finally_node.cpp @@ -4,6 +4,27 @@ namespace BT { +namespace +{ +// Prints the exception being handled. Call only from inside a catch block. +void printCurrentException(const std::string& node_name, const char* context) +{ + std::cerr << "[" << node_name << "]: Finally " << context << ": "; + try + { + throw; + } + 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) { @@ -12,25 +33,39 @@ FinallyNode::FinallyNode(const std::string& name, const NodeConfig& config) void FinallyNode::halt() { + // halt() also runs from ~Tree(), where a propagating exception terminates, so nothing here throws. if(!in_cleanup_ && status() == NodeStatus::RUNNING && children_nodes_.size() == 2) { - haltChild(0); - // halt() also runs from ~Tree(), where a propagating exception terminates. + haltChildNoThrow(0); try { - if(children_nodes_[1]->executeTick() == NodeStatus::RUNNING) - { - haltChild(1); - } + children_nodes_[1]->executeTick(); } - catch(const std::exception& ex) + catch(...) { - std::cerr << "[" << name() << "]: Finally cleanup threw during halt: " << ex.what() - << std::endl; + printCurrentException(name(), "cleanup threw during halt"); } } + for(size_t i = 0; i < children_nodes_.size(); i++) + { + haltChildNoThrow(i); + } in_cleanup_ = false; - ControlNode::halt(); + main_status_ = NodeStatus::IDLE; + resetStatus(); +} + +void FinallyNode::haltChildNoThrow(size_t i) +{ + try + { + haltChild(i); + } + catch(...) + { + printCurrentException(name(), "a child threw while being halted"); + children_nodes_[i]->resetStatus(); + } } NodeStatus FinallyNode::tick() @@ -53,11 +88,10 @@ NodeStatus FinallyNode::tick() { main_status_ = children_nodes_[0]->executeTick(); } - catch(const std::exception& ex) + catch(...) { - std::cerr << "[" << name() << "]: Finally caught an exception from its main child, " - << "running cleanup and returning FAILURE: " << ex.what() << std::endl; - haltChild(0); + printCurrentException(name(), "main threw, running cleanup and returning FAILURE"); + haltChildNoThrow(0); main_status_ = NodeStatus::FAILURE; } diff --git a/tests/gtest_finally.cpp b/tests/gtest_finally.cpp index f85a700ea..eb159990a 100644 --- a/tests/gtest_finally.cpp +++ b/tests/gtest_finally.cpp @@ -6,13 +6,16 @@ using BT::NodeStatus; -// RUNNING until halted; throws on its second tick if `throw_on_running` is set. +// 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) - : StatefulActionNode(name, config), halted_(halted), throw_(throw_on_running) + 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() { @@ -33,11 +36,16 @@ class AsyncMain : public BT::StatefulActionNode void onHalted() override { (*halted_)++; + if(throw_on_halt_) + { + throw std::runtime_error("halt boom"); + } } private: int* halted_; bool throw_; + bool throw_on_halt_; }; class FinallyTest : public testing::Test @@ -48,6 +56,8 @@ class FinallyTest : public testing::Test int main_ticks = 0; int main_halted = 0; int cleanup_ticks = 0; + int throw_once_ticks = 0; + bool gate = true; void SetUp() override { @@ -61,8 +71,22 @@ class FinallyTest : public testing::Test 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); + 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 std::runtime_error("once"); + } + return NodeStatus::SUCCESS; + }); + 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; @@ -181,6 +205,8 @@ 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. tree.haltTree(); EXPECT_EQ(cleanup_count, 1); } @@ -247,3 +273,82 @@ TEST_F(FinallyTest, CleanupThrowsDuringHalt_DoesNotPropagate) EXPECT_NO_THROW(tree.haltTree()); EXPECT_EQ(tree.rootNode()->status(), NodeStatus::IDLE); } + +TEST_F(FinallyTest, MainThrowsAndItsHaltThrows_CleanupStillRuns) +{ + EXPECT_EQ(run(""), NodeStatus::FAILURE); + 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_CleanupRunsAndReturnsFailure) +{ + EXPECT_EQ(run(""), NodeStatus::FAILURE); + 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_NextTickRetriesCleanupOnly) +{ + auto tree = factory.createTreeFromText(R"( + + + )"); + + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + EXPECT_THROW(tree.tickOnce(), BT::RuntimeError); + EXPECT_EQ(tree.tickOnce(), NodeStatus::SUCCESS); + EXPECT_EQ(main_ticks, 3); + 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); +} From 9a764061c84b33dff69ee355d91420e3e031848e Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Mon, 5 Oct 2026 13:10:41 -0600 Subject: [PATCH 4/6] fix: Reset a child in haltChild even when its halt throws ControlNode::haltChild now resets the child's status before rethrowing, so the next reset doesn't halt it again. This replaces the TreeNode friend that Finally needed. Finally also reports a cleanup FAILURE during halt, documents that halt-time cleanup is synchronous and runs on the calling thread, and gains SubTree and nested-Finally tests. Co-Authored-By: Claude Opus 5.5 --- CHANGELOG.rst | 2 +- .../behaviortree_cpp/controls/finally_node.h | 27 ++++---- include/behaviortree_cpp/tree_node.h | 1 - src/control_node.cpp | 11 +++- src/controls/finally_node.cpp | 9 ++- tests/gtest_finally.cpp | 63 +++++++++++++++++++ 6 files changed, 96 insertions(+), 17 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4ee1fcc2e..14333a7d0 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,7 +4,7 @@ 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 +* Add Finally control node: runs a cleanup child after the main child finishes, fails, or throws, and runs it synchronously when halted. 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 786655afe..d283d387c 100644 --- a/include/behaviortree_cpp/controls/finally_node.h +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -5,22 +5,27 @@ namespace BT { /** - * @brief The Finally node ticks its first child ("main") and then always ticks - * its second child ("cleanup"), like try/finally. + * @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. - * - If main throws (any type), 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. + * - If main throws (any type), the exception is printed to stderr, main is + * halted, cleanup runs, and this node returns FAILURE. * - The node returns main's status, or FAILURE if cleanup fails. + * - If this node is halted while main is RUNNING, main is halted and cleanup + * is ticked once, synchronously, on the thread calling halt(). Halt-time + * cleanup must therefore be synchronous: if it returns RUNNING, it is halted. + * A slow cleanup delays the parent, for example a ReactiveSequence whose + * condition changed. Call halt() from the thread that ticks the tree. + * - If this node is halted while cleanup is RUNNING, cleanup is halted and + * does not finish. * - Exceptions thrown by cleanup propagate from tick(), and the next tick - * retries cleanup. halt() never throws, because it also runs from ~Tree(): - * it prints exceptions from cleanup or from halting a child to stderr. - * - Halt-time cleanup runs inside halt(), so a slow cleanup delays the parent, - * for example a ReactiveSequence whose condition changed. + * retries cleanup. If the tree is halted instead, cleanup is not retried. + * - halt() never throws, because it also runs from ~Tree(). It prints + * exceptions from cleanup or from halting a child, and a cleanup FAILURE, + * to stderr. * - * Requires exactly 2 children. + * Requires exactly 2 children, checked when the XML is loaded and on tick. */ class FinallyNode : public ControlNode { diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 3d7fc013c..bd7f22425 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -445,7 +445,6 @@ class TreeNode friend class BehaviorTreeFactory; friend class DecoratorNode; friend class ControlNode; - friend class FinallyNode; // resets a child whose halt() threw friend class Tree; [[nodiscard]] NodeConfig& config(); diff --git a/src/control_node.cpp b/src/control_node.cpp index 535165376..b28fec77c 100644 --- a/src/control_node.cpp +++ b/src/control_node.cpp @@ -57,7 +57,16 @@ void ControlNode::haltChild(size_t i) auto* child = children_nodes_[i]; if(child->status() == NodeStatus::RUNNING) { - child->haltNode(); + try + { + child->haltNode(); + } + catch(...) + { + // Don't leave the child RUNNING, or the next reset would halt it again. + child->resetStatus(); + throw; + } } child->resetStatus(); } diff --git a/src/controls/finally_node.cpp b/src/controls/finally_node.cpp index 0b37ec189..88a4390ef 100644 --- a/src/controls/finally_node.cpp +++ b/src/controls/finally_node.cpp @@ -9,7 +9,7 @@ namespace // Prints the exception being handled. Call only from inside a catch block. void printCurrentException(const std::string& node_name, const char* context) { - std::cerr << "[" << node_name << "]: Finally " << context << ": "; + std::cerr << "[" << node_name << "]: " << context << ": "; try { throw; @@ -39,7 +39,11 @@ void FinallyNode::halt() haltChildNoThrow(0); try { - children_nodes_[1]->executeTick(); + if(children_nodes_[1]->executeTick() == NodeStatus::FAILURE) + { + std::cerr << "[" << name() << "]: cleanup returned FAILURE during halt" + << std::endl; + } } catch(...) { @@ -64,7 +68,6 @@ void FinallyNode::haltChildNoThrow(size_t i) catch(...) { printCurrentException(name(), "a child threw while being halted"); - children_nodes_[i]->resetStatus(); } } diff --git a/tests/gtest_finally.cpp b/tests/gtest_finally.cpp index eb159990a..0758a4fc7 100644 --- a/tests/gtest_finally.cpp +++ b/tests/gtest_finally.cpp @@ -352,3 +352,66 @@ TEST_F(FinallyTest, HaltedByReactiveSequence_CleanupRunsOnce) 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); +} From b2d56de9cc7ed95e171965dcdce0a50bcf2fcd7f Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Mon, 5 Oct 2026 13:24:03 -0600 Subject: [PATCH 5/6] feat: Rethrow main's exception after Finally cleanup When main throws, Finally halts main, runs cleanup, then rethrows the exception instead of returning FAILURE. This matches finally in other languages, keeps the NodeExecutionError context, and stops errors such as a missing port from being retried as ordinary failures. Co-Authored-By: Claude Opus 5.5 --- CHANGELOG.rst | 2 +- .../behaviortree_cpp/controls/finally_node.h | 7 ++- src/controls/finally_node.cpp | 12 ++++- tests/gtest_finally.cpp | 53 ++++++++++++++++--- 4 files changed, 62 insertions(+), 12 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 14333a7d0..fd438c605 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, and runs it synchronously when halted. Registers the built-in ID ``Finally`` +* 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`` 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 d283d387c..ec924b660 100644 --- a/include/behaviortree_cpp/controls/finally_node.h +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -2,6 +2,8 @@ #include "behaviortree_cpp/control_node.h" +#include + namespace BT { /** @@ -9,8 +11,8 @@ namespace BT * second child ("cleanup"), like try/finally. * * - Cleanup runs after main returns SUCCESS, FAILURE or SKIPPED. - * - If main throws (any type), the exception is printed to stderr, main is - * halted, cleanup runs, and this node returns FAILURE. + * - 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 is RUNNING, main is halted and cleanup * is ticked once, synchronously, on the thread calling halt(). Halt-time @@ -49,6 +51,7 @@ class FinallyNode : public ControlNode private: bool in_cleanup_ = false; NodeStatus main_status_ = NodeStatus::IDLE; + std::exception_ptr main_exception_; void haltChildNoThrow(size_t i); diff --git a/src/controls/finally_node.cpp b/src/controls/finally_node.cpp index 88a4390ef..7893c1116 100644 --- a/src/controls/finally_node.cpp +++ b/src/controls/finally_node.cpp @@ -1,6 +1,8 @@ #include "behaviortree_cpp/controls/finally_node.h" +#include #include +#include namespace BT { @@ -56,6 +58,7 @@ void FinallyNode::halt() } in_cleanup_ = false; main_status_ = NodeStatus::IDLE; + main_exception_ = nullptr; resetStatus(); } @@ -81,6 +84,7 @@ NodeStatus FinallyNode::tick() if(!isStatusActive(status())) { in_cleanup_ = false; + main_exception_ = nullptr; } setStatus(NodeStatus::RUNNING); @@ -93,7 +97,7 @@ NodeStatus FinallyNode::tick() } catch(...) { - printCurrentException(name(), "main threw, running cleanup and returning FAILURE"); + main_exception_ = std::current_exception(); haltChildNoThrow(0); main_status_ = NodeStatus::FAILURE; } @@ -117,6 +121,12 @@ NodeStatus FinallyNode::tick() 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; diff --git a/tests/gtest_finally.cpp b/tests/gtest_finally.cpp index 0758a4fc7..c86ef8028 100644 --- a/tests/gtest_finally.cpp +++ b/tests/gtest_finally.cpp @@ -123,15 +123,15 @@ TEST_F(FinallyTest, CleanupFails_ReturnsFailure) EXPECT_EQ(run("", ""), NodeStatus::FAILURE); } -TEST_F(FinallyTest, MainThrows_CleanupRunsAndReturnsFailure) +TEST_F(FinallyTest, MainThrows_CleanupRunsThenRethrows) { - EXPECT_EQ(run(""), NodeStatus::FAILURE); + EXPECT_THROW(run(""), BT::RuntimeError); EXPECT_EQ(cleanup_count, 1); } -TEST_F(FinallyTest, NestedMainThrows_CleanupRunsAndReturnsFailure) +TEST_F(FinallyTest, NestedMainThrows_CleanupRunsThenRethrows) { - EXPECT_EQ(run(""), NodeStatus::FAILURE); + EXPECT_THROW(run(""), BT::RuntimeError); EXPECT_EQ(cleanup_count, 1); } @@ -219,7 +219,7 @@ TEST_F(FinallyTest, AsyncCleanup_ReturnsMainStatusWhenCleanupCompletes) TEST_F(FinallyTest, AsyncMainThrows_MainHaltedAndCleanupRuns) { - EXPECT_EQ(run(""), NodeStatus::FAILURE); + EXPECT_THROW(run(""), BT::RuntimeError); EXPECT_EQ(main_halted, 1); EXPECT_EQ(cleanup_count, 1); } @@ -276,7 +276,7 @@ TEST_F(FinallyTest, CleanupThrowsDuringHalt_DoesNotPropagate) TEST_F(FinallyTest, MainThrowsAndItsHaltThrows_CleanupStillRuns) { - EXPECT_EQ(run(""), NodeStatus::FAILURE); + EXPECT_THROW(run(""), BT::RuntimeError); EXPECT_EQ(main_halted, 1); EXPECT_EQ(cleanup_count, 1); } @@ -294,9 +294,9 @@ TEST_F(FinallyTest, HaltWhereMainHaltThrows_CleanupRunsAndNothingPropagates) EXPECT_EQ(cleanup_count, 1); } -TEST_F(FinallyTest, MainThrowsNonStdException_CleanupRunsAndReturnsFailure) +TEST_F(FinallyTest, MainThrowsNonStdException_CleanupRunsThenRethrows) { - EXPECT_EQ(run(""), NodeStatus::FAILURE); + EXPECT_THROW(run(""), int); EXPECT_EQ(cleanup_count, 1); } @@ -415,3 +415,40 @@ TEST_F(FinallyTest, CleanupFailsDuringHalt_IsReported) "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); +} From c4c6e3b3e1a61ed1882f674a20725ece274e65f3 Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Mon, 5 Oct 2026 13:29:14 -0600 Subject: [PATCH 6/6] fix: Pass the exception explicitly to Finally's printer A bare rethrow outside a catch handler trips Sonar cpp:S1039, so the printer takes an exception_ptr. Tests throw a local TestError instead of std::runtime_error (cpp:S112). Co-Authored-By: Claude Opus 5.5 --- src/controls/finally_node.cpp | 11 ++++++----- tests/gtest_finally.cpp | 13 +++++++++---- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/controls/finally_node.cpp b/src/controls/finally_node.cpp index 7893c1116..a2166c66c 100644 --- a/src/controls/finally_node.cpp +++ b/src/controls/finally_node.cpp @@ -2,19 +2,20 @@ #include #include +#include #include namespace BT { namespace { -// Prints the exception being handled. Call only from inside a catch block. -void printCurrentException(const std::string& node_name, const char* context) +void printException(std::string_view node_name, const char* context, + const std::exception_ptr& exception) { std::cerr << "[" << node_name << "]: " << context << ": "; try { - throw; + std::rethrow_exception(exception); } catch(const std::exception& ex) { @@ -49,7 +50,7 @@ void FinallyNode::halt() } catch(...) { - printCurrentException(name(), "cleanup threw during halt"); + printException(name(), "cleanup threw during halt", std::current_exception()); } } for(size_t i = 0; i < children_nodes_.size(); i++) @@ -70,7 +71,7 @@ void FinallyNode::haltChildNoThrow(size_t i) } catch(...) { - printCurrentException(name(), "a child threw while being halted"); + printException(name(), "a child threw while being halted", std::current_exception()); } } diff --git a/tests/gtest_finally.cpp b/tests/gtest_finally.cpp index c86ef8028..a000fdc31 100644 --- a/tests/gtest_finally.cpp +++ b/tests/gtest_finally.cpp @@ -6,6 +6,11 @@ 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 { @@ -29,7 +34,7 @@ class AsyncMain : public BT::StatefulActionNode { if(throw_) { - throw std::runtime_error("boom"); + throw TestError("boom"); } return NodeStatus::RUNNING; } @@ -38,7 +43,7 @@ class AsyncMain : public BT::StatefulActionNode (*halted_)++; if(throw_on_halt_) { - throw std::runtime_error("halt boom"); + throw TestError("halt boom"); } } @@ -66,7 +71,7 @@ class FinallyTest : public testing::Test return NodeStatus::SUCCESS; }); factory.registerSimpleAction( - "Throw", [](BT::TreeNode&) -> NodeStatus { throw std::runtime_error("boom"); }); + "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; @@ -80,7 +85,7 @@ class FinallyTest : public testing::Test factory.registerSimpleAction("ThrowOnce", [this](BT::TreeNode&) { if(++throw_once_ticks == 1) { - throw std::runtime_error("once"); + throw TestError("once"); } return NodeStatus::SUCCESS; });