diff --git a/CMakeLists.txt b/CMakeLists.txt index 442540383..d6ef0411b 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/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/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index 86e0d4812..06d1b5046 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -42,6 +42,7 @@ #include "behaviortree_cpp/decorators/inverter_node.h" #include "behaviortree_cpp/decorators/keep_running_until_failure_node.h" #include "behaviortree_cpp/decorators/loop_node.h" +#include "behaviortree_cpp/decorators/preempt.h" #include "behaviortree_cpp/decorators/repeat_node.h" #include "behaviortree_cpp/decorators/retry_node.h" #include "behaviortree_cpp/decorators/run_once_node.h" 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..b5f20dd71 --- /dev/null +++ b/include/behaviortree_cpp/controls/reactive_control_node.h @@ -0,0 +1,66 @@ +/* Copyright (C) 2025 Davide Faconti - All Rights Reserved + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#pragma once + +#include "behaviortree_cpp/control_node.h" + +namespace BT +{ +/** + * @brief Common base of the control nodes that tick their children in order and stop + * at the first one that returns RUNNING (ReactiveSequence, ReactiveFallback). + * + * It exists so that a node outside the reactive node can ask it to let go of the + * branches it is running without knowing which of the two it is. + * + * Deliberately not virtual: the class adds state and one non-virtual accessor only, + * so no vtable of an existing node changes. + */ +class ReactiveControlNode : public ControlNode +{ +public: + ReactiveControlNode(const std::string& name, const NodeConfig& config) + : ControlNode(name, config) + {} + + /** Halt every child that is RUNNING, except the one the caller is inside. + * + * Used by Preempt: the ancestor cannot tell by index which branch has to go, + * because the index is only updated by its own tick(), and the halting happens + * below it. Asking the children for their status cannot go stale. + */ + void haltRunningChild(const TreeNode* excluded) + { + running_child_ = -1; + for(size_t i = 0; i < childrenCount(); i++) + { + if(children()[i] != excluded && children()[i]->status() == NodeStatus::RUNNING) + { + haltChild(i); + } + } + } + +protected: + int running_child_ = -1; +}; + +} // namespace BT 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..d4ee695ae --- /dev/null +++ b/include/behaviortree_cpp/decorators/preempt.h @@ -0,0 +1,79 @@ +/* Copyright (C) 2025 Davide Faconti - All Rights Reserved + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#pragma once + +#include "behaviortree_cpp/decorator_node.h" + +namespace BT +{ +/** + * @brief Marks the point where a new branch of the tree starts. + * + * A reactive node knows how to halt the siblings of the child that returned + * RUNNING, but it does so after ticking that child. When a check that returns + * SUCCESS immediately and a new action that will keep running look the same to + * it, the tree cannot tell "start a new branch" from "re-tick a check", and the + * previously running branch is left RUNNING while the new one starts (issue + * #1031). + * + * Preempt answers that question at the one place that knows the answer: the tree + * author, who wraps the action that starts the new branch. + * + * + * + * + * + * + * + * + * + * On the first tick of each activation, before ticking its child, it halts the + * RUNNING child of every reactive ancestor it is not itself inside. Children of a + * Parallel are left alone: that node is allowed to keep several of them RUNNING, + * so halting them because a sibling branch moved on would be wrong. + */ +class Preempt : public DecoratorNode +{ +public: + Preempt(const std::string& name, const NodeConfig& config) : DecoratorNode(name, config) + { + setRegistrationID("Preempt"); + } + + ~Preempt() override = default; + + static BT::PortsList providedPorts() + { + return {}; + } + + Preempt(const Preempt&) = delete; + Preempt& operator=(const Preempt&) = delete; + Preempt(Preempt&&) = delete; + Preempt& operator=(Preempt&&) = delete; + +private: + NodeStatus tick() override; + + void haltPreviousBranch(); +}; + +} // namespace BT diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 8c528ccf6..fcb3ff751 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -205,6 +205,9 @@ class TreeNode virtual NodeType type() const = 0; + /// Parent of this node in the tree, or nullptr for the root of a subtree. + [[nodiscard]] TreeNode* parent() const; + using StatusChangeSignal = Signal; using StatusChangeSubscriber = StatusChangeSignal::Subscriber; using StatusChangeCallback = StatusChangeSignal::CallableFunction; @@ -463,9 +466,15 @@ class TreeNode T parseString(const std::string& str) const; private: + friend class ControlNode; + friend class DecoratorNode; + struct PImpl; std::unique_ptr _p; + /// Set by ControlNode and DecoratorNode when the node is added to the tree. + void setParent(TreeNode* parent); + Expected checkPreConditions(); void checkPostConditions(NodeStatus status); diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index e3e376c50..c93ed978c 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -144,6 +144,7 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl) registerNodeType("Timeout"); registerNodeType("Delay"); registerNodeType("RunOnce"); + registerNodeType("Preempt"); registerNodeType("ForceSuccess"); registerNodeType("ForceFailure"); 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/decorators/preempt.cpp b/src/decorators/preempt.cpp new file mode 100644 index 000000000..fd83964eb --- /dev/null +++ b/src/decorators/preempt.cpp @@ -0,0 +1,57 @@ +/* Copyright (C) 2025 Davide Faconti - All Rights Reserved + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "behaviortree_cpp/decorators/preempt.h" + +#include "behaviortree_cpp/controls/reactive_control_node.h" + +namespace BT +{ +NodeStatus Preempt::tick() +{ + // A new branch starts now: let go of whatever the reactive ancestors are + // still running before the child below us begins. status() is IDLE again + // whenever a parent resets a completed node, so this also covers a second + // preemption after the child returned SUCCESS or FAILURE. + if(status() == NodeStatus::IDLE) + { + haltPreviousBranch(); + } + return child()->executeTick(); +} + +void Preempt::haltPreviousBranch() +{ + // Walk up the tree keeping track of the child we came from: a reactive + // ancestor is only asked to let go of the children that are not on our path, + // so it never halts the branch we are about to start. + TreeNode* child_on_path = this; + + for(TreeNode* ancestor = parent(); ancestor != nullptr; ancestor = ancestor->parent()) + { + if(auto* reactive = dynamic_cast(ancestor)) + { + reactive->haltRunningChild(child_on_path); + } + child_on_path = ancestor; + } +} + +} // namespace BT diff --git a/src/tree_node.cpp b/src/tree_node.cpp index b1f1b93ae..c57440577 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -51,6 +51,8 @@ struct TreeNode::PImpl std::array pre_parsed; std::array post_parsed; + + TreeNode* parent = nullptr; }; TreeNode::TreeNode(std::string name, NodeConfig config) @@ -431,6 +433,16 @@ uint16_t TreeNode::UID() const return _p->config.uid; } +TreeNode* TreeNode::parent() const +{ + return _p->parent; +} + +void TreeNode::setParent(TreeNode* parent) +{ + _p->parent = parent; +} + const std::string& TreeNode::fullPath() const { return _p->config.path; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 5d4e643c9..02f7f368e 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..c4a165825 --- /dev/null +++ b/tests/gtest_preempt.cpp @@ -0,0 +1,574 @@ +#include "behaviortree_cpp/bt_factory.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace BT::test +{ + +struct Tracker +{ + void onStart(const std::string& name) + { + if(running.load() > 0) + { + started_while_running++; + } + { + 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)) + { + } + } + + void onHalted(const std::string& name) + { + { + std::lock_guard lock(mutex_); + events_.push_back(name + ":halted"); + } + 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, size_t occurrence = 0) const + { + std::lock_guard lock(mutex_); + 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 + { + 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_) + { + 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 +{ +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_; +}; + +/// 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: + 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); + }); + factory.registerBuilder( + "Finishing", [&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(tracker.count("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(); +} + +// 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. +static void runThreePhases(const std::string& fallback_tag) +{ + const std::string xml_text = R"( + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + Tracker tracker; + registerNodes(factory, tracker); + + std::string xml = xml_text; + 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; + 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(); +} + +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) +{ + 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 diff --git a/tests/gtest_reactive_backchaining.cpp b/tests/gtest_reactive_backchaining.cpp index 5aed62b52..101562f43 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,61 @@ 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. 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); + + 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 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 @@ +