[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,43 @@ 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) {
+  auto chooser = std::make_shared<frc::SendableChooser<std::string_view>>();
+  ((void)chooser->AddOption(commands.get()->GetName(),
+                            commands.get()->GetName()),
+   ...);
+  publish(chooser.get());
+  return Select<std::string_view>(
+      [sendableChooser = chooser]() mutable {
+        return sendableChooser->GetSelected();
+      },
+      std::pair{std::string_view{commands.get()->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,84 @@
+// 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 <functional>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <networktables/BooleanTopic.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StringArrayTopic.h>
+#include <wpi/sendable/Sendable.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Commands.h"
+#include "make_vector.h"
+
+using namespace frc2;
+
+using SendableChooserTestPublisherFunc = std::function<void(wpi::Sendable*)>;
+using SendableChooserTestArgs =
+    std::pair<std::function<frc2::CommandPtr(SendableChooserTestPublisherFunc)>,
+              std::vector<std::string>>;
+
+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&& [commandFactory, names] = GetParam();
+  nt::StringArraySubscriber optionsSubscriber =
+      nt::NetworkTableInstance::GetDefault()
+          .GetStringArrayTopic("/SmartDashboard/chooser/options")
+          .Subscribe({});
+  auto cmd = commandFactory(
+      [&](wpi::Sendable* c) { frc::SmartDashboard::PutData("chooser", c); });
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_EQ(names, optionsSubscriber.Get());
+}
+
+namespace utils {
+static frc2::CommandPtr CommandNamed(std::string_view name) {
+  return frc2::cmd::Print(name).WithName(name);
+}
+
+static const std::vector<SendableChooserTestArgs> OptionsAreCorrectParams() {
+  SendableChooserTestArgs empty{[](SendableChooserTestPublisherFunc func) {
+                                  return frc2::cmd::Choose(func);
+                                },
+                                std::vector<std::string>()};
+
+  SendableChooserTestArgs duplicateName{
+      [](SendableChooserTestPublisherFunc func) {
+        return frc2::cmd::Choose(func, CommandNamed("a"), CommandNamed("b"),
+                                 CommandNamed("a"));
+      },
+      make_vector<std::string>("a", "b")};
+
+  SendableChooserTestArgs happyPath{[](SendableChooserTestPublisherFunc func) {
+                                      return frc2::cmd::Choose(
+                                          func, CommandNamed("a"),
+                                          CommandNamed("b"), CommandNamed("c"));
+                                    },
+                                    make_vector<std::string>("a", "b", "c")};
+
+  return make_vector<SendableChooserTestArgs>(empty, duplicateName, happyPath);
+}
+
+}  // namespace utils
+
+INSTANTIATE_TEST_SUITE_P(SendableChooserCommandTests,
+                         SendableChooserCommandTest,
+                         testing::ValuesIn(utils::OptionsAreCorrectParams()));