gtest_parallel.cpp
Go to the documentation of this file.
1 /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
2 *
3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
4 * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
5 * 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:
6 * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
7 *
8 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
9 * 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,
10 * 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.
11 */
12 
13 #include <gtest/gtest.h>
14 #include "action_test_node.h"
15 #include "condition_test_node.h"
16 #include "behaviortree_cpp_v3/behavior_tree.h"
17 
18 using BT::NodeStatus;
19 using std::chrono::milliseconds;
20 
21 struct SimpleParallelTest : testing::Test
22 {
23  BT::ParallelNode root;
24  BT::AsyncActionTest action_1;
25  BT::ConditionTestNode condition_1;
26 
27  BT::AsyncActionTest action_2;
28  BT::ConditionTestNode condition_2;
29 
30  SimpleParallelTest() :
31  root("root_parallel", 4),
32  action_1("action_1", milliseconds(100)),
33  condition_1("condition_1"),
34  action_2("action_2", milliseconds(300)),
35  condition_2("condition_2")
36  {
37  root.addChild(&condition_1);
38  root.addChild(&action_1);
39  root.addChild(&condition_2);
40  root.addChild(&action_2);
41  }
42  ~SimpleParallelTest()
43  {}
44 };
45 
46 struct ComplexParallelTest : testing::Test
47 {
48  BT::ParallelNode parallel_root;
49  BT::ParallelNode parallel_left;
50  BT::ParallelNode parallel_right;
51 
52  BT::AsyncActionTest action_L1;
53  BT::ConditionTestNode condition_L1;
54 
55  BT::AsyncActionTest action_L2;
56  BT::ConditionTestNode condition_L2;
57 
58  BT::AsyncActionTest action_R;
59  BT::ConditionTestNode condition_R;
60 
61  ComplexParallelTest() :
62  parallel_root("root", 2),
63  parallel_left("par1", 3),
64  parallel_right("par2", 1),
65  action_L1("action_1", milliseconds(100)),
66  condition_L1("condition_1"),
67  action_L2("action_2", milliseconds(200)),
68  condition_L2("condition_2"),
69  action_R("action_3", milliseconds(400)),
70  condition_R("condition_3")
71  {
72  parallel_root.addChild(&parallel_left);
73  {
74  parallel_left.addChild(&condition_L1);
75  parallel_left.addChild(&action_L1);
76  parallel_left.addChild(&condition_L2);
77  parallel_left.addChild(&action_L2);
78  }
79  parallel_root.addChild(&parallel_right);
80  {
81  parallel_right.addChild(&condition_R);
82  parallel_right.addChild(&action_R);
83  }
84  }
85  ~ComplexParallelTest()
86  {}
87 };
88 
89 /****************TESTS START HERE***************************/
90 
91 TEST_F(SimpleParallelTest, ConditionsTrue)
92 {
93  BT::NodeStatus state = root.executeTick();
94 
95  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
96  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
97  ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
98  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
99  ASSERT_EQ(NodeStatus::RUNNING, state);
100 
101  std::this_thread::sleep_for(milliseconds(200));
102  state = root.executeTick();
103 
104  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
105  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
106  ASSERT_EQ(NodeStatus::SUCCESS, action_1.status());
107  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
108  ASSERT_EQ(NodeStatus::RUNNING, state);
109 
110  std::this_thread::sleep_for(milliseconds(200));
111  state = root.executeTick();
112 
113  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
114  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
115  ASSERT_EQ(NodeStatus::IDLE, action_1.status());
116  ASSERT_EQ(NodeStatus::IDLE, action_2.status());
117  ASSERT_EQ(NodeStatus::SUCCESS, state);
118 }
119 
120 TEST_F(SimpleParallelTest, Threshold_3)
121 {
122  root.setSuccessThreshold(3);
123  action_1.setTime(milliseconds(100));
124  action_2.setTime(milliseconds(500)); // this takes a lot of time
125 
126  BT::NodeStatus state = root.executeTick();
127  // first tick, zero wait
128  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
129  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
130  ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
131  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
132  ASSERT_EQ(NodeStatus::RUNNING, state);
133 
134  std::this_thread::sleep_for(milliseconds(150));
135  state = root.executeTick();
136  // second tick: action1 should be completed, but not action2
137  // nevertheless it is sufficient because threshold is 3
138  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
139  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
140  ASSERT_EQ(NodeStatus::IDLE, action_1.status());
141  ASSERT_EQ(NodeStatus::IDLE, action_2.status());
142  ASSERT_EQ(NodeStatus::SUCCESS, state);
143 }
144 
145 TEST_F(SimpleParallelTest, Threshold_neg2)
146 {
147  root.setSuccessThreshold(-2);
148  action_1.setTime(milliseconds(100));
149  action_2.setTime(milliseconds(500)); // this takes a lot of time
150 
151  BT::NodeStatus state = root.executeTick();
152  // first tick, zero wait
153  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
154  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
155  ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
156  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
157  ASSERT_EQ(NodeStatus::RUNNING, state);
158 
159  std::this_thread::sleep_for(milliseconds(150));
160  state = root.executeTick();
161  // second tick: action1 should be completed, but not action2
162  // nevertheless it is sufficient because threshold is 3
163  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
164  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
165  ASSERT_EQ(NodeStatus::IDLE, action_1.status());
166  ASSERT_EQ(NodeStatus::IDLE, action_2.status());
167  ASSERT_EQ(NodeStatus::SUCCESS, state);
168 }
169 
170 TEST_F(SimpleParallelTest, Threshold_neg1)
171 {
172  root.setSuccessThreshold(-1);
173  action_1.setTime(milliseconds(100));
174  action_2.setTime(milliseconds(500)); // this takes a lot of time
175 
176  BT::NodeStatus state = root.executeTick();
177  // first tick, zero wait
178  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
179  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
180  ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
181  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
182  ASSERT_EQ(NodeStatus::RUNNING, state);
183 
184  std::this_thread::sleep_for(milliseconds(150));
185  state = root.executeTick();
186  // second tick: action1 should be completed, but not action2
187  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
188  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
189  ASSERT_EQ(NodeStatus::SUCCESS, action_1.status());
190  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
191  ASSERT_EQ(NodeStatus::RUNNING, state);
192 
193  std::this_thread::sleep_for(milliseconds(650));
194  state = root.executeTick();
195  // third tick: all actions completed
196  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
197  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
198  ASSERT_EQ(NodeStatus::IDLE, action_1.status());
199  ASSERT_EQ(NodeStatus::IDLE, action_2.status());
200  ASSERT_EQ(NodeStatus::SUCCESS, state);
201 }
202 
203 TEST_F(SimpleParallelTest, Threshold_thresholdFneg1)
204 {
205  root.setSuccessThreshold(1);
206  root.setFailureThreshold(-1);
207  action_1.setTime(milliseconds(100));
208  action_1.setExpectedResult(NodeStatus::FAILURE);
209  condition_1.setExpectedResult(NodeStatus::FAILURE);
210  action_2.setTime(milliseconds(200));
211  condition_2.setExpectedResult(NodeStatus::FAILURE);
212  action_2.setExpectedResult(NodeStatus::FAILURE);
213 
214  BT::NodeStatus state = root.executeTick();
215  ASSERT_EQ(NodeStatus::RUNNING, state);
216 
217  std::this_thread::sleep_for(milliseconds(250));
218  state = root.executeTick();
219  ASSERT_EQ(NodeStatus::FAILURE, state);
220 }
221 
222 TEST_F(SimpleParallelTest, Threshold_2)
223 {
224  root.setSuccessThreshold(2);
225  BT::NodeStatus state = root.executeTick();
226 
227  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
228  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
229  ASSERT_EQ(NodeStatus::IDLE, action_1.status());
230  ASSERT_EQ(NodeStatus::IDLE, action_2.status());
231  ASSERT_EQ(NodeStatus::SUCCESS, state);
232 }
233 
234 TEST_F(ComplexParallelTest, ConditionsTrue)
235 {
236  BT::NodeStatus state = parallel_root.executeTick();
237 
238  ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
239  ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
240  ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
241  ASSERT_EQ(NodeStatus::RUNNING, action_L1.status());
242  ASSERT_EQ(NodeStatus::RUNNING, action_L2.status());
243 
244  ASSERT_EQ(NodeStatus::SUCCESS, parallel_right.status());
245  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
246  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
247 
248  ASSERT_EQ(NodeStatus::RUNNING, state);
249  //----------------------------------------
250  std::this_thread::sleep_for(milliseconds(200));
251  state = parallel_root.executeTick();
252 
253  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
254  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
255  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
256  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
257  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
258 
259  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
260  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
261  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
262 
263  ASSERT_EQ(NodeStatus::SUCCESS, state);
264 }
265 
266 TEST_F(ComplexParallelTest, ConditionsLeftFalse)
267 {
268  parallel_left.setFailureThreshold(3);
269  parallel_left.setSuccessThreshold(3);
270  condition_L1.setExpectedResult(NodeStatus::FAILURE);
271  condition_L2.setExpectedResult(NodeStatus::FAILURE);
272  BT::NodeStatus state = parallel_root.executeTick();
273 
274  // It fails because Parallel Left it will never succeed (two already fail)
275  // even though threshold_failure == 3
276 
277  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
278  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
279  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
280  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
281  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
282 
283  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
284  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
285  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
286 
287  ASSERT_EQ(NodeStatus::FAILURE, state);
288 }
289 
290 TEST_F(ComplexParallelTest, ConditionRightFalse)
291 {
292  condition_R.setExpectedResult(NodeStatus::FAILURE);
293  BT::NodeStatus state = parallel_root.executeTick();
294 
295  // It fails because threshold_failure is 1 for parallel right and
296  // condition_R fails
297 
298  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
299  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
300  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
301  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
302  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
303 
304  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
305  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
306  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
307 
308  ASSERT_EQ(NodeStatus::FAILURE, state);
309 }
310 
311 TEST_F(ComplexParallelTest, ConditionRightFalse_thresholdF_2)
312 {
313  parallel_right.setFailureThreshold(2);
314  condition_R.setExpectedResult(NodeStatus::FAILURE);
315  BT::NodeStatus state = parallel_root.executeTick();
316 
317  // All the actions are running
318 
319  ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
320  ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
321  ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
322  ASSERT_EQ(NodeStatus::RUNNING, action_L1.status());
323  ASSERT_EQ(NodeStatus::RUNNING, action_L2.status());
324 
325  ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
326  ASSERT_EQ(NodeStatus::FAILURE, condition_R.status());
327  ASSERT_EQ(NodeStatus::RUNNING, action_R.status());
328 
329  ASSERT_EQ(NodeStatus::RUNNING, state);
330 
331  //----------------------------------------
332  std::this_thread::sleep_for(milliseconds(500));
333  state = parallel_root.executeTick();
334 
335  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
336  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
337  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
338  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
339  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
340 
341  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
342  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
343  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
344 
345  ASSERT_EQ(NodeStatus::SUCCESS, state);
346 }
347 
348 TEST_F(ComplexParallelTest, ConditionRightFalseAction1Done)
349 {
350  condition_R.setExpectedResult(NodeStatus::FAILURE);
351 
352  parallel_right.setFailureThreshold(2);
353  parallel_left.setSuccessThreshold(4);
354 
355  BT::NodeStatus state = parallel_root.executeTick();
356  std::this_thread::sleep_for(milliseconds(300));
357 
358  // parallel_1 hasn't realize (yet) that action_1 has succeeded
359  ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
360  ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
361  ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
362  ASSERT_EQ(NodeStatus::SUCCESS, action_L1.status());
363  ASSERT_EQ(NodeStatus::SUCCESS, action_L2.status());
364 
365  ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
366 
367  //------------------------
368  state = parallel_root.executeTick();
369 
370  ASSERT_EQ(NodeStatus::SUCCESS, parallel_left.status());
371  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
372  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
373  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
374  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
375 
376  ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
377  ASSERT_EQ(NodeStatus::RUNNING, action_R.status());
378 
379  ASSERT_EQ(NodeStatus::RUNNING, state);
380 
381  //----------------------------------
382  std::this_thread::sleep_for(milliseconds(300));
383  state = parallel_root.executeTick();
384 
385  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
386  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
387 
388  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
389  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
390 
391  ASSERT_EQ(NodeStatus::SUCCESS, state);
392 }
SimpleParallelTest::SimpleParallelTest
SimpleParallelTest()
Definition: gtest_parallel.cpp:30
ComplexParallelTest
Definition: gtest_parallel.cpp:46
ComplexParallelTest::action_L2
BT::AsyncActionTest action_L2
Definition: gtest_parallel.cpp:55
SimpleParallelTest::condition_1
BT::ConditionTestNode condition_1
Definition: gtest_parallel.cpp:25
SimpleParallelTest::action_2
BT::AsyncActionTest action_2
Definition: gtest_parallel.cpp:27
ComplexParallelTest::parallel_left
BT::ParallelNode parallel_left
Definition: gtest_parallel.cpp:49
BT::ParallelNode
The ParallelNode execute all its children concurrently, but not in separate threads!
Definition: parallel_node.h:40
ComplexParallelTest::condition_L2
BT::ConditionTestNode condition_L2
Definition: gtest_parallel.cpp:56
ComplexParallelTest::action_L1
BT::AsyncActionTest action_L1
Definition: gtest_parallel.cpp:52
ComplexParallelTest::ComplexParallelTest
ComplexParallelTest()
Definition: gtest_parallel.cpp:61
ComplexParallelTest::condition_L1
BT::ConditionTestNode condition_L1
Definition: gtest_parallel.cpp:53
BT::AsyncActionTest
Definition: action_test_node.h:32
ComplexParallelTest::parallel_root
BT::ParallelNode parallel_root
Definition: gtest_parallel.cpp:48
TEST_F
TEST_F(SimpleParallelTest, ConditionsTrue)
Definition: gtest_parallel.cpp:91
ComplexParallelTest::action_R
BT::AsyncActionTest action_R
Definition: gtest_parallel.cpp:58
condition_test_node.h
SimpleParallelTest
Definition: gtest_parallel.cpp:21
SimpleParallelTest::condition_2
BT::ConditionTestNode condition_2
Definition: gtest_parallel.cpp:28
ComplexParallelTest::parallel_right
BT::ParallelNode parallel_right
Definition: gtest_parallel.cpp:50
behavior_tree.h
ComplexParallelTest::~ComplexParallelTest
~ComplexParallelTest()
Definition: gtest_parallel.cpp:85
SimpleParallelTest::~SimpleParallelTest
~SimpleParallelTest()
Definition: gtest_parallel.cpp:42
action_test_node.h
ComplexParallelTest::condition_R
BT::ConditionTestNode condition_R
Definition: gtest_parallel.cpp:59
SimpleParallelTest::root
BT::ParallelNode root
Definition: gtest_parallel.cpp:23
BT::ConditionTestNode
Definition: condition_test_node.h:8
BT::ControlNode::addChild
void addChild(TreeNode *child)
The method used to add nodes to the children vector.
Definition: control_node.cpp:22
BT::NodeStatus
NodeStatus
Definition: basic_types.h:35
SimpleParallelTest::action_1
BT::AsyncActionTest action_1
Definition: gtest_parallel.cpp:24

behaviortree_cpp_v3
Author(s): Michele Colledanchise, Davide Faconti
autogenerated on Wed Jun 26 2024 02:51:19