add Finally control node for guaranteed cleanup

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