[cmd] Add Commands.Choose aka SendableChooserCommand

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java

@@ -7,9 +7,13 @@
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import java.util.HashMap;
 import java.util.Map;
 import java.util.Set;
 import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
 import java.util.function.Supplier;
 
 /**
@@ -179,6 +183,37 @@ public static <K> Command select(Map<K, Command> commands, Supplier<? extends K>
     return new SelectCommand<>(commands, selector);
   }
 
+  /**
+   * Runs a command chosen from the dashboard.
+   *
+   * <p>Example usage:
+   *
+   * <pre>
+   * <code>
+   * Command autonomousCommand = Commands.choose(
+   *   chooser -> SmartDashboard.putData("Auto Chooser", chooser),
+   *   myFirstAuto.withName("First Auto"),
+   *   mySecondAuto.withName("Second Auto")
+   * );
+   * </code>
+   * </pre>
+   *
+   * @param publish lambda used for publishing the chooser to the dashboard
+   * @param commands commands to choose from
+   * @return the command
+   */
+  public static Command choose(Consumer<Sendable> publish, Command... commands) {
+    SendableChooser<String> chooser = new SendableChooser<>();
+    HashMap<String, Command> options = new HashMap<>(commands.length);
+    for (Command command : commands) {
+      var name = command.getName();
+      chooser.addOption(name, name);
+      options.put(name, command);
+    }
+    publish.accept(chooser);
+    return select(options, chooser::getSelected);
+  }
+
   /**
    * Runs the command supplied by the supplier.
    *

wpilibNewCommands/src/main/native/include/frc2/command/Commands.h

@@ -8,11 +8,14 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <type_traits>
 #include <utility>
 #include <vector>
 
+#include <frc/smartdashboard/SendableChooser.h>
 #include <wpi/deprecated.h>
+#include <wpi/sendable/Sendable.h>
 
 #include "frc2/command/CommandPtr.h"
 #include "frc2/command/Requirements.h"
@@ -156,6 +159,39 @@ CommandPtr Select(std::function<Key()> selector,
   return SelectCommand(std::move(selector), std::move(vec)).ToPtr();
 }
 
+/**
+ * Runs a command chosen from the dashboard.
+ *
+ * <p>Example usage:
+ *
+ * <pre>
+ * <code>
+ * CommandPtr autonomousCommand = frc2::cmd::Choose(
+ *   [](wpi::Sendable* chooser) { frc::SmartDashboard::PutData("Auto Chooser",
+ * chooser); }, std::move(myFirstAuto).WithName("First Auto"),
+ *   std::move(mySecondAuto).WithName("Second Auto")
+ * );
+ * </code>
+ * </pre>
+ *
+ * @param publish lambda used for publishing the chooser to the dashboard
+ * @param commands commands to choose from
+ * @return the command
+ */
+template <std::convertible_to<CommandPtr>... CommandPtrs>
+[[nodiscard]]
+CommandPtr Choose(std::function<void(wpi::Sendable*)> publish,
+                  CommandPtrs&&... commands) {
+  frc::SendableChooser<std::string_view> chooser;
+  ((void)chooser.AddOption(commands.GetName(), commands.GetName()), ...);
+  publish(&chooser);
+  return Select(
+      [sendableChooser = std::move(chooser)]() mutable {
+        return sendableChooser.GetSelected();
+      },
+      (std::pair{commands.GetName(), std::move(commands)}, ...));
+}
+
 /**
  * Runs the command supplied by the supplier.
  *

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

@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// 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.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class SendableChooserCommandTest extends CommandTestBase {
+  private NetworkTableInstance m_inst;
+  private BooleanPublisher m_publish;
+  private static final String kBasePath = "/SmartDashboard/chooser/";
+
+  @BeforeEach
+  void setUp() {
+    m_inst = NetworkTableInstance.create();
+    SmartDashboard.setNetworkTableInstance(m_inst);
+    m_publish = m_inst.getBooleanTopic("/SmartDashboard/chooser").publish();
+    SmartDashboard.updateValues();
+  }
+
+  @ParameterizedTest(name = "options[{index}]: {0}")
+  @MethodSource
+  void optionsAreCorrect(
+      @SuppressWarnings("unused") String testName, Command[] commands, String[] names) {
+    try (var optionsSubscriber =
+        m_inst.getStringArrayTopic(kBasePath + "options").subscribe(new String[] {})) {
+      @SuppressWarnings("unused")
+      var command = Commands.choose(c -> SmartDashboard.putData("chooser", c), commands);
+      SmartDashboard.updateValues();
+      assertArrayEquals(names, optionsSubscriber.get());
+    }
+  }
+
+  static Stream<Arguments> optionsAreCorrect() {
+    return Stream.of(
+        Arguments.of("empty", new Command[] {}, new String[] {}),
+        Arguments.of(
+            "duplicateName",
+            new Command[] {commandNamed("a"), commandNamed("b"), commandNamed("a")},
+            new String[] {"a", "b"}),
+        Arguments.of(
+            "happyPath",
+            new Command[] {commandNamed("a"), commandNamed("b"), commandNamed("c")},
+            new String[] {"a", "b", "c"}));
+  }
+
+  @AfterEach
+  void tearDown() {
+    m_publish.close();
+    m_inst.close();
+    SmartDashboard.setNetworkTableInstance(NetworkTableInstance.getDefault());
+  }
+
+  private static Command commandNamed(String name) {
+    return Commands.print(name).withName(name);
+  }
+}