Fancy auto gen

src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java

@@ -12,9 +12,7 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
@@ -30,13 +28,11 @@
 import frc.robot.shooter.commands.SpinUpAndShoot;
 import frc.robot.undertaker.UndertakerSubsystem;
 import frc.robot.util.AllianceUtil;
+import frc.robot.util.FilteredChooserGroup;
 import java.util.Collections;
 import java.util.List;
 
 public class PathPlannerAutoGenerator {
-
-    private SendableChooser<String> autoSelector;
-
     private Field2d field;
     private ShuffleboardTab tab;
     private GenericEntry delay;
@@ -46,6 +42,8 @@ public class PathPlannerAutoGenerator {
     private final UndertakerSubsystem undertaker;
     private final LEDSubsystem led;
     private Command ppAuto;
+    private String chooserResult = "none yet";
+    private FilteredChooserGroup chooserGroup;
 
     public PathPlannerAutoGenerator(
             DriveSubsystem drive,
@@ -78,23 +76,6 @@ public PathPlannerAutoGenerator(
                 AllianceUtil::isRedAlliance,
                 drive);
 
-        autoSelector = new SendableChooser<String>();
-        List<String> autos = AutoBuilder.getAllAutoNames();
-        Collections.sort(autos);
-
-        for (String s : autos) {
-            autoSelector.addOption(s, s);
-        }
-        autoSelector.setDefaultOption("SubC-MidDC", "SubC-MidDC");
-
-        autoSelector.onChange((cmd) -> {
-            if (cmd != null) {
-                setStartingPose(cmd);
-                ppAuto = getPathPlannerAuto(); // because we are composing later, we need to cook a fresh
-                // Command every time we select.
-            }
-        });
-
         setupShuffleboard();
     }
 
@@ -116,7 +97,29 @@ private void setStartingPose(String cmdName) {
     private void setupShuffleboard() {
         field = new Field2d();
         tab = Shuffleboard.getTab("Auto");
-        tab.add("Auto Selection", autoSelector).withPosition(0, 0).withSize(2, 1);
+        List<String> autos = AutoBuilder.getAllAutoNames();
+        Collections.sort(autos);
+
+        chooserGroup = new FilteredChooserGroup(tab, "AutoLayer", 3, autos.toArray(new String[0]));
+        chooserGroup.onChange(s -> {
+            chooserResult = s;
+            if (autos.contains(s)) {
+                setStartingPose(s);
+                ppAuto = new PathPlannerAuto(s);
+            }
+        });
+
+        tab.addString("Loaded Auto", () -> {
+            if (ppAuto != null) {
+                return ppAuto.getName();
+            } else {
+                return "no auto initialized";
+            }
+        });
+        tab.addBoolean(
+                "Auto Matches Choices",
+                () -> (ppAuto != null && ppAuto.getName().equals(chooserResult)));
+
         tab.add("Starting Pose", field).withPosition(0, 1).withSize(6, 4);
         tab.addString("Alliance", () -> AllianceUtil.isRedAlliance() ? "Red" : "Blue")
                 .withPosition(6, 1);
@@ -166,14 +169,6 @@ private void registerCommands() {
         NamedCommands.registerCommand("spinDown", new AutoSetTargetSpeed(shooter, 0, 0)); // non-blocking
     }
 
-    private Command getPathPlannerAuto() {
-        String selected = autoSelector.getSelected();
-        if (selected == null || selected.equalsIgnoreCase("none")) {
-            return Commands.none();
-        }
-        return new PathPlannerAuto(selected);
-    }
-
     public Command getAutoCommand() {
         Command autoCmd = new SequentialCommandGroup(
                 // We always want to shoot the preloaded piece, and we want to shoot before the delay.
@@ -185,7 +180,9 @@ public Command getAutoCommand() {
                 new WaitCommand(delay.getDouble(0)),
                 ppAuto,
                 new AutoSetTargetSpeed(shooter, 0, 0));
-        ppAuto = getPathPlannerAuto(); // rebake pp auto... this might cause delay (bad)
+        if (AutoBuilder.getAllAutoNames().contains(chooserResult)) {
+            ppAuto = new PathPlannerAuto(chooserResult); // rebake pp auto... this might cause delay (bad)
+        }
         return autoCmd;
     }
 }

src/main/java/frc/robot/util/ChangableSendableChooser.java

@@ -0,0 +1,263 @@
+package frc.robot.util;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.LinkedHashMap;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
+
+/**
+ * Basically a fork of SendableChooser that allows you to remove/clear the options.
+ *
+ * @param <V> The type of the values to be stored
+ */
+public class ChangableSendableChooser<V> implements Sendable, AutoCloseable {
+    /** The key for the default value. */
+    private static final String DEFAULT = "default";
+
+    /** The key for the selected option. */
+    private static final String SELECTED = "selected";
+
+    /** The key for the active option. */
+    private static final String ACTIVE = "active";
+
+    /** The key for the option array. */
+    private static final String OPTIONS = "options";
+
+    /** The key for the instance number. */
+    private static final String INSTANCE = ".instance";
+
+    /** A map linking strings to the objects they represent. */
+    private final Map<String, V> m_map = new LinkedHashMap<>();
+
+    private String m_defaultChoice = "";
+    private final int m_instance;
+    private String m_previousVal;
+    private Consumer<V> m_listener;
+    private static final AtomicInteger s_instances = new AtomicInteger();
+
+    /** Instantiates a {@link SendableChooser}. */
+    @SuppressWarnings("this-escape")
+    public ChangableSendableChooser() {
+        m_instance = s_instances.getAndIncrement();
+        SendableRegistry.add(this, "SendableChangableChooser", m_instance);
+    }
+
+    @Override
+    public void close() {
+        SendableRegistry.remove(this);
+    }
+
+    /**
+     * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
+     * object will appear as the given name.
+     *
+     * @param name the name of the option
+     * @param object the option
+     */
+    public void addOption(String name, V object) {
+        m_map.put(name, object);
+    }
+
+    /**
+     * Removes the given name and its object from the list of options.
+     * If it was the default choice, the default choice reverts to its default (empty string).
+     * If it was the current selected option, the selected option changes to the default choice.
+     * @param name the name of the option to remove
+     */
+    public void removeOption(String name) {
+        if (m_map.containsKey(name)) {
+            if (m_defaultChoice.equals(name)) {
+                m_defaultChoice = "";
+            }
+
+            if (m_selected.equals(name)) {
+                m_selected = m_defaultChoice;
+            }
+
+            m_map.remove(name);
+        }
+    }
+
+    /**
+     * Removes all options from the chooser.
+     * After this call, the default choice will revert to empty string.
+     * After this call, setting any value will trigger onChange, even if the value is the same as the value before clearing.
+     */
+    public void clearOptions() {
+        m_defaultChoice = "";
+        m_previousVal = "";
+        m_selected = m_defaultChoice;
+
+        m_map.clear();
+    }
+
+    /**
+     * Replaces the current chooser options with the given options.
+     * After this call, the default choice will revert to empty string.
+     * @param options map of (names to options) to add
+     */
+    public void setOptions(Map<String, V> options) {
+        requireNonNullParam(options, "options", "setOptions");
+
+        clearOptions();
+        m_map.putAll(options);
+    }
+
+    /**
+     * Replaces the current chooser options with the given options, then sets a default option.
+     * @param options map of (names to options) to add
+     * @param defaultName the name of the default option
+     * @param defaultObject the default option
+     */
+    public void setOptions(Map<String, V> options, String defaultName, V defaultObject) {
+        requireNonNullParam(options, "options", "setOptions");
+        requireNonNullParam(defaultName, "defaultName", "setOptions");
+        requireNonNullParam(defaultObject, "defaultObject", "setOptions");
+
+        setOptions(options);
+        setDefaultOption(defaultName, defaultObject);
+        if (m_selected.equals("")) {
+            m_selected = defaultName;
+        }
+    }
+
+    /**
+     * Adds the given object to the list of options and marks it as the default. Functionally, this is
+     * very close to {@link #addOption(String, Object)} except that it will use this as the default
+     * option if none other is explicitly selected.
+     *
+     * @param name the name of the option
+     * @param object the option
+     */
+    public void setDefaultOption(String name, V object) {
+        requireNonNullParam(name, "name", "setDefaultOption");
+
+        m_defaultChoice = name;
+        addOption(name, object);
+        m_selected = m_defaultChoice;
+    }
+
+    /**
+     * Returns the selected option. If there is none selected, it will return the default. If there is
+     * none selected and no default, then it will return {@code null}.
+     *
+     * @return the option selected
+     */
+    public V getSelected() {
+        m_mutex.lock();
+        try {
+            if (m_selected != null) {
+                return m_map.get(m_selected);
+            } else {
+                return m_map.get(m_defaultChoice);
+            }
+        } finally {
+            m_mutex.unlock();
+        }
+    }
+
+    /**
+     * Bind a listener that's called when the selected value changes. Only one listener can be bound.
+     * Calling this function will replace the previous listener.
+     *
+     * @param listener The function to call that accepts the new value
+     */
+    public void onChange(Consumer<V> listener) {
+        requireNonNullParam(listener, "listener", "onChange");
+        m_mutex.lock();
+        m_listener = listener;
+        m_mutex.unlock();
+    }
+
+    private String m_selected;
+    private final ReentrantLock m_mutex = new ReentrantLock();
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        builder.setSmartDashboardType("String Chooser");
+        builder.publishConstInteger(INSTANCE, m_instance);
+        builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
+        builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
+        builder.addStringProperty(
+                ACTIVE,
+                () -> {
+                    m_mutex.lock();
+                    try {
+                        if (m_selected != null) {
+                            return m_selected;
+                        } else {
+                            return m_defaultChoice;
+                        }
+                    } finally {
+                        m_mutex.unlock();
+                    }
+                },
+                null);
+        builder.addStringProperty(
+                SELECTED,
+                () -> {
+                    // SELECTED maintains a getter to keep a proper state and fire onChange when items are removed
+                    // if the final item is removed, this handles gracefully (gets empty string and no onChange call)
+                    V choice;
+                    Consumer<V> listener;
+                    String setSelectedTo;
+                    m_mutex.lock();
+                    try {
+                        if (m_selected != null) {
+                            setSelectedTo = m_selected;
+                        } else {
+                            setSelectedTo = m_defaultChoice;
+                        }
+                        if (!setSelectedTo.equals(m_previousVal)
+                                && m_listener != null
+                                && m_map.containsKey(setSelectedTo)) {
+                            choice = m_map.get(setSelectedTo);
+                            listener = m_listener;
+                        } else {
+                            choice = null;
+                            listener = null;
+                        }
+                        m_previousVal = setSelectedTo;
+                    } finally {
+                        m_mutex.unlock();
+                    }
+                    if (listener != null) {
+                        listener.accept(choice);
+                    }
+                    return setSelectedTo;
+                },
+                val -> {
+                    V choice;
+                    Consumer<V> listener;
+                    m_mutex.lock();
+                    try {
+                        m_selected = val;
+                        // If dashboard loads with a selected that isn't there anymore, reset it
+                        if (!m_map.containsKey(m_selected)) {
+                            m_selected = m_defaultChoice;
+                        }
+                        if (!m_selected.equals(m_previousVal) && m_listener != null) {
+                            choice = m_map.get(val);
+                            listener = m_listener;
+                        } else {
+                            choice = null;
+                            listener = null;
+                        }
+                        m_previousVal = val;
+                    } finally {
+                        m_mutex.unlock();
+                    }
+                    if (listener != null) {
+                        listener.accept(choice);
+                    }
+                });
+    }
+}