[choreolib] AutoChooser Fixes and Sim behavior changes

choreolib/src/main/java/choreo/auto/AutoChooser.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import java.util.HashMap;
@@ -43,17 +44,20 @@ public class AutoChooser implements Sendable {
   private final HashMap<String, Supplier<Command>> autoRoutines =
       new HashMap<>(Map.of(NONE_NAME, Commands::none));
 
-  // private final StringEntry selected, active;
-  // private final StringArrayEntry options;
+  private int allianceId = -1;
   private String selected = NONE_NAME;
   private String[] options = new String[] {NONE_NAME};
 
   private String activeCommandName = NONE_NAME;
-  private Command selectedCommand = Commands.none();
+  private Command activeCommand = Commands.none();
 
   /** Constructs a new {@link AutoChooser}. */
   public AutoChooser() {}
 
+  private int fetchAllianceId() {
+    return DriverStation.getAlliance().map(alliance -> alliance.ordinal()).orElse(-1);
+  }
+
   /**
    * Select a new option in the chooser.
    *
@@ -63,33 +67,43 @@ public AutoChooser() {}
    * @return The name of the selected option.
    */
   public String select(String selectStr) {
+    return select(selectStr, false);
+  }
+
+  private String select(String selectStr, boolean force) {
     selected = selectStr;
-    if (DriverStation.isDisabled()
-        && DriverStation.isDSAttached()
-        && DriverStation.getAlliance().isPresent()) {
-      if (selected.equals(activeCommandName)) return activeCommandName;
+    if (selected.equals(activeCommandName) && fetchAllianceId() == allianceId) {
+      // early return if the selected auto matches the active auto
+      return activeCommandName;
+    }
+    boolean dsValid =
+        DriverStation.isDisabled() && DriverStation.isDSAttached() && fetchAllianceId() != -1;
+    if (dsValid || force) {
       if (!autoRoutines.containsKey(selected) && !selected.equals(NONE_NAME)) {
         selected = NONE_NAME;
         selectedNonexistentAuto.set(true);
       } else {
         selectedNonexistentAuto.set(false);
       }
+      allianceId = fetchAllianceId();
       activeCommandName = selected;
+      activeCommand = autoRoutines.get(activeCommandName).get().withName(activeCommandName);
+    } else {
+      allianceId = -1;
+      activeCommandName = NONE_NAME;
+      activeCommand = Commands.none();
     }
-    selectedCommand = autoRoutines.get(activeCommandName).get();
     return activeCommandName;
   }
 
   /**
    * Add an AutoRoutine to the chooser.
    *
-   * <p>The options of the chooser are actually a function that takes an {@link AutoFactory} and
-   * returns a {@link AutoRoutine}. These functions can be static, a lambda or belong to a local
-   * variable.
-   *
    * <p>This is done to load AutoRoutines when and only when they are selected, in order to save
    * memory and file loading time for unused AutoRoutines.
    *
+   * <p>The generators are only run when the DriverStation is disabled and the alliance is known.
+   *
    * <p>One way to keep this clean is to make an `Autos` class that all of your subsystems/resources
    * are <a href="https://en.wikipedia.org/wiki/Dependency_injection">dependency injected</a> into.
    * Then create methods inside that class that take an {@link AutoFactory} and return an {@link
@@ -124,6 +138,8 @@ public void addRoutine(String name, Supplier<AutoRoutine> generator) {
    * <p>This is done to load autonomous commands when and only when they are selected, in order to
    * save memory and file loading time for unused autonomous commands.
    *
+   * <p>The generators are only run when the DriverStation is disabled and the alliance is known.
+   *
    * <h3>Example:</h3>
    *
    * <pre><code>
@@ -148,8 +164,8 @@ public void addCmd(String name, Supplier<Command> generator) {
   }
 
   /**
-   * Gets a Command that schedules the selected auto routine. This Command finishes immediately as
-   * it simply schedules another Command. This Command can directly be bound to a trigger, like so:
+   * Gets a Command that schedules the selected auto routine. This Command shares the lifetime of
+   * the scheduled Command. This Command can directly be bound to a trigger, like so:
    *
    * <pre><code>
    *     AutoChooser chooser = ...;
@@ -162,7 +178,7 @@ public void addCmd(String name, Supplier<Command> generator) {
    * @return A command that runs the selected {@link AutoRoutine}
    */
   public Command selectedCommandScheduler() {
-    return Commands.defer(() -> selectedCommand.asProxy(), Set.of());
+    return Commands.defer(() -> selectedCommand().asProxy(), Set.of());
   }
 
   /**
@@ -174,7 +190,10 @@ public Command selectedCommandScheduler() {
    * @return The currently selected command.
    */
   public Command selectedCommand() {
-    return selectedCommand.withName(activeCommandName);
+    if (RobotBase.isSimulation() && activeCommandName == NONE_NAME) {
+      select(selected, true);
+    }
+    return activeCommand;
   }
 
   @Override
@@ -183,7 +202,7 @@ public void initSendable(SendableBuilder builder) {
     builder.publishConstBoolean(".controllable", true);
     builder.publishConstString("default", NONE_NAME);
     builder.addStringArrayProperty("options", () -> options, null);
-    builder.addStringProperty("selected", () -> selected, this::select);
+    builder.addStringProperty("selected", null, this::select);
     builder.addStringProperty("active", () -> select(selected), null);
   }
 }

choreolib/src/test/java/choreo/auto/AutoChooserTest.java

@@ -20,6 +20,7 @@
 
 public class AutoChooserTest {
   private static final String NONE_NAME = AutoChooser.NONE_NAME;
+  private static final String NOT_FOUND = "__NOT_FOUND__";
 
   private SendableBuilderImpl builder;
   private NetworkTableInstance ntInstance;
@@ -55,22 +56,22 @@ private NetworkTable table(String testFuncName) {
   }
 
   private void assertNTType(String testFuncName) {
-    String type = table(testFuncName).getEntry(".type").getString("");
+    String type = table(testFuncName).getEntry(".type").getString(NOT_FOUND);
     assertEquals("String Chooser", type);
   }
 
   private void assertNTSelected(String testFuncName, String expected) {
-    String type = table(testFuncName).getEntry("selected").getString("");
+    String type = table(testFuncName).getEntry("selected").getString(NOT_FOUND);
     assertEquals(expected, type);
   }
 
   private void assertNTActive(String testFuncName, String expected) {
-    String type = table(testFuncName).getEntry("active").getString("");
+    String type = table(testFuncName).getEntry("active").getString(NOT_FOUND);
     assertEquals(expected, type);
   }
 
   private void assertNTDefault(String testFuncName) {
-    String type = table(testFuncName).getEntry("default").getString("");
+    String type = table(testFuncName).getEntry("default").getString(NOT_FOUND);
     assertEquals(NONE_NAME, type);
   }
 
@@ -95,7 +96,7 @@ public void initializeTest() {
     new AutoChooser().initSendable(builder);
     builder.update();
     assertNTType(fnName);
-    assertNTSelected(fnName, NONE_NAME);
+    assertNTSelected(fnName, NOT_FOUND);
     assertNTActive(fnName, NONE_NAME);
     assertNTDefault(fnName);
     assertNTOptions(fnName, NONE_NAME);