diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index b9183146c0..ff5e8f6aa5 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -99,11 +99,19 @@ CommandPtr Command::BeforeStarting(std::function toRun, requirements); } +CommandPtr Command::BeforeStarting(CommandPtr&& before) && { + return std::move(*this).ToPtr().BeforeStarting(std::move(before)); +} + CommandPtr Command::AndThen(std::function toRun, Requirements requirements) && { return std::move(*this).ToPtr().AndThen(std::move(toRun), requirements); } +CommandPtr Command::AndThen(CommandPtr&& next) && { + return std::move(*this).ToPtr().AndThen(std::move(next)); +} + CommandPtr Command::Repeatedly() && { return std::move(*this).ToPtr().Repeatedly(); } @@ -120,6 +128,18 @@ CommandPtr Command::OnlyIf(std::function condition) && { return std::move(*this).ToPtr().OnlyIf(std::move(condition)); } +CommandPtr Command::DeadlineFor(CommandPtr&& parallel) && { + return std::move(*this).ToPtr().DeadlineFor(std::move(parallel)); +} + +CommandPtr Command::AlongWith(CommandPtr&& parallel) && { + return std::move(*this).ToPtr().AlongWith(std::move(parallel)); +} + +CommandPtr Command::RaceWith(CommandPtr&& parallel) && { + return std::move(*this).ToPtr().RaceWith(std::move(parallel)); +} + CommandPtr Command::FinallyDo(std::function end) && { return std::move(*this).ToPtr().FinallyDo(std::move(end)); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index 27ac7f3582..aabbf05372 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -228,6 +228,16 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { CommandPtr BeforeStarting(std::function toRun, Requirements requirements = {}) &&; + /** + * Decorates this command with another command to run before this command + * starts. + * + * @param before the command to run before this one + * @return the decorated command + */ + [[nodiscard]] + CommandPtr BeforeStarting(CommandPtr&& before) &&; + /** * Decorates this command with a runnable to run after the command finishes. * @@ -239,6 +249,17 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { CommandPtr AndThen(std::function toRun, Requirements requirements = {}) &&; + /** + * Decorates this command with a set of commands to run after it in sequence. + * Often more convenient/less-verbose than constructing a + * SequentialCommandGroup explicitly. + * + * @param next the commands to run next + * @return the decorated command + */ + [[nodiscard]] + CommandPtr AndThen(CommandPtr&& next) &&; + /** * Decorates this command to run repeatedly, restarting it when it ends, until * this command is interrupted. The decorated command can still be canceled. @@ -288,6 +309,40 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { [[nodiscard]] CommandPtr OnlyIf(std::function condition) &&; + /** + * Decorates this command with a set of commands to run parallel to it, ending + * when the calling command ends and interrupting all the others. Often more + * convenient/less-verbose than constructing a new {@link + * ParallelDeadlineGroup} explicitly. + * + * @param parallel the commands to run in parallel. Note the parallel commands + * will be interupted when the deadline command ends + * @return the decorated command + */ + [[nodiscard]] + CommandPtr DeadlineFor(CommandPtr&& parallel) &&; + /** + * Decorates this command with a set of commands to run parallel to it, ending + * when the last command ends. Often more convenient/less-verbose than + * constructing a new {@link ParallelCommandGroup} explicitly. + * + * @param parallel the commands to run in parallel + * @return the decorated command + */ + [[nodiscard]] + CommandPtr AlongWith(CommandPtr&& parallel) &&; + + /** + * Decorates this command with a set of commands to run parallel to it, ending + * when the first command ends. Often more convenient/less-verbose than + * constructing a new {@link ParallelRaceGroup} explicitly. + * + * @param parallel the commands to run in parallel + * @return the decorated command + */ + [[nodiscard]] + CommandPtr RaceWith(CommandPtr&& parallel) &&; + /** * Decorates this command to run or stop when disabled. * diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java index 582c176596..61cbffd61b 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java @@ -22,17 +22,14 @@ class CommandDecoratorTest extends CommandTestBase { HAL.initialize(500, 0); SimHooks.pauseTiming(); try (CommandScheduler scheduler = new CommandScheduler()) { - Command command1 = new WaitCommand(10); - - Command timeout = command1.withTimeout(2); + Command timeout = new RunCommand(() -> {}).withTimeout(0.1); scheduler.schedule(timeout); scheduler.run(); - assertFalse(scheduler.isScheduled(command1)); assertTrue(scheduler.isScheduled(timeout)); - SimHooks.stepTiming(3); + SimHooks.stepTiming(0.15); scheduler.run(); assertFalse(scheduler.isScheduled(timeout)); @@ -44,15 +41,18 @@ class CommandDecoratorTest extends CommandTestBase { @Test void untilTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - AtomicBoolean condition = new AtomicBoolean(); + AtomicBoolean finish = new AtomicBoolean(); - Command command = new WaitCommand(10).until(condition::get); + Command command = new RunCommand(() -> {}).until(finish::get); scheduler.schedule(command); scheduler.run(); + assertTrue(scheduler.isScheduled(command)); - condition.set(true); + + finish.set(true); scheduler.run(); + assertFalse(scheduler.isScheduled(command)); } } @@ -60,15 +60,18 @@ class CommandDecoratorTest extends CommandTestBase { @Test void onlyWhileTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - AtomicBoolean condition = new AtomicBoolean(true); + AtomicBoolean run = new AtomicBoolean(true); - Command command = new WaitCommand(10).onlyWhile(condition::get); + Command command = new RunCommand(() -> {}).onlyWhile(run::get); scheduler.schedule(command); scheduler.run(); + assertTrue(scheduler.isScheduled(command)); - condition.set(false); + + run.set(false); scheduler.run(); + assertFalse(scheduler.isScheduled(command)); } } @@ -90,61 +93,75 @@ class CommandDecoratorTest extends CommandTestBase { @Test void beforeStartingTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - AtomicBoolean condition = new AtomicBoolean(); - condition.set(false); + AtomicBoolean finished = new AtomicBoolean(); + finished.set(false); - Command command = new InstantCommand(); + Command command = new InstantCommand().beforeStarting(() -> finished.set(true)); - scheduler.schedule(command.beforeStarting(() -> condition.set(true))); + scheduler.schedule(command); - assertTrue(condition.get()); + assertTrue(finished.get()); + + scheduler.run(); + + assertTrue(scheduler.isScheduled(command)); + + scheduler.run(); + + assertFalse(scheduler.isScheduled(command)); } } @Test void andThenLambdaTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - AtomicBoolean condition = new AtomicBoolean(); - condition.set(false); + AtomicBoolean finished = new AtomicBoolean(false); - Command command = new InstantCommand(); + Command command = new InstantCommand().andThen(() -> finished.set(true)); - scheduler.schedule(command.andThen(() -> condition.set(true))); + scheduler.schedule(command); - assertFalse(condition.get()); + assertFalse(finished.get()); scheduler.run(); - assertTrue(condition.get()); + assertTrue(finished.get()); + + scheduler.run(); + + assertFalse(scheduler.isScheduled(command)); } } @Test void andThenTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - AtomicBoolean condition = new AtomicBoolean(); - condition.set(false); + AtomicBoolean condition = new AtomicBoolean(false); Command command1 = new InstantCommand(); Command command2 = new InstantCommand(() -> condition.set(true)); + Command group = command1.andThen(command2); - scheduler.schedule(command1.andThen(command2)); + scheduler.schedule(group); assertFalse(condition.get()); scheduler.run(); assertTrue(condition.get()); + + scheduler.run(); + + assertFalse(scheduler.isScheduled(group)); } } @Test - void deadlineWithTest() { + void deadlineForTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - AtomicBoolean condition = new AtomicBoolean(); - condition.set(false); + AtomicBoolean finish = new AtomicBoolean(false); - Command dictator = new WaitUntilCommand(condition::get); + Command dictator = new WaitUntilCommand(finish::get); Command endsBefore = new InstantCommand(); Command endsAfter = new WaitUntilCommand(() -> false); @@ -155,7 +172,7 @@ class CommandDecoratorTest extends CommandTestBase { assertTrue(scheduler.isScheduled(group)); - condition.set(true); + finish.set(true); scheduler.run(); assertFalse(scheduler.isScheduled(group)); @@ -165,10 +182,9 @@ class CommandDecoratorTest extends CommandTestBase { @Test void alongWithTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - AtomicBoolean condition = new AtomicBoolean(); - condition.set(false); + AtomicBoolean finish = new AtomicBoolean(false); - Command command1 = new WaitUntilCommand(condition::get); + Command command1 = new WaitUntilCommand(finish::get); Command command2 = new InstantCommand(); Command group = command1.alongWith(command2); @@ -178,7 +194,7 @@ class CommandDecoratorTest extends CommandTestBase { assertTrue(scheduler.isScheduled(group)); - condition.set(true); + finish.set(true); scheduler.run(); assertFalse(scheduler.isScheduled(group)); @@ -203,40 +219,38 @@ class CommandDecoratorTest extends CommandTestBase { @Test void unlessTest() { try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean hasRun = new AtomicBoolean(false); AtomicBoolean unlessCondition = new AtomicBoolean(true); - AtomicBoolean hasRunCondition = new AtomicBoolean(false); - Command command = - new InstantCommand(() -> hasRunCondition.set(true)).unless(unlessCondition::get); + Command command = new InstantCommand(() -> hasRun.set(true)).unless(unlessCondition::get); scheduler.schedule(command); scheduler.run(); - assertFalse(hasRunCondition.get()); + assertFalse(hasRun.get()); unlessCondition.set(false); scheduler.schedule(command); scheduler.run(); - assertTrue(hasRunCondition.get()); + assertTrue(hasRun.get()); } } @Test void onlyIfTest() { try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean hasRun = new AtomicBoolean(false); AtomicBoolean onlyIfCondition = new AtomicBoolean(false); - AtomicBoolean hasRunCondition = new AtomicBoolean(false); - Command command = - new InstantCommand(() -> hasRunCondition.set(true)).onlyIf(onlyIfCondition::get); + Command command = new InstantCommand(() -> hasRun.set(true)).onlyIf(onlyIfCondition::get); scheduler.schedule(command); scheduler.run(); - assertFalse(hasRunCondition.get()); + assertFalse(hasRun.get()); onlyIfCondition.set(true); scheduler.schedule(command); scheduler.run(); - assertTrue(hasRunCondition.get()); + assertTrue(hasRun.get()); } } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp index 7c342476fb..2cd54ba161 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp @@ -8,6 +8,7 @@ #include "frc2/command/FunctionalCommand.h" #include "frc2/command/InstantCommand.h" #include "frc2/command/RunCommand.h" +#include "frc2/command/WaitUntilCommand.h" using namespace frc2; class CommandDecoratorTest : public CommandTestBase {}; @@ -20,13 +21,14 @@ TEST_F(CommandDecoratorTest, WithTimeout) { auto command = RunCommand([] {}, {}).WithTimeout(100_ms); scheduler.Schedule(command); - scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(command)); frc::sim::StepTiming(150_ms); scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(command)); frc::sim::ResumeTiming(); @@ -35,18 +37,18 @@ TEST_F(CommandDecoratorTest, WithTimeout) { TEST_F(CommandDecoratorTest, Until) { CommandScheduler scheduler = GetScheduler(); - bool finished = false; + bool finish = false; - auto command = RunCommand([] {}, {}).Until([&finished] { return finished; }); + auto command = RunCommand([] {}, {}).Until([&finish] { return finish; }); scheduler.Schedule(command); - scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(command)); - finished = true; - + finish = true; scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(command)); } @@ -58,13 +60,13 @@ TEST_F(CommandDecoratorTest, OnlyWhile) { auto command = RunCommand([] {}, {}).OnlyWhile([&run] { return run; }); scheduler.Schedule(command); - scheduler.Run(); + EXPECT_TRUE(scheduler.IsScheduled(command)); run = false; - scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(command)); } @@ -94,12 +96,15 @@ TEST_F(CommandDecoratorTest, BeforeStarting) { EXPECT_TRUE(finished); scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(command)); + scheduler.Run(); EXPECT_FALSE(scheduler.IsScheduled(command)); } -TEST_F(CommandDecoratorTest, AndThen) { +TEST_F(CommandDecoratorTest, AndThenLambda) { CommandScheduler scheduler = GetScheduler(); bool finished = false; @@ -112,28 +117,106 @@ TEST_F(CommandDecoratorTest, AndThen) { EXPECT_FALSE(finished); scheduler.Run(); + + EXPECT_TRUE(finished); + scheduler.Run(); EXPECT_FALSE(scheduler.IsScheduled(command)); +} + +TEST_F(CommandDecoratorTest, AndThen) { + CommandScheduler scheduler = GetScheduler(); + + bool finished = false; + + auto command1 = InstantCommand(); + auto command2 = InstantCommand([&finished] { finished = true; }); + auto group = std::move(command1).AndThen(std::move(command2).ToPtr()); + + scheduler.Schedule(group); + + EXPECT_FALSE(finished); + + scheduler.Run(); + EXPECT_TRUE(finished); + + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(group)); +} + +TEST_F(CommandDecoratorTest, DeadlineFor) { + CommandScheduler scheduler = GetScheduler(); + + bool finish = false; + + auto dictator = WaitUntilCommand([&finish] { return finish; }); + auto endsAfter = WaitUntilCommand([] { return false; }); + + auto group = std::move(dictator).DeadlineFor(std::move(endsAfter).ToPtr()); + + scheduler.Schedule(group); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(group)); + + finish = true; + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(group)); +} + +TEST_F(CommandDecoratorTest, AlongWith) { + CommandScheduler scheduler = GetScheduler(); + + bool finish = false; + + auto command1 = WaitUntilCommand([&finish] { return finish; }); + auto command2 = InstantCommand(); + + auto group = std::move(command1).AlongWith(std::move(command2).ToPtr()); + + scheduler.Schedule(group); + scheduler.Run(); + + EXPECT_TRUE(scheduler.IsScheduled(group)); + + finish = true; + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(group)); +} + +TEST_F(CommandDecoratorTest, RaceWith) { + CommandScheduler scheduler = GetScheduler(); + + auto command1 = WaitUntilCommand([] { return false; }); + auto command2 = InstantCommand(); + + auto group = std::move(command1).RaceWith(std::move(command2).ToPtr()); + + scheduler.Schedule(group); + scheduler.Run(); + + EXPECT_FALSE(scheduler.IsScheduled(group)); } TEST_F(CommandDecoratorTest, Unless) { CommandScheduler scheduler = GetScheduler(); bool hasRun = false; - bool unlessBool = true; + bool unlessCondition = true; - auto command = - InstantCommand([&hasRun] { hasRun = true; }, {}).Unless([&unlessBool] { - return unlessBool; - }); + auto command = InstantCommand([&hasRun] { hasRun = true; }, {}) + .Unless([&unlessCondition] { return unlessCondition; }); scheduler.Schedule(command); scheduler.Run(); EXPECT_FALSE(hasRun); - unlessBool = false; + unlessCondition = false; scheduler.Schedule(command); scheduler.Run(); EXPECT_TRUE(hasRun); @@ -143,18 +226,16 @@ TEST_F(CommandDecoratorTest, OnlyIf) { CommandScheduler scheduler = GetScheduler(); bool hasRun = false; - bool onlyIfBool = false; + bool onlyIfCondition = false; - auto command = - InstantCommand([&hasRun] { hasRun = true; }, {}).OnlyIf([&onlyIfBool] { - return onlyIfBool; - }); + auto command = InstantCommand([&hasRun] { hasRun = true; }, {}) + .OnlyIf([&onlyIfCondition] { return onlyIfCondition; }); scheduler.Schedule(command); scheduler.Run(); EXPECT_FALSE(hasRun); - onlyIfBool = true; + onlyIfCondition = true; scheduler.Schedule(command); scheduler.Run(); EXPECT_TRUE(hasRun);