Halt the running branch before starting another asynchronous action

CMakeLists.txt

@@ -171,6 +171,7 @@ list(APPEND BT_SOURCE
     src/actions/updated_action.cpp
 
     src/decorators/delay_node.cpp
+    src/decorators/preempt.cpp
     src/decorators/inverter_node.cpp
     src/decorators/repeat_node.cpp
     src/decorators/retry_node.cpp

docs/preempt.md

@@ -0,0 +1,33 @@
+# Preempt
+
+`Preempt` marks the point where a new branch of a reactive tree starts. Without it,
+a reactive node can only halt the siblings of a child *after* that child returned
+RUNNING, so an asynchronous action started in `onStart()` runs for a moment next to
+the action that is about to be preempted (issue #1031). With the ROS action wrappers
+that means a new goal is sent before the previous one is cancelled.
+
+Wrap the action that starts the new branch, and the RUNNING branches of the reactive
+ancestors above it are halted first:
+
+```xml
+<ReactiveSequence>
+  <Fallback>
+    <BatteryOk/>
+    <Preempt>
+      <GotoCharging/>
+    </Preempt>
+  </Fallback>
+  <GotoKitchen/>
+</ReactiveSequence>
+```
+
+When `battery_ok` turns false, `GotoKitchen` is halted before `GotoCharging` starts.
+
+The preemption runs on the first tick of `Preempt` in each activation, that is,
+whenever the node is not already RUNNING or completed. It walks up through the
+parents and stops at `ReactiveSequence` and `ReactiveFallback`; the children of a
+`Parallel` are left alone, since that node is allowed to keep several of them
+RUNNING. `Preempt` is opt-in: a tree that does not use it behaves exactly as before.
+
+Note that `Preempt` is a builtin node, so registering a custom node with that ID will
+now throw.

include/behaviortree_cpp/behavior_tree.h

@@ -42,6 +42,7 @@
 #include "behaviortree_cpp/decorators/inverter_node.h"
 #include "behaviortree_cpp/decorators/keep_running_until_failure_node.h"
 #include "behaviortree_cpp/decorators/loop_node.h"
+#include "behaviortree_cpp/decorators/preempt.h"
 #include "behaviortree_cpp/decorators/repeat_node.h"
 #include "behaviortree_cpp/decorators/retry_node.h"
 #include "behaviortree_cpp/decorators/run_once_node.h"

include/behaviortree_cpp/controls/reactive_control_node.h

@@ -0,0 +1,66 @@
+/* Copyright (C) 2025 Davide Faconti -  All Rights Reserved
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy of
+ *   this software and associated documentation files (the "Software"), to deal in
+ *   the Software without restriction, including without limitation the rights to
+ *   use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ *   the Software, and to permit persons to whom the Software is furnished to do so,
+ *   subject to the following conditions:
+ *
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ *   FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ *   COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ *   IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *   CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+#include "behaviortree_cpp/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief Common base of the control nodes that tick their children in order and stop
+ * at the first one that returns RUNNING (ReactiveSequence, ReactiveFallback).
+ *
+ * It exists so that a node outside the reactive node can ask it to let go of the
+ * branches it is running without knowing which of the two it is.
+ *
+ * Deliberately not virtual: the class adds state and one non-virtual accessor only,
+ * so no vtable of an existing node changes.
+ */
+class ReactiveControlNode : public ControlNode
+{
+public:
+  ReactiveControlNode(const std::string& name, const NodeConfig& config)
+    : ControlNode(name, config)
+  {}
+
+  /** Halt every child that is RUNNING, except the one the caller is inside.
+   *
+   * Used by Preempt: the ancestor cannot tell by index which branch has to go,
+   * because the index is only updated by its own tick(), and the halting happens
+   * below it. Asking the children for their status cannot go stale.
+   */
+  void haltRunningChild(const TreeNode* excluded)
+  {
+    running_child_ = -1;
+    for(size_t i = 0; i < childrenCount(); i++)
+    {
+      if(children()[i] != excluded && children()[i]->status() == NodeStatus::RUNNING)
+      {
+        haltChild(i);
+      }
+    }
+  }
+
+protected:
+  int running_child_ = -1;
+};
+
+}  // namespace BT

include/behaviortree_cpp/controls/reactive_fallback.h

@@ -12,7 +12,7 @@
 
 #pragma once
 
-#include "behaviortree_cpp/control_node.h"
+#include "behaviortree_cpp/controls/reactive_control_node.h"
 
 namespace BT
 {
@@ -30,10 +30,10 @@ namespace BT
  *            a single asynchronous child.
  *
  */
-class ReactiveFallback : public ControlNode
+class ReactiveFallback : public ReactiveControlNode
 {
 public:
-  ReactiveFallback(const std::string& name) : ControlNode(name, {})
+  ReactiveFallback(const std::string& name) : ReactiveControlNode(name, {})
   {}
 
   /** A ReactiveFallback is not supposed to have more than a single
@@ -47,7 +47,6 @@ class ReactiveFallback : public ControlNode
 
   void halt() override;
 
-  int running_child_ = -1;
   static bool throw_if_multiple_running;
 };
 

include/behaviortree_cpp/controls/reactive_sequence.h

@@ -12,7 +12,7 @@
 
 #pragma once
 
-#include "behaviortree_cpp/control_node.h"
+#include "behaviortree_cpp/controls/reactive_control_node.h"
 
 namespace BT
 {
@@ -30,10 +30,10 @@ namespace BT
  *            asynchronous child.
  *
  */
-class ReactiveSequence : public ControlNode
+class ReactiveSequence : public ReactiveControlNode
 {
 public:
-  ReactiveSequence(const std::string& name) : ControlNode(name, {})
+  ReactiveSequence(const std::string& name) : ReactiveControlNode(name, {})
   {}
 
   /** A ReactiveSequence is not supposed to have more than a single
@@ -47,8 +47,6 @@ class ReactiveSequence : public ControlNode
 
   void halt() override;
 
-  int running_child_ = -1;
-
   static bool throw_if_multiple_running;
 };
 

include/behaviortree_cpp/decorators/preempt.h

@@ -0,0 +1,79 @@
+/* Copyright (C) 2025 Davide Faconti -  All Rights Reserved
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy of
+ *   this software and associated documentation files (the "Software"), to deal in
+ *   the Software without restriction, including without limitation the rights to
+ *   use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ *   the Software, and to permit persons to whom the Software is furnished to do so,
+ *   subject to the following conditions:
+ *
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ *   FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ *   COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ *   IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *   CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+#include "behaviortree_cpp/decorator_node.h"
+
+namespace BT
+{
+/**
+ * @brief Marks the point where a new branch of the tree starts.
+ *
+ * A reactive node knows how to halt the siblings of the child that returned
+ * RUNNING, but it does so after ticking that child. When a check that returns
+ * SUCCESS immediately and a new action that will keep running look the same to
+ * it, the tree cannot tell "start a new branch" from "re-tick a check", and the
+ * previously running branch is left RUNNING while the new one starts (issue
+ * #1031).
+ *
+ * Preempt answers that question at the one place that knows the answer: the tree
+ * author, who wraps the action that starts the new branch.
+ *
+ *   <ReactiveSequence>
+ *     <Fallback>
+ *       <BatteryOk/>
+ *       <Preempt><GotoCharging/></Preempt>
+ *     </Fallback>
+ *     <GotoKitchen/>
+ *   </ReactiveSequence>
+ *
+ * On the first tick of each activation, before ticking its child, it halts the
+ * RUNNING child of every reactive ancestor it is not itself inside. Children of a
+ * Parallel are left alone: that node is allowed to keep several of them RUNNING,
+ * so halting them because a sibling branch moved on would be wrong.
+ */
+class Preempt : public DecoratorNode
+{
+public:
+  Preempt(const std::string& name, const NodeConfig& config) : DecoratorNode(name, config)
+  {
+    setRegistrationID("Preempt");
+  }
+
+  ~Preempt() override = default;
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+
+  Preempt(const Preempt&) = delete;
+  Preempt& operator=(const Preempt&) = delete;
+  Preempt(Preempt&&) = delete;
+  Preempt& operator=(Preempt&&) = delete;
+
+private:
+  NodeStatus tick() override;
+
+  void haltPreviousBranch();
+};
+
+}  // namespace BT

include/behaviortree_cpp/tree_node.h

@@ -205,6 +205,9 @@ class TreeNode
 
   virtual NodeType type() const = 0;
 
+  /// Parent of this node in the tree, or nullptr for the root of a subtree.
+  [[nodiscard]] TreeNode* parent() const;
+
   using StatusChangeSignal = Signal<TimePoint, const TreeNode&, NodeStatus, NodeStatus>;
   using StatusChangeSubscriber = StatusChangeSignal::Subscriber;
   using StatusChangeCallback = StatusChangeSignal::CallableFunction;
@@ -463,9 +466,15 @@ class TreeNode
   T parseString(const std::string& str) const;
 
 private:
+  friend class ControlNode;
+  friend class DecoratorNode;
+
   struct PImpl;
   std::unique_ptr<PImpl> _p;
 
+  /// Set by ControlNode and DecoratorNode when the node is added to the tree.
+  void setParent(TreeNode* parent);
+
   Expected<NodeStatus> checkPreConditions();
   void checkPostConditions(NodeStatus status);
 

src/bt_factory.cpp

@@ -144,6 +144,7 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl)
   registerNodeType<TimeoutNode>("Timeout");
   registerNodeType<DelayNode>("Delay");
   registerNodeType<RunOnceNode>("RunOnce");
+  registerNodeType<Preempt>("Preempt");
 
   registerNodeType<ForceSuccessNode>("ForceSuccess");
   registerNodeType<ForceFailureNode>("ForceFailure");

src/control_node.cpp

@@ -21,6 +21,7 @@ ControlNode::ControlNode(const std::string& name, const NodeConfig& config)
 
 void ControlNode::addChild(TreeNode* child)
 {
+  child->setParent(this);
   children_nodes_.push_back(child);
 }
 

src/decorator_node.cpp

@@ -26,6 +26,7 @@ void DecoratorNode::setChild(TreeNode* child)
     throw BehaviorTreeException("Decorator [", name(), "] has already a child assigned");
   }
 
+  child->setParent(this);
   child_node_ = child;
 }
 

