[commands] Make command scheduling order consistent (#5470)

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -92,6 +92,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
   private final Set<Command> m_toSchedule = new LinkedHashSet<>();
   private final List<Command> m_toCancelCommands = new ArrayList<>();
   private final List<Optional<Command>> m_toCancelInterruptors = new ArrayList<>();
+  private final Set<Command> m_endingCommands = new LinkedHashSet<>();
 
   private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
 
@@ -271,18 +272,13 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
     m_watchdog.addEpoch("buttons.run()");
 
     m_inRunLoop = true;
+    boolean isDisabled = RobotState.isDisabled();
     // Run scheduled commands, remove finished commands.
     for (Iterator<Command> iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) {
       Command command = iterator.next();
 
-      if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
-        command.end(true);
-        for (BiConsumer<Command, Optional<Command>> 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, kNoInterruptor);
         continue;
       }
 
@@ -292,10 +288,12 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
       }
       m_watchdog.addEpoch(command.getName() + ".execute()");
       if (command.isFinished()) {
+        m_endingCommands.add(command);
         command.end(false);
         for (Consumer<Command> action : m_finishActions) {
           action.accept(command);
         }
+        m_endingCommands.remove(command);
         iterator.remove();
 
         m_requirements.keySet().removeAll(command.getRequirements());
@@ -466,6 +464,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
       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 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
       return;
     }
 
-    m_scheduledCommands.remove(command);
-    m_requirements.keySet().removeAll(command.getRequirements());
+    m_endingCommands.add(command);
     command.end(true);
     for (BiConsumer<Command, Optional<Command>> 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)");
   }
 

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<Command*, 4> toSchedule;
   wpi::SmallVector<Command*, 4> toCancelCommands;
   wpi::SmallVector<std::optional<Command*>, 4> toCancelInterruptors;
+  wpi::SmallSet<Command*, 4> endingCommands;
 };
 
 template <typename TMap, typename TKey>
@@ -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)");
 }
 

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 @@ class SchedulingRecursionTest extends CommandTestBase {
               scheduler.cancel(this);
             }
 
+            @Override
+            public void end(boolean interrupted) {
+              counter.incrementAndGet();
+            }
+
             @Override
             public InterruptionBehavior getInterruptionBehavior() {
               return interruptionBehavior;
@@ -52,6 +58,47 @@ class SchedulingRecursionTest extends CommandTestBase {
           });
       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 @@ class SchedulingRecursionTest extends CommandTestBase {
   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 @@ class SchedulingRecursionTest extends CommandTestBase {
               scheduler.cancel(this);
             }
 
+            @Override
+            public void end(boolean interrupted) {
+              counter.incrementAndGet();
+            }
+
             @Override
             public InterruptionBehavior getInterruptionBehavior() {
               return interruptionBehavior;
@@ -90,6 +143,7 @@ class SchedulingRecursionTest extends CommandTestBase {
       scheduler.run();
       assertFalse(scheduler.isScheduled(selfCancels));
       assertTrue(scheduler.isScheduled(other));
+      assertEquals(1, counter.get());
       scheduler.run();
       assertTrue(hasOtherRun.get());
     }
@@ -115,24 +169,177 @@ class SchedulingRecursionTest extends CommandTestBase {
     }
   }
 
+  @Test
+  void cancelFromInterruptAction() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      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.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);
-      Command selfCancels =
-          new Command() {
-            {
-              addRequirements(requirement);
-            }
-
-            @Override
-            public void end(boolean interrupted) {
-              counter.incrementAndGet();
-              scheduler.schedule(other);
-            }
-          };
+      FunctionalCommand selfCancels =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.schedule(other);
+              },
+              () -> false,
+              requirement);
 
       scheduler.schedule(selfCancels);
 
@@ -148,19 +355,38 @@ class SchedulingRecursionTest extends CommandTestBase {
       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 @@ class SchedulingRecursionTest extends CommandTestBase {
       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 @@ class SchedulingRecursionTest extends CommandTestBase {
       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));
+    }
+  }
 }

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<Command, SelfCancellingCommand> {
  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 <a
  * href="https://github.com/wpilibsuite/allwpilib/issues/4259">wpilibsuite/allwpilib#4259</a>.
  */
-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<Command, EndCommand> {
+ public:
+  explicit EndCommand(std::function<void(bool)> end) : m_end(end) {}
+  void End(bool interrupted) override { m_end(interrupted); }
+  bool IsFinished() override { return true; }
+
+ private:
+  std::function<void(bool)> 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<Command, MultiCancelCommand> {
+ 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,