add Finally control node for guaranteed cleanup

CMakeLists.txt

@@ -187,6 +187,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,86 @@
+#pragma once
+
+#include "behaviortree_cpp/control_node.h"
+
+#include <atomic>
+#include <exception>
+#include <thread>
+
+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 or FAILURE. If main returns SKIPPED on
+ *   its first tick it did nothing, so cleanup is not run and the node returns
+ *   SKIPPED. If main is skipped after it returned RUNNING, for example by _while,
+ *   cleanup runs and the node returns 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 gets one tick, like TryCatch with catch_on_halt="true". A cleanup that
+ *   needs several ticks is cut short by the final halt of its subtree. That tick
+ *   runs inside halt(), on the calling thread and under any lock the caller holds,
+ *   so a cleanup action that blocks also blocks the caller.
+ * - To let a multi-step cleanup finish, set "halt_timeout_msec" above 0. Then halt()
+ *   ticks cleanup once, and again every 10 ms until it finishes, fails or throws,
+ *   or the timeout runs out, in which case cleanup is halted unfinished. No tick
+ *   starts after the timeout, and a tick that blocks is not cut short. halt()
+ *   blocks its caller, and any lock the caller holds, so a slow cleanup also
+ *   delays a parent such as a ReactiveSequence.
+ * - The loop runs only when halt() is called from the thread that last ticked this
+ *   node. A halt from another thread, such as a Timeout decorator's timer or a
+ *   service callback calling Tree::haltTree(), gives cleanup the single tick and
+ *   prints a note to stderr. A Finally nested inside that halt also gets a single
+ *   tick. halt() is not synchronized with a concurrent tick().
+ * - 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. "
+        "Below 10 (the tick period) cleanup gets a single tick") };
+  }
+
+  void halt() override;
+
+private:
+  static constexpr unsigned kDefaultHaltTimeoutMsec = 0;
+
+  // Written in tick(), read in halt(), which may run on another thread.
+  std::atomic<std::thread::id> tick_thread_{};
+  bool in_cleanup_ = false;
+  bool main_started_ = false;
+  NodeStatus main_status_ = NodeStatus::IDLE;
+  std::exception_ptr main_exception_;
+
+  void haltChildNoThrow(size_t i) noexcept;
+  void finishCleanupDuringHalt(bool foreign_thread);
+
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace BT

src/bt_factory.cpp

@@ -134,6 +134,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,254 @@
+#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 };
+
+// True while this thread runs a halt() that came from a thread other than the one ticking the
+// node. A Finally ticked or halted inside it, e.g. as cleanup, must not run its blocking loop.
+thread_local bool tls_in_foreign_halt = false;
+
+class ForeignHaltScope
+{
+public:
+  explicit ForeignHaltScope(bool foreign)
+    : previous_(std::exchange(tls_in_foreign_halt, tls_in_foreign_halt || foreign))
+  {}
+  ~ForeignHaltScope()
+  {
+    tls_in_foreign_halt = previous_;
+  }
+  ForeignHaltScope(const ForeignHaltScope&) = delete;
+  ForeignHaltScope& operator=(const ForeignHaltScope&) = delete;
+  ForeignHaltScope(ForeignHaltScope&&) = delete;
+  ForeignHaltScope& operator=(ForeignHaltScope&&) = delete;
+
+private:
+  bool previous_;
+};
+
+void printException(std::string_view node_name, const char* context,
+                    const std::exception_ptr& exception) noexcept
+{
+  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.
+  const bool foreign =
+      tls_in_foreign_halt || std::this_thread::get_id() != tick_thread_.load();
+  {
+    const ForeignHaltScope scope(foreign);
+    if(status() == NodeStatus::RUNNING && children_nodes_.size() == 2)
+    {
+      if(!in_cleanup_)
+      {
+        haltChildNoThrow(0);
+      }
+      finishCleanupDuringHalt(foreign);
+    }
+    for(size_t i = 0; i < children_nodes_.size(); i++)
+    {
+      haltChildNoThrow(i);
+    }
+  }
+  tick_thread_.store(std::thread::id{});
+  in_cleanup_ = false;
+  main_started_ = false;
+  main_status_ = NodeStatus::IDLE;
+  main_exception_ = nullptr;
+  resetStatus();
+}
+
+void FinallyNode::finishCleanupDuringHalt(bool foreign_thread)
+{
+  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
+  {
+    NodeStatus cleanup_status = children_nodes_[1]->executeTick();
+    // Cleanup gets one tick. Keep ticking only if halt_timeout_msec > 0 and this is the thread
+    // that ticks the tree, so a halt from a timer or service thread does not block its caller.
+    if(timeout_msec > 0 && foreign_thread && cleanup_status == NodeStatus::RUNNING)
+    {
+      std::cerr << "[" << name()
+                << "]: halted from a thread that does not tick this node, cleanup gets a "
+                   "single tick instead of halt_timeout_msec"
+                << std::endl;
+    }
+    const bool keep_ticking = timeout_msec > 0 && !foreign_thread;
+    while(keep_ticking && 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) noexcept
+{
+  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_started_ = false;
+    main_exception_ = nullptr;
+  }
+
+  setStatus(NodeStatus::RUNNING);
+  tick_thread_.store(std::this_thread::get_id());
+
+  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)
+    {
+      main_started_ = true;
+      return NodeStatus::RUNNING;
+    }
+    if(main_status_ == NodeStatus::IDLE)
+    {
+      throw LogicError("[", name(), "]: A child should not return IDLE");
+    }
+    if(main_status_ == NodeStatus::SKIPPED && !main_started_)
+    {
+      // Main did nothing, so there is nothing to clean up. executeTick() keeps our RUNNING
+      // status on SKIPPED, and halt() would run cleanup.
+      resetChildren();
+      resetStatus();
+      return NodeStatus::SKIPPED;
+    }
+    in_cleanup_ = true;
+    main_started_ = false;
+  }
+
+  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)
+  {
+    // Main was skipped after it started, e.g. by _while. executeTick() keeps our RUNNING status
+    // on SKIPPED, and halt() would rerun cleanup.
+    resetStatus();
+  }
+  return main_status_;
+}
+
+}  // namespace BT

src/xml_parsing.cpp

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