diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java index c53409a033d..c9f1efe4b91 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java +++ b/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 Command select(Map commands, Supplier return new SelectCommand<>(commands, selector); } + /** + * Runs a command chosen from the dashboard. + * + *

Example usage: + * + * 

+   * 
+   * Command autonomousCommand = Commands.choose(
+   *   chooser -> SmartDashboard.putData("Auto Chooser", chooser),
+   *   myFirstAuto.withName("First Auto"),
+   *   mySecondAuto.withName("Second Auto")
+   * );
+   * 
+   *
+ * + * @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 publish, Command... commands) { + SendableChooser chooser = new SendableChooser<>(); + HashMap 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. * diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h index 6cfad01b1f0..8d09f6e7e14 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h @@ -8,11 +8,14 @@ #include #include #include +#include #include #include #include +#include #include +#include #include "frc2/command/CommandPtr.h" #include "frc2/command/Requirements.h" @@ -156,6 +159,43 @@ CommandPtr Select(std::function 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 ... CommandPtrs> +[[nodiscard]] +CommandPtr Choose(std::function publish, + CommandPtrs&&... commands) { + auto chooser = std::make_shared>(); + ((void)chooser->AddOption(commands.get()->GetName(), + commands.get()->GetName()), + ...); + publish(chooser.get()); + return Select( + [sendableChooser = chooser]() mutable { + return sendableChooser->GetSelected(); + }, + std::pair{std::string_view{commands.get()->GetName()}, + std::move(commands)}...); +} + /** * Runs the command supplied by the supplier. * diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SendableChooserCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SendableChooserCommandTest.java new file mode 100644 index 00000000000..814afaa7da7 --- /dev/null +++ b/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 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); + } +} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SendableChooserCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SendableChooserCommandTest.cpp new file mode 100644 index 00000000000..c35d988ab85 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "CommandTestBase.h" +#include "frc2/command/CommandPtr.h" +#include "frc2/command/Commands.h" +#include "make_vector.h" + +using namespace frc2; + +using SendableChooserTestPublisherFunc = std::function; +using SendableChooserTestArgs = + std::pair, + std::vector>; + +class SendableChooserCommandTest + : public CommandTestBaseWithParam { + 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 OptionsAreCorrectParams() { + SendableChooserTestArgs empty{[](SendableChooserTestPublisherFunc func) { + return frc2::cmd::Choose(func); + }, + std::vector()}; + + SendableChooserTestArgs duplicateName{ + [](SendableChooserTestPublisherFunc func) { + return frc2::cmd::Choose(func, CommandNamed("a"), CommandNamed("b"), + CommandNamed("a")); + }, + make_vector("a", "b")}; + + SendableChooserTestArgs happyPath{[](SendableChooserTestPublisherFunc func) { + return frc2::cmd::Choose( + func, CommandNamed("a"), + CommandNamed("b"), CommandNamed("c")); + }, + make_vector("a", "b", "c")}; + + return make_vector(empty, duplicateName, happyPath); +} + +} // namespace utils + +INSTANTIATE_TEST_SUITE_P(SendableChooserCommandTests, + SendableChooserCommandTest, + testing::ValuesIn(utils::OptionsAreCorrectParams()));