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. An exception from cleanup resets the node and propagates.

When halted while main or cleanup is RUNNING, it halts main and keeps ticking cleanup every 10 ms until cleanup finishes or the halt_timeout_msec port (default 10000) runs out, so asynchronous cleanup completes. halt() never throws, because it also runs from ~Tree(), so it prints problems 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,68 @@
+#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 or cleanup is RUNNING, main is halted
+ *   and cleanup is ticked again every 10 ms on the thread calling halt(), so
+ *   asynchronous cleanup can finish. This stops when cleanup finishes, fails
+ *   or throws, or when "halt_timeout_msec" runs out, in which case cleanup is
+ *   halted unfinished. Cleanup always gets at least one tick, no later tick
+ *   starts after the timeout, and a tick that blocks is not cut short. halt() blocks its caller, and any lock the
+ *   caller holds, until then, so a slow cleanup also delays a parent such as
+ *   a ReactiveSequence. Call halt() from the thread that ticks the tree.
+ * - 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 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<unsigned>("halt_timeout_msec", kDefaultHaltTimeoutMsec,
+                                 "When halted, how long to keep ticking cleanup "
+                                 "before halting it unfinished, in milliseconds") };
+  }
+
+  void halt() override;
+
+private:
+  static constexpr unsigned kDefaultHaltTimeoutMsec = 10000;
+
+  bool in_cleanup_ = false;
+  NodeStatus main_status_ = NodeStatus::IDLE;
+  std::exception_ptr main_exception_;
+
+  void haltChildNoThrow(size_t i);
+  void finishCleanupDuringHalt();
+
+  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,203 @@
+#include "behaviortree_cpp/controls/finally_node.h"
+
+#include <chrono>
+#include <exception>
+#include <iostream>
+#include <string_view>
+#include <thread>
+#include <utility>
+
+namespace BT
+{
+namespace
+{
+constexpr std::chrono::milliseconds kHaltTickPeriod{ 10 };
+
+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(status() == NodeStatus::RUNNING && children_nodes_.size() == 2)
+  {
+    if(!in_cleanup_)
+    {
+      haltChildNoThrow(0);
+    }
+    finishCleanupDuringHalt();
+  }
+  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::finishCleanupDuringHalt()
+{
+  unsigned timeout_msec = kDefaultHaltTimeoutMsec;
+  if(auto input = getInput<unsigned>("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
+  {
+    // Cleanup is commonly asynchronous, so keep ticking it until it finishes rather than
+    // halting it after its first tick.
+    NodeStatus cleanup_status = children_nodes_[1]->executeTick();
+    while(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)
+{
+  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;
+  }
+
+  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)
+  {
+    // 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