add Finally control node for guaranteed cleanup

Finally ticks its first child (main), then ticks its second child (cleanup) after main returns SUCCESS, FAILURE or SKIPPED, or throws. If main threw, the exception is rethrown once cleanup finishes. Otherwise the node returns main's status, or FAILURE if cleanup failed. When halted while main is RUNNING, it halts main and ticks cleanup once, synchronously. halt() never throws, because it also runs from ~Tree(), so it prints exceptions to stderr instead.

ControlNode::haltChild now resets the child's status before rethrowing an exception from its halt(), so a later reset does not halt that child a second time.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

Author: dv-picknik

CMakeLists.txt

@@ -182,6 +182,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
 

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"

include/behaviortree_cpp/controls/finally_node.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include "behaviortree_cpp/control_node.h"
+
+#include <exception>
+
+namespace BT
+{
+/**
+ * @brief The Finally node ticks its first child ("main") and then ticks its
+ * second child ("cleanup"), like try/finally.
+ *
+ * - Cleanup runs after main returns SUCCESS, FAILURE or SKIPPED.
+ * - 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
+ *   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. 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, 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 {};
+  }
+
+  void halt() override;
+
+private:
+  bool in_cleanup_ = false;
+  NodeStatus main_status_ = NodeStatus::IDLE;
+  std::exception_ptr main_exception_;
+
+  void haltChildNoThrow(size_t i);
+
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace BT

src/bt_factory.cpp

@@ -132,6 +132,7 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl)
   registerNodeType<IfThenElseNode>("IfThenElse");
   registerNodeType<WhileDoElseNode>("WhileDoElse");
   registerNodeType<TryCatchNode>("TryCatch");
+  registerNodeType<FinallyNode>("Finally");
 
   registerNodeType<InverterNode>("Inverter");
 

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

src/controls/finally_node.cpp

@@ -0,0 +1,143 @@
+#include "behaviortree_cpp/controls/finally_node.h"
+
+#include <exception>
+#include <iostream>
+#include <string_view>
+#include <utility>
+
+namespace BT
+{
+namespace
+{
+void printException(std::string_view node_name, const char* context,
+                    const std::exception_ptr& exception)
+{
+  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()
+{
+  // halt() also runs from ~Tree(), where a propagating exception terminates, so nothing here throws.
+  if(!in_cleanup_ && status() == NodeStatus::RUNNING && children_nodes_.size() == 2)
+  {
+    haltChildNoThrow(0);
+    try
+    {
+      if(children_nodes_[1]->executeTick() == NodeStatus::FAILURE)
+      {
+        std::cerr << "[" << name() << "]: cleanup returned FAILURE during halt"
+                  << std::endl;
+      }
+    }
+    catch(...)
+    {
+      printException(name(), "cleanup threw during halt", std::current_exception());
+    }
+  }
+  for(size_t i = 0; i < children_nodes_.size(); i++)
+  {
+    haltChildNoThrow(i);
+  }
+  in_cleanup_ = false;
+  main_status_ = NodeStatus::IDLE;
+  main_exception_ = nullptr;
+  resetStatus();
+}
+
+void FinallyNode::haltChildNoThrow(size_t i)
+{
+  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_exception_ = nullptr;
+  }
+
+  setStatus(NodeStatus::RUNNING);
+
+  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)
+    {
+      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(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)
+  {
+    // executeTick() keeps our RUNNING status on SKIPPED, and halt() would rerun cleanup.
+    resetStatus();
+  }
+  return main_status_;
+}
+
+}  // namespace BT

src/xml_parsing.cpp

@@ -642,6 +642,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;

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