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 using std::chrono::milliseconds;
00020
00021 struct SimpleSequenceTest : testing::Test
00022 {
00023 BT::SequenceNode root;
00024 BT::AsyncActionTest action;
00025 BT::ConditionTestNode condition;
00026
00027 SimpleSequenceTest() :
00028 root("root_sequence")
00029 , action("action", milliseconds(100))
00030 , condition("condition")
00031 {
00032 root.addChild(&condition);
00033 root.addChild(&action);
00034 }
00035 ~SimpleSequenceTest()
00036 {
00037 haltAllActions(&root);
00038 }
00039 };
00040
00041 struct ComplexSequenceTest : testing::Test
00042 {
00043 BT::ReactiveSequence root;
00044 BT::AsyncActionTest action_1;
00045 BT::ConditionTestNode condition_1;
00046 BT::ConditionTestNode condition_2;
00047
00048 BT::SequenceNode seq_conditions;
00049
00050 ComplexSequenceTest()
00051 : root("root")
00052 , action_1("action_1", milliseconds(100))
00053 , condition_1("condition_1")
00054 , condition_2("condition_2")
00055 , seq_conditions("sequence_conditions")
00056 {
00057 root.addChild(&seq_conditions);
00058 {
00059 seq_conditions.addChild(&condition_1);
00060 seq_conditions.addChild(&condition_2);
00061 }
00062 root.addChild(&action_1);
00063 }
00064 ~ComplexSequenceTest()
00065 {
00066 haltAllActions(&root);
00067 }
00068 };
00069
00070 struct SequenceTripleActionTest : testing::Test
00071 {
00072 BT::SequenceNode root;
00073 BT::ConditionTestNode condition;
00074 BT::AsyncActionTest action_1;
00075 BT::SyncActionTest action_2;
00076 BT::AsyncActionTest action_3;
00077
00078 SequenceTripleActionTest()
00079 : root("root_sequence")
00080 , condition("condition")
00081 , action_1("action_1", milliseconds(100))
00082 , action_2("action_2")
00083 , action_3("action_3", milliseconds(100))
00084 {
00085 root.addChild(&condition);
00086 root.addChild(&action_1);
00087 root.addChild(&action_2);
00088 root.addChild(&action_3);
00089 }
00090 ~SequenceTripleActionTest()
00091 {
00092 haltAllActions(&root);
00093 }
00094 };
00095
00096 struct ComplexSequence2ActionsTest : testing::Test
00097 {
00098 BT::SequenceNode root;
00099 BT::AsyncActionTest action_1;
00100 BT::AsyncActionTest action_2;
00101 BT::SequenceNode seq_1;
00102 BT::SequenceNode seq_2;
00103
00104 BT::ConditionTestNode condition_1;
00105 BT::ConditionTestNode condition_2;
00106
00107 ComplexSequence2ActionsTest()
00108 : root("root_sequence")
00109 , action_1("action_1", milliseconds(100))
00110 , action_2("action_2", milliseconds(100))
00111 , seq_1("sequence_1")
00112 , seq_2("sequence_2")
00113 , condition_1("condition_1")
00114 , condition_2("condition_2")
00115 {
00116 root.addChild(&seq_1);
00117 {
00118 seq_1.addChild(&condition_1);
00119 seq_1.addChild(&action_1);
00120 }
00121 root.addChild(&seq_2);
00122 {
00123 seq_2.addChild(&condition_2);
00124 seq_2.addChild(&action_2);
00125 }
00126 }
00127 ~ComplexSequence2ActionsTest()
00128 {
00129 haltAllActions(&root);
00130 }
00131 };
00132
00133 struct SimpleSequenceWithMemoryTest : testing::Test
00134 {
00135 BT::SequenceStarNode root;
00136 BT::AsyncActionTest action;
00137 BT::ConditionTestNode condition;
00138
00139 SimpleSequenceWithMemoryTest() :
00140 root("root_sequence")
00141 , action("action", milliseconds(100))
00142 , condition("condition")
00143 {
00144 root.addChild(&condition);
00145 root.addChild(&action);
00146 }
00147 ~SimpleSequenceWithMemoryTest()
00148 {
00149 haltAllActions(&root);
00150 }
00151 };
00152
00153 struct ComplexSequenceWithMemoryTest : testing::Test
00154 {
00155 BT::SequenceStarNode root;
00156
00157 BT::AsyncActionTest action_1;
00158 BT::AsyncActionTest action_2;
00159
00160 BT::ConditionTestNode condition_1;
00161 BT::ConditionTestNode condition_2;
00162
00163 BT::SequenceStarNode seq_conditions;
00164 BT::SequenceStarNode seq_actions;
00165
00166 ComplexSequenceWithMemoryTest()
00167 : root("root_sequence")
00168 , action_1("action_1", milliseconds(100))
00169 , action_2("action_2", milliseconds(100))
00170 , condition_1("condition_1")
00171 , condition_2("condition_2")
00172 , seq_conditions("sequence_conditions")
00173 , seq_actions("sequence_actions")
00174 {
00175 root.addChild(&seq_conditions);
00176 {
00177 seq_conditions.addChild(&condition_1);
00178 seq_conditions.addChild(&condition_2);
00179 }
00180 root.addChild(&seq_actions);
00181 {
00182 seq_actions.addChild(&action_1);
00183 seq_actions.addChild(&action_2);
00184 }
00185 }
00186 ~ComplexSequenceWithMemoryTest()
00187 {
00188 haltAllActions(&root);
00189 }
00190 };
00191
00192 struct SimpleParallelTest : testing::Test
00193 {
00194 BT::ParallelNode root;
00195 BT::AsyncActionTest action_1;
00196 BT::ConditionTestNode condition_1;
00197
00198 BT::AsyncActionTest action_2;
00199 BT::ConditionTestNode condition_2;
00200
00201 SimpleParallelTest()
00202 : root("root_parallel", 4)
00203 , action_1("action_1", milliseconds(100))
00204 , condition_1("condition_1")
00205 , action_2("action_2", milliseconds(100))
00206 , condition_2("condition_2")
00207 {
00208 root.addChild(&condition_1);
00209 root.addChild(&action_1);
00210 root.addChild(&condition_2);
00211 root.addChild(&action_2);
00212 }
00213 ~SimpleParallelTest()
00214 {
00215 haltAllActions(&root);
00216 }
00217 };
00218
00219
00220
00221 TEST_F(SimpleSequenceTest, ConditionTrue)
00222 {
00223 std::cout << "Ticking the root node !" << std::endl << std::endl;
00224
00225 BT::NodeStatus state = root.executeTick();
00226
00227 ASSERT_EQ(NodeStatus::RUNNING, action.status());
00228 ASSERT_EQ(NodeStatus::RUNNING, state);
00229 }
00230
00231 TEST_F(SimpleSequenceTest, ConditionTurnToFalse)
00232 {
00233 condition.setBoolean(false);
00234 BT::NodeStatus state = root.executeTick();
00235
00236 state = root.executeTick();
00237 ASSERT_EQ(NodeStatus::FAILURE, state);
00238 ASSERT_EQ(NodeStatus::IDLE, condition.status());
00239 ASSERT_EQ(NodeStatus::IDLE, action.status());
00240 }
00241
00242 TEST_F(ComplexSequenceTest, ComplexSequenceConditionsTrue)
00243 {
00244 BT::NodeStatus state = root.executeTick();
00245
00246 ASSERT_EQ(NodeStatus::RUNNING, state);
00247 ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
00248 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00249 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00250 ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
00251 }
00252
00253 TEST_F(SequenceTripleActionTest, TripleAction)
00254 {
00255 using namespace BT;
00256 using namespace std::chrono;
00257 const auto timeout = system_clock::now() + milliseconds(650);
00258
00259 action_1.setTime( milliseconds(300) );
00260 action_3.setTime( milliseconds(300) );
00261
00262
00263
00264 NodeStatus state = root.executeTick();
00265
00266 ASSERT_EQ(NodeStatus::RUNNING, state);
00267 ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
00268 ASSERT_EQ(NodeStatus::IDLE, action_2.status());
00269 ASSERT_EQ(NodeStatus::IDLE, action_3.status());
00270
00271
00272 while (state != NodeStatus::SUCCESS && system_clock::now() < timeout)
00273 {
00274 std::this_thread::sleep_for(milliseconds(10));
00275 state = root.executeTick();
00276 }
00277
00278 ASSERT_EQ(NodeStatus::SUCCESS, state);
00279
00280
00281 ASSERT_EQ(condition.tickCount(), 1);
00282
00283 ASSERT_EQ(action_1.tickCount(), 1);
00284 ASSERT_EQ(action_2.tickCount(), 1);
00285 ASSERT_EQ(action_3.tickCount(), 1);
00286
00287 ASSERT_EQ(NodeStatus::IDLE, action_1.status());
00288 ASSERT_EQ(NodeStatus::IDLE, action_2.status());
00289 ASSERT_EQ(NodeStatus::IDLE, action_3.status());
00290 ASSERT_TRUE(system_clock::now() < timeout);
00291 }
00292
00293 TEST_F(ComplexSequence2ActionsTest, ConditionsTrue)
00294 {
00295 BT::NodeStatus state = root.executeTick();
00296
00297 state = root.executeTick();
00298
00299 ASSERT_EQ(NodeStatus::RUNNING, state);
00300 ASSERT_EQ(NodeStatus::RUNNING, seq_1.status());
00301 ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
00302 ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
00303 ASSERT_EQ(NodeStatus::IDLE, seq_2.status());
00304 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00305 ASSERT_EQ(NodeStatus::IDLE, action_2.status());
00306
00307 std::this_thread::sleep_for(milliseconds(300));
00308 state = root.executeTick();
00309
00310 ASSERT_EQ(NodeStatus::RUNNING, state);
00311 ASSERT_EQ(NodeStatus::SUCCESS, seq_1.status());
00312 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00313 ASSERT_EQ(NodeStatus::IDLE, action_1.status());
00314 ASSERT_EQ(NodeStatus::RUNNING, seq_2.status());
00315 ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
00316 ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
00317
00318 state = root.executeTick();
00319 }
00320
00321 TEST_F(ComplexSequenceTest, ComplexSequenceConditions1ToFalse)
00322 {
00323 BT::NodeStatus state = root.executeTick();
00324
00325 condition_1.setBoolean(false);
00326
00327 state = root.executeTick();
00328
00329 ASSERT_EQ(NodeStatus::FAILURE, state);
00330 ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status());
00331 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00332 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00333 ASSERT_EQ(NodeStatus::IDLE, action_1.status());
00334 }
00335
00336 TEST_F(ComplexSequenceTest, ComplexSequenceConditions2ToFalse)
00337 {
00338 BT::NodeStatus state = root.executeTick();
00339
00340 condition_2.setBoolean(false);
00341
00342 state = root.executeTick();
00343
00344 ASSERT_EQ(NodeStatus::FAILURE, state);
00345 ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status());
00346 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00347 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00348 ASSERT_EQ(NodeStatus::IDLE, action_1.status());
00349 }
00350
00351 TEST_F(SimpleSequenceWithMemoryTest, ConditionTrue)
00352 {
00353 BT::NodeStatus state = root.executeTick();
00354 std::this_thread::sleep_for( milliseconds(50) );
00355
00356 ASSERT_EQ(NodeStatus::RUNNING, state);
00357 ASSERT_EQ(NodeStatus::SUCCESS, condition.status());
00358 ASSERT_EQ(NodeStatus::RUNNING, action.status());
00359 }
00360
00361 TEST_F(SimpleSequenceWithMemoryTest, ConditionTurnToFalse)
00362 {
00363 BT::NodeStatus state = root.executeTick();
00364
00365 ASSERT_EQ(NodeStatus::RUNNING, state);
00366 ASSERT_EQ(NodeStatus::SUCCESS, condition.status());
00367 ASSERT_EQ(NodeStatus::RUNNING, action.status());
00368
00369 condition.setBoolean(false);
00370 state = root.executeTick();
00371
00372 ASSERT_EQ(NodeStatus::RUNNING, state);
00373 ASSERT_EQ(NodeStatus::SUCCESS, condition.status());
00374 ASSERT_EQ(NodeStatus::RUNNING, action.status());
00375 }
00376
00377 TEST_F(ComplexSequenceWithMemoryTest, ConditionsTrue)
00378 {
00379 BT::NodeStatus state = root.executeTick();
00380
00381 ASSERT_EQ(NodeStatus::RUNNING, state);
00382 ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
00383 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00384 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00385 ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
00386 ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
00387 ASSERT_EQ(NodeStatus::IDLE, action_2.status());
00388 }
00389
00390 TEST_F(ComplexSequenceWithMemoryTest, Conditions1ToFase)
00391 {
00392 BT::NodeStatus state = root.executeTick();
00393
00394 condition_1.setBoolean(false);
00395 state = root.executeTick();
00396
00397
00398 ASSERT_EQ(NodeStatus::RUNNING, state);
00399 ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
00400 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00401 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00402 ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
00403 ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
00404 ASSERT_EQ(NodeStatus::IDLE, action_2.status());
00405 }
00406
00407 TEST_F(ComplexSequenceWithMemoryTest, Conditions2ToFalse)
00408 {
00409 BT::NodeStatus state = root.executeTick();
00410
00411 condition_2.setBoolean(false);
00412 state = root.executeTick();
00413
00414
00415 ASSERT_EQ(NodeStatus::RUNNING, state);
00416 ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
00417 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00418 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00419 ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
00420 ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
00421 ASSERT_EQ(NodeStatus::IDLE, action_2.status());
00422 }
00423
00424 TEST_F(ComplexSequenceWithMemoryTest, Action1DoneSeq)
00425 {
00426 root.executeTick();
00427
00428 condition_2.setBoolean(false);
00429 root.executeTick();
00430
00431
00432
00433 ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
00434 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00435 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00436 ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
00437 ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
00438 ASSERT_EQ(NodeStatus::IDLE, action_2.status());
00439
00440 std::this_thread::sleep_for(milliseconds(150));
00441 root.executeTick();
00442
00443 ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
00444 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00445 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00446 ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
00447 ASSERT_EQ(NodeStatus::SUCCESS, action_1.status());
00448 ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
00449
00450 std::this_thread::sleep_for(milliseconds(150));
00451 root.executeTick();
00452
00453 ASSERT_EQ(NodeStatus::SUCCESS, root.status());
00454 ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status());
00455 ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
00456 ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
00457 ASSERT_EQ(NodeStatus::IDLE, seq_actions.status());
00458 ASSERT_EQ(NodeStatus::IDLE, action_1.status());
00459 ASSERT_EQ(NodeStatus::IDLE, action_2.status());
00460 }