[choreolib] AutoChooser Fixes and Sim behavior changes (SleipnirGroup…

…#1057)

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

@@ -9,10 +9,13 @@
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import java.util.HashMap;
 import java.util.Map;
+import java.util.Optional;
 import java.util.Set;
 import java.util.function.Supplier;
 
@@ -43,13 +46,12 @@ 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 String selected = NONE_NAME;
   private String[] options = new String[] {NONE_NAME};
 
-  private String activeCommandName = NONE_NAME;
-  private Command selectedCommand = Commands.none();
+  private Optional<Alliance> allianceAtGeneration = Optional.empty();
+  private String nameAtGeneration = NONE_NAME;
+  private Command generatedCommand = Commands.none();
 
   /** Constructs a new {@link AutoChooser}. */
   public AutoChooser() {}
@@ -63,33 +65,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(nameAtGeneration)
+        && allianceAtGeneration.equals(DriverStation.getAlliance())) {
+      // early return if the selected auto matches the active auto
+      return nameAtGeneration;
+    }
+    boolean dsValid = DriverStation.isDisabled() && DriverStation.getAlliance().isPresent();
+    if (dsValid || force) {
       if (!autoRoutines.containsKey(selected) && !selected.equals(NONE_NAME)) {
         selected = NONE_NAME;
         selectedNonexistentAuto.set(true);
       } else {
         selectedNonexistentAuto.set(false);
       }
-      activeCommandName = selected;
+      allianceAtGeneration = DriverStation.getAlliance();
+      nameAtGeneration = selected;
+      generatedCommand = autoRoutines.get(nameAtGeneration).get().withName(nameAtGeneration);
+    } else {
+      allianceAtGeneration = Optional.empty();
+      nameAtGeneration = NONE_NAME;
+      generatedCommand = Commands.none();
     }
-    selectedCommand = autoRoutines.get(activeCommandName).get();
-    return activeCommandName;
+    return nameAtGeneration;
   }
 
   /**
    * 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 +136,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 +162,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 +176,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 +188,10 @@ public Command selectedCommandScheduler() {
    * @return The currently selected command.
    */
   public Command selectedCommand() {
-    return selectedCommand.withName(activeCommandName);
+    if (RobotBase.isSimulation() && nameAtGeneration == NONE_NAME) {
+      select(selected, true);
+    }
+    return generatedCommand;
   }
 
   @Override
@@ -183,7 +200,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);