diff --git a/include/behaviortree_cpp/action_node.h b/include/behaviortree_cpp/action_node.h index 2cfd60e63..e62400465 100644 --- a/include/behaviortree_cpp/action_node.h +++ b/include/behaviortree_cpp/action_node.h @@ -38,7 +38,7 @@ class ActionNodeBase : public LeafNode ActionNodeBase(const std::string& name, const NodeConfiguration& config); ~ActionNodeBase() override = default; - virtual NodeType type() const override final + virtual NodeType type() const override { return NodeType::ACTION; } @@ -114,6 +114,11 @@ class AsyncActionNode : public ActionNodeBase void stopAndJoinThread(); + virtual NodeType type() const override final + { + return NodeType::ACTION_ASYNC; + } + private: // The method that will be executed by the thread diff --git a/include/behaviortree_cpp/basic_types.h b/include/behaviortree_cpp/basic_types.h index b53bd89b8..97c136534 100644 --- a/include/behaviortree_cpp/basic_types.h +++ b/include/behaviortree_cpp/basic_types.h @@ -26,7 +26,8 @@ enum class NodeType CONDITION, CONTROL, DECORATOR, - SUBTREE + SUBTREE, + ACTION_ASYNC }; /// Enumerates the states every node can be in after execution during a particular diff --git a/include/behaviortree_cpp/control_node.h b/include/behaviortree_cpp/control_node.h index d77abb25a..72f9ef6c2 100644 --- a/include/behaviortree_cpp/control_node.h +++ b/include/behaviortree_cpp/control_node.h @@ -50,6 +50,7 @@ class ControlNode : public TreeNode { return NodeType::CONTROL; } + void propagateHalt(unsigned i); }; } diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 91060b9ee..885a6881d 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved +/* Copyright (C) 2015-2019 Michele Colledanchise - All Rights Reserved * Copyright (C) 2018-2019 Davide Faconti, Eurecat - 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"), @@ -21,13 +21,17 @@ #include "behaviortree_cpp/basic_types.h" #include "behaviortree_cpp/blackboard.h" #include "behaviortree_cpp/utils/strcat.hpp" +//#include #ifdef _MSC_VER #pragma warning(disable : 4127) #endif + namespace BT { +class ControlNode; + /// This information is used mostly by the XMLParser. struct TreeNodeManifest { @@ -147,8 +151,13 @@ class TreeNode static StringView stripBlackboardPointer(StringView str); static Optional getRemappedKey(StringView port_name, StringView remapping_value); + void set_parent_prt(ControlNode *parent_ptr); - protected: + ControlNode *parent_prt() const; + unsigned int child_index() const; + void set_child_index(unsigned int child_index); + +protected: /// Method to be implemented by the user virtual BT::NodeStatus tick() = 0; @@ -161,6 +170,9 @@ class TreeNode } void modifyPortsRemapping(const PortsRemapping& new_remapping); + unsigned int child_index_; + + ControlNode* parent_prt_ = nullptr; private: const std::string name_; diff --git a/src/control_node.cpp b/src/control_node.cpp index 52c7fd4b0..7d66aceb6 100644 --- a/src/control_node.cpp +++ b/src/control_node.cpp @@ -22,6 +22,8 @@ ControlNode::ControlNode(const std::string& name, const NodeConfiguration& confi void ControlNode::addChild(TreeNode* child) { + child->set_child_index(children_nodes_.size()); + child->set_parent_prt(this); children_nodes_.push_back(child); } @@ -54,4 +56,13 @@ void ControlNode::haltChildren(unsigned i) } } +void ControlNode::propagateHalt(unsigned i) +{ + haltChildren(i + 1); + if (parent_prt_ != nullptr) + { + ((ControlNode*)parent_prt_)->propagateHalt(child_index_); + } +} + } // end namespace diff --git a/src/controls/reactive_fallback.cpp b/src/controls/reactive_fallback.cpp index 8d930eb19..8840fcaee 100644 --- a/src/controls/reactive_fallback.cpp +++ b/src/controls/reactive_fallback.cpp @@ -1,5 +1,5 @@ /* Copyright (C) 2019 Davide Faconti, Eurecat - All Rights Reserved -* +* Copyright (C) 2019 Michele Colledanchise - 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: @@ -11,7 +11,7 @@ */ #include "behaviortree_cpp/controls/reactive_fallback.h" - +#include namespace BT { @@ -22,31 +22,70 @@ NodeStatus ReactiveFallback::tick() for (size_t index = 0; index < childrenCount(); index++) { TreeNode* current_child_node = children_nodes_[index]; - const NodeStatus child_status = current_child_node->executeTick(); + NodeStatus child_status = NodeStatus::IDLE; - switch (child_status) + if (current_child_node->type() != NodeType::ACTION_ASYNC) { - case NodeStatus::RUNNING: + child_status = current_child_node->executeTick(); + } + else + { + if (current_child_node->status() != NodeStatus::RUNNING) { - haltChildren(index+1); - return NodeStatus::RUNNING; - } + /* if not running already, tick it. I assume that ACTION_ASYNC returns running for (at least the first tick), + hence the halt of possibly other async actions should be sent before ticking current_child_node */ - case NodeStatus::FAILURE: + if (parent_prt_ != nullptr) + { + parent_prt_->propagateHalt(child_index_); + } + + current_child_node->executeTick(); + + do + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + child_status = current_child_node->status(); + + } while (child_status == NodeStatus::IDLE); + } + else { - failure_count++; - }break; + child_status = NodeStatus::RUNNING; + } + } - case NodeStatus::SUCCESS: + switch (child_status) + { + case NodeStatus::RUNNING: + { + haltChildren(index+1); + return NodeStatus::RUNNING; + }break; + + case NodeStatus::FAILURE: + { + failure_count++; + if (current_child_node->type() == NodeType::ACTION_ASYNC) { - haltChildren(0); - return NodeStatus::SUCCESS; + current_child_node->setStatus(NodeStatus::IDLE); } + }break; - case NodeStatus::IDLE: + case NodeStatus::SUCCESS: + { + haltChildren(index+1); + if (current_child_node->type() == NodeType::ACTION_ASYNC) { - throw LogicError("A child node must never return IDLE"); + current_child_node->setStatus(NodeStatus::IDLE); } + return NodeStatus::SUCCESS; + }break; + + case NodeStatus::IDLE: + { + throw LogicError("A child node must never return IDLE"); + } } // end switch } //end for @@ -60,4 +99,3 @@ NodeStatus ReactiveFallback::tick() } } - diff --git a/src/controls/reactive_sequence.cpp b/src/controls/reactive_sequence.cpp index 35f5c5046..dcdd4fb74 100644 --- a/src/controls/reactive_sequence.cpp +++ b/src/controls/reactive_sequence.cpp @@ -1,5 +1,5 @@ /* Copyright (C) 2019 Davide Faconti, Eurecat - All Rights Reserved -* +* Copyright (C) 2019 Michele Colledanchise - 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: @@ -11,6 +11,7 @@ */ #include "behaviortree_cpp/controls/reactive_sequence.h" +#include namespace BT { @@ -18,36 +19,73 @@ namespace BT NodeStatus ReactiveSequence::tick() { size_t success_count = 0; - size_t running_count = 0; for (size_t index = 0; index < childrenCount(); index++) { TreeNode* current_child_node = children_nodes_[index]; - const NodeStatus child_status = current_child_node->executeTick(); + NodeStatus child_status = NodeStatus::IDLE; - switch (child_status) + if (current_child_node->type() != NodeType::ACTION_ASYNC) + { + child_status = current_child_node->executeTick(); + } + else { - case NodeStatus::RUNNING: + if (current_child_node->status() != NodeStatus::RUNNING) { - running_count++; - haltChildren(index+1); - return NodeStatus::RUNNING; - } + /* if not running already, tick it. I assume that ACTION_ASYNC returns running for (at least the first tick), + hence the halt of possibly other async actions should be sent before ticking current_child_node */ - case NodeStatus::FAILURE: - { - haltChildren(0); - return NodeStatus::FAILURE; + if (parent_prt_ != nullptr) + { + parent_prt_->propagateHalt(child_index_); + } + + current_child_node->executeTick(); + + do + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + child_status = current_child_node->status(); + + } while (child_status == NodeStatus::IDLE); } - case NodeStatus::SUCCESS: + else { - success_count++; - }break; + child_status = NodeStatus::RUNNING; + } + } + + switch (child_status) + { + case NodeStatus::RUNNING: + { + haltChildren(index+1); + return NodeStatus::RUNNING; + } - case NodeStatus::IDLE: + case NodeStatus::FAILURE: + { + haltChildren(index+1); + if (current_child_node->type() == NodeType::ACTION_ASYNC) + { + current_child_node->setStatus(NodeStatus::IDLE); + } + return NodeStatus::FAILURE; + } + case NodeStatus::SUCCESS: + { + success_count++; + if (current_child_node->type() == NodeType::ACTION_ASYNC) { - throw LogicError("A child node must never return IDLE"); + current_child_node->setStatus(NodeStatus::IDLE); } + }break; + + case NodeStatus::IDLE: + { + throw LogicError("A child node must never return IDLE"); + } } // end switch } //end for @@ -59,5 +97,4 @@ NodeStatus ReactiveSequence::tick() return NodeStatus::RUNNING; } - } diff --git a/src/tree_node.cpp b/src/tree_node.cpp index 3cd3e1756..3771573fc 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved +/* Copyright (C) 2015-2019 Michele Colledanchise - All Rights Reserved * Copyright (C) 2018-2019 Davide Faconti, Eurecat - 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"), @@ -13,6 +13,7 @@ #include "behaviortree_cpp/tree_node.h" #include +#include namespace BT { @@ -161,4 +162,24 @@ void TreeNode::modifyPortsRemapping(const PortsRemapping &new_remapping) } } +unsigned int TreeNode::child_index() const +{ + return child_index_; +} + +void TreeNode::set_child_index(unsigned int child_index) +{ + child_index_ = child_index; +} + +void TreeNode::set_parent_prt(ControlNode *parent_ptr) +{ + parent_prt_ = parent_ptr; +} + +ControlNode *TreeNode::parent_prt() const +{ + return parent_prt_; +} + } // end namespace diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 355e5c728..58c646d8c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -16,6 +16,10 @@ set(BT_TESTS gtest_coroutines.cpp navigation_test.cpp gtest_subtree.cpp + gtest_reactive_sequence.cpp + gtest_reactive_fallback.cpp + gtest_reactive_tree.cpp + ) if(ament_cmake_FOUND AND BUILD_TESTING) diff --git a/tests/gtest_fallback.cpp b/tests/gtest_fallback.cpp index e8fc87e85..b6a500f1f 100644 --- a/tests/gtest_fallback.cpp +++ b/tests/gtest_fallback.cpp @@ -38,29 +38,6 @@ struct SimpleFallbackTest : testing::Test } }; -struct ReactiveFallbackTest : testing::Test -{ - BT::ReactiveFallback root; - BT::ConditionTestNode condition_1; - BT::ConditionTestNode condition_2; - BT::AsyncActionTest action_1; - - ReactiveFallbackTest() - : root("root_first") - , condition_1("condition_1") - , condition_2("condition_2") - , action_1("action_1", milliseconds(100) ) - { - root.addChild(&condition_1); - root.addChild(&condition_2); - root.addChild(&action_1); - } - ~ReactiveFallbackTest() - { - haltAllActions(&root); - } -}; - struct SimpleFallbackWithMemoryTest : testing::Test { BT::FallbackNode root; @@ -152,49 +129,6 @@ TEST_F(SimpleFallbackTest, ConditionChangeWhileRunning) ASSERT_EQ(NodeStatus::RUNNING, action.status()); } -TEST_F(ReactiveFallbackTest, Condition1ToTrue) -{ - condition_1.setBoolean(false); - condition_2.setBoolean(false); - - BT::NodeStatus state = root.executeTick(); - - ASSERT_EQ(NodeStatus::RUNNING, state); - ASSERT_EQ(NodeStatus::FAILURE, condition_1.status()); - ASSERT_EQ(NodeStatus::FAILURE, condition_2.status()); - ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); - - condition_1.setBoolean(true); - - state = root.executeTick(); - - ASSERT_EQ(NodeStatus::SUCCESS, state); - ASSERT_EQ(NodeStatus::IDLE, condition_1.status()); - ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); - ASSERT_EQ(NodeStatus::IDLE, action_1.status()); -} - -TEST_F(ReactiveFallbackTest, Condition2ToTrue) -{ - condition_1.setBoolean(false); - condition_2.setBoolean(false); - - BT::NodeStatus state = root.executeTick(); - - ASSERT_EQ(NodeStatus::RUNNING, state); - ASSERT_EQ(NodeStatus::FAILURE, condition_1.status()); - ASSERT_EQ(NodeStatus::FAILURE, condition_2.status()); - ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); - - condition_2.setBoolean(true); - - state = root.executeTick(); - - ASSERT_EQ(NodeStatus::SUCCESS, state); - ASSERT_EQ(NodeStatus::IDLE, condition_1.status()); - ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); - ASSERT_EQ(NodeStatus::IDLE, action_1.status()); -} TEST_F(SimpleFallbackWithMemoryTest, ConditionFalse) { diff --git a/tests/gtest_reactive_fallback.cpp b/tests/gtest_reactive_fallback.cpp new file mode 100644 index 000000000..4a8bdd9e5 --- /dev/null +++ b/tests/gtest_reactive_fallback.cpp @@ -0,0 +1,214 @@ +/* Copyright (C) 2015-2019 Michele Colledanchise - 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 +#include "action_test_node.h" +#include "condition_test_node.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/loggers/bt_cout_logger.h" +#include "behaviortree_cpp/bt_factory.h" + +using BT::NodeStatus; +using std::chrono::milliseconds; + +struct ReactiveFallbackTest : testing::Test +{ + BT::ReactiveFallback root; + BT::ConditionTestNode condition_1; + BT::ConditionTestNode condition_2; + BT::AsyncActionTest action_1; + + ReactiveFallbackTest() + : root("root_first") + , condition_1("condition_1") + , condition_2("condition_2") + , action_1("action_1", milliseconds(100) ) + { + root.addChild(&condition_1); + root.addChild(&condition_2); + root.addChild(&action_1); + } + ~ReactiveFallbackTest() + { + haltAllActions(&root); + } +}; + +struct ReactiveFallback2ActionsTest : testing::Test +{ + BT::ReactiveFallback root; + BT::AsyncActionTest action_1; + BT::AsyncActionTest action_2; + + ReactiveFallback2ActionsTest() + : root("root_Fallback") + , action_1("action_1", milliseconds(100)) + , action_2("action_2", milliseconds(100)) + { + root.addChild(&action_1); + root.addChild(&action_2); + } + ~ReactiveFallback2ActionsTest() + { + haltAllActions(&root); + } +}; + +struct ComplexReactiveFallback2ActionsTest : testing::Test +{ + BT::ReactiveFallback root; + BT::AsyncActionTest action_1; + BT::AsyncActionTest action_2; + BT::ReactiveFallback seq_1; + BT::ReactiveFallback seq_2; + + BT::ConditionTestNode condition_1; + BT::ConditionTestNode condition_2; + + ComplexReactiveFallback2ActionsTest() + : root("root_Fallback") + , action_1("action_1", milliseconds(100)) + , action_2("action_2", milliseconds(100)) + , seq_1("Fallback_1") + , seq_2("Fallback_2") + , condition_1("condition_1") + , condition_2("condition_2") + { + root.addChild(&seq_1); + { + seq_1.addChild(&condition_1); + seq_1.addChild(&action_1); + } + root.addChild(&seq_2); + { + seq_2.addChild(&condition_2); + seq_2.addChild(&action_2); + } + } + ~ComplexReactiveFallback2ActionsTest() + { + haltAllActions(&root); + } +}; + +/****************TESTS START HERE***************************/ + +TEST_F(ReactiveFallbackTest, Condition1ToTrue) +{ + condition_1.setBoolean(false); + condition_2.setBoolean(false); + + BT::NodeStatus state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::FAILURE, condition_1.status()); + ASSERT_EQ(NodeStatus::FAILURE, condition_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + + condition_1.setBoolean(true); + + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::SUCCESS, state); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); +} + +TEST_F(ReactiveFallbackTest, Condition2ToTrue) +{ + condition_1.setBoolean(false); + condition_2.setBoolean(false); + + BT::NodeStatus state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::FAILURE, condition_1.status()); + ASSERT_EQ(NodeStatus::FAILURE, condition_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + + condition_2.setBoolean(true); + + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::SUCCESS, state); + ASSERT_EQ(NodeStatus::FAILURE, condition_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); +} + +TEST_F(ReactiveFallback2ActionsTest, Actions) +{ + + root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, root.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + + std::this_thread::sleep_for(milliseconds(1000)); + + ASSERT_EQ(NodeStatus::SUCCESS, action_1.status()); + + root.executeTick(); + + ASSERT_EQ(NodeStatus::SUCCESS, root.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + + root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, root.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + +} + +TEST_F(ComplexReactiveFallback2ActionsTest, ConditionsTrue) +{ + BT::NodeStatus state = root.executeTick(); + + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::SUCCESS, state); + ASSERT_EQ(NodeStatus::SUCCESS, seq_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, seq_2.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + + std::this_thread::sleep_for(milliseconds(300)); + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::SUCCESS, state); + ASSERT_EQ(NodeStatus::SUCCESS, seq_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, seq_2.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + + std::this_thread::sleep_for(milliseconds(300)); + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::SUCCESS, state); + ASSERT_EQ(NodeStatus::SUCCESS, seq_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, seq_2.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); +} + + + diff --git a/tests/gtest_reactive_sequence.cpp b/tests/gtest_reactive_sequence.cpp new file mode 100644 index 000000000..d10d74acc --- /dev/null +++ b/tests/gtest_reactive_sequence.cpp @@ -0,0 +1,212 @@ +/* Copyright (C) 2015-2019 Michele Colledanchise - 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 +#include "action_test_node.h" +#include "condition_test_node.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/loggers/bt_cout_logger.h" +#include "behaviortree_cpp/bt_factory.h" + +using BT::NodeStatus; +using std::chrono::milliseconds; + + +struct ReactiveSequenceTest : testing::Test +{ + BT::ReactiveSequence root; + BT::ConditionTestNode condition_1; + BT::ConditionTestNode condition_2; + BT::AsyncActionTest action_1; + + ReactiveSequenceTest() + : root("root_first") + , condition_1("condition_1") + , condition_2("condition_2") + , action_1("action_1", milliseconds(100) ) + { + root.addChild(&condition_1); + root.addChild(&condition_2); + root.addChild(&action_1); + } + ~ReactiveSequenceTest() + { + haltAllActions(&root); + } +}; + +struct ReactiveSequence2ActionsTest : testing::Test +{ + BT::ReactiveSequence root; + BT::AsyncActionTest action_1; + BT::AsyncActionTest action_2; + + ReactiveSequence2ActionsTest() + : root("root_sequence") + , action_1("action_1", milliseconds(100)) + , action_2("action_2", milliseconds(100)) + { + root.addChild(&action_1); + root.addChild(&action_2); + } + ~ReactiveSequence2ActionsTest() + { + haltAllActions(&root); + } +}; + +struct ComplexReactiveSequence2ActionsTest : testing::Test +{ + BT::ReactiveSequence root; + BT::AsyncActionTest action_1; + BT::AsyncActionTest action_2; + BT::ReactiveSequence seq_1; + BT::ReactiveSequence seq_2; + + BT::ConditionTestNode condition_1; + BT::ConditionTestNode condition_2; + + ComplexReactiveSequence2ActionsTest() + : root("root_sequence") + , action_1("action_1", milliseconds(100)) + , action_2("action_2", milliseconds(100)) + , seq_1("sequence_1") + , seq_2("sequence_2") + , condition_1("condition_1") + , condition_2("condition_2") + { + root.addChild(&seq_1); + { + seq_1.addChild(&condition_1); + seq_1.addChild(&action_1); + } + root.addChild(&seq_2); + { + seq_2.addChild(&condition_2); + seq_2.addChild(&action_2); + } + } + ~ComplexReactiveSequence2ActionsTest() + { + haltAllActions(&root); + } +}; + +/****************TESTS START HERE***************************/ + +TEST_F(ReactiveSequenceTest, Condition1ToFalse) +{ + condition_1.setBoolean(true); + condition_2.setBoolean(true); + + BT::NodeStatus state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + + condition_1.setBoolean(false); + + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::FAILURE, state); + ASSERT_EQ(NodeStatus::FAILURE, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); +} + +TEST_F(ReactiveSequenceTest, Condition2ToFalse) +{ + condition_1.setBoolean(true); + condition_2.setBoolean(true); + + BT::NodeStatus state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + + condition_2.setBoolean(false); + + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::FAILURE, state); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::FAILURE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); +} + +TEST_F(ReactiveSequence2ActionsTest, ConditionsTrue) +{ + root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, root.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + + std::this_thread::sleep_for(milliseconds(1000)); + + ASSERT_EQ(NodeStatus::SUCCESS, action_1.status()); + + root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, root.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_2.status()); + + root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, root.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + +} + +TEST_F(ComplexReactiveSequence2ActionsTest, ConditionsTrue) +{ + BT::NodeStatus state = root.executeTick(); + + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::RUNNING, seq_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, seq_2.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + + std::this_thread::sleep_for(milliseconds(300)); + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::SUCCESS, seq_1.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::RUNNING, seq_2.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_2.status()); + + + std::this_thread::sleep_for(milliseconds(300)); + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::RUNNING, seq_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, seq_2.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status());; +} diff --git a/tests/gtest_reactive_tree.cpp b/tests/gtest_reactive_tree.cpp new file mode 100644 index 000000000..a2682d65e --- /dev/null +++ b/tests/gtest_reactive_tree.cpp @@ -0,0 +1,102 @@ +/* Copyright (C) 2015-2019 Michele Colledanchise - 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 +#include "action_test_node.h" +#include "condition_test_node.h" +#include "behaviortree_cpp/behavior_tree.h" + +using BT::NodeStatus; +using std::chrono::milliseconds; + + +struct ComplexReactiveTree : testing::Test +{ + BT::ReactiveSequence root; + BT::AsyncActionTest action_1; + BT::AsyncActionTest action_2; + BT::ReactiveFallback fal_1; + BT::ReactiveFallback fal_2; + + BT::ConditionTestNode condition_1; + BT::ConditionTestNode condition_2; + + ComplexReactiveTree() + : root("root_sequence") + , action_1("action_1", milliseconds(5000)) + , action_2("action_2", milliseconds(5000)) + , fal_1("fallback_1") + , fal_2("fallback_2") + , condition_1("condition_1") + , condition_2("condition_2") + { + root.addChild(&fal_1); + { + fal_1.addChild(&condition_1); + fal_1.addChild(&action_1); + } + root.addChild(&fal_2); + { + fal_2.addChild(&condition_2); + fal_2.addChild(&action_2); + } + } + ~ComplexReactiveTree() + { + haltAllActions(&root); + } +}; + +/****************TESTS START HERE***************************/ + + +TEST_F(ComplexReactiveTree, CorrectHaltTiming) +{ + auto t0 = std::chrono::high_resolution_clock::now(); + + condition_1.setBoolean(false); + condition_2.setBoolean(false); + + BT::NodeStatus state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::RUNNING, fal_1.status()); + ASSERT_EQ(NodeStatus::FAILURE, condition_1.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, fal_2.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + + std::this_thread::sleep_for(milliseconds(300)); + + condition_1.setBoolean(true); // condition 1 set to true" + + state = root.executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, state); + ASSERT_EQ(NodeStatus::SUCCESS, fal_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::RUNNING, fal_2.status()); + ASSERT_EQ(NodeStatus::FAILURE, condition_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_2.status()); + + std::this_thread::sleep_for(milliseconds(300)); + condition_1.setBoolean(false); // condition 1 set to false" + + state = root.executeTick(); + + std::this_thread::sleep_for(milliseconds(300)); + + ASSERT_TRUE(action_1.startTimePoint().time_since_epoch().count() > + action_2.stopTimePoint().time_since_epoch().count() ); +} diff --git a/tests/gtest_sequence.cpp b/tests/gtest_sequence.cpp index 110a6c6eb..618913526 100644 --- a/tests/gtest_sequence.cpp +++ b/tests/gtest_sequence.cpp @@ -327,7 +327,7 @@ TEST_F(ComplexSequenceTest, ComplexSequenceConditions1ToFalse) state = root.executeTick(); ASSERT_EQ(NodeStatus::FAILURE, state); - ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status()); + ASSERT_EQ(NodeStatus::FAILURE, seq_conditions.status()); ASSERT_EQ(NodeStatus::IDLE, condition_1.status()); ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); ASSERT_EQ(NodeStatus::IDLE, action_1.status()); @@ -342,7 +342,7 @@ TEST_F(ComplexSequenceTest, ComplexSequenceConditions2ToFalse) state = root.executeTick(); ASSERT_EQ(NodeStatus::FAILURE, state); - ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status()); + ASSERT_EQ(NodeStatus::FAILURE, seq_conditions.status()); ASSERT_EQ(NodeStatus::IDLE, condition_1.status()); ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); ASSERT_EQ(NodeStatus::IDLE, action_1.status()); @@ -458,3 +458,4 @@ TEST_F(ComplexSequenceWithMemoryTest, Action1DoneSeq) ASSERT_EQ(NodeStatus::IDLE, action_1.status()); ASSERT_EQ(NodeStatus::IDLE, action_2.status()); } + diff --git a/tests/include/action_test_node.h b/tests/include/action_test_node.h index 6f6a15700..c58320895 100644 --- a/tests/include/action_test_node.h +++ b/tests/include/action_test_node.h @@ -1,7 +1,10 @@ #ifndef ACTIONTEST_H #define ACTIONTEST_H +#include +#include #include "behaviortree_cpp/action_node.h" +#include namespace BT { @@ -56,12 +59,22 @@ class AsyncActionTest : public AsyncActionNode tick_count_ = 0; } - private: + + void setStartTimePoint(std::chrono::high_resolution_clock::time_point now); + std::chrono::high_resolution_clock::time_point startTimePoint() const; + void setStopTimePoint(std::chrono::high_resolution_clock::time_point now); + std::chrono::high_resolution_clock::time_point stopTimePoint() const; + +private: // using atomic because these variables might be accessed from different threads BT::Duration time_; std::atomic_bool boolean_value_; std::atomic tick_count_; - std::atomic_bool stop_loop_; + std::atomic_bool stop_loop_, has_started_; + std::chrono::high_resolution_clock::time_point start_time_, stop_time_; + + mutable std::mutex start_time_mutex_, stop_time_mutex_; + }; } diff --git a/tests/src/action_test_node.cpp b/tests/src/action_test_node.cpp index 4e0812a70..5b153431e 100644 --- a/tests/src/action_test_node.cpp +++ b/tests/src/action_test_node.cpp @@ -13,6 +13,12 @@ #include "action_test_node.h" #include +#include +#include +#include +#include +#include +using namespace std::chrono; BT::AsyncActionTest::AsyncActionTest(const std::string& name, BT::Duration deadline_ms) : AsyncActionNode(name, {}) @@ -20,6 +26,7 @@ BT::AsyncActionTest::AsyncActionTest(const std::string& name, BT::Duration deadl boolean_value_ = true; time_ = deadline_ms; stop_loop_ = false; + has_started_ = false; tick_count_ = 0; } @@ -30,6 +37,14 @@ BT::AsyncActionTest::~AsyncActionTest() BT::NodeStatus BT::AsyncActionTest::tick() { + + has_started_ = true; + + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + + + setStartTimePoint(t2); + setStatus(NodeStatus::RUNNING); using std::chrono::high_resolution_clock; tick_count_++; stop_loop_ = false; @@ -40,6 +55,8 @@ BT::NodeStatus BT::AsyncActionTest::tick() std::this_thread::sleep_for(std::chrono::milliseconds(1)); } + setStopTimePoint(high_resolution_clock::now()); + if (!stop_loop_) { return boolean_value_ ? NodeStatus::SUCCESS : NodeStatus::FAILURE; @@ -50,9 +67,18 @@ BT::NodeStatus BT::AsyncActionTest::tick() } } + + void BT::AsyncActionTest::halt() { stop_loop_ = true; + NodeStatus node_status; + do + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + node_status = status(); + } while (node_status == NodeStatus::RUNNING && has_started_); + } void BT::AsyncActionTest::setTime(BT::Duration time) @@ -65,6 +91,37 @@ void BT::AsyncActionTest::setBoolean(bool boolean_value) boolean_value_ = boolean_value; } + +void BT::AsyncActionTest::setStartTimePoint(std::chrono::high_resolution_clock::time_point now) +{ + std::lock_guard lock(start_time_mutex_); + + start_time_ = now; +} + +std::chrono::high_resolution_clock::time_point BT::AsyncActionTest::startTimePoint() const +{ + std::lock_guard lock(start_time_mutex_); + + return start_time_; +} + + +void BT::AsyncActionTest::setStopTimePoint(std::chrono::high_resolution_clock::time_point now) +{ + std::lock_guard lock(stop_time_mutex_); + + stop_time_ = now; +} + +std::chrono::high_resolution_clock::time_point BT::AsyncActionTest::stopTimePoint() const +{ + std::lock_guard lock(stop_time_mutex_); + + return stop_time_; +} + + //---------------------------------------------- BT::SyncActionTest::SyncActionTest(const std::string& name) :