From 30e5fd6be124ac89102bc31ffc293ca4e33e86e5 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Tue, 25 Jul 2023 16:37:23 -0400 Subject: [PATCH 1/2] Make command scheduling order consistent --- .../wpilibj2/command/CommandScheduler.java | 23 +- .../cpp/frc2/command/CommandScheduler.cpp | 29 +- .../command/SchedulingRecursionTest.java | 325 ++++++++++++++++-- .../frc2/command/SchedulingRecursionTest.cpp | 253 +++++++++++++- 4 files changed, 566 insertions(+), 64 deletions(-) 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 4e995f70d32..6980eb30eb6 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 @@ -92,6 +92,7 @@ public static synchronized CommandScheduler getInstance() { private final Set m_toSchedule = new LinkedHashSet<>(); private final List m_toCancelCommands = new ArrayList<>(); private final List> m_toCancelInterruptors = new ArrayList<>(); + private final Set m_endingCommands = new LinkedHashSet<>(); private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {}); @@ -271,18 +272,13 @@ public void run() { m_watchdog.addEpoch("buttons.run()"); m_inRunLoop = true; + boolean isDisabled = RobotState.isDisabled(); // Run scheduled commands, remove finished commands. for (Iterator iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) { Command command = iterator.next(); - if (!command.runsWhenDisabled() && RobotState.isDisabled()) { - command.end(true); - for (BiConsumer> action : m_interruptActions) { - action.accept(command, kNoInterruptor); - } - m_requirements.keySet().removeAll(command.getRequirements()); - iterator.remove(); - m_watchdog.addEpoch(command.getName() + ".end(true)"); + if (isDisabled && !command.runsWhenDisabled()) { + cancel(command); continue; } @@ -292,10 +288,12 @@ public void run() { } m_watchdog.addEpoch(command.getName() + ".execute()"); if (command.isFinished()) { + m_endingCommands.add(command); command.end(false); for (Consumer action : m_finishActions) { action.accept(command); } + m_endingCommands.remove(command); iterator.remove(); m_requirements.keySet().removeAll(command.getRequirements()); @@ -466,6 +464,9 @@ private void cancel(Command command, Optional interruptor) { DriverStation.reportWarning("Tried to cancel a null command", true); return; } + if (m_endingCommands.contains(command)) { + return; + } if (m_inRunLoop) { m_toCancelCommands.add(command); m_toCancelInterruptors.add(interruptor); @@ -475,12 +476,14 @@ private void cancel(Command command, Optional interruptor) { return; } - m_scheduledCommands.remove(command); - m_requirements.keySet().removeAll(command.getRequirements()); + m_endingCommands.add(command); command.end(true); for (BiConsumer> action : m_interruptActions) { action.accept(command, interruptor); } + m_endingCommands.remove(command); + m_scheduledCommands.remove(command); + m_requirements.keySet().removeAll(command.getRequirements()); m_watchdog.addEpoch(command.getName() + ".end(true)"); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index fdf9bfcf2a3..d61ffbda3e4 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -53,11 +53,11 @@ class CommandScheduler::Impl { // Flag and queues for avoiding concurrent modification if commands are // scheduled/canceled during run - bool inRunLoop = false; wpi::SmallVector toSchedule; wpi::SmallVector toCancelCommands; wpi::SmallVector, 4> toCancelInterruptors; + wpi::SmallSet endingCommands; }; template @@ -194,9 +194,10 @@ void CommandScheduler::Run() { m_watchdog.AddEpoch("buttons.Run()"); m_impl->inRunLoop = true; + bool isDisabled = frc::RobotState::IsDisabled(); // Run scheduled commands, remove finished commands. for (Command* command : m_impl->scheduledCommands) { - if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) { + if (isDisabled && !command->RunsWhenDisabled()) { Cancel(command, std::nullopt); continue; } @@ -208,16 +209,18 @@ void CommandScheduler::Run() { m_watchdog.AddEpoch(command->GetName() + ".Execute()"); if (command->IsFinished()) { + m_impl->endingCommands.insert(command); command->End(false); for (auto&& action : m_impl->finishActions) { action(*command); } + m_impl->endingCommands.erase(command); + m_impl->scheduledCommands.erase(command); for (auto&& requirement : command->GetRequirements()) { m_impl->requirements.erase(requirement); } - m_impl->scheduledCommands.erase(command); m_watchdog.AddEpoch(command->GetName() + ".End(false)"); } } @@ -326,27 +329,29 @@ void CommandScheduler::Cancel(Command* command, if (!m_impl) { return; } - + if (m_impl->endingCommands.contains(command)) { + return; + } if (m_impl->inRunLoop) { m_impl->toCancelCommands.emplace_back(command); m_impl->toCancelInterruptors.emplace_back(interruptor); return; } - - auto find = m_impl->scheduledCommands.find(command); - if (find == m_impl->scheduledCommands.end()) { + if (!IsScheduled(command)) { return; } - m_impl->scheduledCommands.erase(*find); + m_impl->endingCommands.insert(command); + command->End(true); + for (auto&& action : m_impl->interruptActions) { + action(*command, interruptor); + } + m_impl->endingCommands.erase(command); + m_impl->scheduledCommands.erase(command); for (auto&& requirement : m_impl->requirements) { if (requirement.second == command) { m_impl->requirements.erase(requirement.first); } } - command->End(true); - for (auto&& action : m_impl->interruptActions) { - action(*command, interruptor); - } m_watchdog.AddEpoch(command->GetName() + ".End(true)"); } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java index 4b14b25cd85..5e8fd392e46 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java @@ -25,6 +25,7 @@ class SchedulingRecursionTest extends CommandTestBase { void cancelFromInitialize(InterruptionBehavior interruptionBehavior) { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicBoolean hasOtherRun = new AtomicBoolean(); + AtomicInteger counter = new AtomicInteger(); Subsystem requirement = new SubsystemBase() {}; Command selfCancels = new Command() { @@ -37,6 +38,11 @@ public void initialize() { scheduler.cancel(this); } + @Override + public void end(boolean interrupted) { + counter.incrementAndGet(); + } + @Override public InterruptionBehavior getInterruptionBehavior() { return interruptionBehavior; @@ -52,6 +58,47 @@ public InterruptionBehavior getInterruptionBehavior() { }); assertFalse(scheduler.isScheduled(selfCancels)); assertTrue(scheduler.isScheduled(other)); + assertEquals(1, counter.get()); + scheduler.run(); + assertTrue(hasOtherRun.get()); + } + } + + @EnumSource(InterruptionBehavior.class) + @ParameterizedTest + void cancelFromInitializeAction(InterruptionBehavior interruptionBehavior) { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean hasOtherRun = new AtomicBoolean(); + AtomicInteger counter = new AtomicInteger(); + Subsystem requirement = new Subsystem() {}; + Command selfCancels = + new Command() { + { + addRequirements(requirement); + } + + @Override + public void end(boolean interrupted) { + counter.incrementAndGet(); + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + return interruptionBehavior; + } + }; + Command other = new RunCommand(() -> hasOtherRun.set(true), requirement); + + assertDoesNotThrow( + () -> { + scheduler.onCommandInitialize(cmd -> scheduler.cancel(selfCancels)); + scheduler.schedule(selfCancels); + scheduler.run(); + scheduler.schedule(other); + }); + assertFalse(scheduler.isScheduled(selfCancels)); + assertTrue(scheduler.isScheduled(other)); + assertEquals(1, counter.get()); scheduler.run(); assertTrue(hasOtherRun.get()); } @@ -62,6 +109,7 @@ public InterruptionBehavior getInterruptionBehavior() { void defaultCommandGetsRescheduledAfterSelfCanceling(InterruptionBehavior interruptionBehavior) { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicBoolean hasOtherRun = new AtomicBoolean(); + AtomicInteger counter = new AtomicInteger(); Subsystem requirement = new SubsystemBase() {}; Command selfCancels = new Command() { @@ -74,6 +122,11 @@ public void initialize() { scheduler.cancel(this); } + @Override + public void end(boolean interrupted) { + counter.incrementAndGet(); + } + @Override public InterruptionBehavior getInterruptionBehavior() { return interruptionBehavior; @@ -90,6 +143,7 @@ public InterruptionBehavior getInterruptionBehavior() { scheduler.run(); assertFalse(scheduler.isScheduled(selfCancels)); assertTrue(scheduler.isScheduled(other)); + assertEquals(1, counter.get()); scheduler.run(); assertTrue(hasOtherRun.get()); } @@ -116,24 +170,177 @@ public void end(boolean interrupted) { } @Test - void scheduleFromEndCancel() { + void cancelFromInterruptAction() { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicInteger counter = new AtomicInteger(); - Subsystem requirement = new SubsystemBase() {}; - InstantCommand other = new InstantCommand(() -> {}, requirement); - Command selfCancels = - new Command() { - { - addRequirements(requirement); - } + Command selfCancels = new RunCommand(() -> {}); + scheduler.onCommandInterrupt( + cmd -> { + counter.incrementAndGet(); + scheduler.cancel(selfCancels); + }); + scheduler.schedule(selfCancels); + + assertDoesNotThrow(() -> scheduler.cancel(selfCancels)); + assertEquals(1, counter.get()); + assertFalse(scheduler.isScheduled(selfCancels)); + } + } + + @Test + void cancelFromEndLoop() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + FunctionalCommand dCancelsAll = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.cancelAll(); + }, + () -> true); + FunctionalCommand cCancelsD = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.cancel(dCancelsAll); + }, + () -> true); + FunctionalCommand bCancelsC = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.cancel(cCancelsD); + }, + () -> true); + FunctionalCommand aCancelsB = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.cancel(bCancelsC); + }, + () -> true); + + scheduler.schedule(aCancelsB); + scheduler.schedule(bCancelsC); + scheduler.schedule(cCancelsD); + scheduler.schedule(dCancelsAll); + + assertDoesNotThrow(() -> scheduler.cancel(aCancelsB)); + assertEquals(4, counter.get()); + assertFalse(scheduler.isScheduled(aCancelsB)); + assertFalse(scheduler.isScheduled(bCancelsC)); + assertFalse(scheduler.isScheduled(cCancelsD)); + assertFalse(scheduler.isScheduled(dCancelsAll)); + } + } + @Test + void cancelFromEndLoopWhileInRunLoop() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + FunctionalCommand dCancelsAll = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.cancelAll(); + }, + () -> true); + FunctionalCommand cCancelsD = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.cancel(dCancelsAll); + }, + () -> true); + FunctionalCommand bCancelsC = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.cancel(cCancelsD); + }, + () -> true); + FunctionalCommand aCancelsB = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.cancel(bCancelsC); + }, + () -> true); + + scheduler.schedule(aCancelsB); + scheduler.schedule(bCancelsC); + scheduler.schedule(cCancelsD); + scheduler.schedule(dCancelsAll); + + assertDoesNotThrow(() -> scheduler.run()); + assertEquals(4, counter.get()); + assertFalse(scheduler.isScheduled(aCancelsB)); + assertFalse(scheduler.isScheduled(bCancelsC)); + assertFalse(scheduler.isScheduled(cCancelsD)); + assertFalse(scheduler.isScheduled(dCancelsAll)); + } + } + + @Test + void multiCancelFromEnd() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + FunctionalCommand bIncrementsCounter = + new FunctionalCommand( + () -> {}, () -> {}, interrupted -> counter.incrementAndGet(), () -> true); + Command aCancelsB = + new Command() { @Override public void end(boolean interrupted) { counter.incrementAndGet(); - scheduler.schedule(other); + scheduler.cancel(bIncrementsCounter); + scheduler.cancel(this); } }; + scheduler.schedule(aCancelsB); + scheduler.schedule(bIncrementsCounter); + + assertDoesNotThrow(() -> scheduler.cancel(aCancelsB)); + assertEquals(2, counter.get()); + assertFalse(scheduler.isScheduled(aCancelsB)); + assertFalse(scheduler.isScheduled(bIncrementsCounter)); + } + } + + @Test + void scheduleFromEndCancel() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + Subsystem requirement = new SubsystemBase() {}; + InstantCommand other = new InstantCommand(() -> {}, requirement); + FunctionalCommand selfCancels = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.schedule(other); + }, + () -> false, + requirement); + scheduler.schedule(selfCancels); assertDoesNotThrow(() -> scheduler.cancel(selfCancels)); @@ -148,19 +355,38 @@ void scheduleFromEndInterrupt() { AtomicInteger counter = new AtomicInteger(); Subsystem requirement = new SubsystemBase() {}; InstantCommand other = new InstantCommand(() -> {}, requirement); - Command selfCancels = - new Command() { - { - addRequirements(requirement); - } + FunctionalCommand selfCancels = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.schedule(other); + }, + () -> false, + requirement); - @Override - public void end(boolean interrupted) { - counter.incrementAndGet(); - scheduler.schedule(other); - } - }; + scheduler.schedule(selfCancels); + + assertDoesNotThrow(() -> scheduler.schedule(other)); + assertEquals(1, counter.get()); + assertFalse(scheduler.isScheduled(selfCancels)); + assertTrue(scheduler.isScheduled(other)); + } + } + @Test + void scheduleFromEndInterruptAction() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + Subsystem requirement = new Subsystem() {}; + InstantCommand other = new InstantCommand(() -> {}, requirement); + InstantCommand selfCancels = new InstantCommand(() -> {}, requirement); + scheduler.onCommandInterrupt( + cmd -> { + counter.incrementAndGet(); + scheduler.schedule(other); + }); scheduler.schedule(selfCancels); assertDoesNotThrow(() -> scheduler.schedule(other)); @@ -178,18 +404,16 @@ void scheduleInitializeFromDefaultCommand(InterruptionBehavior interruptionBehav Subsystem requirement = new SubsystemBase() {}; Command other = new InstantCommand(() -> {}, requirement).withInterruptBehavior(interruptionBehavior); - Command defaultCommand = - new Command() { - { - addRequirements(requirement); - } - - @Override - public void initialize() { - counter.incrementAndGet(); - scheduler.schedule(other); - } - }; + FunctionalCommand defaultCommand = + new FunctionalCommand( + () -> { + counter.incrementAndGet(); + scheduler.schedule(other); + }, + () -> {}, + interrupted -> {}, + () -> false, + requirement); scheduler.setDefaultCommand(requirement, defaultCommand); @@ -201,4 +425,41 @@ public void initialize() { assertTrue(scheduler.isScheduled(other)); } } + + @Test + void cancelDefaultCommandFromEnd() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(); + Subsystem requirement = new Subsystem() {}; + Command defaultCommand = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> counter.incrementAndGet(), + () -> false, + requirement); + Command other = new InstantCommand(() -> {}, requirement); + Command cancelDefaultCommand = + new FunctionalCommand( + () -> {}, + () -> {}, + interrupted -> { + counter.incrementAndGet(); + scheduler.schedule(other); + }, + () -> false); + + assertDoesNotThrow( + () -> { + scheduler.schedule(cancelDefaultCommand); + scheduler.setDefaultCommand(requirement, defaultCommand); + + scheduler.run(); + scheduler.cancel(cancelDefaultCommand); + }); + assertEquals(2, counter.get()); + assertFalse(scheduler.isScheduled(defaultCommand)); + assertTrue(scheduler.isScheduled(other)); + } + } } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp index f1bbfd6143d..37353032a12 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp @@ -7,6 +7,7 @@ #include "CommandTestBase.h" #include "frc2/command/Command.h" #include "frc2/command/CommandHelper.h" +#include "frc2/command/FunctionalCommand.h" #include "frc2/command/RunCommand.h" using namespace frc2; @@ -17,21 +18,27 @@ class SchedulingRecursionTest class SelfCancellingCommand : public CommandHelper { public: - SelfCancellingCommand(CommandScheduler* scheduler, Subsystem* requirement, + SelfCancellingCommand(CommandScheduler* scheduler, int& counter, + Subsystem* requirement, Command::InterruptionBehavior interruptionBehavior = Command::InterruptionBehavior::kCancelSelf) - : m_scheduler(scheduler), m_interrupt(interruptionBehavior) { + : m_scheduler(scheduler), + m_counter(counter), + m_interrupt(interruptionBehavior) { AddRequirements(requirement); } void Initialize() override { m_scheduler->Cancel(this); } + void End(bool interrupted) override { m_counter++; } + InterruptionBehavior GetInterruptionBehavior() const override { return m_interrupt; } private: CommandScheduler* m_scheduler; + int& m_counter; InterruptionBehavior m_interrupt; }; @@ -39,13 +46,14 @@ class SelfCancellingCommand * Checks wpilibsuite/allwpilib#4259. */ -TEST_F(SchedulingRecursionTest, CancelFromInitialize) { +TEST_P(SchedulingRecursionTest, CancelFromInitialize) { CommandScheduler scheduler = GetScheduler(); bool hasOtherRun = false; + int counter = 0; TestSubsystem requirement; - auto selfCancels = SelfCancellingCommand(&scheduler, &requirement); - RunCommand other = - RunCommand([&hasOtherRun] { hasOtherRun = true; }, {&requirement}); + SelfCancellingCommand selfCancels{&scheduler, counter, &requirement, + GetParam()}; + RunCommand other{[&hasOtherRun] { hasOtherRun = true; }, {&requirement}}; scheduler.Schedule(&selfCancels); scheduler.Run(); @@ -53,6 +61,32 @@ TEST_F(SchedulingRecursionTest, CancelFromInitialize) { EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); EXPECT_TRUE(scheduler.IsScheduled(&other)); + EXPECT_EQ(1, counter); + scheduler.Run(); + EXPECT_TRUE(hasOtherRun); +} + +TEST_F(SchedulingRecursionTest, CancelFromInitializeAction) { + CommandScheduler scheduler = GetScheduler(); + bool hasOtherRun = false; + int counter = 0; + TestSubsystem requirement; + FunctionalCommand selfCancels{[] {}, + [] {}, + [&counter](bool) { counter++; }, + [] { return false; }, + {&requirement}}; + RunCommand other{[&hasOtherRun] { hasOtherRun = true; }, {&requirement}}; + scheduler.OnCommandInitialize([&scheduler, &selfCancels](const Command&) { + scheduler.Cancel(&selfCancels); + }); + scheduler.Schedule(&selfCancels); + scheduler.Run(); + scheduler.Schedule(&other); + + EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); + EXPECT_TRUE(scheduler.IsScheduled(&other)); + EXPECT_EQ(1, counter); scheduler.Run(); EXPECT_TRUE(hasOtherRun); } @@ -61,11 +95,11 @@ TEST_P(SchedulingRecursionTest, DefaultCommandGetsRescheduledAfterSelfCanceling) { CommandScheduler scheduler = GetScheduler(); bool hasOtherRun = false; + int counter = 0; TestSubsystem requirement; - auto selfCancels = - SelfCancellingCommand(&scheduler, &requirement, GetParam()); - RunCommand other = - RunCommand([&hasOtherRun] { hasOtherRun = true; }, {&requirement}); + SelfCancellingCommand selfCancels{&scheduler, counter, &requirement, + GetParam()}; + RunCommand other{[&hasOtherRun] { hasOtherRun = true; }, {&requirement}}; scheduler.SetDefaultCommand(&requirement, std::move(other)); scheduler.Schedule(&selfCancels); @@ -73,6 +107,7 @@ TEST_P(SchedulingRecursionTest, scheduler.Run(); EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); EXPECT_TRUE(scheduler.IsScheduled(scheduler.GetDefaultCommand(&requirement))); + EXPECT_EQ(1, counter); scheduler.Run(); EXPECT_TRUE(hasOtherRun); } @@ -104,6 +139,204 @@ TEST_F(SchedulingRecursionTest, CancelFromEnd) { EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); } +TEST_F(SchedulingRecursionTest, CancelFromInterruptAction) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + FunctionalCommand selfCancels{[] {}, [] {}, [](bool) {}, + [] { return false; }}; + scheduler.OnCommandInterrupt([&](const Command&) { + counter++; + scheduler.Cancel(&selfCancels); + }); + scheduler.Schedule(&selfCancels); + + EXPECT_NO_THROW({ scheduler.Cancel(&selfCancels); }); + EXPECT_EQ(1, counter); + EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); +} + +class EndCommand : public CommandHelper { + public: + explicit EndCommand(std::function end) : m_end(end) {} + void End(bool interrupted) override { m_end(interrupted); } + bool IsFinished() override { return true; } + + private: + std::function m_end; +}; + +TEST_F(SchedulingRecursionTest, CancelFromEndLoop) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + EndCommand dCancelsAll([&](bool) { + counter++; + scheduler.CancelAll(); + }); + EndCommand cCancelsD([&](bool) { + counter++; + scheduler.Cancel(&dCancelsAll); + }); + EndCommand bCancelsC([&](bool) { + counter++; + scheduler.Cancel(&cCancelsD); + }); + EndCommand aCancelsB([&](bool) { + counter++; + scheduler.Cancel(&bCancelsC); + }); + scheduler.Schedule(&aCancelsB); + scheduler.Schedule(&bCancelsC); + scheduler.Schedule(&cCancelsD); + scheduler.Schedule(&dCancelsAll); + + EXPECT_NO_THROW({ scheduler.Cancel(&aCancelsB); }); + EXPECT_EQ(4, counter); + EXPECT_FALSE(scheduler.IsScheduled(&aCancelsB)); + EXPECT_FALSE(scheduler.IsScheduled(&bCancelsC)); + EXPECT_FALSE(scheduler.IsScheduled(&cCancelsD)); + EXPECT_FALSE(scheduler.IsScheduled(&dCancelsAll)); +} + +TEST_F(SchedulingRecursionTest, CancelFromEndLoopWhileInRunLoop) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + EndCommand dCancelsAll([&](bool) { + counter++; + scheduler.CancelAll(); + }); + EndCommand cCancelsD([&](bool) { + counter++; + scheduler.Cancel(&dCancelsAll); + }); + EndCommand bCancelsC([&](bool) { + counter++; + scheduler.Cancel(&cCancelsD); + }); + EndCommand aCancelsB([&](bool) { + counter++; + scheduler.Cancel(&bCancelsC); + }); + scheduler.Schedule(&aCancelsB); + scheduler.Schedule(&bCancelsC); + scheduler.Schedule(&cCancelsD); + scheduler.Schedule(&dCancelsAll); + + EXPECT_NO_THROW({ scheduler.Run(); }); + EXPECT_EQ(4, counter); + EXPECT_FALSE(scheduler.IsScheduled(&aCancelsB)); + EXPECT_FALSE(scheduler.IsScheduled(&bCancelsC)); + EXPECT_FALSE(scheduler.IsScheduled(&cCancelsD)); + EXPECT_FALSE(scheduler.IsScheduled(&dCancelsAll)); +} + +class MultiCancelCommand : public CommandHelper { + public: + MultiCancelCommand(CommandScheduler* scheduler, int& counter, + Command* command) + : m_scheduler(scheduler), m_counter(counter), m_command(command) {} + + void End(bool interrupted) override { + m_counter++; + m_scheduler->Cancel(m_command); + m_scheduler->Cancel(this); + } + + private: + CommandScheduler* m_scheduler; + int& m_counter; + Command* m_command; +}; + +TEST_F(SchedulingRecursionTest, MultiCancelFromEnd) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + EndCommand bIncrementsCounter([&counter](bool) { counter++; }); + MultiCancelCommand aCancelsB{&scheduler, counter, &bIncrementsCounter}; + + scheduler.Schedule(&aCancelsB); + scheduler.Schedule(&bIncrementsCounter); + + EXPECT_NO_THROW({ scheduler.Cancel(&aCancelsB); }); + EXPECT_EQ(2, counter); + EXPECT_FALSE(scheduler.IsScheduled(&aCancelsB)); + EXPECT_FALSE(scheduler.IsScheduled(&bIncrementsCounter)); +} + +TEST_P(SchedulingRecursionTest, ScheduleFromEndCancel) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + TestSubsystem requirement; + SelfCancellingCommand selfCancels{&scheduler, counter, &requirement, + GetParam()}; + RunCommand other{[] {}, {&requirement}}; + + scheduler.Schedule(&selfCancels); + EXPECT_NO_THROW({ scheduler.Cancel(&selfCancels); }); + EXPECT_EQ(1, counter); + EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); +} + +TEST_P(SchedulingRecursionTest, ScheduleFromEndInterrupt) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + TestSubsystem requirement; + SelfCancellingCommand selfCancels{&scheduler, counter, &requirement, + GetParam()}; + RunCommand other{[] {}, {&requirement}}; + + scheduler.Schedule(&selfCancels); + EXPECT_NO_THROW({ scheduler.Schedule(&other); }); + EXPECT_EQ(1, counter); + EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); + EXPECT_TRUE(scheduler.IsScheduled(&other)); +} + +TEST_F(SchedulingRecursionTest, ScheduleFromEndInterruptAction) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + TestSubsystem requirement; + RunCommand selfCancels{[] {}, {&requirement}}; + RunCommand other{[] {}, {&requirement}}; + scheduler.OnCommandInterrupt([&](const Command&) { + counter++; + scheduler.Schedule(&other); + }); + scheduler.Schedule(&selfCancels); + EXPECT_NO_THROW({ scheduler.Schedule(&other); }); + EXPECT_EQ(1, counter); + EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); + EXPECT_TRUE(scheduler.IsScheduled(&other)); +} + +TEST_F(SchedulingRecursionTest, CancelDefaultCommandFromEnd) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + TestSubsystem requirement; + FunctionalCommand defaultCommand{[] {}, + [] {}, + [&counter](bool) { counter++; }, + [] { return false; }, + {&requirement}}; + RunCommand other{[] {}, {&requirement}}; + FunctionalCommand cancelDefaultCommand{[] {}, [] {}, + [&](bool) { + counter++; + scheduler.Schedule(&other); + }, + [] { return false; }}; + + EXPECT_NO_THROW({ + scheduler.Schedule(&cancelDefaultCommand); + scheduler.SetDefaultCommand(&requirement, std::move(defaultCommand)); + + scheduler.Run(); + scheduler.Cancel(&cancelDefaultCommand); + }); + EXPECT_EQ(2, counter); + EXPECT_FALSE(scheduler.IsScheduled(&defaultCommand)); + EXPECT_TRUE(scheduler.IsScheduled(&other)); +} + INSTANTIATE_TEST_SUITE_P( SchedulingRecursionTests, SchedulingRecursionTest, testing::Values(Command::InterruptionBehavior::kCancelSelf, From 367292996279556b006fc45e16dd5d9098c3f0b2 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Mon, 18 Sep 2023 00:27:29 -0400 Subject: [PATCH 2/2] Use direct version of cancel for consistency --- .../java/edu/wpi/first/wpilibj2/command/CommandScheduler.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6980eb30eb6..c868539ff46 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 @@ -278,7 +278,7 @@ public void run() { Command command = iterator.next(); if (isDisabled && !command.runsWhenDisabled()) { - cancel(command); + cancel(command, kNoInterruptor); continue; }