From 1828fdaaa43c11892d3078f951b7fb9625b8fa38 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Sat, 1 Jun 2024 12:01:15 -0700 Subject: [PATCH] [commands] Define order of parallel groups (#6602) --- .../command/ParallelCommandGroup.java | 6 +- .../command/ParallelDeadlineGroup.java | 8 +- .../wpilibj2/command/ParallelRaceGroup.java | 5 +- .../native/cpp/frc2/command/CommandPtr.cpp | 4 +- .../command/CommandDecoratorTest.java | 153 ++++++++++++++++++ .../cpp/frc2/command/CommandDecoratorTest.cpp | 129 +++++++++++++++ 6 files changed, 296 insertions(+), 9 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java index 8604189415..6ea58698c6 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java @@ -5,7 +5,7 @@ package edu.wpi.first.wpilibj2.command; import java.util.Collections; -import java.util.HashMap; +import java.util.LinkedHashMap; import java.util.Map; /** @@ -19,7 +19,9 @@ import java.util.Map; */ public class ParallelCommandGroup extends Command { // maps commands in this composition to whether they are still running - private final Map m_commands = new HashMap<>(); + // LinkedHashMap guarantees we iterate over commands in the order they were added (Note that + // changing the value associated with a command does NOT change the order) + private final Map m_commands = new LinkedHashMap<>(); private boolean m_runWhenDisabled = true; private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming; diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java index 8acfea8a55..eb576d7587 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java @@ -6,7 +6,7 @@ package edu.wpi.first.wpilibj2.command; import edu.wpi.first.util.sendable.SendableBuilder; import java.util.Collections; -import java.util.HashMap; +import java.util.LinkedHashMap; import java.util.Map; /** @@ -22,7 +22,9 @@ import java.util.Map; */ public class ParallelDeadlineGroup extends Command { // maps commands in this composition to whether they are still running - private final Map m_commands = new HashMap<>(); + // LinkedHashMap guarantees we iterate over commands in the order they were added (Note that + // changing the value associated with a command does NOT change the order) + private final Map m_commands = new LinkedHashMap<>(); private boolean m_runWhenDisabled = true; private boolean m_finished = true; private Command m_deadline; @@ -39,8 +41,8 @@ public class ParallelDeadlineGroup extends Command { * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument */ public ParallelDeadlineGroup(Command deadline, Command... otherCommands) { - addCommands(otherCommands); setDeadline(deadline); + addCommands(otherCommands); } /** diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java index 7195cd70bf..f2ff29920d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java @@ -5,7 +5,7 @@ package edu.wpi.first.wpilibj2.command; import java.util.Collections; -import java.util.HashSet; +import java.util.LinkedHashSet; import java.util.Set; /** @@ -19,7 +19,8 @@ import java.util.Set; *

This class is provided by the NewCommands VendorDep */ public class ParallelRaceGroup extends Command { - private final Set m_commands = new HashSet<>(); + // LinkedHashSet guarantees we iterate over commands in the order they were added + private final Set m_commands = new LinkedHashSet<>(); private boolean m_runWhenDisabled = true; private boolean m_finished = true; private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp index 4cd0cbee9d..ee7182a784 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp @@ -132,8 +132,8 @@ CommandPtr CommandPtr::BeforeStarting(CommandPtr&& before) && { CommandPtr CommandPtr::WithTimeout(units::second_t duration) && { AssertValid(); std::vector> temp; - temp.emplace_back(std::make_unique(duration)); temp.emplace_back(std::move(m_ptr)); + temp.emplace_back(std::make_unique(duration)); m_ptr = std::make_unique(std::move(temp)); return std::move(*this); } @@ -141,8 +141,8 @@ CommandPtr CommandPtr::WithTimeout(units::second_t duration) && { CommandPtr CommandPtr::Until(std::function condition) && { AssertValid(); std::vector> temp; - temp.emplace_back(std::make_unique(std::move(condition))); temp.emplace_back(std::move(m_ptr)); + temp.emplace_back(std::make_unique(std::move(condition))); m_ptr = std::make_unique(std::move(temp)); return std::move(*this); } 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 61cbffd61b..1f9605d010 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 @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj2.command; +import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -57,6 +58,36 @@ class CommandDecoratorTest extends CommandTestBase { } } + @Test + void untilOrderTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean firstHasRun = new AtomicBoolean(false); + AtomicBoolean firstWasPolled = new AtomicBoolean(false); + + Command first = + new FunctionalCommand( + () -> {}, + () -> firstHasRun.set(true), + interrupted -> {}, + () -> { + firstWasPolled.set(true); + return true; + }); + Command command = + first.until( + () -> { + assertAll( + () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get())); + return true; + }); + + scheduler.schedule(command); + scheduler.run(); + + assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get())); + } + } + @Test void onlyWhileTest() { try (CommandScheduler scheduler = new CommandScheduler()) { @@ -76,6 +107,36 @@ class CommandDecoratorTest extends CommandTestBase { } } + @Test + void onlyWhileOrderTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean firstHasRun = new AtomicBoolean(false); + AtomicBoolean firstWasPolled = new AtomicBoolean(false); + + Command first = + new FunctionalCommand( + () -> {}, + () -> firstHasRun.set(true), + interrupted -> {}, + () -> { + firstWasPolled.set(true); + return true; + }); + Command command = + first.onlyWhile( + () -> { + assertAll( + () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get())); + return false; + }); + + scheduler.schedule(command); + scheduler.run(); + + assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get())); + } + } + @Test void ignoringDisableTest() { try (CommandScheduler scheduler = new CommandScheduler()) { @@ -179,6 +240,37 @@ class CommandDecoratorTest extends CommandTestBase { } } + @Test + void deadlineForOrderTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean dictatorHasRun = new AtomicBoolean(false); + AtomicBoolean dictatorWasPolled = new AtomicBoolean(false); + + Command dictator = + new FunctionalCommand( + () -> {}, + () -> dictatorHasRun.set(true), + interrupted -> {}, + () -> { + dictatorWasPolled.set(true); + return true; + }); + Command other = + new RunCommand( + () -> + assertAll( + () -> assertTrue(dictatorHasRun.get()), + () -> assertTrue(dictatorWasPolled.get()))); + + Command group = dictator.deadlineFor(other); + + scheduler.schedule(group); + scheduler.run(); + + assertAll(() -> assertTrue(dictatorHasRun.get()), () -> assertTrue(dictatorWasPolled.get())); + } + } + @Test void alongWithTest() { try (CommandScheduler scheduler = new CommandScheduler()) { @@ -201,6 +293,36 @@ class CommandDecoratorTest extends CommandTestBase { } } + @Test + void alongWithOrderTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean firstHasRun = new AtomicBoolean(false); + AtomicBoolean firstWasPolled = new AtomicBoolean(false); + + Command command1 = + new FunctionalCommand( + () -> {}, + () -> firstHasRun.set(true), + interrupted -> {}, + () -> { + firstWasPolled.set(true); + return true; + }); + Command command2 = + new RunCommand( + () -> + assertAll( + () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()))); + + Command group = command1.alongWith(command2); + + scheduler.schedule(group); + scheduler.run(); + + assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get())); + } + } + @Test void raceWithTest() { try (CommandScheduler scheduler = new CommandScheduler()) { @@ -216,6 +338,37 @@ class CommandDecoratorTest extends CommandTestBase { } } + @Test + void raceWithOrderTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicBoolean firstHasRun = new AtomicBoolean(false); + AtomicBoolean firstWasPolled = new AtomicBoolean(false); + + Command command1 = + new FunctionalCommand( + () -> {}, + () -> firstHasRun.set(true), + interrupted -> {}, + () -> { + firstWasPolled.set(true); + return true; + }); + Command command2 = + new RunCommand( + () -> { + assertTrue(firstHasRun.get()); + assertTrue(firstWasPolled.get()); + }); + + Command group = command1.raceWith(command2); + + scheduler.schedule(group); + scheduler.run(); + + assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get())); + } + } + @Test void unlessTest() { try (CommandScheduler scheduler = new CommandScheduler()) { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp index 2cd54ba161..a5059b16e1 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp @@ -52,6 +52,31 @@ TEST_F(CommandDecoratorTest, Until) { EXPECT_FALSE(scheduler.IsScheduled(command)); } +TEST_F(CommandDecoratorTest, UntilOrder) { + CommandScheduler scheduler = GetScheduler(); + + bool firstHasRun = false; + bool firstWasPolled = false; + + auto first = FunctionalCommand([] {}, [&firstHasRun] { firstHasRun = true; }, + [](bool interrupted) {}, + [&firstWasPolled] { + firstWasPolled = true; + return true; + }); + auto command = std::move(first).Until([&firstHasRun, &firstWasPolled] { + EXPECT_TRUE(firstHasRun); + EXPECT_TRUE(firstWasPolled); + return true; + }); + + scheduler.Schedule(command); + scheduler.Run(); + + EXPECT_TRUE(firstHasRun); + EXPECT_TRUE(firstWasPolled); +} + TEST_F(CommandDecoratorTest, OnlyWhile) { CommandScheduler scheduler = GetScheduler(); @@ -70,6 +95,31 @@ TEST_F(CommandDecoratorTest, OnlyWhile) { EXPECT_FALSE(scheduler.IsScheduled(command)); } +TEST_F(CommandDecoratorTest, OnlyWhileOrder) { + CommandScheduler scheduler = GetScheduler(); + + bool firstHasRun = false; + bool firstWasPolled = false; + + auto first = FunctionalCommand([] {}, [&firstHasRun] { firstHasRun = true; }, + [](bool interrupted) {}, + [&firstWasPolled] { + firstWasPolled = true; + return true; + }); + auto command = std::move(first).Until([&firstHasRun, &firstWasPolled] { + EXPECT_TRUE(firstHasRun); + EXPECT_TRUE(firstWasPolled); + return false; + }); + + scheduler.Schedule(command); + scheduler.Run(); + + EXPECT_TRUE(firstHasRun); + EXPECT_TRUE(firstWasPolled); +} + TEST_F(CommandDecoratorTest, IgnoringDisable) { CommandScheduler scheduler = GetScheduler(); @@ -203,6 +253,85 @@ TEST_F(CommandDecoratorTest, RaceWith) { EXPECT_FALSE(scheduler.IsScheduled(group)); } +TEST_F(CommandDecoratorTest, DeadlineForOrder) { + CommandScheduler scheduler = GetScheduler(); + + bool dictatorHasRun = false; + bool dictatorWasPolled = false; + + auto dictator = + FunctionalCommand([] {}, [&dictatorHasRun] { dictatorHasRun = true; }, + [](bool interrupted) {}, + [&dictatorWasPolled] { + dictatorWasPolled = true; + return true; + }); + auto other = RunCommand([&dictatorHasRun, &dictatorWasPolled] { + EXPECT_TRUE(dictatorHasRun); + EXPECT_TRUE(dictatorWasPolled); + }); + + auto group = std::move(dictator).DeadlineFor(std::move(other).ToPtr()); + + scheduler.Schedule(group); + scheduler.Run(); + + EXPECT_TRUE(dictatorHasRun); + EXPECT_TRUE(dictatorWasPolled); +} + +TEST_F(CommandDecoratorTest, AlongWithOrder) { + CommandScheduler scheduler = GetScheduler(); + + bool firstHasRun = false; + bool firstWasPolled = false; + + auto command1 = FunctionalCommand( + [] {}, [&firstHasRun] { firstHasRun = true; }, [](bool interrupted) {}, + [&firstWasPolled] { + firstWasPolled = true; + return true; + }); + auto command2 = RunCommand([&firstHasRun, &firstWasPolled] { + EXPECT_TRUE(firstHasRun); + EXPECT_TRUE(firstWasPolled); + }); + + auto group = std::move(command1).AlongWith(std::move(command2).ToPtr()); + + scheduler.Schedule(group); + scheduler.Run(); + + EXPECT_TRUE(firstHasRun); + EXPECT_TRUE(firstWasPolled); +} + +TEST_F(CommandDecoratorTest, RaceWithOrder) { + CommandScheduler scheduler = GetScheduler(); + + bool firstHasRun = false; + bool firstWasPolled = false; + + auto command1 = FunctionalCommand( + [] {}, [&firstHasRun] { firstHasRun = true; }, [](bool interrupted) {}, + [&firstWasPolled] { + firstWasPolled = true; + return true; + }); + auto command2 = RunCommand([&firstHasRun, &firstWasPolled] { + EXPECT_TRUE(firstHasRun); + EXPECT_TRUE(firstWasPolled); + }); + + auto group = std::move(command1).RaceWith(std::move(command2).ToPtr()); + + scheduler.Schedule(group); + scheduler.Run(); + + EXPECT_TRUE(firstHasRun); + EXPECT_TRUE(firstWasPolled); +} + TEST_F(CommandDecoratorTest, Unless) { CommandScheduler scheduler = GetScheduler();