Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 1a67511767f0d422c346bbe181debaf7bd9e03a9 / . / wpilibcIntegrationTests / src / command / CommandTest.cpp
blob: 9b3f7d0cc93c8ebb9ac821f4bcac94887ad2cde1 [file] [log] [blame]
Brian Silverman26e4e522015-12-17 01:56:40 -0500[diff] [blame]1/*----------------------------------------------------------------------------*/
Brian Silverman1a675112016-02-20 20:42:49 -0500[diff] [blame^]2/* Copyright (c) FIRST 2014-2016. All Rights Reserved. */
Brian Silverman26e4e522015-12-17 01:56:40 -0500[diff] [blame]3/* Open Source Software - may be modified and shared by FRC teams. The code */
4/* must be accompanied by the FIRST BSD license file in the root directory of */
5/* the project. */
6/*----------------------------------------------------------------------------*/
7
8#include <Commands/CommandGroup.h>
9#include <Commands/Scheduler.h>
10#include <Commands/Subsystem.h>
11#include <DriverStation.h>
12#include <RobotState.h>
13#include <Timer.h>
14#include "command/MockCommand.h"
15#include "gtest/gtest.h"
16
17class CommandTest : public testing::Test {
18 protected:
19 virtual void SetUp() override {
20 RobotState::SetImplementation(DriverStation::GetInstance());
21 Scheduler::GetInstance()->SetEnabled(true);
22 }
23
24 /**
25 * Tears Down the Scheduler at the end of each test.
26 * Must be called at the end of each test inside each test in order to prevent
27 * them being deallocated
28 * when they leave the scope of the test (causing a segfault).
29 * This can not be done within the virtual void Teardown() method because it
30 * is called outside of the
31 * scope of the test.
32 */
33 void TeardownScheduler() { Scheduler::GetInstance()->ResetAll(); }
34
35 void AssertCommandState(MockCommand &command, int initialize, int execute,
36 int isFinished, int end, int interrupted) {
37 EXPECT_EQ(initialize, command.GetInitializeCount());
38 EXPECT_EQ(execute, command.GetExecuteCount());
39 EXPECT_EQ(isFinished, command.GetIsFinishedCount());
40 EXPECT_EQ(end, command.GetEndCount());
41 EXPECT_EQ(interrupted, command.GetInterruptedCount());
42 }
43};
44
45class ASubsystem : public Subsystem {
46 private:
47 Command *m_command = nullptr;
48
49 public:
50 ASubsystem(const std::string &name) : Subsystem(name) {}
51
52 virtual void InitDefaultCommand() override {
53 if (m_command != nullptr) {
54 SetDefaultCommand(m_command);
55 }
56 }
57
58 void Init(Command *command) { m_command = command; }
59};
60
61// CommandParallelGroupTest ported from CommandParallelGroupTest.java
62TEST_F(CommandTest, ParallelCommands) {
63 MockCommand command1;
64 MockCommand command2;
65 CommandGroup commandGroup;
66
67 commandGroup.AddParallel(&command1);
68 commandGroup.AddParallel(&command2);
69
70 AssertCommandState(command1, 0, 0, 0, 0, 0);
71 AssertCommandState(command2, 0, 0, 0, 0, 0);
72 commandGroup.Start();
73 AssertCommandState(command1, 0, 0, 0, 0, 0);
74 AssertCommandState(command2, 0, 0, 0, 0, 0);
75 Scheduler::GetInstance()->Run();
76 AssertCommandState(command1, 0, 0, 0, 0, 0);
77 AssertCommandState(command2, 0, 0, 0, 0, 0);
78 Scheduler::GetInstance()->Run();
79 AssertCommandState(command1, 1, 1, 1, 0, 0);
80 AssertCommandState(command2, 1, 1, 1, 0, 0);
81 Scheduler::GetInstance()->Run();
82 AssertCommandState(command1, 1, 2, 2, 0, 0);
83 AssertCommandState(command2, 1, 2, 2, 0, 0);
84 command1.SetHasFinished(true);
85 Scheduler::GetInstance()->Run();
86 AssertCommandState(command1, 1, 3, 3, 1, 0);
87 AssertCommandState(command2, 1, 3, 3, 0, 0);
88 Scheduler::GetInstance()->Run();
89 AssertCommandState(command1, 1, 3, 3, 1, 0);
90 AssertCommandState(command2, 1, 4, 4, 0, 0);
91 command2.SetHasFinished(true);
92 Scheduler::GetInstance()->Run();
93 AssertCommandState(command1, 1, 3, 3, 1, 0);
94 AssertCommandState(command2, 1, 5, 5, 1, 0);
95
96 TeardownScheduler();
97}
98// END CommandParallelGroupTest
99
100// CommandScheduleTest ported from CommandScheduleTest.java
101TEST_F(CommandTest, RunAndTerminate) {
102 MockCommand command;
103 command.Start();
104 AssertCommandState(command, 0, 0, 0, 0, 0);
105 Scheduler::GetInstance()->Run();
106 AssertCommandState(command, 0, 0, 0, 0, 0);
107 Scheduler::GetInstance()->Run();
108 AssertCommandState(command, 1, 1, 1, 0, 0);
109 Scheduler::GetInstance()->Run();
110 AssertCommandState(command, 1, 2, 2, 0, 0);
111 command.SetHasFinished(true);
112 AssertCommandState(command, 1, 2, 2, 0, 0);
113 Scheduler::GetInstance()->Run();
114 AssertCommandState(command, 1, 3, 3, 1, 0);
115 Scheduler::GetInstance()->Run();
116 AssertCommandState(command, 1, 3, 3, 1, 0);
117
118 TeardownScheduler();
119}
120
121TEST_F(CommandTest, RunAndCancel) {
122 MockCommand command;
123 command.Start();
124 AssertCommandState(command, 0, 0, 0, 0, 0);
125 Scheduler::GetInstance()->Run();
126 AssertCommandState(command, 0, 0, 0, 0, 0);
127 Scheduler::GetInstance()->Run();
128 AssertCommandState(command, 1, 1, 1, 0, 0);
129 Scheduler::GetInstance()->Run();
130 AssertCommandState(command, 1, 2, 2, 0, 0);
131 Scheduler::GetInstance()->Run();
132 AssertCommandState(command, 1, 3, 3, 0, 0);
133 command.Cancel();
134 AssertCommandState(command, 1, 3, 3, 0, 0);
135 Scheduler::GetInstance()->Run();
136 AssertCommandState(command, 1, 3, 3, 0, 1);
137 Scheduler::GetInstance()->Run();
138 AssertCommandState(command, 1, 3, 3, 0, 1);
139
140 TeardownScheduler();
141}
142// END CommandScheduleTest
143
144// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
145TEST_F(CommandTest, ThreeCommandOnSubSystem) {
146 ASubsystem subsystem("Three Command Test Subsystem");
147 MockCommand command1;
148 command1.Requires(&subsystem);
149 MockCommand command2;
150 command2.Requires(&subsystem);
151 MockCommand command3;
152 command3.Requires(&subsystem);
153
154 CommandGroup commandGroup;
155 commandGroup.AddSequential(&command1, 1.0);
156 commandGroup.AddSequential(&command2, 2.0);
157 commandGroup.AddSequential(&command3);
158
159 AssertCommandState(command1, 0, 0, 0, 0, 0);
160 AssertCommandState(command2, 0, 0, 0, 0, 0);
161 AssertCommandState(command3, 0, 0, 0, 0, 0);
162
163 commandGroup.Start();
164 AssertCommandState(command1, 0, 0, 0, 0, 0);
165 AssertCommandState(command2, 0, 0, 0, 0, 0);
166 AssertCommandState(command3, 0, 0, 0, 0, 0);
167
168 Scheduler::GetInstance()->Run();
169 AssertCommandState(command1, 0, 0, 0, 0, 0);
170 AssertCommandState(command2, 0, 0, 0, 0, 0);
171 AssertCommandState(command3, 0, 0, 0, 0, 0);
172
173 Scheduler::GetInstance()->Run();
174 AssertCommandState(command1, 1, 1, 1, 0, 0);
175 AssertCommandState(command2, 0, 0, 0, 0, 0);
176 AssertCommandState(command3, 0, 0, 0, 0, 0);
177 Wait(1); // command 1 timeout
178
179 Scheduler::GetInstance()->Run();
180 AssertCommandState(command1, 1, 1, 1, 0, 1);
181 AssertCommandState(command2, 1, 1, 1, 0, 0);
182 AssertCommandState(command3, 0, 0, 0, 0, 0);
183
184 Scheduler::GetInstance()->Run();
185 AssertCommandState(command1, 1, 1, 1, 0, 1);
186 AssertCommandState(command2, 1, 2, 2, 0, 0);
187 AssertCommandState(command3, 0, 0, 0, 0, 0);
188 Wait(2); // command 2 timeout
189
190 Scheduler::GetInstance()->Run();
191 AssertCommandState(command1, 1, 1, 1, 0, 1);
192 AssertCommandState(command2, 1, 2, 2, 0, 1);
193 AssertCommandState(command3, 1, 1, 1, 0, 0);
194
195 Scheduler::GetInstance()->Run();
196 AssertCommandState(command1, 1, 1, 1, 0, 1);
197 AssertCommandState(command2, 1, 2, 2, 0, 1);
198 AssertCommandState(command3, 1, 2, 2, 0, 0);
199 command3.SetHasFinished(true);
200 AssertCommandState(command1, 1, 1, 1, 0, 1);
201 AssertCommandState(command2, 1, 2, 2, 0, 1);
202 AssertCommandState(command3, 1, 2, 2, 0, 0);
203
204 Scheduler::GetInstance()->Run();
205 AssertCommandState(command1, 1, 1, 1, 0, 1);
206 AssertCommandState(command2, 1, 2, 2, 0, 1);
207 AssertCommandState(command3, 1, 3, 3, 1, 0);
208
209 Scheduler::GetInstance()->Run();
210 AssertCommandState(command1, 1, 1, 1, 0, 1);
211 AssertCommandState(command2, 1, 2, 2, 0, 1);
212 AssertCommandState(command3, 1, 3, 3, 1, 0);
213
214 TeardownScheduler();
215}
216// END CommandSequentialGroupTest
217
218// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
219TEST_F(CommandTest, OneCommandSupersedingAnotherBecauseOfDependencies) {
220 auto subsystem = new ASubsystem("Command Superseding Test Subsystem");
221 MockCommand command1;
222 command1.Requires(subsystem);
223 MockCommand command2;
224 command2.Requires(subsystem);
225
226 AssertCommandState(command1, 0, 0, 0, 0, 0);
227 AssertCommandState(command2, 0, 0, 0, 0, 0);
228
229 command1.Start();
230 AssertCommandState(command1, 0, 0, 0, 0, 0);
231 AssertCommandState(command2, 0, 0, 0, 0, 0);
232
233 Scheduler::GetInstance()->Run();
234 AssertCommandState(command1, 0, 0, 0, 0, 0);
235 AssertCommandState(command2, 0, 0, 0, 0, 0);
236
237 Scheduler::GetInstance()->Run();
238 AssertCommandState(command1, 1, 1, 1, 0, 0);
239 AssertCommandState(command2, 0, 0, 0, 0, 0);
240
241 Scheduler::GetInstance()->Run();
242 AssertCommandState(command1, 1, 2, 2, 0, 0);
243 AssertCommandState(command2, 0, 0, 0, 0, 0);
244
245 Scheduler::GetInstance()->Run();
246 AssertCommandState(command1, 1, 3, 3, 0, 0);
247 AssertCommandState(command2, 0, 0, 0, 0, 0);
248
249 command2.Start();
250 AssertCommandState(command1, 1, 3, 3, 0, 0);
251 AssertCommandState(command2, 0, 0, 0, 0, 0);
252
253 Scheduler::GetInstance()->Run();
254 AssertCommandState(command1, 1, 4, 4, 0, 1);
255 AssertCommandState(command2, 0, 0, 0, 0, 0);
256
257 Scheduler::GetInstance()->Run();
258 AssertCommandState(command1, 1, 4, 4, 0, 1);
259 AssertCommandState(command2, 1, 1, 1, 0, 0);
260
261 Scheduler::GetInstance()->Run();
262 AssertCommandState(command1, 1, 4, 4, 0, 1);
263 AssertCommandState(command2, 1, 2, 2, 0, 0);
264
265 Scheduler::GetInstance()->Run();
266 AssertCommandState(command1, 1, 4, 4, 0, 1);
267 AssertCommandState(command2, 1, 3, 3, 0, 0);
268
269 TeardownScheduler();
270}
271
272TEST_F(CommandTest,
273 OneCommandFailingSupersedingBecauseFirstCanNotBeInterrupted) {
274 ASubsystem subsystem("Command Superseding Test Subsystem");
275 MockCommand command1;
276
277 command1.Requires(&subsystem);
278
279 command1.SetInterruptible(false);
280 MockCommand command2;
281 command2.Requires(&subsystem);
282
283 AssertCommandState(command1, 0, 0, 0, 0, 0);
284 AssertCommandState(command2, 0, 0, 0, 0, 0);
285
286 command1.Start();
287 AssertCommandState(command1, 0, 0, 0, 0, 0);
288 AssertCommandState(command2, 0, 0, 0, 0, 0);
289
290 Scheduler::GetInstance()->Run();
291 AssertCommandState(command1, 0, 0, 0, 0, 0);
292 AssertCommandState(command2, 0, 0, 0, 0, 0);
293
294 Scheduler::GetInstance()->Run();
295 AssertCommandState(command1, 1, 1, 1, 0, 0);
296 AssertCommandState(command2, 0, 0, 0, 0, 0);
297
298 Scheduler::GetInstance()->Run();
299 AssertCommandState(command1, 1, 2, 2, 0, 0);
300 AssertCommandState(command2, 0, 0, 0, 0, 0);
301
302 Scheduler::GetInstance()->Run();
303 AssertCommandState(command1, 1, 3, 3, 0, 0);
304 AssertCommandState(command2, 0, 0, 0, 0, 0);
305
306 command2.Start();
307 AssertCommandState(command1, 1, 3, 3, 0, 0);
308 AssertCommandState(command2, 0, 0, 0, 0, 0);
309
310 Scheduler::GetInstance()->Run();
311 AssertCommandState(command1, 1, 4, 4, 0, 0);
312 AssertCommandState(command2, 0, 0, 0, 0, 0);
313
314 TeardownScheduler();
315}
316
317// END CommandSequentialGroupTest
318
319class ModifiedMockCommand : public MockCommand {
320 public:
321 ModifiedMockCommand() : MockCommand() { SetTimeout(2.0); }
322 bool IsFinished() override {
323 return MockCommand::IsFinished() || IsTimedOut();
324 }
325};
326
327TEST_F(CommandTest, TwoSecondTimeout) {
328 ASubsystem subsystem("Two Second Timeout Test Subsystem");
329 ModifiedMockCommand command;
330 command.Requires(&subsystem);
331
332 command.Start();
333 AssertCommandState(command, 0, 0, 0, 0, 0);
334 Scheduler::GetInstance()->Run();
335 AssertCommandState(command, 0, 0, 0, 0, 0);
336 Scheduler::GetInstance()->Run();
337 AssertCommandState(command, 1, 1, 1, 0, 0);
338 Scheduler::GetInstance()->Run();
339 AssertCommandState(command, 1, 2, 2, 0, 0);
340 Scheduler::GetInstance()->Run();
341 AssertCommandState(command, 1, 3, 3, 0, 0);
342 Wait(2);
343 Scheduler::GetInstance()->Run();
344 AssertCommandState(command, 1, 4, 4, 1, 0);
345 Scheduler::GetInstance()->Run();
346 AssertCommandState(command, 1, 4, 4, 1, 0);
347
348 TeardownScheduler();
349}
350
351TEST_F(CommandTest, DefaultCommandWhereTheInteruptingCommandEndsItself) {
352 ASubsystem subsystem("Default Command Test Subsystem");
353 MockCommand defaultCommand;
354 defaultCommand.Requires(&subsystem);
355 MockCommand anotherCommand;
356 anotherCommand.Requires(&subsystem);
357
358 AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
359 subsystem.Init(&defaultCommand);
360
361 AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
362 Scheduler::GetInstance()->Run();
363 AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
364 Scheduler::GetInstance()->Run();
365 AssertCommandState(defaultCommand, 1, 1, 1, 0, 0);
366 Scheduler::GetInstance()->Run();
367 AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
368
369 anotherCommand.Start();
370 AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
371 AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
372 Scheduler::GetInstance()->Run();
373 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
374 AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
375 Scheduler::GetInstance()->Run();
376 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
377 AssertCommandState(anotherCommand, 1, 1, 1, 0, 0);
378 Scheduler::GetInstance()->Run();
379 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
380 AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
381 anotherCommand.SetHasFinished(true);
382 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
383 AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
384 Scheduler::GetInstance()->Run();
385 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
386 AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
387 Scheduler::GetInstance()->Run();
388 AssertCommandState(defaultCommand, 2, 4, 4, 0, 1);
389 AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
390 Scheduler::GetInstance()->Run();
391 AssertCommandState(defaultCommand, 2, 5, 5, 0, 1);
392 AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
393
394 TeardownScheduler();
395}
396
397TEST_F(CommandTest, DefaultCommandsInterruptingCommandCanceled) {
398 ASubsystem subsystem("Default Command Test Subsystem");
399 MockCommand defaultCommand;
400 defaultCommand.Requires(&subsystem);
401 MockCommand anotherCommand;
402 anotherCommand.Requires(&subsystem);
403
404 AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
405 subsystem.Init(&defaultCommand);
406 subsystem.InitDefaultCommand();
407 AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
408 Scheduler::GetInstance()->Run();
409 AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
410 Scheduler::GetInstance()->Run();
411 AssertCommandState(defaultCommand, 1, 1, 1, 0, 0);
412 Scheduler::GetInstance()->Run();
413 AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
414
415 anotherCommand.Start();
416 AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
417 AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
418 Scheduler::GetInstance()->Run();
419 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
420 AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
421 Scheduler::GetInstance()->Run();
422 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
423 AssertCommandState(anotherCommand, 1, 1, 1, 0, 0);
424 Scheduler::GetInstance()->Run();
425 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
426 AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
427 anotherCommand.Cancel();
428 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
429 AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
430 Scheduler::GetInstance()->Run();
431 AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
432 AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
433 Scheduler::GetInstance()->Run();
434 AssertCommandState(defaultCommand, 2, 4, 4, 0, 1);
435 AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
436 Scheduler::GetInstance()->Run();
437 AssertCommandState(defaultCommand, 2, 5, 5, 0, 1);
438 AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
439
440 TeardownScheduler();
441}