diff --git a/tests/gtest_reactive.cpp b/tests/gtest_reactive.cpp index c38ef11c7..391874a7b 100644 --- a/tests/gtest_reactive.cpp +++ b/tests/gtest_reactive.cpp @@ -3,6 +3,10 @@ #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/loggers/bt_observer.h" +#include +#include +#include + #include using BT::NodeStatus; @@ -495,3 +499,185 @@ TEST(Reactive, ReactiveSequence_ReEvaluatesOnEveryTick) // Condition should be ticked multiple times (re-evaluated while Sleep is running) ASSERT_GE(condition_tick_count, 2); } + +namespace +{ +// Logs onStart() and onHalted(); onStart() returns the given status, onRunning() keeps +// running. +class LoggedAction : public BT::StatefulActionNode +{ +public: + LoggedAction(const std::string& name, const BT::NodeConfig& config, + std::vector* log, NodeStatus start_status) + : BT::StatefulActionNode(name, config), log_(log), start_status_(start_status) + {} + static BT::PortsList providedPorts() + { + return {}; + } + NodeStatus onStart() override + { + log_->push_back(name() + ":start"); + return start_status_; + } + NodeStatus onRunning() override + { + return NodeStatus::RUNNING; + } + void onHalted() override + { + log_->push_back(name() + ":halted"); + } + +private: + std::vector* log_; + NodeStatus start_status_; +}; + +// A coroutine action that completes with SUCCESS on its first tick. +class LoggedCoroCheck : public BT::CoroActionNode +{ +public: + LoggedCoroCheck(const std::string& name, const BT::NodeConfig& config, + std::vector* log) + : BT::CoroActionNode(name, config), log_(log) + {} + static BT::PortsList providedPorts() + { + return {}; + } + NodeStatus tick() override + { + log_->push_back(name() + ":start"); + return NodeStatus::SUCCESS; + } + +private: + std::vector* log_; +}; + +// Keeps all its children running at once, like nav2's PipelineSequence: every tick it +// ticks each child and restarts the ones that completed. +class TickAllChildren : public BT::ControlNode +{ +public: + TickAllChildren(const std::string& name, const BT::NodeConfig& config) + : BT::ControlNode(name, config) + {} + static BT::PortsList providedPorts() + { + return {}; + } + NodeStatus tick() override + { + setStatus(NodeStatus::RUNNING); + for(size_t i = 0; i < childrenCount(); i++) + { + if(BT::isStatusCompleted(children_nodes_[i]->executeTick())) + { + haltChild(i); + } + } + return NodeStatus::RUNNING; + } +}; + +long countOf(const std::vector& log, const std::string& event) +{ + return std::count(log.begin(), log.end(), event); +} + +void registerLoggedNodes(BT::BehaviorTreeFactory& factory, std::vector* log) +{ + factory.registerNodeType("QuickSuccess", log, NodeStatus::SUCCESS); + factory.registerNodeType("QuickFailure", log, NodeStatus::FAILURE); + factory.registerNodeType("LongAction", log, NodeStatus::RUNNING); + factory.registerNodeType("CoroCheck", log); + factory.registerNodeType("TickAllChildren"); +} +} // namespace + +// The children before a RUNNING child are re-ticked on every tick. When they complete +// immediately (a check implemented as an action), the RUNNING child must keep running: +// it is not halted and not started again (see #1031 and #1217). +TEST(Reactive, ReactiveSequence_CompletedSiblingsDoNotRestartRunningChild) +{ + BT::BehaviorTreeFactory factory; + std::vector log; + registerLoggedNodes(factory, &log); + + auto tree = factory.createTreeFromText(R"( + + + + + + + + +)"); + + for(int i = 0; i < 5; i++) + { + ASSERT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + } + EXPECT_EQ(countOf(log, "Check:start"), 5); + EXPECT_EQ(countOf(log, "CoroCheck:start"), 5); + EXPECT_EQ(countOf(log, "Long:start"), 1); + EXPECT_EQ(countOf(log, "Long:halted"), 0); +} + +TEST(Reactive, ReactiveFallback_CompletedSiblingsDoNotRestartRunningChild) +{ + BT::BehaviorTreeFactory factory; + std::vector log; + registerLoggedNodes(factory, &log); + + auto tree = factory.createTreeFromText(R"( + + + + + + + +)"); + + for(int i = 0; i < 5; i++) + { + ASSERT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + } + EXPECT_EQ(countOf(log, "Check:start"), 5); + EXPECT_EQ(countOf(log, "Long:start"), 1); + EXPECT_EQ(countOf(log, "Long:halted"), 0); +} + +// A control node may keep several children running at once (nav2's PipelineSequence). +// Starting one of them must not halt the others. +TEST(Reactive, ConcurrentChildrenOfCustomControlAreNotHalted) +{ + BT::BehaviorTreeFactory factory; + std::vector log; + registerLoggedNodes(factory, &log); + + auto tree = factory.createTreeFromText(R"( + + + + + + + + + + +)"); + + for(int i = 0; i < 5; i++) + { + ASSERT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + } + EXPECT_EQ(countOf(log, "Replan:start"), 5); + EXPECT_EQ(countOf(log, "Follow:start"), 1); + EXPECT_EQ(countOf(log, "Follow:halted"), 0); +}