[commands] Add interruptor parameter to onCommandInterrupt callbacks (#5461)

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);

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<Command*>& 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<Command*>& 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<Command*>& 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<Command*>& interruptor) {
+        EXPECT_FALSE(interruptor);
+      });
 
   scheduler.Schedule(&command);
   scheduler.Schedule(&command2);