Fix running ParallelRaceGroup multiple times

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,6 +15,7 @@ ParallelRaceGroup::ParallelRaceGroup(
 }
 
 void ParallelRaceGroup::Initialize() {
+  m_finished = false;
   for (auto& commandRunning : m_commands) {
     commandRunning->Initialize();
   }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,6 +15,7 @@
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.reset;
 import static org.mockito.Mockito.times;
 import static org.mockito.Mockito.verify;
 
@@ -160,4 +161,50 @@ void parallelRaceOnlyCallsEndOnceTest() {
 
   }
 
+  @Test
+  void parallelRaceScheduleTwiceTest() {
+    CommandScheduler scheduler = new CommandScheduler();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+
+    Command group = new ParallelRaceGroup(command1, command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2).initialize();
+
+    command1Holder.setFinished(true);
+    scheduler.run();
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    verify(command1).execute();
+    verify(command1).end(false);
+    verify(command2).execute();
+    verify(command2).end(true);
+    verify(command2, never()).end(false);
+
+    assertFalse(scheduler.isScheduled(group));
+
+    reset(command1);
+    reset(command2);
+
+    scheduler.schedule(group);
+
+    verify(command1).initialize();
+    verify(command2).initialize();
+
+    scheduler.run();
+    scheduler.run();
+    assertTrue(scheduler.isScheduled(group));
+    command2Holder.setFinished(true);
+    scheduler.run();
+
+    assertFalse(scheduler.isScheduled(group));
+  }
+
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -154,3 +154,54 @@ TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnceTest) {
   EXPECT_NO_FATAL_FAILURE(scheduler.Run());
   EXPECT_FALSE(scheduler.IsScheduled(&group2));
 }
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTwiceTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize()).Times(2);
+  EXPECT_CALL(*command1, Execute()).Times(5);
+  EXPECT_CALL(*command1, End(true)).Times(2);
+
+  EXPECT_CALL(*command2, Initialize()).Times(2);
+  EXPECT_CALL(*command2, Execute()).Times(5);
+  EXPECT_CALL(*command2, End(false)).Times(2);
+
+  EXPECT_CALL(*command3, Initialize()).Times(2);
+  EXPECT_CALL(*command3, Execute()).Times(5);
+  EXPECT_CALL(*command3, End(true)).Times(2);
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+
+  command2->SetFinished(false);
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}