From b6ccb8388d95f152cdc98f25697ffa5e28975099 Mon Sep 17 00:00:00 2001 From: renan028 Date: Tue, 17 Mar 2020 23:29:09 -0300 Subject: [PATCH 1/2] add unittest switch_node --- tests/CMakeLists.txt | 1 + tests/gtest_switch.cpp | 210 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 211 insertions(+) create mode 100644 tests/gtest_switch.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 1227f3137..4a3e6200f 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -15,6 +15,7 @@ set(BT_TESTS gtest_blackboard.cpp navigation_test.cpp gtest_subtree.cpp + gtest_switch.cpp ) if (NOT MINGW) diff --git a/tests/gtest_switch.cpp b/tests/gtest_switch.cpp new file mode 100644 index 000000000..80d61b7ad --- /dev/null +++ b/tests/gtest_switch.cpp @@ -0,0 +1,210 @@ +#include +#include "action_test_node.h" +#include "condition_test_node.h" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/tree_node.h" + +using BT::NodeStatus; +using std::chrono::milliseconds; + +struct SwitchTest : testing::Test +{ + + std::unique_ptr> root; + BT::AsyncActionTest action_1; + BT::AsyncActionTest action_2; + BT::AsyncActionTest action_3; + BT::Blackboard::Ptr bb = BT::Blackboard::create(); + BT::NodeConfiguration simple_switch_config_; + + SwitchTest() : + action_1("action_1", milliseconds(100)), + action_2("action_2", milliseconds(100)), + action_3("action_3", milliseconds(100)) + { + BT::PortsRemapping input; + input.insert(std::make_pair("case_1", "1")); + input.insert(std::make_pair("case_2", "2")); + + BT::NodeConfiguration simple_switch_config_; + simple_switch_config_.blackboard = bb; + simple_switch_config_.input_ports = input; + + root = std::make_unique> + ("simple_switch", simple_switch_config_); + root->addChild(&action_1); + root->addChild(&action_2); + root->addChild(&action_3); + } + ~SwitchTest() + { + root->halt(); + } +}; + +TEST_F(SwitchTest, DefaultCase) +{ + BT::NodeStatus state = root->executeTick(); + + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + std::this_thread::sleep_for(milliseconds(110)); + + // Switch Node does not halt action after success ? + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::SUCCESS, action_3.status()); + ASSERT_EQ(NodeStatus::SUCCESS, state); +} + +TEST_F(SwitchTest, Case1) +{ + bb->set("variable", "1"); + BT::NodeStatus state = root->executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + std::this_thread::sleep_for(milliseconds(110)); + + ASSERT_EQ(NodeStatus::SUCCESS, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::SUCCESS, state); +} + +TEST_F(SwitchTest, Case2) +{ + bb->set("variable", "2"); + BT::NodeStatus state = root->executeTick(); + + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + std::this_thread::sleep_for(milliseconds(110)); + + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::SUCCESS, state); +} + +TEST_F(SwitchTest, CaseNone) +{ + bb->set("variable", "none"); + BT::NodeStatus state = root->executeTick(); + + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + std::this_thread::sleep_for(milliseconds(110)); + + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::SUCCESS, action_3.status()); + ASSERT_EQ(NodeStatus::SUCCESS, state); +} + +TEST_F(SwitchTest, CaseSwitchToDefault) +{ + bb->set("variable", "1"); + BT::NodeStatus state = root->executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + std::this_thread::sleep_for(milliseconds(10)); + state = root->executeTick(); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + // Switch Node feels changes only when tick ? (no while loop inside, + // not reactive) + std::this_thread::sleep_for(milliseconds(10)); + bb->set("variable", ""); + std::this_thread::sleep_for(milliseconds(10)); + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, root->status()); + + std::this_thread::sleep_for(milliseconds(10)); + state = root->executeTick(); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + std::this_thread::sleep_for(milliseconds(110)); + + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::SUCCESS, action_3.status()); + ASSERT_EQ(NodeStatus::SUCCESS, root->status()); +} + +TEST_F(SwitchTest, CaseSwitchToAction2) +{ + bb->set("variable", "1"); + BT::NodeStatus state = root->executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + bb->set("variable", "2"); + std::this_thread::sleep_for(milliseconds(10)); + state = root->executeTick(); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::RUNNING, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + std::this_thread::sleep_for(milliseconds(110)); + + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); + ASSERT_EQ(NodeStatus::SUCCESS, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::SUCCESS, root->status()); +} + +TEST_F(SwitchTest, ActionFailure) +{ + bb->set("variable", "1"); + BT::NodeStatus state = root->executeTick(); + + ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::RUNNING, state); + + // Switch Node does not halt after failure ? + std::this_thread::sleep_for(milliseconds(10)); + action_1.setStatus(NodeStatus::FAILURE); + state = root->executeTick(); + ASSERT_EQ(NodeStatus::FAILURE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::FAILURE, state); + + state = root->executeTick(); + state = root->executeTick(); + ASSERT_EQ(NodeStatus::FAILURE, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); + ASSERT_EQ(NodeStatus::FAILURE, state); +} \ No newline at end of file From 59094736c86505c7aa94e4f04aab2194324c7116 Mon Sep 17 00:00:00 2001 From: renan028 Date: Wed, 18 Mar 2020 11:59:03 -0300 Subject: [PATCH 2/2] fix unittest switch should halt --- tests/gtest_switch.cpp | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/tests/gtest_switch.cpp b/tests/gtest_switch.cpp index 80d61b7ad..d4d602a56 100644 --- a/tests/gtest_switch.cpp +++ b/tests/gtest_switch.cpp @@ -52,11 +52,11 @@ TEST_F(SwitchTest, DefaultCase) ASSERT_EQ(NodeStatus::RUNNING, state); std::this_thread::sleep_for(milliseconds(110)); + state = root->executeTick(); - // Switch Node does not halt action after success ? ASSERT_EQ(NodeStatus::IDLE, action_1.status()); ASSERT_EQ(NodeStatus::IDLE, action_2.status()); - ASSERT_EQ(NodeStatus::SUCCESS, action_3.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); ASSERT_EQ(NodeStatus::SUCCESS, state); } @@ -71,8 +71,9 @@ TEST_F(SwitchTest, Case1) ASSERT_EQ(NodeStatus::RUNNING, state); std::this_thread::sleep_for(milliseconds(110)); + state = root->executeTick(); - ASSERT_EQ(NodeStatus::SUCCESS, action_1.status()); + ASSERT_EQ(NodeStatus::IDLE, action_1.status()); ASSERT_EQ(NodeStatus::IDLE, action_2.status()); ASSERT_EQ(NodeStatus::IDLE, action_3.status()); ASSERT_EQ(NodeStatus::SUCCESS, state); @@ -89,9 +90,10 @@ TEST_F(SwitchTest, Case2) ASSERT_EQ(NodeStatus::RUNNING, state); std::this_thread::sleep_for(milliseconds(110)); + state = root->executeTick(); ASSERT_EQ(NodeStatus::IDLE, action_1.status()); - ASSERT_EQ(NodeStatus::SUCCESS, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); ASSERT_EQ(NodeStatus::IDLE, action_3.status()); ASSERT_EQ(NodeStatus::SUCCESS, state); } @@ -107,10 +109,11 @@ TEST_F(SwitchTest, CaseNone) ASSERT_EQ(NodeStatus::RUNNING, state); std::this_thread::sleep_for(milliseconds(110)); + state = root->executeTick(); ASSERT_EQ(NodeStatus::IDLE, action_1.status()); ASSERT_EQ(NodeStatus::IDLE, action_2.status()); - ASSERT_EQ(NodeStatus::SUCCESS, action_3.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); ASSERT_EQ(NodeStatus::SUCCESS, state); } @@ -131,8 +134,8 @@ TEST_F(SwitchTest, CaseSwitchToDefault) ASSERT_EQ(NodeStatus::IDLE, action_3.status()); ASSERT_EQ(NodeStatus::RUNNING, state); - // Switch Node feels changes only when tick ? (no while loop inside, - // not reactive) + // Switch Node does not feels changes. Only when tick. + // (not reactive) std::this_thread::sleep_for(milliseconds(10)); bb->set("variable", ""); std::this_thread::sleep_for(milliseconds(10)); @@ -149,10 +152,11 @@ TEST_F(SwitchTest, CaseSwitchToDefault) ASSERT_EQ(NodeStatus::RUNNING, state); std::this_thread::sleep_for(milliseconds(110)); + state = root->executeTick(); ASSERT_EQ(NodeStatus::IDLE, action_1.status()); ASSERT_EQ(NodeStatus::IDLE, action_2.status()); - ASSERT_EQ(NodeStatus::SUCCESS, action_3.status()); + ASSERT_EQ(NodeStatus::IDLE, action_3.status()); ASSERT_EQ(NodeStatus::SUCCESS, root->status()); } @@ -175,9 +179,10 @@ TEST_F(SwitchTest, CaseSwitchToAction2) ASSERT_EQ(NodeStatus::RUNNING, state); std::this_thread::sleep_for(milliseconds(110)); + state = root->executeTick(); ASSERT_EQ(NodeStatus::IDLE, action_1.status()); - ASSERT_EQ(NodeStatus::SUCCESS, action_2.status()); + ASSERT_EQ(NodeStatus::IDLE, action_2.status()); ASSERT_EQ(NodeStatus::IDLE, action_3.status()); ASSERT_EQ(NodeStatus::SUCCESS, root->status()); } @@ -192,19 +197,18 @@ TEST_F(SwitchTest, ActionFailure) ASSERT_EQ(NodeStatus::IDLE, action_3.status()); ASSERT_EQ(NodeStatus::RUNNING, state); - // Switch Node does not halt after failure ? std::this_thread::sleep_for(milliseconds(10)); action_1.setStatus(NodeStatus::FAILURE); state = root->executeTick(); ASSERT_EQ(NodeStatus::FAILURE, action_1.status()); ASSERT_EQ(NodeStatus::IDLE, action_2.status()); ASSERT_EQ(NodeStatus::IDLE, action_3.status()); - ASSERT_EQ(NodeStatus::FAILURE, state); + ASSERT_EQ(NodeStatus::IDLE, state); state = root->executeTick(); state = root->executeTick(); ASSERT_EQ(NodeStatus::FAILURE, action_1.status()); ASSERT_EQ(NodeStatus::IDLE, action_2.status()); ASSERT_EQ(NodeStatus::IDLE, action_3.status()); - ASSERT_EQ(NodeStatus::FAILURE, state); + ASSERT_EQ(NodeStatus::IDLE, state); } \ No newline at end of file