From 3a073f0acbfcf29898c060280fa824a3ebbf9486 Mon Sep 17 00:00:00 2001 From: Quchaosheng Date: Tue, 29 Sep 2026 16:49:55 +0800 Subject: [PATCH 1/9] Halt the running branch before starting another asynchronous action ReactiveSequence and ReactiveFallback halt the siblings of a child only after that child returned RUNNING. When the new child starts an asynchronous action in its onStart(), the action of the branch that is about to be preempted is still alive, so for a short time two actions run concurrently. With the ROS action wrappers that means that a new goal is sent before the previous one is cancelled, which is the race described in issue #1031. Let an asynchronous node stop the branches that are still RUNNING right before it transitions out of IDLE, walking up to the root. Parallel is the only control node designed to keep several children running at the same time, so it stops the walk. To know the ancestors, every node now stores the parent that the parser sets when the children are added to the tree. --- .../controls/parallel_all_node.h | 5 + .../behaviortree_cpp/controls/parallel_node.h | 5 + include/behaviortree_cpp/tree_node.h | 29 +++++ src/action_node.cpp | 11 ++ src/control_node.cpp | 1 + src/decorator_node.cpp | 1 + src/tree_node.cpp | 43 +++++++ tests/gtest_reactive_backchaining.cpp | 119 ++++++++++++++++++ 8 files changed, 214 insertions(+) diff --git a/include/behaviortree_cpp/controls/parallel_all_node.h b/include/behaviortree_cpp/controls/parallel_all_node.h index 4deff7050..a998bb342 100644 --- a/include/behaviortree_cpp/controls/parallel_all_node.h +++ b/include/behaviortree_cpp/controls/parallel_all_node.h @@ -36,6 +36,11 @@ class ParallelAllNode : public ControlNode public: ParallelAllNode(const std::string& name, const NodeConfig& config); + [[nodiscard]] bool allowsConcurrentChildren() const override + { + return true; + } + static PortsList providedPorts() { return { InputPort("max_failures", 1, diff --git a/include/behaviortree_cpp/controls/parallel_node.h b/include/behaviortree_cpp/controls/parallel_node.h index 063f68954..b6a4bd0a3 100644 --- a/include/behaviortree_cpp/controls/parallel_node.h +++ b/include/behaviortree_cpp/controls/parallel_node.h @@ -64,6 +64,11 @@ class ParallelNode : public ControlNode virtual void halt() override; + [[nodiscard]] bool allowsConcurrentChildren() const override + { + return true; + } + size_t successThreshold() const; size_t failureThreshold() const; void setSuccessThreshold(int threshold); diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 9fe48568d..eb9cecd20 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -193,6 +193,21 @@ 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; + + /// Set by the parent when the node is added to the tree. + void setParent(TreeNode* parent); + + /** Control nodes that are meant to keep several children RUNNING at the same + * time (only Parallel) override this to true, so that the branches below + * them are not halted when an asynchronous node is (re)started. + */ + [[nodiscard]] virtual bool allowsConcurrentChildren() const + { + return false; + } + using StatusChangeSignal = Signal; using StatusChangeSubscriber = StatusChangeSignal::Subscriber; using StatusChangeCallback = StatusChangeSignal::CallableFunction; @@ -412,6 +427,20 @@ class TreeNode /// Set the status to IDLE void resetStatus(); + /** Halt the siblings that are still RUNNING while this node is about to + * start, walking up the tree to the root. + * + * Reactive control nodes such as ReactiveSequence and ReactiveFallback are + * only able to halt the siblings of the child that returned RUNNING, and + * they do so *after* that child has been ticked. An asynchronous node in a + * different branch of the tree is therefore still RUNNING when the new + * action starts, which is the race described in issue #1031. + * + * Asynchronous nodes call this method when they transition from IDLE, so + * that the previously running branch is halted before onStart(). + */ + void haltRunningSiblings(); + // Only BehaviorTreeFactory should call this void setRegistrationID(StringView ID); diff --git a/src/action_node.cpp b/src/action_node.cpp index 77f1f440b..bd420d8f3 100644 --- a/src/action_node.cpp +++ b/src/action_node.cpp @@ -109,6 +109,10 @@ NodeStatus CoroActionNode::executeTick() // create a new coroutine, if necessary if(_p->coro == nullptr) { + // The previous branch must be stopped before this action starts, otherwise + // both would be running at the same time (see issue #1031). + haltRunningSiblings(); + // First initialize a `desc` object through `mco_desc_init`. _p->desc = mco_desc_init(CoroEntry, 0); _p->desc.user_data = this; @@ -169,6 +173,9 @@ NodeStatus StatefulActionNode::tick() if(prev_status == NodeStatus::IDLE) { + // The previous branch must be stopped before this action starts, otherwise + // both would be running at the same time (see issue #1031). + haltRunningSiblings(); const NodeStatus new_status = onStart(); if(new_status == NodeStatus::IDLE) { @@ -206,6 +213,10 @@ NodeStatus BT::ThreadedAction::executeTick() // The other thread is in charge for changing the status if(status() == NodeStatus::IDLE) { + // The previous branch must be stopped before this action starts, otherwise + // both would be running at the same time (see issue #1031). + haltRunningSiblings(); + setStatus(NodeStatus::RUNNING); halt_requested_ = false; thread_handle_ = std::async(std::launch::async, [this]() { diff --git a/src/control_node.cpp b/src/control_node.cpp index 535165376..65c3f899b 100644 --- a/src/control_node.cpp +++ b/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); } diff --git a/src/decorator_node.cpp b/src/decorator_node.cpp index 993ef2719..08e9201a8 100644 --- a/src/decorator_node.cpp +++ b/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; } diff --git a/src/tree_node.cpp b/src/tree_node.cpp index e43f19706..c77cb4d22 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -13,6 +13,8 @@ #include "behaviortree_cpp/tree_node.h" +#include "behaviortree_cpp/control_node.h" + #include #include #include @@ -51,6 +53,8 @@ struct TreeNode::PImpl std::array pre_parsed; std::array post_parsed; + + TreeNode* parent = nullptr; }; TreeNode::TreeNode(std::string name, NodeConfig config) @@ -353,6 +357,45 @@ uint16_t TreeNode::UID() const return _p->config.uid; } +TreeNode* TreeNode::parent() const +{ + return _p->parent; +} + +void TreeNode::setParent(TreeNode* parent) +{ + _p->parent = parent; +} + +void TreeNode::haltRunningSiblings() +{ + TreeNode* child = this; + + for(TreeNode* ancestor = _p->parent; ancestor != nullptr; ancestor = ancestor->parent()) + { + // A Parallel node is the only one allowed to have several children RUNNING + // at the same time, so its other branches must be left untouched. + if(ancestor->allowsConcurrentChildren()) + { + return; + } + + if(auto* control = dynamic_cast(ancestor)) + { + const auto& children = control->children(); + for(size_t i = 0; i < children.size(); i++) + { + if(children[i] != child && children[i]->status() == NodeStatus::RUNNING) + { + control->haltChild(i); + } + } + } + + child = ancestor; + } +} + const std::string& TreeNode::fullPath() const { return _p->config.path; diff --git a/tests/gtest_reactive_backchaining.cpp b/tests/gtest_reactive_backchaining.cpp index 5aed62b52..5549018d3 100644 --- a/tests/gtest_reactive_backchaining.cpp +++ b/tests/gtest_reactive_backchaining.cpp @@ -1,6 +1,9 @@ #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/loggers/bt_observer.h" +#include +#include + #include namespace BT::test @@ -62,6 +65,69 @@ class AsyncTestAction : public BT::StatefulActionNode void onHalted() override {} }; + +// Records when asynchronous actions are started and halted, and keeps track of +// how many of them are alive at the same time. Used to check that a branch is +// stopped *before* the next one is started (issue #1031). +struct AsyncActionTracker +{ + std::vector events; + int running_actions = 0; + int max_running_actions = 0; + int started_while_another_was_running = 0; + + bool hasEvent(const std::string& event) const + { + return std::find(events.begin(), events.end(), event) != events.end(); + } + + size_t indexOf(const std::string& event) const + { + const auto it = std::find(events.begin(), events.end(), event); + return it == events.end() ? events.size() : size_t(std::distance(events.begin(), it)); + } +}; + +class TrackedAsyncAction : public BT::StatefulActionNode +{ +public: + TrackedAsyncAction(const std::string& name, const BT::NodeConfig& config, + AsyncActionTracker* tracker) + : BT::StatefulActionNode(name, config), tracker_(tracker) + {} + + static BT::PortsList providedPorts() + { + return {}; + } + + NodeStatus onStart() override + { + if(tracker_->running_actions > 0) + { + tracker_->started_while_another_was_running++; + } + tracker_->events.push_back(name() + ":start"); + tracker_->running_actions++; + tracker_->max_running_actions = + (std::max)(tracker_->max_running_actions, tracker_->running_actions); + return NodeStatus::RUNNING; + } + + NodeStatus onRunning() override + { + return NodeStatus::RUNNING; + } + + void onHalted() override + { + tracker_->events.push_back(name() + ":halted"); + tracker_->running_actions--; + } + +private: + AsyncActionTracker* tracker_; +}; //-------------------------- TEST(ReactiveBackchaining, EnsureWarm) @@ -189,4 +255,57 @@ TEST(ReactiveBackchaining, EnsureWarmWithEnsureHoldingHacket) EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::SUCCESS); } +// When the second branch of a reactive fallback takes over, the asynchronous +// action of the first branch must be halted before the new one is started. +// Otherwise both would be alive at the same time and, for instance, two ROS +// actions would run concurrently (issue #1031). +TEST(ReactiveBackchaining, HaltBranchBeforeStartingAnotherAsyncAction) +{ + static const char* xml_text = R"( + + + + + + + + + + + + )"; + + AsyncActionTracker tracker; + + BehaviorTreeFactory factory; + factory.registerNodeType("IsBatteryOk", "battery_ok"); + factory.registerBuilder( + "GotoCharging", [&tracker](const std::string& name, const BT::NodeConfig& config) { + return std::make_unique(name, config, &tracker); + }); + factory.registerBuilder( + "GotoKitchen", [&tracker](const std::string& name, const BT::NodeConfig& config) { + return std::make_unique(name, config, &tracker); + }); + + auto tree = factory.createTreeFromText(xml_text); + auto blackboard = tree.subtrees.front()->blackboard; + blackboard->set("battery_ok", true); + + // The battery is still fine, so the robot goes to the kitchen. + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + EXPECT_TRUE(tracker.hasEvent("GotoKitchen:start")); + EXPECT_EQ(tracker.running_actions, 1); + + // The battery runs low while the robot is still moving: the goal must be + // cancelled before the charging action is started. + blackboard->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + + EXPECT_EQ(tracker.started_while_another_was_running, 0); + EXPECT_EQ(tracker.max_running_actions, 1); + EXPECT_EQ(tracker.running_actions, 1); + EXPECT_LT(tracker.indexOf("GotoKitchen:halted"), tracker.indexOf("GotoCharging:start")); +} + } // namespace BT::test From 5d7c6c3fa7a175f44404b064d4b485d2a87916d3 Mon Sep 17 00:00:00 2001 From: Chaosheng QU <203742003+Quchaosheng@users.noreply.github.com> Date: Wed, 28 Oct 2026 10:51:31 +0800 Subject: [PATCH 2/9] Let the tree author mark where a new branch starts The previous version halted the running siblings from inside the asynchronous node, whenever it left IDLE. That cannot be made to work: before onStart() runs, a check that will return SUCCESS and a new action that will keep running look identical, so the rule has to guess from the shape of the tree. It also misses ThreadedAction, whose executeTick() is final and does not run the body of TreeNode::executeTick(). Replace it with a decorator that answers the question where the answer is known. Preempt wraps the action that starts the new branch; on its first tick, before ticking its child, it halts the RUNNING child of every reactive ancestor it is not itself inside. Children of a Parallel are not touched, since that node is allowed to keep several of them RUNNING. ReactiveSequence and ReactiveFallback now share a ReactiveControlNode base that owns the running child index and can let go of it on request, so the index stays consistent with the child that is actually RUNNING. The base adds state and non-virtual members only: no existing vtable changes, and the parent pointer lives in the existing PImpl. Tests: both fallbacks, an action inside a SubTree, a ThreadedAction, and a Parallel inside the new branch whose children must keep running together. The three cases pinned by #1258 pass, as does the rest of the suite (591 passed, 2 skipped). Suggested-by: Davide Faconti --- CMakeLists.txt | 1 + include/behaviortree_cpp/behavior_tree.h | 1 + .../controls/parallel_all_node.h | 5 - .../behaviortree_cpp/controls/parallel_node.h | 5 - .../controls/reactive_control_node.h | 69 ++++ .../controls/reactive_fallback.h | 7 +- .../controls/reactive_sequence.h | 8 +- include/behaviortree_cpp/decorators/preempt.h | 85 ++++ include/behaviortree_cpp/tree_node.h | 23 -- src/action_node.cpp | 9 - src/bt_factory.cpp | 1 + src/decorators/preempt.cpp | 66 +++ src/tree_node.cpp | 28 -- tests/CMakeLists.txt | 1 + tests/gtest_preempt.cpp | 379 ++++++++++++++++++ tests/gtest_reactive_backchaining.cpp | 8 +- 16 files changed, 615 insertions(+), 81 deletions(-) create mode 100644 include/behaviortree_cpp/controls/reactive_control_node.h create mode 100644 include/behaviortree_cpp/decorators/preempt.h create mode 100644 src/decorators/preempt.cpp create mode 100644 tests/gtest_preempt.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f82c6bf52..0aac41666 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index e7373d4dc..cb8599226 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -42,6 +42,7 @@ #include "behaviortree_cpp/decorators/keep_running_until_failure_node.h" #include "behaviortree_cpp/decorators/loop_node.h" #include "behaviortree_cpp/decorators/repeat_node.h" +#include "behaviortree_cpp/decorators/preempt.h" #include "behaviortree_cpp/decorators/retry_node.h" #include "behaviortree_cpp/decorators/run_once_node.h" #include "behaviortree_cpp/decorators/script_precondition.h" diff --git a/include/behaviortree_cpp/controls/parallel_all_node.h b/include/behaviortree_cpp/controls/parallel_all_node.h index a998bb342..4deff7050 100644 --- a/include/behaviortree_cpp/controls/parallel_all_node.h +++ b/include/behaviortree_cpp/controls/parallel_all_node.h @@ -36,11 +36,6 @@ class ParallelAllNode : public ControlNode public: ParallelAllNode(const std::string& name, const NodeConfig& config); - [[nodiscard]] bool allowsConcurrentChildren() const override - { - return true; - } - static PortsList providedPorts() { return { InputPort("max_failures", 1, diff --git a/include/behaviortree_cpp/controls/parallel_node.h b/include/behaviortree_cpp/controls/parallel_node.h index b6a4bd0a3..063f68954 100644 --- a/include/behaviortree_cpp/controls/parallel_node.h +++ b/include/behaviortree_cpp/controls/parallel_node.h @@ -64,11 +64,6 @@ class ParallelNode : public ControlNode virtual void halt() override; - [[nodiscard]] bool allowsConcurrentChildren() const override - { - return true; - } - size_t successThreshold() const; size_t failureThreshold() const; void setSuccessThreshold(int threshold); diff --git a/include/behaviortree_cpp/controls/reactive_control_node.h b/include/behaviortree_cpp/controls/reactive_control_node.h new file mode 100644 index 000000000..b94b593c2 --- /dev/null +++ b/include/behaviortree_cpp/controls/reactive_control_node.h @@ -0,0 +1,69 @@ +/* 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 its + * running child without knowing which of the two it is, and so that the index stays + * consistent with the child that is actually RUNNING. + * + * Deliberately not virtual: the class adds state and two non-virtual accessors 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) + {} + + /// Index of the child that returned RUNNING, or -1 if there is none. + [[nodiscard]] int runningChild() const + { + return running_child_; + } + + /** Halt the child that is currently RUNNING, if any, and forget it. + * + * Used by Preempt: the reactive ancestor cannot halt that child on its own, + * because from its point of view the child is still the one it has to tick. + */ + void haltRunningChild() + { + if(running_child_ >= 0 && static_cast(running_child_) < childrenCount()) + { + haltChild(static_cast(running_child_)); + } + running_child_ = -1; + } + +protected: + int running_child_ = -1; +}; + +} // namespace BT diff --git a/include/behaviortree_cpp/controls/reactive_fallback.h b/include/behaviortree_cpp/controls/reactive_fallback.h index ceea54f9f..2ecca12d3 100644 --- a/include/behaviortree_cpp/controls/reactive_fallback.h +++ b/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; }; diff --git a/include/behaviortree_cpp/controls/reactive_sequence.h b/include/behaviortree_cpp/controls/reactive_sequence.h index eba0f41ff..e5d9d8771 100644 --- a/include/behaviortree_cpp/controls/reactive_sequence.h +++ b/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; }; diff --git a/include/behaviortree_cpp/decorators/preempt.h b/include/behaviortree_cpp/decorators/preempt.h new file mode 100644 index 000000000..c568dd731 --- /dev/null +++ b/include/behaviortree_cpp/decorators/preempt.h @@ -0,0 +1,85 @@ +/* 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. + * + * + * + * + * + * + * + * + * + * On its first tick, 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. + * + * The name is a placeholder. + */ +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 halt() override; + + void haltPreviousBranch(); + + bool started_ = false; +}; + +} // namespace BT diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index f966b5d9d..025ca896f 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -199,15 +199,6 @@ class TreeNode /// Set by the parent when the node is added to the tree. void setParent(TreeNode* parent); - /** Control nodes that are meant to keep several children RUNNING at the same - * time (only Parallel) override this to true, so that the branches below - * them are not halted when an asynchronous node is (re)started. - */ - [[nodiscard]] virtual bool allowsConcurrentChildren() const - { - return false; - } - using StatusChangeSignal = Signal; using StatusChangeSubscriber = StatusChangeSignal::Subscriber; using StatusChangeCallback = StatusChangeSignal::CallableFunction; @@ -442,20 +433,6 @@ class TreeNode /// Set the status to IDLE void resetStatus(); - /** Halt the siblings that are still RUNNING while this node is about to - * start, walking up the tree to the root. - * - * Reactive control nodes such as ReactiveSequence and ReactiveFallback are - * only able to halt the siblings of the child that returned RUNNING, and - * they do so *after* that child has been ticked. An asynchronous node in a - * different branch of the tree is therefore still RUNNING when the new - * action starts, which is the race described in issue #1031. - * - * Asynchronous nodes call this method when they transition from IDLE, so - * that the previously running branch is halted before onStart(). - */ - void haltRunningSiblings(); - // Only BehaviorTreeFactory should call this void setRegistrationID(StringView ID); diff --git a/src/action_node.cpp b/src/action_node.cpp index f525891d7..2ee6871a0 100644 --- a/src/action_node.cpp +++ b/src/action_node.cpp @@ -114,9 +114,6 @@ NodeStatus CoroActionNode::executeTick() // create a new coroutine, if necessary if(_p->coro == nullptr) { - // The previous branch must be stopped before this action starts, otherwise - // both would be running at the same time (see issue #1031). - haltRunningSiblings(); // First initialize a `desc` object through `mco_desc_init`. _p->desc = mco_desc_init(CoroEntry, 0); @@ -178,9 +175,6 @@ NodeStatus StatefulActionNode::tick() if(prev_status == NodeStatus::IDLE) { - // The previous branch must be stopped before this action starts, otherwise - // both would be running at the same time (see issue #1031). - haltRunningSiblings(); const NodeStatus new_status = onStart(); if(new_status == NodeStatus::IDLE) { @@ -218,9 +212,6 @@ NodeStatus BT::ThreadedAction::executeTick() // The other thread is in charge for changing the status if(status() == NodeStatus::IDLE) { - // The previous branch must be stopped before this action starts, otherwise - // both would be running at the same time (see issue #1031). - haltRunningSiblings(); setStatus(NodeStatus::RUNNING); halt_requested_ = false; diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 67ff3ea67..3958a54ce 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -143,6 +143,7 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl) registerNodeType("Timeout"); registerNodeType("Delay"); registerNodeType("RunOnce"); + registerNodeType("Preempt"); registerNodeType("ForceSuccess"); registerNodeType("ForceFailure"); diff --git a/src/decorators/preempt.cpp b/src/decorators/preempt.cpp new file mode 100644 index 000000000..73aca325f --- /dev/null +++ b/src/decorators/preempt.cpp @@ -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. + */ + +#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 were + // still running before the child below us begins. + if(!started_) + { + started_ = true; + haltPreviousBranch(); + } + return child()->executeTick(); +} + +void Preempt::halt() +{ + started_ = false; + DecoratorNode::halt(); +} + +void Preempt::haltPreviousBranch() +{ + // Walk up the tree keeping track of the child we came from: a reactive + // ancestor may only be told to let go when the child it has RUNNING is not + // the one we are inside. If it were, halting it would halt ourselves. + TreeNode* child_on_path = this; + + for(TreeNode* ancestor = parent(); ancestor != nullptr; ancestor = ancestor->parent()) + { + if(auto* reactive = dynamic_cast(ancestor)) + { + const int running = reactive->runningChild(); + if(running >= 0 && reactive->child(static_cast(running)) != child_on_path) + { + reactive->haltRunningChild(); + } + } + child_on_path = ancestor; + } +} + +} // namespace BT diff --git a/src/tree_node.cpp b/src/tree_node.cpp index 28c12e68f..f9327a767 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -421,34 +421,6 @@ void TreeNode::setParent(TreeNode* parent) _p->parent = parent; } -void TreeNode::haltRunningSiblings() -{ - TreeNode* child = this; - - for(TreeNode* ancestor = _p->parent; ancestor != nullptr; ancestor = ancestor->parent()) - { - // A Parallel node is the only one allowed to have several children RUNNING - // at the same time, so its other branches must be left untouched. - if(ancestor->allowsConcurrentChildren()) - { - return; - } - - if(auto* control = dynamic_cast(ancestor)) - { - const auto& children = control->children(); - for(size_t i = 0; i < children.size(); i++) - { - if(children[i] != child && children[i]->status() == NodeStatus::RUNNING) - { - control->haltChild(i); - } - } - } - - child = ancestor; - } -} const std::string& TreeNode::fullPath() const { diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ad39d7879..ba92e8dbf 100644 --- a/tests/CMakeLists.txt +++ b/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 diff --git a/tests/gtest_preempt.cpp b/tests/gtest_preempt.cpp new file mode 100644 index 000000000..bc6ef52ea --- /dev/null +++ b/tests/gtest_preempt.cpp @@ -0,0 +1,379 @@ +#include "behaviortree_cpp/bt_factory.h" + +#include +#include +#include +#include +#include +#include + +#include + +namespace BT::test +{ + +struct Tracker +{ + std::vector events; + std::atomic running{ 0 }; + std::atomic max_running{ 0 }; + std::atomic started_while_running{ 0 }; + + void onStart(const std::string& name) + { + if(running.load() > 0) + { + started_while_running++; + } + events.push_back(name + ":start"); + const int now = ++running; + int previous = max_running.load(); + while(now > previous && !max_running.compare_exchange_weak(previous, now)) + { + } + } + + void onHalted(const std::string& name) + { + events.push_back(name + ":halted"); + running--; + } + + bool hasEvent(const std::string& event) const + { + return std::find(events.begin(), events.end(), event) != events.end(); + } + + [[nodiscard]] size_t indexOf(const std::string& event) const + { + const auto it = std::find(events.begin(), events.end(), event); + return it == events.end() ? events.size() : size_t(std::distance(events.begin(), it)); + } + + [[nodiscard]] std::string dump() const + { + std::string out; + for(const auto& e : events) + { + out += e + " "; + } + return out; + } +}; + +class ForeverAction : public BT::StatefulActionNode +{ +public: + ForeverAction(const std::string& name, const BT::NodeConfig& config, Tracker* tracker) + : BT::StatefulActionNode(name, config), tracker_(tracker) + {} + + static BT::PortsList providedPorts() + { + return {}; + } + + NodeStatus onStart() override + { + tracker_->onStart(name()); + return NodeStatus::RUNNING; + } + + NodeStatus onRunning() override + { + return NodeStatus::RUNNING; + } + + void onHalted() override + { + tracker_->onHalted(name()); + } + +private: + Tracker* tracker_; +}; + +class ForeverThreadedAction : public BT::ThreadedAction +{ +public: + ForeverThreadedAction(const std::string& name, const BT::NodeConfig& config, + Tracker* tracker) + : BT::ThreadedAction(name, config), tracker_(tracker) + {} + + static BT::PortsList providedPorts() + { + return {}; + } + +protected: + NodeStatus tick() override + { + tracker_->onStart(name()); + while(!isHaltRequested()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + tracker_->onHalted(name()); + return NodeStatus::FAILURE; + } + +private: + Tracker* tracker_; +}; + +class FlagCondition : public BT::ConditionNode +{ +public: + FlagCondition(const std::string& name, const BT::NodeConfig& config, std::string key) + : BT::ConditionNode(name, config), key_(std::move(key)) + {} + + static BT::PortsList providedPorts() + { + return {}; + } + + NodeStatus tick() override + { + return config().blackboard->get(key_) ? NodeStatus::SUCCESS : + NodeStatus::FAILURE; + } + +private: + std::string key_; +}; + +static void registerNodes(BehaviorTreeFactory& factory, Tracker& tracker) +{ + factory.registerNodeType("BatteryOk", "battery_ok"); + factory.registerBuilder( + "Forever", [&tracker](const std::string& name, const NodeConfig& config) { + return std::make_unique(name, config, &tracker); + }); + factory.registerBuilder( + "ForeverThreaded", [&tracker](const std::string& name, const NodeConfig& config) { + return std::make_unique(name, config, &tracker); + }); +} + +template +static bool waitFor(Predicate predicate) +{ + for(int i = 0; i < 200; i++) + { + if(predicate()) + { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + return predicate(); +} + +TEST(Preempt, plainFallbackStopsTheOldBranchBeforeStartingTheNewOne) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + Tracker tracker; + registerNodes(factory, tracker); + auto tree = factory.createTreeFromText(xml_text); + + auto bb = tree.subtrees.front()->blackboard; + bb->set("battery_ok", true); + + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_TRUE(tracker.hasEvent("GotoKitchen:start")) << tracker.dump(); + EXPECT_EQ(tracker.running.load(), 1); + + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + + EXPECT_EQ(tracker.started_while_running.load(), 0) << tracker.dump(); + EXPECT_LT(tracker.indexOf("GotoKitchen:halted"), tracker.indexOf("GotoCharging:start")) + << tracker.dump(); + EXPECT_EQ(tracker.max_running.load(), 1); + EXPECT_EQ(tracker.running.load(), 1); +} + +TEST(Preempt, reactiveFallbackStopsTheOldBranchBeforeStartingTheNewOne) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + Tracker tracker; + registerNodes(factory, tracker); + auto tree = factory.createTreeFromText(xml_text); + + auto bb = tree.subtrees.front()->blackboard; + bb->set("battery_ok", true); + + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_TRUE(tracker.hasEvent("GotoKitchen:start")) << tracker.dump(); + + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + + EXPECT_EQ(tracker.started_while_running.load(), 0) << tracker.dump(); + EXPECT_LT(tracker.indexOf("GotoKitchen:halted"), tracker.indexOf("GotoCharging:start")) + << tracker.dump(); + EXPECT_EQ( + std::count(tracker.events.begin(), tracker.events.end(), "GotoKitchen:halted"), 1) + << tracker.dump(); +} + +TEST(Preempt, actionInsideSubTree) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + Tracker tracker; + registerNodes(factory, tracker); + auto tree = factory.createTreeFromText(xml_text); + + auto bb = tree.subtrees.front()->blackboard; + bb->set("battery_ok", true); + + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_TRUE(tracker.hasEvent("GotoKitchen:start")) << tracker.dump(); + + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + + EXPECT_EQ(tracker.started_while_running.load(), 0) << tracker.dump(); + EXPECT_LT(tracker.indexOf("GotoKitchen:halted"), tracker.indexOf("GotoCharging:start")) + << tracker.dump(); +} + +TEST(Preempt, threadedAction) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + Tracker tracker; + registerNodes(factory, tracker); + auto tree = factory.createTreeFromText(xml_text); + + auto bb = tree.subtrees.front()->blackboard; + bb->set("battery_ok", true); + + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_TRUE(waitFor([&] { return tracker.hasEvent("GotoKitchen:start"); })) + << tracker.dump(); + + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_TRUE(waitFor([&] { return tracker.hasEvent("GotoCharging:start"); })) + << tracker.dump(); + + EXPECT_EQ(tracker.started_while_running.load(), 0) << tracker.dump(); + EXPECT_LT(tracker.indexOf("GotoKitchen:halted"), tracker.indexOf("GotoCharging:start")) + << tracker.dump(); +} + +TEST(Preempt, parallelInsideTheNewBranchKeepsItsChildrenRunning) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + Tracker tracker; + registerNodes(factory, tracker); + auto tree = factory.createTreeFromText(xml_text); + + auto bb = tree.subtrees.front()->blackboard; + bb->set("battery_ok", true); + + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_TRUE(tracker.hasEvent("GotoKitchen:start")) << tracker.dump(); + + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + + EXPECT_LT(tracker.indexOf("GotoKitchen:halted"), tracker.indexOf("GotoCharging:start")) + << tracker.dump(); + EXPECT_LT(tracker.indexOf("GotoKitchen:halted"), tracker.indexOf("OpenDoor:start")) + << tracker.dump(); + EXPECT_EQ(tracker.running.load(), 2) << tracker.dump(); + EXPECT_FALSE(tracker.hasEvent("GotoCharging:halted")) << tracker.dump(); + EXPECT_FALSE(tracker.hasEvent("OpenDoor:halted")) << tracker.dump(); +} + +} // namespace BT::test diff --git a/tests/gtest_reactive_backchaining.cpp b/tests/gtest_reactive_backchaining.cpp index 5549018d3..101562f43 100644 --- a/tests/gtest_reactive_backchaining.cpp +++ b/tests/gtest_reactive_backchaining.cpp @@ -267,7 +267,9 @@ TEST(ReactiveBackchaining, HaltBranchBeforeStartingAnotherAsyncAction) - + + + @@ -298,7 +300,9 @@ TEST(ReactiveBackchaining, HaltBranchBeforeStartingAnotherAsyncAction) EXPECT_EQ(tracker.running_actions, 1); // The battery runs low while the robot is still moving: the goal must be - // cancelled before the charging action is started. + // cancelled before the charging action is started. That is what the Preempt + // decorator marks: without it the fallback and the reactive sequence above it + // cannot tell this switch from a check being re-ticked. blackboard->set("battery_ok", false); EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); From b23139907db2fd61743c02b9ab7f537c3d48c503 Mon Sep 17 00:00:00 2001 From: Chaosheng QU <203742003+Quchaosheng@users.noreply.github.com> Date: Wed, 28 Oct 2026 14:38:44 +0800 Subject: [PATCH 3/9] Make the preempt test fixture thread-safe, and format Two separate CI failures. The tsan job fails on Preempt.threadedAction, and the race is in the new test rather than in the library: Tracker::events is written by the ThreadedAction worker thread from onStart/onHalted while waitFor() polls hasEvent() on the main thread, so the vector can be reallocated while it is being read. Tracker now guards the vector with a mutex and exposes count(), so the fixture no longer reaches into the list directly. Reproduced on the previous commit with setarch -R: 3/3 runs report the race and exit 66. After the change, 5/5 runs are clean and the whole Preempt and ReactiveBackchaining suites pass under tsan. The clang-format failure is unrelated: it reorders the preempt.h include and drops three blank lines. No behaviour change. --- include/behaviortree_cpp/behavior_tree.h | 2 +- src/action_node.cpp | 2 -- src/tree_node.cpp | 1 - tests/gtest_preempt.cpp | 46 ++++++++++++++++-------- 4 files changed, 33 insertions(+), 18 deletions(-) diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index cb8599226..52e83ab23 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -41,8 +41,8 @@ #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/repeat_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/decorators/script_precondition.h" diff --git a/src/action_node.cpp b/src/action_node.cpp index 2ee6871a0..8e47ad940 100644 --- a/src/action_node.cpp +++ b/src/action_node.cpp @@ -114,7 +114,6 @@ NodeStatus CoroActionNode::executeTick() // create a new coroutine, if necessary if(_p->coro == nullptr) { - // First initialize a `desc` object through `mco_desc_init`. _p->desc = mco_desc_init(CoroEntry, 0); _p->desc.user_data = this; @@ -212,7 +211,6 @@ NodeStatus BT::ThreadedAction::executeTick() // The other thread is in charge for changing the status if(status() == NodeStatus::IDLE) { - setStatus(NodeStatus::RUNNING); halt_requested_ = false; thread_handle_ = std::async(std::launch::async, [this]() { diff --git a/src/tree_node.cpp b/src/tree_node.cpp index f9327a767..d8ec91dac 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -421,7 +421,6 @@ void TreeNode::setParent(TreeNode* parent) _p->parent = parent; } - const std::string& TreeNode::fullPath() const { return _p->config.path; diff --git a/tests/gtest_preempt.cpp b/tests/gtest_preempt.cpp index bc6ef52ea..4959e3f1c 100644 --- a/tests/gtest_preempt.cpp +++ b/tests/gtest_preempt.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -14,18 +15,16 @@ namespace BT::test struct Tracker { - std::vector events; - std::atomic running{ 0 }; - std::atomic max_running{ 0 }; - std::atomic started_while_running{ 0 }; - void onStart(const std::string& name) { if(running.load() > 0) { started_while_running++; } - events.push_back(name + ":start"); + { + std::lock_guard lock(mutex_); + events_.push_back(name + ":start"); + } const int now = ++running; int previous = max_running.load(); while(now > previous && !max_running.compare_exchange_weak(previous, now)) @@ -35,30 +34,51 @@ struct Tracker void onHalted(const std::string& name) { - events.push_back(name + ":halted"); + { + std::lock_guard lock(mutex_); + events_.push_back(name + ":halted"); + } running--; } bool hasEvent(const std::string& event) const { - return std::find(events.begin(), events.end(), event) != events.end(); + std::lock_guard lock(mutex_); + return std::find(events_.begin(), events_.end(), event) != events_.end(); } [[nodiscard]] size_t indexOf(const std::string& event) const { - const auto it = std::find(events.begin(), events.end(), event); - return it == events.end() ? events.size() : size_t(std::distance(events.begin(), it)); + std::lock_guard lock(mutex_); + const auto it = std::find(events_.begin(), events_.end(), event); + return it == events_.end() ? events_.size() : + size_t(std::distance(events_.begin(), it)); + } + + [[nodiscard]] size_t count(const std::string& event) const + { + std::lock_guard lock(mutex_); + return size_t(std::count(events_.begin(), events_.end(), event)); } [[nodiscard]] std::string dump() const { + std::lock_guard lock(mutex_); std::string out; - for(const auto& e : events) + for(const auto& e : events_) { out += e + " "; } return out; } + + std::atomic running{ 0 }; + std::atomic max_running{ 0 }; + std::atomic started_while_running{ 0 }; + +private: + mutable std::mutex mutex_; + std::vector events_; }; class ForeverAction : public BT::StatefulActionNode @@ -246,9 +266,7 @@ TEST(Preempt, reactiveFallbackStopsTheOldBranchBeforeStartingTheNewOne) EXPECT_EQ(tracker.started_while_running.load(), 0) << tracker.dump(); EXPECT_LT(tracker.indexOf("GotoKitchen:halted"), tracker.indexOf("GotoCharging:start")) << tracker.dump(); - EXPECT_EQ( - std::count(tracker.events.begin(), tracker.events.end(), "GotoKitchen:halted"), 1) - << tracker.dump(); + EXPECT_EQ(tracker.count("GotoKitchen:halted"), 1) << tracker.dump(); } TEST(Preempt, actionInsideSubTree) From afbef5a4329371b0e8fe9e5be853ab10aaac1096 Mon Sep 17 00:00:00 2001 From: Chaosheng QU <203742003+Quchaosheng@users.noreply.github.com> Date: Wed, 11 Nov 2026 10:34:56 +0800 Subject: [PATCH 4/9] Let the preemption happen more than once Two defects made Preempt preempt only the first time, both reported in review. Preempt::started_ was cleared in halt() only, and a parent calls haltNode() only on a RUNNING child: a Preempt that returned SUCCESS or FAILURE is reset to IDLE with the flag still set, so the next activation skipped haltPreviousBranch(). Test status() == IDLE instead, which is true again after every reset, and drop the member. ReactiveControlNode::running_child_ is only written by the ancestor's own tick(), so it means "the first child that ran in this activation" rather than "the child RUNNING now", and it was never cleared on completion. Halting by that index either missed the branch that had to be preempted or halted the wrong one. Ask the children for their status instead and halt every RUNNING child except the one the caller is inside, which cannot go stale. Also from the review: set the index to -1 in haltRunningChild() before halting, in case a halt() throws; make setParent() private to ControlNode/DecoratorNode and drop the now unused include; document Preempt in docs/, including the fact that the new builtin ID can collide with a user node of the same name. The two new tests fail on the previous head and pass now. Full suite: 593 passed, 2 skipped, same two as master. --- docs/preempt.md | 33 ++++ .../controls/reactive_control_node.h | 31 ++-- include/behaviortree_cpp/decorators/preempt.h | 4 - include/behaviortree_cpp/tree_node.h | 6 +- src/decorators/preempt.cpp | 25 +-- src/tree_node.cpp | 2 - tests/gtest_preempt.cpp | 168 +++++++++++++++++- 7 files changed, 222 insertions(+), 47 deletions(-) create mode 100644 docs/preempt.md diff --git a/docs/preempt.md b/docs/preempt.md new file mode 100644 index 000000000..086379588 --- /dev/null +++ b/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 + + + + + + + + + +``` + +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. diff --git a/include/behaviortree_cpp/controls/reactive_control_node.h b/include/behaviortree_cpp/controls/reactive_control_node.h index b94b593c2..b5f20dd71 100644 --- a/include/behaviortree_cpp/controls/reactive_control_node.h +++ b/include/behaviortree_cpp/controls/reactive_control_node.h @@ -28,11 +28,10 @@ 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 its - * running child without knowing which of the two it is, and so that the index stays - * consistent with the child that is actually RUNNING. + * 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 two non-virtual accessors only, + * 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 @@ -42,24 +41,22 @@ class ReactiveControlNode : public ControlNode : ControlNode(name, config) {} - /// Index of the child that returned RUNNING, or -1 if there is none. - [[nodiscard]] int runningChild() const - { - return running_child_; - } - - /** Halt the child that is currently RUNNING, if any, and forget it. + /** Halt every child that is RUNNING, except the one the caller is inside. * - * Used by Preempt: the reactive ancestor cannot halt that child on its own, - * because from its point of view the child is still the one it has to tick. + * 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() + void haltRunningChild(const TreeNode* excluded) { - if(running_child_ >= 0 && static_cast(running_child_) < childrenCount()) + running_child_ = -1; + for(size_t i = 0; i < childrenCount(); i++) { - haltChild(static_cast(running_child_)); + if(children()[i] != excluded && children()[i]->status() == NodeStatus::RUNNING) + { + haltChild(i); + } } - running_child_ = -1; } protected: diff --git a/include/behaviortree_cpp/decorators/preempt.h b/include/behaviortree_cpp/decorators/preempt.h index c568dd731..c03ac26fa 100644 --- a/include/behaviortree_cpp/decorators/preempt.h +++ b/include/behaviortree_cpp/decorators/preempt.h @@ -75,11 +75,7 @@ class Preempt : public DecoratorNode private: NodeStatus tick() override; - void halt() override; - void haltPreviousBranch(); - - bool started_ = false; }; } // namespace BT diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 025ca896f..5298fbed8 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -196,9 +196,6 @@ class TreeNode /// Parent of this node in the tree, or nullptr for the root of a subtree. [[nodiscard]] TreeNode* parent() const; - /// Set by the parent when the node is added to the tree. - void setParent(TreeNode* parent); - using StatusChangeSignal = Signal; using StatusChangeSubscriber = StatusChangeSignal::Subscriber; using StatusChangeCallback = StatusChangeSignal::CallableFunction; @@ -425,6 +422,9 @@ class TreeNode friend class ControlNode; friend class Tree; + /// Set by ControlNode and DecoratorNode when the node is added to the tree. + void setParent(TreeNode* parent); + [[nodiscard]] NodeConfig& config(); /// Method to be implemented by the user diff --git a/src/decorators/preempt.cpp b/src/decorators/preempt.cpp index 73aca325f..fd83964eb 100644 --- a/src/decorators/preempt.cpp +++ b/src/decorators/preempt.cpp @@ -26,38 +26,29 @@ namespace BT { NodeStatus Preempt::tick() { - // A new branch starts now: let go of whatever the reactive ancestors were - // still running before the child below us begins. - if(!started_) + // 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) { - started_ = true; haltPreviousBranch(); } return child()->executeTick(); } -void Preempt::halt() -{ - started_ = false; - DecoratorNode::halt(); -} - void Preempt::haltPreviousBranch() { // Walk up the tree keeping track of the child we came from: a reactive - // ancestor may only be told to let go when the child it has RUNNING is not - // the one we are inside. If it were, halting it would halt ourselves. + // 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(ancestor)) { - const int running = reactive->runningChild(); - if(running >= 0 && reactive->child(static_cast(running)) != child_on_path) - { - reactive->haltRunningChild(); - } + reactive->haltRunningChild(child_on_path); } child_on_path = ancestor; } diff --git a/src/tree_node.cpp b/src/tree_node.cpp index d8ec91dac..ceeb676f3 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -13,8 +13,6 @@ #include "behaviortree_cpp/tree_node.h" -#include "behaviortree_cpp/control_node.h" - #include #include #include diff --git a/tests/gtest_preempt.cpp b/tests/gtest_preempt.cpp index 4959e3f1c..7dd8f8a34 100644 --- a/tests/gtest_preempt.cpp +++ b/tests/gtest_preempt.cpp @@ -41,18 +41,32 @@ struct Tracker running--; } + /// A node that returns SUCCESS or FAILURE is reset, not halted. + void onCompleted() + { + running--; + } + bool hasEvent(const std::string& event) const { std::lock_guard lock(mutex_); return std::find(events_.begin(), events_.end(), event) != events_.end(); } - [[nodiscard]] size_t indexOf(const std::string& event) const + [[nodiscard]] size_t indexOf(const std::string& event, size_t occurrence = 0) const { std::lock_guard lock(mutex_); - const auto it = std::find(events_.begin(), events_.end(), event); - return it == events_.end() ? events_.size() : - size_t(std::distance(events_.begin(), it)); + auto it = events_.begin(); + while(true) + { + it = std::find(it, events_.end(), event); + if(it == events_.end() || occurrence-- == 0) + { + return it == events_.end() ? events_.size() : + size_t(std::distance(events_.begin(), it)); + } + ++it; + } } [[nodiscard]] size_t count(const std::string& event) const @@ -113,6 +127,40 @@ class ForeverAction : public BT::StatefulActionNode Tracker* tracker_; }; +/// Runs for one tick, then reports SUCCESS, so the branch above it completes. +class FinishingAction : public BT::StatefulActionNode +{ +public: + FinishingAction(const std::string& name, const BT::NodeConfig& config, Tracker* tracker) + : BT::StatefulActionNode(name, config), tracker_(tracker) + {} + + static BT::PortsList providedPorts() + { + return {}; + } + + NodeStatus onStart() override + { + tracker_->onStart(name()); + return NodeStatus::RUNNING; + } + + NodeStatus onRunning() override + { + tracker_->onCompleted(); + return NodeStatus::SUCCESS; + } + + void onHalted() override + { + tracker_->onHalted(name()); + } + +private: + Tracker* tracker_; +}; + class ForeverThreadedAction : public BT::ThreadedAction { public: @@ -175,6 +223,10 @@ static void registerNodes(BehaviorTreeFactory& factory, Tracker& tracker) "ForeverThreaded", [&tracker](const std::string& name, const NodeConfig& config) { return std::make_unique(name, config, &tracker); }); + factory.registerBuilder( + "Finishing", [&tracker](const std::string& name, const NodeConfig& config) { + return std::make_unique(name, config, &tracker); + }); } template @@ -394,4 +446,112 @@ TEST(Preempt, parallelInsideTheNewBranchKeepsItsChildrenRunning) EXPECT_FALSE(tracker.hasEvent("OpenDoor:halted")) << tracker.dump(); } +// The preemption has to work again after the branch that preempted has completed: +// 1. battery low: charging starts, kitchen is halted; +// 2. charging completes, battery is OK, so the kitchen runs again; +// 3. battery low once more: the kitchen must be halted before charging restarts. +TEST(Preempt, preemptsAgainAfterTheBranchCompleted) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + Tracker tracker; + registerNodes(factory, tracker); + auto tree = factory.createTreeFromText(xml_text); + + auto bb = tree.subtrees.front()->blackboard; + bb->set("battery_ok", true); + + // phase 1: the kitchen is running, then the battery goes low + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_TRUE(tracker.hasEvent("GotoKitchen:start")) << tracker.dump(); + + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_TRUE(tracker.hasEvent("GotoCharging:start")) << tracker.dump(); + + // phase 2: charging completes, the battery check passes again, kitchen restarts + for(int i = 0; i < 4 && tracker.count("GotoKitchen:start") < 2; i++) + { + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING) << tracker.dump(); + } + ASSERT_EQ(tracker.count("GotoKitchen:start"), 2) << tracker.dump(); + EXPECT_EQ(tracker.running.load(), 1) << tracker.dump(); + + // phase 3: low again, and the second preemption has to halt the kitchen first + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + + EXPECT_EQ(tracker.started_while_running.load(), 0) << tracker.dump(); + EXPECT_LT(tracker.indexOf("GotoKitchen:halted", 1), + tracker.indexOf("GotoCharging:start", 1)) + << tracker.dump(); + EXPECT_EQ(tracker.max_running.load(), 1) << tracker.dump(); +} + +// A branch that already completed leaves nothing for the reactive ancestor to +// remember: RS[Fallback[Cond, Preempt[A]], B] must still halt B before A restarts. +TEST(Preempt, siblingThatCompletedIsStillPreempted) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + Tracker tracker; + registerNodes(factory, tracker); + auto tree = factory.createTreeFromText(xml_text); + + auto bb = tree.subtrees.front()->blackboard; + bb->set("battery_ok", true); + + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_EQ(tracker.count("GotoKitchen:start"), 1) << tracker.dump(); + + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + ASSERT_EQ(tracker.count("GotoCharging:start"), 1) << tracker.dump(); + + // charging finished, the fallback fails again, the kitchen is back in charge + for(int i = 0; i < 4 && tracker.count("GotoKitchen:start") < 2; i++) + { + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING) << tracker.dump(); + } + ASSERT_EQ(tracker.count("GotoKitchen:start"), 2) << tracker.dump(); + + bb->set("battery_ok", false); + EXPECT_EQ(tree.tickExactlyOnce(), NodeStatus::RUNNING); + + EXPECT_EQ(tracker.started_while_running.load(), 0) << tracker.dump(); + EXPECT_LT(tracker.indexOf("GotoKitchen:halted", 1), + tracker.indexOf("GotoCharging:start", 1)) + << tracker.dump(); +} + } // namespace BT::test From 6fc933da8cbc0dd188e9270a1bacf310cb8cb37c Mon Sep 17 00:00:00 2001 From: Chaosheng QU <203742003+Quchaosheng@users.noreply.github.com> Date: Wed, 11 Nov 2026 10:38:09 +0800 Subject: [PATCH 5/9] Keep setParent() private, as asked in review The friends are already declared on TreeNode, so no other class needs it. --- include/behaviortree_cpp/tree_node.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 5298fbed8..baf8806e6 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -422,9 +422,6 @@ class TreeNode friend class ControlNode; friend class Tree; - /// Set by ControlNode and DecoratorNode when the node is added to the tree. - void setParent(TreeNode* parent); - [[nodiscard]] NodeConfig& config(); /// Method to be implemented by the user @@ -457,9 +454,15 @@ class TreeNode T parseString(const std::string& str) const; private: + friend class ControlNode; + friend class DecoratorNode; + struct PImpl; std::unique_ptr _p; + /// Set by ControlNode and DecoratorNode when the node is added to the tree. + void setParent(TreeNode* parent); + Expected checkPreConditions(); void checkPostConditions(NodeStatus status); From 9b3db8df09e8a7ba7b30e9b3b8cd1d4eb679cf77 Mon Sep 17 00:00:00 2001 From: Chaosheng QU <203742003+Quchaosheng@users.noreply.github.com> Date: Wed, 11 Nov 2026 10:43:17 +0800 Subject: [PATCH 6/9] Run the three-phase preemption test with both fallbacks The review asked for the battery tree over three phases with a plain Fallback and with a ReactiveFallback; both variants fail before the fix. Full suite: 688 tests, 686 passed, 2 skipped. --- tests/gtest_preempt.cpp | 24 +++++++++++++++++++----- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/tests/gtest_preempt.cpp b/tests/gtest_preempt.cpp index 7dd8f8a34..6f7f8bee5 100644 --- a/tests/gtest_preempt.cpp +++ b/tests/gtest_preempt.cpp @@ -450,18 +450,18 @@ TEST(Preempt, parallelInsideTheNewBranchKeepsItsChildrenRunning) // 1. battery low: charging starts, kitchen is halted; // 2. charging completes, battery is OK, so the kitchen runs again; // 3. battery low once more: the kitchen must be halted before charging restarts. -TEST(Preempt, preemptsAgainAfterTheBranchCompleted) +static void runThreePhases(const std::string& fallback_tag) { - static const char* xml_text = R"( + const std::string xml_text = R"( - + - + @@ -471,7 +471,11 @@ TEST(Preempt, preemptsAgainAfterTheBranchCompleted) BehaviorTreeFactory factory; Tracker tracker; registerNodes(factory, tracker); - auto tree = factory.createTreeFromText(xml_text); + + std::string xml = xml_text; + xml.replace(xml.find("FALLBACK_TAG"), strlen("FALLBACK_TAG"), fallback_tag); + xml.replace(xml.find("FALLBACK_TAG"), strlen("FALLBACK_TAG"), fallback_tag); + auto tree = factory.createTreeFromText(xml); auto bb = tree.subtrees.front()->blackboard; bb->set("battery_ok", true); @@ -503,6 +507,16 @@ TEST(Preempt, preemptsAgainAfterTheBranchCompleted) EXPECT_EQ(tracker.max_running.load(), 1) << tracker.dump(); } +TEST(Preempt, preemptsAgainWithReactiveFallback) +{ + runThreePhases("ReactiveFallback"); +} + +TEST(Preempt, preemptsAgainWithPlainFallback) +{ + runThreePhases("Fallback"); +} + // A branch that already completed leaves nothing for the reactive ancestor to // remember: RS[Fallback[Cond, Preempt[A]], B] must still halt B before A restarts. TEST(Preempt, siblingThatCompletedIsStillPreempted) From 52f4fc2a74592a36c9c9e0f8a4e011b963a7facf Mon Sep 17 00:00:00 2001 From: Chaosheng QU <203742003+Quchaosheng@users.noreply.github.com> Date: Wed, 11 Nov 2026 10:44:23 +0800 Subject: [PATCH 7/9] Fix the Preempt doc comment: it is per activation, not per first tick --- include/behaviortree_cpp/decorators/preempt.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/include/behaviortree_cpp/decorators/preempt.h b/include/behaviortree_cpp/decorators/preempt.h index c03ac26fa..d4ee695ae 100644 --- a/include/behaviortree_cpp/decorators/preempt.h +++ b/include/behaviortree_cpp/decorators/preempt.h @@ -45,12 +45,10 @@ namespace BT * * * - * On its first tick, 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. - * - * The name is a placeholder. + * 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 { From 6c717b5661ec2108fd8da3ddf41d5bb2b85c74f1 Mon Sep 17 00:00:00 2001 From: Chaosheng QU <203742003+Quchaosheng@users.noreply.github.com> Date: Wed, 11 Nov 2026 10:49:09 +0800 Subject: [PATCH 8/9] Replace the tag without strlen in the three-phase test --- tests/gtest_preempt.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/tests/gtest_preempt.cpp b/tests/gtest_preempt.cpp index 6f7f8bee5..c4a165825 100644 --- a/tests/gtest_preempt.cpp +++ b/tests/gtest_preempt.cpp @@ -473,8 +473,11 @@ static void runThreePhases(const std::string& fallback_tag) registerNodes(factory, tracker); std::string xml = xml_text; - xml.replace(xml.find("FALLBACK_TAG"), strlen("FALLBACK_TAG"), fallback_tag); - xml.replace(xml.find("FALLBACK_TAG"), strlen("FALLBACK_TAG"), fallback_tag); + const std::string tag = "FALLBACK_TAG"; + for(auto pos = xml.find(tag); pos != std::string::npos; pos = xml.find(tag, pos + 1)) + { + xml.replace(pos, tag.size(), fallback_tag); + } auto tree = factory.createTreeFromText(xml); auto bb = tree.subtrees.front()->blackboard; From b2099c0238b65bd1133bcc49a3205ed42c344988 Mon Sep 17 00:00:00 2001 From: Chaosheng QU <203742003+Quchaosheng@users.noreply.github.com> Date: Wed, 11 Nov 2026 10:59:43 +0800 Subject: [PATCH 9/9] Regenerate the schemas with the new builtin Preempt is registered in the factory, so bt4_generate_xsd and bt4_generate_schematron emit a PreemptType and add the name to the allowed list. The CI step that diffs them against tools/generated/ was failing because they were not regenerated. ./build/tools/bt4_generate_xsd > tools/generated/bt4.xsd ./build/tools/bt4_generate_schematron > tools/generated/bt4.sch The diff is the Preempt entry only. --- tools/generated/bt4.sch | 2 +- tools/generated/bt4.xsd | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/tools/generated/bt4.sch b/tools/generated/bt4.sch index 1b70a024f..b79b78ab8 100644 --- a/tools/generated/bt4.sch +++ b/tools/generated/bt4.sch @@ -23,7 +23,7 @@ + + + + @@ -366,6 +370,7 @@ +