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..52e83ab23 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -41,6 +41,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..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 b4ae9506f..025ca896f 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -193,6 +193,12 @@ 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); + using StatusChangeSignal = Signal; using StatusChangeSubscriber = StatusChangeSignal::Subscriber; using StatusChangeCallback = StatusChangeSignal::CallableFunction; 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/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..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 b0c612f7d..d8ec91dac 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) @@ -407,6 +411,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 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..4959e3f1c --- /dev/null +++ b/tests/gtest_preempt.cpp @@ -0,0 +1,397 @@ +#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--; + } + + 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 + { + 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_) + { + 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_; +}; + +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(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(); +} + +} // 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