src/decorators/preempt.cpp

@@ -0,0 +1,57 @@
+/* Copyright (C) 2025 Davide Faconti -  All Rights Reserved
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy of
+ *   this software and associated documentation files (the "Software"), to deal in
+ *   the Software without restriction, including without limitation the rights to
+ *   use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ *   the Software, and to permit persons to whom the Software is furnished to do so,
+ *   subject to the following conditions:
+ *
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ *   FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ *   COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ *   IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *   CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "behaviortree_cpp/decorators/preempt.h"
+
+#include "behaviortree_cpp/controls/reactive_control_node.h"
+
+namespace BT
+{
+NodeStatus Preempt::tick()
+{
+  // A new branch starts now: let go of whatever the reactive ancestors are
+  // still running before the child below us begins. status() is IDLE again
+  // whenever a parent resets a completed node, so this also covers a second
+  // preemption after the child returned SUCCESS or FAILURE.
+  if(status() == NodeStatus::IDLE)
+  {
+    haltPreviousBranch();
+  }
+  return child()->executeTick();
+}
+
+void Preempt::haltPreviousBranch()
+{
+  // Walk up the tree keeping track of the child we came from: a reactive
+  // ancestor is only asked to let go of the children that are not on our path,
+  // so it never halts the branch we are about to start.
+  TreeNode* child_on_path = this;
+
+  for(TreeNode* ancestor = parent(); ancestor != nullptr; ancestor = ancestor->parent())
+  {
+    if(auto* reactive = dynamic_cast<ReactiveControlNode*>(ancestor))
+    {
+      reactive->haltRunningChild(child_on_path);
+    }
+    child_on_path = ancestor;
+  }
+}
+
+}  // namespace BT

src/tree_node.cpp

@@ -51,6 +51,8 @@ struct TreeNode::PImpl
 
   std::array<ScriptFunction, size_t(PreCond::COUNT_)> pre_parsed;
   std::array<ScriptFunction, size_t(PostCond::COUNT_)> post_parsed;
+
+  TreeNode* parent = nullptr;
 };
 
 TreeNode::TreeNode(std::string name, NodeConfig config)
@@ -431,6 +433,16 @@ uint16_t TreeNode::UID() const
   return _p->config.uid;
 }
 
+TreeNode* TreeNode::parent() const
+{
+  return _p->parent;
+}
+
+void TreeNode::setParent(TreeNode* parent)
+{
+  _p->parent = parent;
+}
+
 const std::string& TreeNode::fullPath() const
 {
   return _p->config.path;

tests/CMakeLists.txt

@@ -42,6 +42,7 @@ set(BT_TESTS
   gtest_loop.cpp
   gtest_reactive.cpp
   gtest_reactive_backchaining.cpp
+  gtest_preempt.cpp
   gtest_sequence.cpp
   gtest_skipping.cpp
   gtest_substitution.cpp