Merge branch 'wpilibsuite:main' into Fix-cpp-commandScheduler

wpilibsuite · Jun 2, 2024 · 3dc304c · 3dc304c
2 parents dae1de3 + 1828fda
commit 3dc304c
Show file tree

Hide file tree

Showing 47 changed files with 623 additions and 395 deletions.
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
@@ -5,7 +5,7 @@
 package edu.wpi.first.wpilibj2.command;
 
 import java.util.Collections;
-import java.util.HashMap;
+import java.util.LinkedHashMap;
 import java.util.Map;
 
 /**
@@ -19,7 +19,9 @@
  */
 public class ParallelCommandGroup extends Command {
   // maps commands in this composition to whether they are still running
-  private final Map<Command, Boolean> m_commands = new HashMap<>();
+  // LinkedHashMap guarantees we iterate over commands in the order they were added (Note that
+  // changing the value associated with a command does NOT change the order)
+  private final Map<Command, Boolean> m_commands = new LinkedHashMap<>();
   private boolean m_runWhenDisabled = true;
   private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
 

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
@@ -6,7 +6,7 @@
 
 import edu.wpi.first.util.sendable.SendableBuilder;
 import java.util.Collections;
-import java.util.HashMap;
+import java.util.LinkedHashMap;
 import java.util.Map;
 
 /**
@@ -22,7 +22,9 @@
  */
 public class ParallelDeadlineGroup extends Command {
   // maps commands in this composition to whether they are still running
-  private final Map<Command, Boolean> m_commands = new HashMap<>();
+  // LinkedHashMap guarantees we iterate over commands in the order they were added (Note that
+  // changing the value associated with a command does NOT change the order)
+  private final Map<Command, Boolean> m_commands = new LinkedHashMap<>();
   private boolean m_runWhenDisabled = true;
   private boolean m_finished = true;
   private Command m_deadline;
@@ -39,8 +41,8 @@ public class ParallelDeadlineGroup extends Command {
    * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
    */
   public ParallelDeadlineGroup(Command deadline, Command... otherCommands) {
-    addCommands(otherCommands);
     setDeadline(deadline);
+    addCommands(otherCommands);
   }
 
   /**

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
@@ -5,7 +5,7 @@
 package edu.wpi.first.wpilibj2.command;
 
 import java.util.Collections;
-import java.util.HashSet;
+import java.util.LinkedHashSet;
 import java.util.Set;
 
 /**
@@ -19,7 +19,8 @@
  * <p>This class is provided by the NewCommands VendorDep
  */
 public class ParallelRaceGroup extends Command {
-  private final Set<Command> m_commands = new HashSet<>();
+  // LinkedHashSet guarantees we iterate over commands in the order they were added
+  private final Set<Command> m_commands = new LinkedHashSet<>();
   private boolean m_runWhenDisabled = true;
   private boolean m_finished = true;
   private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp
@@ -132,17 +132,17 @@ CommandPtr CommandPtr::BeforeStarting(CommandPtr&& before) && {
 CommandPtr CommandPtr::WithTimeout(units::second_t duration) && {
   AssertValid();
   std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::make_unique<WaitCommand>(duration));
   temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(std::make_unique<WaitCommand>(duration));
   m_ptr = std::make_unique<ParallelRaceGroup>(std::move(temp));
   return std::move(*this);
 }
 
 CommandPtr CommandPtr::Until(std::function<bool()> condition) && {
   AssertValid();
   std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
   temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
   m_ptr = std::make_unique<ParallelRaceGroup>(std::move(temp));
   return std::move(*this);
 }

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -57,6 +58,36 @@ void untilTest() {
     }
   }
 
+  @Test
+  void untilOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean firstHasRun = new AtomicBoolean(false);
+      AtomicBoolean firstWasPolled = new AtomicBoolean(false);
+
+      Command first =
+          new FunctionalCommand(
+              () -> {},
+              () -> firstHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                firstWasPolled.set(true);
+                return true;
+              });
+      Command command =
+          first.until(
+              () -> {
+                assertAll(
+                    () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+                return true;
+              });
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+    }
+  }
+
   @Test
   void onlyWhileTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
@@ -76,6 +107,36 @@ void onlyWhileTest() {
     }
   }
 
+  @Test
+  void onlyWhileOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean firstHasRun = new AtomicBoolean(false);
+      AtomicBoolean firstWasPolled = new AtomicBoolean(false);
+
+      Command first =
+          new FunctionalCommand(
+              () -> {},
+              () -> firstHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                firstWasPolled.set(true);
+                return true;
+              });
+      Command command =
+          first.onlyWhile(
+              () -> {
+                assertAll(
+                    () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+                return false;
+              });
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+    }
+  }
+
   @Test
   void ignoringDisableTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
@@ -179,6 +240,37 @@ void deadlineForTest() {
     }
   }
 
+  @Test
+  void deadlineForOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean dictatorHasRun = new AtomicBoolean(false);
+      AtomicBoolean dictatorWasPolled = new AtomicBoolean(false);
+
+      Command dictator =
+          new FunctionalCommand(
+              () -> {},
+              () -> dictatorHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                dictatorWasPolled.set(true);
+                return true;
+              });
+      Command other =
+          new RunCommand(
+              () ->
+                  assertAll(
+                      () -> assertTrue(dictatorHasRun.get()),
+                      () -> assertTrue(dictatorWasPolled.get())));
+
+      Command group = dictator.deadlineFor(other);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(dictatorHasRun.get()), () -> assertTrue(dictatorWasPolled.get()));
+    }
+  }
+
   @Test
   void alongWithTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
@@ -201,6 +293,36 @@ void alongWithTest() {
     }
   }
 
+  @Test
+  void alongWithOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean firstHasRun = new AtomicBoolean(false);
+      AtomicBoolean firstWasPolled = new AtomicBoolean(false);
+
+      Command command1 =
+          new FunctionalCommand(
+              () -> {},
+              () -> firstHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                firstWasPolled.set(true);
+                return true;
+              });
+      Command command2 =
+          new RunCommand(
+              () ->
+                  assertAll(
+                      () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get())));
+
+      Command group = command1.alongWith(command2);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+    }
+  }
+
   @Test
   void raceWithTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
@@ -216,6 +338,37 @@ void raceWithTest() {
     }
   }
 
+  @Test
+  void raceWithOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean firstHasRun = new AtomicBoolean(false);
+      AtomicBoolean firstWasPolled = new AtomicBoolean(false);
+
+      Command command1 =
+          new FunctionalCommand(
+              () -> {},
+              () -> firstHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                firstWasPolled.set(true);
+                return true;
+              });
+      Command command2 =
+          new RunCommand(
+              () -> {
+                assertTrue(firstHasRun.get());
+                assertTrue(firstWasPolled.get());
+              });
+
+      Command group = command1.raceWith(command2);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+    }
+  }
+
   @Test
   void unlessTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {