add a test to cancel the next command from the first one

Author: kytpbs

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java

@@ -5,6 +5,7 @@
 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.assertTrue;
 import static org.mockito.Mockito.never;
@@ -13,6 +14,7 @@
 
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.concurrent.atomic.AtomicInteger;
 import org.junit.jupiter.api.Test;
 
 class CommandScheduleTest extends CommandTestBase {
@@ -109,6 +111,49 @@ void schedulerCancelTest() {
     }
   }
 
+  @Test
+  void cancelNextCommandTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command[] commands = new Command[2];
+      var commandRunCounter = new AtomicInteger(0);
+      // due to the ordering of sets being non-deterministic, we cancel the other command in the
+      // first command and check that the second command is not run
+      Command command1 =
+          new RunCommand(
+              () -> {
+                scheduler.cancel(commands[1]);
+                commandRunCounter.incrementAndGet();
+              });
+      Command command2 =
+          new RunCommand(
+              () -> {
+                scheduler.cancel(commands[0]);
+                commandRunCounter.incrementAndGet();
+              });
+
+      commands[0] = command1;
+      commands[1] = command2;
+
+      scheduler.schedule(command1, command2);
+      scheduler.run();
+
+      assertEquals(
+          1, commandRunCounter.get(), "Second command was run when it shouldn't have been");
+
+      // only one of the commands should be canceled.
+      assertFalse(
+          scheduler.isScheduled(command1) && scheduler.isScheduled(command2),
+          "None of the commands were canceled when one should have been");
+      // one of the commands shouldn't be canceled because the other one is canceled first
+      assertTrue(
+          scheduler.isScheduled(command1) || scheduler.isScheduled(command2),
+          "Both commands were canceled when only one should have been");
+
+      scheduler.run();
+      assertEquals(2, commandRunCounter.get());
+    }
+  }
+
   @Test
   void notScheduledCancelTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp

@@ -2,6 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <frc2/command/RunCommand.h>
+
+#include <memory>
+
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <networktables/NetworkTableInstance.h>
 
@@ -102,6 +106,43 @@ TEST_F(CommandScheduleTest, NotScheduledCancel) {
   EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command));
 }
 
+TEST_F(CommandScheduleTest, CancelNextCommand) {
+  CommandScheduler scheduler = GetScheduler();
+
+  frc2::RunCommand** command1PtrPtr = new frc2::RunCommand*;
+  frc2::RunCommand** command2PtrPtr = new frc2::RunCommand*;
+  auto counterPtr = std::make_shared<int>(0);
+
+  auto command1 = frc2::RunCommand([counterPtr, &command2PtrPtr, &scheduler] {
+    scheduler.Cancel(*command2PtrPtr);
+    (*counterPtr)++;
+  });
+  auto command2 = frc2::RunCommand([counterPtr, &command1PtrPtr, &scheduler] {
+    scheduler.Cancel(*command1PtrPtr);
+    (*counterPtr)++;
+  });
+
+  *command1PtrPtr = &command1;
+  *command2PtrPtr = &command2;
+
+  scheduler.Schedule(&command1);
+  scheduler.Schedule(&command2);
+  scheduler.Run();
+
+  EXPECT_EQ(*counterPtr, 1);
+
+  // only one of the commands should be canceled.
+  EXPECT_FALSE(scheduler.IsScheduled(&command1) &&
+               scheduler.IsScheduled(&command2));
+  // one of the commands shouldn't be canceled because the other one is canceled
+  // first
+  EXPECT_TRUE(scheduler.IsScheduled(&command1) ||
+              scheduler.IsScheduled(&command2));
+
+  scheduler.Run();
+  EXPECT_EQ(*counterPtr, 2);
+}
+
 TEST_F(CommandScheduleTest, SmartDashboardCancel) {
   CommandScheduler scheduler = GetScheduler();
   frc::SmartDashboard::PutData("Scheduler", &scheduler);