diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp index 4f9881b7c5..8a027172fd 100644 --- a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp +++ b/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; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp index 9d51609a4a..1aa19e4b6d 100644 --- a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp +++ b/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; diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp index dad02a128c..54762ca9b5 100644 --- a/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp +++ b/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)); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java index a84d8aaa2f..dcaf5f5596 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java +++ b/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()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java index d834018796..0d01f1119b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java +++ b/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; diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java index c9b21e4bad..8a4781f87d 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java +++ b/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()); + + } + }