13 #include <gtest/gtest.h> 19 using std::chrono::milliseconds;
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")
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")
74 parallel_root.
addChild(¶llel_left);
76 parallel_left.
addChild(&condition_L1);
78 parallel_left.
addChild(&condition_L2);
81 parallel_root.
addChild(¶llel_right);
83 parallel_right.
addChild(&condition_R);
102 ASSERT_EQ(NodeStatus::RUNNING, state);
104 std::this_thread::sleep_for( milliseconds(200) );
111 ASSERT_EQ(NodeStatus::RUNNING, state);
113 std::this_thread::sleep_for( milliseconds(200) );
120 ASSERT_EQ(NodeStatus::SUCCESS, state);
135 ASSERT_EQ(NodeStatus::RUNNING, state);
137 std::this_thread::sleep_for( milliseconds(150) );
145 ASSERT_EQ(NodeStatus::SUCCESS, state);
157 ASSERT_EQ(NodeStatus::SUCCESS, state);
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());
170 ASSERT_EQ(NodeStatus::SUCCESS, parallel_right.status());
171 ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
172 ASSERT_EQ(NodeStatus::IDLE, action_R.status());
174 ASSERT_EQ(NodeStatus::RUNNING, state);
176 std::this_thread::sleep_for(milliseconds(200));
177 state = parallel_root.executeTick();
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());
185 ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
186 ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
187 ASSERT_EQ(NodeStatus::IDLE, action_R.status());
189 ASSERT_EQ(NodeStatus::SUCCESS, state);
194 parallel_left.setThresholdFM(3);
195 parallel_left.setThresholdM(3);
196 condition_L1.setExpectedResult(NodeStatus::FAILURE);
197 condition_L2.setExpectedResult(NodeStatus::FAILURE);
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());
209 ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
210 ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
211 ASSERT_EQ(NodeStatus::IDLE, action_R.status());
213 ASSERT_EQ(NodeStatus::FAILURE, state);
218 condition_R.setExpectedResult(NodeStatus::FAILURE);
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());
230 ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
231 ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
232 ASSERT_EQ(NodeStatus::IDLE, action_R.status());
234 ASSERT_EQ(NodeStatus::FAILURE, state);
239 parallel_right.setThresholdFM(2);
240 condition_R.setExpectedResult(NodeStatus::FAILURE);
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());
251 ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
252 ASSERT_EQ(NodeStatus::FAILURE, condition_R.status());
253 ASSERT_EQ(NodeStatus::RUNNING, action_R.status());
255 ASSERT_EQ(NodeStatus::RUNNING, state);
258 std::this_thread::sleep_for(milliseconds(500));
259 state = parallel_root.executeTick();
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());
267 ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
268 ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
269 ASSERT_EQ(NodeStatus::IDLE, action_R.status());
271 ASSERT_EQ(NodeStatus::SUCCESS, state);
276 condition_R.setExpectedResult(NodeStatus::FAILURE);
278 parallel_right.setThresholdFM(2);
279 parallel_left.setThresholdM(4);
282 std::this_thread::sleep_for(milliseconds(300));
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());
291 ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
294 state = parallel_root.executeTick();
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());
302 ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
303 ASSERT_EQ(NodeStatus::RUNNING, action_R.status());
305 ASSERT_EQ(NodeStatus::RUNNING, state);
308 std::this_thread::sleep_for(milliseconds(300));
309 state = parallel_root.executeTick();
311 ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
312 ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
314 ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
315 ASSERT_EQ(NodeStatus::IDLE, action_R.status());
317 ASSERT_EQ(NodeStatus::SUCCESS, state);
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 parallel_root
NodeStatus status() const
TEST_F(SimpleParallelTest, ConditionsTrue)
virtual BT::NodeStatus executeTick()
The method that should be used to invoke tick() and setStatus();.
BT::ConditionTestNode condition_1
BT::AsyncActionTest action_2