Fix array out of bounds exception caused by parallel race group (#1935)

The current index would be set to -1 by the execute method of ParallelRaceGroup,
and then an index out of bounds exception would be thrown by the end() method of
SequentialCommandGroup. This change bound checks the current command index as well
as only calls end at the end of parallel race group rather than during execute.

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

@@ -26,16 +26,13 @@ void ParallelRaceGroup::Execute() {
     commandRunning->Execute();
     if (commandRunning->IsFinished()) {
       m_finished = true;
-      commandRunning->End(false);
     }
   }
 }
 
 void ParallelRaceGroup::End(bool interrupted) {
   for (auto& commandRunning : m_commands) {
-    if (!commandRunning->IsFinished()) {
-      commandRunning->End(true);
-    }
+    commandRunning->End(!commandRunning->IsFinished());
   }
   isRunning = false;
 }

wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp

@@ -38,7 +38,9 @@ void SequentialCommandGroup::Execute() {
 }
 
 void SequentialCommandGroup::End(bool interrupted) {
-  if (interrupted && !m_commands.empty()) {
+  if (interrupted && !m_commands.empty() &&
+      m_currentCommandIndex != invalid_index &&
+      m_currentCommandIndex < m_commands.size()) {
     m_commands[m_currentCommandIndex]->End(interrupted);
   }
   m_currentCommandIndex = invalid_index;

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

@@ -8,6 +8,7 @@
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SequentialCommandGroup.h"
 #include "frc2/command/WaitUntilCommand.h"
 
 using namespace frc2;
@@ -129,3 +130,27 @@ TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) {
   EXPECT_TRUE(scheduler.IsScheduled(&command3));
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnceTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished1 = false;
+  bool finished2 = false;
+  bool finished3 = false;
+
+  WaitUntilCommand command1([&finished1] { return finished1; });
+  WaitUntilCommand command2([&finished2] { return finished2; });
+  WaitUntilCommand command3([&finished3] { return finished3; });
+
+  SequentialCommandGroup group1(command1, command2);
+  ParallelRaceGroup group2(std::move(group1), command3);
+
+  scheduler.Schedule(&group2);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group2));
+  finished1 = true;
+  scheduler.Run();
+  finished2 = true;
+  EXPECT_NO_FATAL_FAILURE(scheduler.Run());
+  EXPECT_FALSE(scheduler.IsScheduled(&group2));
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java

@@ -69,7 +69,6 @@ public class ParallelRaceGroup extends CommandGroupBase {
       command.execute();
       if (command.isFinished()) {
         m_finished = true;
-        command.end(false);
       }
     }
   }
@@ -77,9 +76,7 @@ public class ParallelRaceGroup extends CommandGroupBase {
   @Override
   public void end(boolean interrupted) {
     for (Command command : m_commands) {
-      if (!command.isFinished()) {
-        command.end(true);
-      }
+      command.end(!command.isFinished());
     }
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java

@@ -77,7 +77,9 @@ public class SequentialCommandGroup extends CommandGroupBase {
 
   @Override
   public void end(boolean interrupted) {
-    if (interrupted && !m_commands.isEmpty()) {
+    if (interrupted && !m_commands.isEmpty() && m_currentCommandIndex > -1
+        && m_currentCommandIndex < m_commands.size()
+    ) {
       m_commands.get(m_currentCommandIndex).end(true);
     }
     m_currentCommandIndex = -1;

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

@@ -11,6 +11,7 @@ import org.junit.jupiter.api.Test;
 
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.never;
@@ -129,4 +130,34 @@ class ParallelRaceGroupTest extends CommandTestBase {
 
     assertThrows(IllegalArgumentException.class, () -> new ParallelRaceGroup(command1, command2));
   }
+
+  @Test
+  void parallelRaceOnlyCallsEndOnceTest() {
+    Subsystem system1 = new TestSubsystem();
+    Subsystem system2 = new TestSubsystem();
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder perpetualCommandHolder = new MockCommandHolder(true);
+    Command command3 = new PerpetualCommand(perpetualCommandHolder.getMock());
+
+    Command group1 = new SequentialCommandGroup(command1, command2);
+    assertNotNull(group1);
+    assertNotNull(command3);
+    Command group2 = new ParallelRaceGroup(group1, command3);
+
+    CommandScheduler scheduler = new CommandScheduler();
+
+    scheduler.schedule(group2);
+    scheduler.run();
+    command1Holder.setFinished(true);
+    scheduler.run();
+    command2Holder.setFinished(true);
+    // at this point the sequential group should be done
+    assertDoesNotThrow(() -> scheduler.run());
+
+  }
+
 }