From 3a073f0acbfcf29898c060280fa824a3ebbf9486 Mon Sep 17 00:00:00 2001 From: Quchaosheng Date: Tue, 29 Sep 2026 16:49:55 +0800 Subject: [PATCH 1/3] 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/3] 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/3] 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)