diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp index bb0bf8ede1..464a4a6239 100644 --- a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -39,6 +39,11 @@ void CommandScheduler::AddButton(wpi::unique_function button) { void CommandScheduler::ClearButtons() { m_buttons.clear(); } void CommandScheduler::Schedule(bool interruptible, Command* command) { + if (m_inRunLoop) { + m_toSchedule.try_emplace(command, interruptible); + return; + } + if (command->IsGrouped()) { wpi_setWPIErrorWithContext(CommandIllegalUse, "A command that is part of a command group " @@ -125,6 +130,7 @@ void CommandScheduler::Run() { button(); } + m_inRunLoop = true; // Run scheduled commands, remove finished commands. for (auto iterator = m_scheduledCommands.begin(); iterator != m_scheduledCommands.end(); iterator++) { @@ -153,6 +159,18 @@ void CommandScheduler::Run() { m_scheduledCommands.erase(iterator); } } + m_inRunLoop = false; + + for (auto&& commandInterruptible : m_toSchedule) { + Schedule(commandInterruptible.second, commandInterruptible.first); + } + + for (auto&& command : m_toCancel) { + Cancel(command); + } + + m_toSchedule.clear(); + m_toCancel.clear(); // Add default commands for un-required registered subsystems. for (auto&& subsystem : m_subsystems) { @@ -198,6 +216,11 @@ Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const { } void CommandScheduler::Cancel(Command* command) { + if (m_inRunLoop) { + m_toCancel.emplace_back(command); + return; + } + auto find = m_scheduledCommands.find(command); if (find == m_scheduledCommands.end()) return; command->End(true); diff --git a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h index 9f1d81a684..2e9cdd5b26 100644 --- a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h @@ -360,6 +360,13 @@ class CommandScheduler final : public frc::Sendable, wpi::SmallVector m_interruptActions; wpi::SmallVector m_finishActions; + // Flag and queues for avoiding concurrent modification if commands are + // scheduled/canceled during run + + bool m_inRunLoop = false; + wpi::DenseMap m_toSchedule; + wpi::SmallVector m_toCancel; + friend class CommandTestBase; }; } // namespace frc2 diff --git a/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp index 249561b24c..29e8dc5f7e 100644 --- a/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp +++ b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp @@ -10,6 +10,7 @@ #include "CommandTestBase.h" #include "frc2/command/InstantCommand.h" #include "frc2/command/ScheduleCommand.h" +#include "frc2/command/SequentialCommandGroup.h" using namespace frc2; class ScheduleCommandTest : public CommandTestBase {}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 13e873d7af..b34f20e1fc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -81,6 +81,13 @@ public final class CommandScheduler implements Sendable { private final List> m_interruptActions = new ArrayList<>(); private final List> m_finishActions = new ArrayList<>(); + // Flag and queues for avoiding ConcurrentModificationException if commands are + // scheduled/canceled during run + private boolean m_inRunLoop; + private final Map m_toSchedule = new LinkedHashMap<>(); + private final List m_toCancel = new ArrayList<>(); + + CommandScheduler() { HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler); SendableRegistry.addLW(this, "Scheduler"); @@ -132,6 +139,11 @@ public final class CommandScheduler implements Sendable { */ @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"}) private void schedule(boolean interruptible, Command command) { + if (m_inRunLoop) { + m_toSchedule.put(command, interruptible); + return; + } + if (CommandGroupBase.getGroupedCommands().contains(command)) { throw new IllegalArgumentException( "A command that is part of a command group cannot be independently scheduled"); @@ -222,6 +234,7 @@ public final class CommandScheduler implements Sendable { button.run(); } + m_inRunLoop = true; //Run scheduled commands, remove finished commands. for (Iterator iterator = m_scheduledCommands.keySet().iterator(); iterator.hasNext(); ) { @@ -251,6 +264,19 @@ public final class CommandScheduler implements Sendable { m_requirements.keySet().removeAll(command.getRequirements()); } } + m_inRunLoop = false; + + //Schedule/cancel commands from queues populated during loop + for (Map.Entry commandInterruptible : m_toSchedule.entrySet()) { + schedule(commandInterruptible.getValue(), commandInterruptible.getKey()); + } + + for (Command command : m_toCancel) { + cancel(command); + } + + m_toSchedule.clear(); + m_toCancel.clear(); //Add default commands for un-required registered subsystems. for (Map.Entry subsystemCommand : m_subsystems.entrySet()) { @@ -326,6 +352,11 @@ public final class CommandScheduler implements Sendable { * @param commands the commands to cancel */ public void cancel(Command... commands) { + if (m_inRunLoop) { + m_toCancel.addAll(List.of(commands)); + return; + } + for (Command command : commands) { if (!m_scheduledCommands.containsKey(command)) { continue; diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java index 0a5035233b..9041627a03 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java @@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj2.command; import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.mockito.Mockito.verify; class ScheduleCommandTest extends CommandTestBase { @@ -28,4 +29,19 @@ class ScheduleCommandTest extends CommandTestBase { verify(command1).schedule(); verify(command2).schedule(); } + + @Test + void scheduleCommandDuringRunTest() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + + InstantCommand toSchedule = new InstantCommand(); + ScheduleCommand scheduleCommand = new ScheduleCommand(toSchedule); + SequentialCommandGroup group = + new SequentialCommandGroup(new InstantCommand(), scheduleCommand); + + scheduler.schedule(group); + scheduler.schedule(new InstantCommand().perpetually()); + scheduler.run(); + assertDoesNotThrow(scheduler::run); + } }