Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #include <gtest/gtest.h>
00014 #include "action_test_node.h"
00015 #include "condition_test_node.h"
00016 #include "behaviortree_cpp/behavior_tree.h"
00017
00018 using BT::NodeStatus;
00019
00020 struct DeadlineTest : testing::Test
00021 {
00022 BT::TimeoutNode root;
00023 BT::AsyncActionTest action;
00024
00025 DeadlineTest() : root("deadline", 250), action("action")
00026 {
00027 root.setChild(&action);
00028 }
00029 ~DeadlineTest()
00030 {
00031 haltAllActions(&root);
00032 }
00033 };
00034
00035 struct RepeatTest : testing::Test
00036 {
00037 BT::RepeatNode root;
00038 BT::SyncActionTest action;
00039
00040 RepeatTest() : root("repeat", 3), action("action")
00041 {
00042 root.setChild(&action);
00043 }
00044 ~RepeatTest()
00045 {
00046 haltAllActions(&root);
00047 }
00048 };
00049
00050 struct RetryTest : testing::Test
00051 {
00052 BT::RetryNode root;
00053 BT::SyncActionTest action;
00054
00055 RetryTest() : root("retry", 3), action("action")
00056 {
00057 root.setChild(&action);
00058 }
00059 ~RetryTest()
00060 {
00061 haltAllActions(&root);
00062 }
00063 };
00064
00065
00066
00067 TEST_F(DeadlineTest, DeadlineTriggeredTest)
00068 {
00069 BT::NodeStatus state = root.executeTick();
00070
00071 action.setTime(3);
00072
00073 ASSERT_EQ(NodeStatus::RUNNING, action.status());
00074 ASSERT_EQ(NodeStatus::RUNNING, state);
00075
00076 std::this_thread::sleep_for(std::chrono::milliseconds(350));
00077 state = root.executeTick();
00078 ASSERT_EQ(NodeStatus::IDLE, action.status());
00079 ASSERT_EQ(NodeStatus::FAILURE, state);
00080 }
00081
00082 TEST_F(DeadlineTest, DeadlineNotTriggeredTest)
00083 {
00084 BT::NodeStatus state = root.executeTick();
00085
00086 action.setTime(2);
00087
00088 ASSERT_EQ(NodeStatus::RUNNING, action.status());
00089 ASSERT_EQ(NodeStatus::RUNNING, state);
00090
00091 std::this_thread::sleep_for(std::chrono::milliseconds(350));
00092 state = root.executeTick();
00093 ASSERT_EQ(NodeStatus::IDLE, action.status());
00094 ASSERT_EQ(NodeStatus::SUCCESS, state);
00095 }
00096
00097 TEST_F(RetryTest, RetryTestA)
00098 {
00099 action.setBoolean(false);
00100
00101 root.executeTick();
00102 ASSERT_EQ(NodeStatus::RUNNING, root.status());
00103 ASSERT_EQ(1, action.tickCount() );
00104
00105 root.executeTick();
00106 ASSERT_EQ(NodeStatus::RUNNING, root.status());
00107 ASSERT_EQ(2, action.tickCount() );
00108
00109 root.executeTick();
00110 ASSERT_EQ(NodeStatus::FAILURE, root.status());
00111 ASSERT_EQ(3, action.tickCount() );
00112
00113
00114 action.resetTicks();
00115 root.executeTick();
00116 ASSERT_EQ(NodeStatus::RUNNING, root.status());
00117 ASSERT_EQ(1, action.tickCount() );
00118
00119 action.setBoolean(true);
00120
00121 root.executeTick();
00122 ASSERT_EQ(NodeStatus::SUCCESS, root.status());
00123 ASSERT_EQ(2, action.tickCount() );
00124 }
00125
00126 TEST_F(RepeatTest, RepeatTestA)
00127 {
00128 action.setBoolean(false);
00129
00130 root.executeTick();
00131 ASSERT_EQ(NodeStatus::FAILURE, root.status());
00132 ASSERT_EQ(1, action.tickCount() );
00133
00134 root.executeTick();
00135 ASSERT_EQ(NodeStatus::FAILURE, root.status());
00136 ASSERT_EQ(2, action.tickCount() );
00137
00138
00139 action.resetTicks();
00140 action.setBoolean(true);
00141
00142 root.executeTick();
00143 ASSERT_EQ(NodeStatus::RUNNING, root.status());
00144 ASSERT_EQ(1, action.tickCount() );
00145
00146 root.executeTick();
00147 ASSERT_EQ(NodeStatus::RUNNING, root.status());
00148 ASSERT_EQ(2, action.tickCount() );
00149
00150 root.executeTick();
00151 ASSERT_EQ(NodeStatus::SUCCESS, root.status());
00152 ASSERT_EQ(3, action.tickCount() );
00153
00154
00155 action.resetTicks();
00156 action.setBoolean(true);
00157
00158 root.executeTick();
00159 ASSERT_EQ(NodeStatus::RUNNING, root.status());
00160 ASSERT_EQ(1, action.tickCount() );
00161
00162 root.executeTick();
00163 ASSERT_EQ(NodeStatus::RUNNING, root.status());
00164 ASSERT_EQ(2, action.tickCount() );
00165
00166 action.setBoolean(false);
00167 root.executeTick();
00168 ASSERT_EQ(NodeStatus::FAILURE, root.status());
00169 ASSERT_EQ(3, action.tickCount() );
00170
00171 }