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"
17 
18 using BT::NodeStatus;
19 using std::chrono::milliseconds;
20 
21 struct SimpleParallelTest : testing::Test
22 {
26 
29 
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  }
43  {
44 
45  }
46 };
47 
48 struct ComplexParallelTest : testing::Test
49 {
53 
56 
59 
62 
64  : parallel_root("root", 2)
65  , parallel_left("par1", 3)
66  , parallel_right("par2", 1)
67  , action_L1("action_1", milliseconds(100) )
68  , condition_L1("condition_1")
69  , action_L2("action_2", milliseconds(200) )
70  , condition_L2("condition_2")
71  , action_R("action_3", milliseconds(400) )
72  , condition_R("condition_3")
73  {
74  parallel_root.addChild(&parallel_left);
75  {
76  parallel_left.addChild(&condition_L1);
77  parallel_left.addChild(&action_L1);
78  parallel_left.addChild(&condition_L2);
79  parallel_left.addChild(&action_L2);
80  }
81  parallel_root.addChild(&parallel_right);
82  {
83  parallel_right.addChild(&condition_R);
84  parallel_right.addChild(&action_R);
85  }
86  }
88  {
89  }
90 };
91 
92 /****************TESTS START HERE***************************/
93 
94 TEST_F(SimpleParallelTest, ConditionsTrue)
95 {
97 
98  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
99  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
100  ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
101  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
102  ASSERT_EQ(NodeStatus::RUNNING, state);
103 
104  std::this_thread::sleep_for( milliseconds(200) );
105  state = root.executeTick();
106 
107  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
108  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
109  ASSERT_EQ(NodeStatus::SUCCESS, action_1.status());
110  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
111  ASSERT_EQ(NodeStatus::RUNNING, state);
112 
113  std::this_thread::sleep_for( milliseconds(200) );
114  state = root.executeTick();
115 
116  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
117  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
118  ASSERT_EQ(NodeStatus::IDLE, action_1.status());
119  ASSERT_EQ(NodeStatus::IDLE, action_2.status());
120  ASSERT_EQ(NodeStatus::SUCCESS, state);
121 }
122 
124 {
125  root.setThresholdM(3);
126  action_1.setTime( milliseconds(100) );
127  action_2.setTime( milliseconds(500) ); // this takes a lot of time
128 
129  BT::NodeStatus state = root.executeTick();
130  // first tick, zero wait
131  ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
132  ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
133  ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
134  ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
135  ASSERT_EQ(NodeStatus::RUNNING, state);
136 
137  std::this_thread::sleep_for( milliseconds(150) );
138  state = root.executeTick();
139  // second tick: action1 should be completed, but not action2
140  // nevertheless it is sufficient because threshold is 3
141  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
142  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
143  ASSERT_EQ(NodeStatus::IDLE, action_1.status());
144  ASSERT_EQ(NodeStatus::IDLE, action_2.status());
145  ASSERT_EQ(NodeStatus::SUCCESS, state);
146 }
147 
149 {
150  root.setThresholdM(2);
151  BT::NodeStatus state = root.executeTick();
152 
153  ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
154  ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
155  ASSERT_EQ(NodeStatus::IDLE, action_1.status());
156  ASSERT_EQ(NodeStatus::IDLE, action_2.status());
157  ASSERT_EQ(NodeStatus::SUCCESS, state);
158 }
159 
160 TEST_F(ComplexParallelTest, ConditionsTrue)
161 {
162  BT::NodeStatus state = parallel_root.executeTick();
163 
164  ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
165  ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
166  ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
167  ASSERT_EQ(NodeStatus::RUNNING, action_L1.status());
168  ASSERT_EQ(NodeStatus::RUNNING, action_L2.status());
169 
170  ASSERT_EQ(NodeStatus::SUCCESS, parallel_right.status());
171  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
172  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
173 
174  ASSERT_EQ(NodeStatus::RUNNING, state);
175  //----------------------------------------
176  std::this_thread::sleep_for(milliseconds(200));
177  state = parallel_root.executeTick();
178 
179  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
180  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
181  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
182  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
183  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
184 
185  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
186  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
187  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
188 
189  ASSERT_EQ(NodeStatus::SUCCESS, state);
190 }
191 
192 TEST_F(ComplexParallelTest, ConditionsLeftFalse)
193 {
194  parallel_left.setThresholdFM(3);
195  parallel_left.setThresholdM(3);
196  condition_L1.setExpectedResult(NodeStatus::FAILURE);
197  condition_L2.setExpectedResult(NodeStatus::FAILURE);
198  BT::NodeStatus state = parallel_root.executeTick();
199 
200  // It fails because Parallel Left it will never succeed (two already fail)
201  // even though threshold_failure == 3
202 
203  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
204  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
205  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
206  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
207  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
208 
209  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
210  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
211  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
212 
213  ASSERT_EQ(NodeStatus::FAILURE, state);
214 }
215 
216 TEST_F(ComplexParallelTest, ConditionRightFalse)
217 {
218  condition_R.setExpectedResult(NodeStatus::FAILURE);
219  BT::NodeStatus state = parallel_root.executeTick();
220 
221  // It fails because threshold_failure is 1 for parallel right and
222  // condition_R fails
223 
224  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
225  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
226  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
227  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
228  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
229 
230  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
231  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
232  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
233 
234  ASSERT_EQ(NodeStatus::FAILURE, state);
235 }
236 
237 TEST_F(ComplexParallelTest, ConditionRightFalse_thresholdF_2)
238 {
239  parallel_right.setThresholdFM(2);
240  condition_R.setExpectedResult(NodeStatus::FAILURE);
241  BT::NodeStatus state = parallel_root.executeTick();
242 
243  // All the actions are running
244 
245  ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
246  ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
247  ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
248  ASSERT_EQ(NodeStatus::RUNNING, action_L1.status());
249  ASSERT_EQ(NodeStatus::RUNNING, action_L2.status());
250 
251  ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
252  ASSERT_EQ(NodeStatus::FAILURE, condition_R.status());
253  ASSERT_EQ(NodeStatus::RUNNING, action_R.status());
254 
255  ASSERT_EQ(NodeStatus::RUNNING, state);
256 
257  //----------------------------------------
258  std::this_thread::sleep_for(milliseconds(500));
259  state = parallel_root.executeTick();
260 
261  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
262  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
263  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
264  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
265  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
266 
267  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
268  ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
269  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
270 
271  ASSERT_EQ(NodeStatus::SUCCESS, state);
272 }
273 
274 TEST_F(ComplexParallelTest, ConditionRightFalseAction1Done)
275 {
276  condition_R.setExpectedResult(NodeStatus::FAILURE);
277 
278  parallel_right.setThresholdFM(2);
279  parallel_left.setThresholdM(4);
280 
281  BT::NodeStatus state = parallel_root.executeTick();
282  std::this_thread::sleep_for(milliseconds(300));
283 
284  // parallel_1 hasn't realize (yet) that action_1 has succeeded
285  ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
286  ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
287  ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
288  ASSERT_EQ(NodeStatus::SUCCESS, action_L1.status());
289  ASSERT_EQ(NodeStatus::SUCCESS, action_L2.status());
290 
291  ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
292 
293  //------------------------
294  state = parallel_root.executeTick();
295 
296  ASSERT_EQ(NodeStatus::SUCCESS, parallel_left.status());
297  ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
298  ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
299  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
300  ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
301 
302  ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
303  ASSERT_EQ(NodeStatus::RUNNING, action_R.status());
304 
305  ASSERT_EQ(NodeStatus::RUNNING, state);
306 
307  //----------------------------------
308  std::this_thread::sleep_for(milliseconds(300));
309  state = parallel_root.executeTick();
310 
311  ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
312  ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
313 
314  ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
315  ASSERT_EQ(NodeStatus::IDLE, action_R.status());
316 
317  ASSERT_EQ(NodeStatus::SUCCESS, state);
318 }
BT::AsyncActionTest action_L2
BT::ConditionTestNode condition_L2
BT::ConditionTestNode condition_2
BT::ConditionTestNode condition_R
BT::AsyncActionTest action_L1
BT::ParallelNode parallel_right
void setThresholdM(unsigned int threshold_M)
BT::ParallelNode parallel_left
BT::AsyncActionTest action_R
void setTime(BT::Duration time)
BT::ConditionTestNode condition_L1
void addChild(TreeNode *child)
The method used to add nodes to the children vector.
BT::AsyncActionTest action_1
BT::ParallelNode root
BT::ParallelNode parallel_root
NodeStatus status() const
Definition: tree_node.cpp:56
TEST_F(SimpleParallelTest, ConditionsTrue)
NodeStatus
Definition: basic_types.h:35
virtual BT::NodeStatus executeTick()
The method that should be used to invoke tick() and setStatus();.
Definition: tree_node.cpp:33
BT::ConditionTestNode condition_1
BT::AsyncActionTest action_2


behaviotree_cpp_v3
Author(s): Michele Colledanchise, Davide Faconti
autogenerated on Tue May 4 2021 02:56:24