From a5f90a04d98dcbc6544613b4d94b3826a5dcf9d0 Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Wed, 30 Sep 2026 16:01:26 -0600 Subject: [PATCH 1/2] feat: Add Finally control node for guaranteed cleanup Finally ticks its main child, then always ticks its cleanup child: after SUCCESS, FAILURE, or SKIPPED, after main throws (printed to stderr, surfaced as FAILURE), and synchronously when halted while main is RUNNING. It returns main's status, or FAILURE if cleanup fails. Refs PickNikRobotics/moveit_pro#20169 Co-Authored-By: Claude Opus 5.5 --- CHANGELOG.rst | 4 + CMakeLists.txt | 1 + include/behaviortree_cpp/behavior_tree.h | 1 + .../behaviortree_cpp/controls/finally_node.h | 47 +++++++ src/bt_factory.cpp | 1 + src/controls/finally_node.cpp | 77 ++++++++++ src/xml_parsing.cpp | 5 + tests/CMakeLists.txt | 1 + tests/gtest_finally.cpp | 132 ++++++++++++++++++ 9 files changed, 269 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/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..caeb49c85 --- /dev/null +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -0,0 +1,47 @@ +#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. + * + * 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..754f0122f --- /dev/null +++ b/src/controls/finally_node.cpp @@ -0,0 +1,77 @@ +#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_ && isStatusActive(status()) && children_nodes_.size() == 2) + { + haltChild(0); + if(children_nodes_[1]->executeTick() == NodeStatus::RUNNING) + { + haltChild(1); + } + } + 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; + return cleanup_status == NodeStatus::FAILURE ? NodeStatus::FAILURE : 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..548e66991 --- /dev/null +++ b/tests/gtest_finally.cpp @@ -0,0 +1,132 @@ +#include "behaviortree_cpp/bt_factory.h" + +#include + +#include + +using BT::NodeStatus; + +class FinallyTest : public testing::Test +{ +protected: + BT::BehaviorTreeFactory factory; + int cleanup_count = 0; + int main_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.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); +} From de12692208398bd407fb5584c4b2667cdd4e15ee Mon Sep 17 00:00:00 2001 From: David Vadovszki Date: Wed, 30 Sep 2026 16:17:12 -0600 Subject: [PATCH 2/2] fix: Keep Finally cleanup to one run on halt and SKIPPED halt() also runs from ~Tree(), so a cleanup exception there is printed instead of propagated. A SKIPPED result leaves the node RUNNING in executeTick(), so the node resets itself, keeping a later halt from rerunning cleanup. Adds tests for async main and cleanup, halts in each phase, and SKIPPED main. Co-Authored-By: Claude Opus 5.5 --- .../behaviortree_cpp/controls/finally_node.h | 3 +- src/controls/finally_node.cpp | 26 +++- tests/gtest_finally.cpp | 117 ++++++++++++++++++ 3 files changed, 141 insertions(+), 5 deletions(-) diff --git a/include/behaviortree_cpp/controls/finally_node.h b/include/behaviortree_cpp/controls/finally_node.h index caeb49c85..f90c9392d 100644 --- a/include/behaviortree_cpp/controls/finally_node.h +++ b/include/behaviortree_cpp/controls/finally_node.h @@ -14,7 +14,8 @@ namespace BT * - 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. + * - 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. */ diff --git a/src/controls/finally_node.cpp b/src/controls/finally_node.cpp index 754f0122f..9cbf4debf 100644 --- a/src/controls/finally_node.cpp +++ b/src/controls/finally_node.cpp @@ -12,12 +12,21 @@ FinallyNode::FinallyNode(const std::string& name, const NodeConfig& config) void FinallyNode::halt() { - if(!in_cleanup_ && isStatusActive(status()) && children_nodes_.size() == 2) + if(!in_cleanup_ && status() == NodeStatus::RUNNING && children_nodes_.size() == 2) { haltChild(0); - if(children_nodes_[1]->executeTick() == NodeStatus::RUNNING) + // 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) { - haltChild(1); + std::cerr << "[" << name() << "]: Finally cleanup threw during halt: " << ex.what() + << std::endl; } } in_cleanup_ = false; @@ -71,7 +80,16 @@ NodeStatus FinallyNode::tick() resetChildren(); in_cleanup_ = false; - return cleanup_status == NodeStatus::FAILURE ? NodeStatus::FAILURE : main_status_; + 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/tests/gtest_finally.cpp b/tests/gtest_finally.cpp index 548e66991..f85a700ea 100644 --- a/tests/gtest_finally.cpp +++ b/tests/gtest_finally.cpp @@ -6,12 +6,48 @@ 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 { @@ -25,6 +61,12 @@ 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); + // 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; @@ -130,3 +172,78 @@ TEST_F(FinallyTest, WrongChildCount_RejectedAtLoad) )"), 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); +}