diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 8408b0b37a..4e995f70d3 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -28,8 +28,10 @@ import java.util.LinkedHashMap; import java.util.LinkedHashSet; import java.util.List; import java.util.Map; +import java.util.Optional; import java.util.Set; import java.util.WeakHashMap; +import java.util.function.BiConsumer; import java.util.function.Consumer; /** @@ -57,6 +59,8 @@ public final class CommandScheduler implements Sendable, AutoCloseable { return instance; } + private static final Optional kNoInterruptor = Optional.empty(); + private final Set m_composedCommands = Collections.newSetFromMap(new WeakHashMap<>()); // A set of the currently-running commands. @@ -79,14 +83,15 @@ public final class CommandScheduler implements Sendable, AutoCloseable { // Lists of user-supplied actions to be executed on scheduling events for every command. private final List> m_initActions = new ArrayList<>(); private final List> m_executeActions = new ArrayList<>(); - private final List> m_interruptActions = new ArrayList<>(); + 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 Set m_toSchedule = new LinkedHashSet<>(); - private final List m_toCancel = new ArrayList<>(); + private final List m_toCancelCommands = new ArrayList<>(); + private final List> m_toCancelInterruptors = new ArrayList<>(); private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {}); @@ -211,7 +216,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable { for (Subsystem requirement : requirements) { Command requiring = requiring(requirement); if (requiring != null) { - cancel(requiring); + cancel(requiring, Optional.of(command)); } } initCommand(command, requirements); @@ -272,8 +277,8 @@ public final class CommandScheduler implements Sendable, AutoCloseable { if (!command.runsWhenDisabled() && RobotState.isDisabled()) { command.end(true); - for (Consumer action : m_interruptActions) { - action.accept(command); + for (BiConsumer> action : m_interruptActions) { + action.accept(command, kNoInterruptor); } m_requirements.keySet().removeAll(command.getRequirements()); iterator.remove(); @@ -304,12 +309,13 @@ public final class CommandScheduler implements Sendable, AutoCloseable { schedule(command); } - for (Command command : m_toCancel) { - cancel(command); + for (int i = 0; i < m_toCancelCommands.size(); i++) { + cancel(m_toCancelCommands.get(i), m_toCancelInterruptors.get(i)); } m_toSchedule.clear(); - m_toCancel.clear(); + m_toCancelCommands.clear(); + m_toCancelInterruptors.clear(); // Add default commands for un-required registered subsystems. for (Map.Entry subsystemCommand : m_subsystems.entrySet()) { @@ -440,28 +446,42 @@ public final class CommandScheduler implements Sendable, AutoCloseable { * @param commands the commands to cancel */ public void cancel(Command... commands) { + for (Command command : commands) { + cancel(command, kNoInterruptor); + } + } + + /** + * Cancels a command. The scheduler will only call {@link Command#end(boolean)} method of the + * canceled command with {@code true}, indicating they were canceled (as opposed to finishing + * normally). + * + *

Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}. + * + * @param command the command to cancel + * @param interruptor the interrupting command, if any + */ + private void cancel(Command command, Optional interruptor) { + if (command == null) { + DriverStation.reportWarning("Tried to cancel a null command", true); + return; + } if (m_inRunLoop) { - m_toCancel.addAll(List.of(commands)); + m_toCancelCommands.add(command); + m_toCancelInterruptors.add(interruptor); + return; + } + if (!isScheduled(command)) { return; } - for (Command command : commands) { - if (command == null) { - DriverStation.reportWarning("Tried to cancel a null command", true); - continue; - } - if (!isScheduled(command)) { - continue; - } - - m_scheduledCommands.remove(command); - m_requirements.keySet().removeAll(command.getRequirements()); - command.end(true); - for (Consumer action : m_interruptActions) { - action.accept(command); - } - m_watchdog.addEpoch(command.getName() + ".end(true)"); + m_scheduledCommands.remove(command); + m_requirements.keySet().removeAll(command.getRequirements()); + command.end(true); + for (BiConsumer> action : m_interruptActions) { + action.accept(command, interruptor); } + m_watchdog.addEpoch(command.getName() + ".end(true)"); } /** Cancels all commands that are currently scheduled. */ @@ -528,6 +548,19 @@ public final class CommandScheduler implements Sendable, AutoCloseable { * @param action the action to perform */ public void onCommandInterrupt(Consumer action) { + requireNonNullParam(action, "action", "onCommandInterrupt"); + m_interruptActions.add((command, interruptor) -> action.accept(command)); + } + + /** + * Adds an action to perform on the interruption of any command by the scheduler. The action + * receives the interrupted command and an Optional containing the interrupting command, or + * Optional.empty() if it was not canceled by a command (e.g., by {@link + * CommandScheduler#cancel}). + * + * @param action the action to perform + */ + public void onCommandInterrupt(BiConsumer> action) { m_interruptActions.add(requireNonNullParam(action, "action", "onCommandInterrupt")); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 41dcacdba0..fdf9bfcf2a 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -48,7 +48,7 @@ class CommandScheduler::Impl { // every command. wpi::SmallVector initActions; wpi::SmallVector executeActions; - wpi::SmallVector interruptActions; + wpi::SmallVector interruptActions; wpi::SmallVector finishActions; // Flag and queues for avoiding concurrent modification if commands are @@ -56,7 +56,8 @@ class CommandScheduler::Impl { bool inRunLoop = false; wpi::SmallVector toSchedule; - wpi::SmallVector toCancel; + wpi::SmallVector toCancelCommands; + wpi::SmallVector, 4> toCancelInterruptors; }; template @@ -138,7 +139,7 @@ void CommandScheduler::Schedule(Command* command) { if (isDisjoint || allInterruptible) { if (allInterruptible) { for (auto&& cmdToCancel : intersection) { - Cancel(cmdToCancel); + Cancel(cmdToCancel, std::make_optional(command)); } } m_impl->scheduledCommands.insert(command); @@ -196,7 +197,7 @@ void CommandScheduler::Run() { // Run scheduled commands, remove finished commands. for (Command* command : m_impl->scheduledCommands) { if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) { - Cancel(command); + Cancel(command, std::nullopt); continue; } @@ -226,12 +227,13 @@ void CommandScheduler::Run() { Schedule(command); } - for (auto&& command : m_impl->toCancel) { - Cancel(command); + for (size_t i = 0; i < m_impl->toCancelCommands.size(); i++) { + Cancel(m_impl->toCancelCommands[i], m_impl->toCancelInterruptors[i]); } m_impl->toSchedule.clear(); - m_impl->toCancel.clear(); + m_impl->toCancelCommands.clear(); + m_impl->toCancelInterruptors.clear(); // Add default commands for un-required registered subsystems. for (auto&& subsystem : m_impl->subsystems) { @@ -319,13 +321,15 @@ Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const { } } -void CommandScheduler::Cancel(Command* command) { +void CommandScheduler::Cancel(Command* command, + std::optional interruptor) { if (!m_impl) { return; } if (m_impl->inRunLoop) { - m_impl->toCancel.emplace_back(command); + m_impl->toCancelCommands.emplace_back(command); + m_impl->toCancelInterruptors.emplace_back(interruptor); return; } @@ -341,11 +345,15 @@ void CommandScheduler::Cancel(Command* command) { } command->End(true); for (auto&& action : m_impl->interruptActions) { - action(*command); + action(*command, interruptor); } m_watchdog.AddEpoch(command->GetName() + ".End(true)"); } +void CommandScheduler::Cancel(Command* command) { + Cancel(command, std::nullopt); +} + void CommandScheduler::Cancel(const CommandPtr& command) { Cancel(command.get()); } @@ -424,6 +432,14 @@ void CommandScheduler::OnCommandExecute(Action action) { } void CommandScheduler::OnCommandInterrupt(Action action) { + m_impl->interruptActions.emplace_back( + [action = std::move(action)](const Command& command, + const std::optional& interruptor) { + action(command); + }); +} + +void CommandScheduler::OnCommandInterrupt(InterruptAction action) { m_impl->interruptActions.emplace_back(std::move(action)); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index 188d273aa0..ce4dc914c2 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,8 @@ class CommandScheduler final : public wpi::Sendable, CommandScheduler& operator=(const CommandScheduler&) = delete; using Action = std::function; + using InterruptAction = + std::function&)>; /** * Changes the period of the loop overrun watchdog. This should be kept in @@ -353,6 +356,16 @@ class CommandScheduler final : public wpi::Sendable, */ void OnCommandInterrupt(Action action); + /** + * Adds an action to perform on the interruption of any command by the + * scheduler. The action receives the interrupted command and an optional + * containing the interrupting command, or nullopt if it was not canceled by a + * command (e.g., by Cancel()). + * + * @param action the action to perform + */ + void OnCommandInterrupt(InterruptAction action); + /** * Adds an action to perform on the finishing of any command by the scheduler. * @@ -397,6 +410,8 @@ class CommandScheduler final : public wpi::Sendable, void SetDefaultCommandImpl(Subsystem* subsystem, std::unique_ptr command); + void Cancel(Command* command, std::optional interruptor); + class Impl; std::unique_ptr m_impl; diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java index ee1f8eb37c..1e0b33332e 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java @@ -6,6 +6,9 @@ package edu.wpi.first.wpilibj2.command; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertSame; +import static org.junit.jupiter.api.Assertions.assertTrue; import java.util.concurrent.atomic.AtomicInteger; import org.junit.jupiter.api.Test; @@ -43,6 +46,76 @@ class SchedulerTest extends CommandTestBase { } } + @Test + void schedulerInterruptNoCauseLambdaTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + + scheduler.onCommandInterrupt( + (interrupted, cause) -> { + assertFalse(cause.isPresent()); + counter.incrementAndGet(); + }); + + Command command = Commands.run(() -> {}); + + scheduler.schedule(command); + scheduler.cancel(command); + + assertEquals(1, counter.get()); + } + } + + @Test + void schedulerInterruptCauseLambdaTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + + Subsystem subsystem = new Subsystem() {}; + Command command = subsystem.run(() -> {}); + Command interruptor = subsystem.runOnce(() -> {}); + + scheduler.onCommandInterrupt( + (interrupted, cause) -> { + assertTrue(cause.isPresent()); + assertSame(interruptor, cause.get()); + counter.incrementAndGet(); + }); + + scheduler.schedule(command); + scheduler.schedule(interruptor); + + assertEquals(1, counter.get()); + } + } + + @Test + void schedulerInterruptCauseLambdaInRunLoopTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + + Subsystem subsystem = new Subsystem() {}; + Command command = subsystem.run(() -> {}); + Command interruptor = subsystem.runOnce(() -> {}); + // This command will schedule interruptor in execute() inside the run loop + Command interruptorScheduler = Commands.runOnce(() -> scheduler.schedule(interruptor)); + + scheduler.onCommandInterrupt( + (interrupted, cause) -> { + assertTrue(cause.isPresent()); + assertSame(interruptor, cause.get()); + counter.incrementAndGet(); + }); + + scheduler.schedule(command); + scheduler.schedule(interruptorScheduler); + + scheduler.run(); + + assertEquals(1, counter.get()); + } + } + @Test void registerSubsystemTest() { try (CommandScheduler scheduler = new CommandScheduler()) { @@ -87,6 +160,7 @@ class SchedulerTest extends CommandTestBase { AtomicInteger counter = new AtomicInteger(); scheduler.onCommandInterrupt(command -> counter.incrementAndGet()); + scheduler.onCommandInterrupt((command, interruptor) -> assertFalse(interruptor.isPresent())); Command command = new WaitCommand(10); Command command2 = new WaitCommand(10); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp index 3bade0ab09..ef97bb0769 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp @@ -43,6 +43,74 @@ TEST_F(SchedulerTest, SchedulerLambdaInterrupt) { EXPECT_EQ(counter, 1); } +TEST_F(SchedulerTest, SchedulerLambdaInterruptNoCause) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + scheduler.OnCommandInterrupt( + [&counter](const Command&, const std::optional& interruptor) { + EXPECT_FALSE(interruptor); + counter++; + }); + + RunCommand command([] {}); + + scheduler.Schedule(&command); + scheduler.Cancel(&command); + + EXPECT_EQ(1, counter); +} + +TEST_F(SchedulerTest, SchedulerLambdaInterruptCause) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + TestSubsystem subsystem{}; + RunCommand command([] {}, {&subsystem}); + InstantCommand interruptor([] {}, {&subsystem}); + + scheduler.OnCommandInterrupt( + [&](const Command&, const std::optional& cause) { + ASSERT_TRUE(cause); + EXPECT_EQ(&interruptor, *cause); + counter++; + }); + + scheduler.Schedule(&command); + scheduler.Schedule(&interruptor); + + EXPECT_EQ(1, counter); +} + +TEST_F(SchedulerTest, SchedulerLambdaInterruptCauseInRunLoop) { + CommandScheduler scheduler = GetScheduler(); + + int counter = 0; + + TestSubsystem subsystem{}; + RunCommand command([] {}, {&subsystem}); + InstantCommand interruptor([] {}, {&subsystem}); + // This command will schedule interruptor in execute() inside the run loop + InstantCommand interruptorScheduler( + [&] { scheduler.Schedule(&interruptor); }); + + scheduler.OnCommandInterrupt( + [&](const Command&, const std::optional& cause) { + ASSERT_TRUE(cause); + EXPECT_EQ(&interruptor, *cause); + counter++; + }); + + scheduler.Schedule(&command); + scheduler.Schedule(&interruptorScheduler); + + scheduler.Run(); + + EXPECT_EQ(1, counter); +} + TEST_F(SchedulerTest, RegisterSubsystem) { CommandScheduler scheduler = GetScheduler(); @@ -78,6 +146,10 @@ TEST_F(SchedulerTest, SchedulerCancelAll) { int counter = 0; scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; }); + scheduler.OnCommandInterrupt( + [](const Command&, const std::optional& interruptor) { + EXPECT_FALSE(interruptor); + }); scheduler.Schedule(&command); scheduler.Schedule(&command2);