[cmd] Add Commands.Choose aka SendableChooserCommand

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

@@ -6,9 +6,14 @@
 
 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;
 
 /**
@@ -154,6 +159,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,10 +8,15 @@
 #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"
 #include "frc2/command/SelectCommand.h"
@@ -142,6 +147,40 @@ CommandPtr Select(std::function<Key()> selector,
   return SelectCommand(std::move(selector), std::move(vec)).ToPtr();
 }
 
+/**
+ * Runs a command chosen from the dashboard.
+ *
+ *
+ * Example usage:
+ *
+ * ```cpp
+ * frc2::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"));
+ * ```
+ *
+ *
+ * @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);
+  }
+}

wpilibNewCommands/src/test/native/cpp/frc2/command/SendableChooserCommandTest.cpp

@@ -0,0 +1,71 @@
+// 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.
+
+#include "CommandTestBase.h"
+#include "frc/smartdashboard/SmartDashboard.h"
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Commands.h"
+#include "wpi/sendable/Sendable.h"
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/BooleanTopic.h>
+#include <networktables/StringArrayTopic.h>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include "make_vector.h"
+
+using namespace frc2;
+
+typename SendableChooserTestArgs =
+    std::pair<std::vector<frc2::CommandPtr>, std::vector<std::string_view>>;
+
+class SendableChooserCommandTest
+    : public CommandTestBaseWithParam<SendableChooserTestArgs> {
+ protected:
+  nt::BooleanPublisher m_publish;
+  void SetUp() override {
+    m_publish = nt::NetworkTableInstance::GetDefault()
+                    .GetBooleanTopic("/SmartDashboard/chooser")
+                    .Publish();
+    frc::SmartDashboard::UpdateValues();
+  }
+};
+
+TEST_P(SendableChooserCommandTest, OptionsAreCorrect) {
+  auto&& [commands, names] = GetParam();
+  nt::StringArraySubscriber optionsSubscriber =
+      nt::NetworkTableInstance::GetDefault()
+          .GetStringArrayTopic("/SmartDashboard/chooser/options")
+          .Subscribe();
+  auto cmd = frc2::cmd::Choose(
+      [&](wpi::Sendable* c) { frc::SmartDashboard::PutData("chooser", c); },
+      std::move(commands));
+  frc::SmartDashboard::UpdateValues();
+  //  EXPECT_THAT(optionsSubscriber.Get(), ::testing::ElementsAre)
+}
+
+namespace utils {
+const static frc2::CommandPtr CommandNamed(std::string_view name) {
+  return frc2::cmd::Print(name).WithName(name);
+}
+
+const static auto OptionsAreCorrectParams() {
+  SendableChooserTestArgs empty{make_vector<frc2::CommandPtr>(), make_vector<std::string_view>()};
+
+  SendableChooserTestArgs duplicateName{
+    make_vector<frc2::CommandPtr>(CommandNamed("a"), CommandNamed("b"), CommandNamed("a")),
+    make_vector<std::string_view>("a", "b")};
+
+  SendableChooserTestArgs happyPath{
+    make_vector<frc2::CommandPtr>(CommandNamed("a"), CommandNamed("b"), CommandNamed("c")),
+    make_vector<std::string_view>("a", "b", "c")};
+
+  return testing::Values(empty, duplicateName, happyPath);
+}
+
+} // namespace utils
+
+INSTANTIATE_TEST_SUITE_P(SendableChooserCommandTests, SendableChooserCommandTest,
+                         utils::OptionsAreCorrectParams());