From 184566c2d213fed703781f0b1dbaac716c7e981a Mon Sep 17 00:00:00 2001 From: MEFThunders7035 <95276965+kytpbs@users.noreply.github.com> Date: Tue, 7 May 2024 02:42:42 +0300 Subject: [PATCH 01/17] fix scheduledCommands.erase() being called in the same sets iterator --- .../main/native/cpp/frc2/command/CommandScheduler.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index aa57bf00d08..d17c9a64ba5 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -200,6 +200,7 @@ void CommandScheduler::Run() { m_impl->inRunLoop = true; bool isDisabled = frc::RobotState::IsDisabled(); // Run scheduled commands, remove finished commands. + auto toRemove = std::vector(m_impl->scheduledCommands.size()); for (Command* command : m_impl->scheduledCommands) { if (isDisabled && !command->RunsWhenDisabled()) { Cancel(command, std::nullopt); @@ -220,7 +221,7 @@ void CommandScheduler::Run() { } m_impl->endingCommands.erase(command); - m_impl->scheduledCommands.erase(command); + toRemove.push_back(command); for (auto&& requirement : command->GetRequirements()) { m_impl->requirements.erase(requirement); } @@ -228,6 +229,12 @@ void CommandScheduler::Run() { m_watchdog.AddEpoch(command->GetName() + ".End(false)"); } } + + // remove the commands that should be removed + for (auto&& command : toRemove) { + m_impl->scheduledCommands.erase(command); + } + m_impl->inRunLoop = false; for (Command* command : m_impl->toSchedule) { From 9c943229e45955ed1e2880edb9ac3cad7aa127a9 Mon Sep 17 00:00:00 2001 From: MEFThunders7035 <95276965+kytpbs@users.noreply.github.com> Date: Tue, 7 May 2024 13:48:15 +0300 Subject: [PATCH 02/17] added comment explaning why we defer from java this is also to retrigger workflows --- .../src/main/native/cpp/frc2/command/CommandScheduler.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index d17c9a64ba5..19ca9b268d5 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -221,6 +221,8 @@ void CommandScheduler::Run() { } m_impl->endingCommands.erase(command); + // do not remove the command from the scheduled commands until the end + // because this causes undefined behavior in the next iteration of the loop toRemove.push_back(command); for (auto&& requirement : command->GetRequirements()) { m_impl->requirements.erase(requirement); From cd753b1978a77b4936bc958ac98224070cbed3c1 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 7 May 2024 12:18:55 +0000 Subject: [PATCH 03/17] Formatting fixes --- .../src/main/native/cpp/frc2/command/CommandScheduler.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 19ca9b268d5..2e8f3d402a7 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -222,7 +222,8 @@ void CommandScheduler::Run() { m_impl->endingCommands.erase(command); // do not remove the command from the scheduled commands until the end - // because this causes undefined behavior in the next iteration of the loop + // because this causes undefined behavior in the next iteration of the + // loop toRemove.push_back(command); for (auto&& requirement : command->GetRequirements()) { m_impl->requirements.erase(requirement); From 23547a2fe5c74e95eaa9072f731b2dfbc90b0c44 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Thu, 16 May 2024 05:03:46 +0300 Subject: [PATCH 04/17] use small Vector instead size is set to 10, as I don't think any normal team is going to end 20 commands at the same time... --- .../src/main/native/cpp/frc2/command/CommandScheduler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 2e8f3d402a7..9a5ade1d09f 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -200,7 +200,7 @@ void CommandScheduler::Run() { m_impl->inRunLoop = true; bool isDisabled = frc::RobotState::IsDisabled(); // Run scheduled commands, remove finished commands. - auto toRemove = std::vector(m_impl->scheduledCommands.size()); + wpi::SmallVector toRemove; for (Command* command : m_impl->scheduledCommands) { if (isDisabled && !command->RunsWhenDisabled()) { Cancel(command, std::nullopt); From ec62f62361263ade30182eb75326937b8ec780f9 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Wed, 22 May 2024 00:49:25 +0300 Subject: [PATCH 05/17] create a new SmallSet instead this is similar to the way robot.py has done it, and because we are only copying a pointer array, it shouldn't have a big impact on performance. --- .../native/cpp/frc2/command/CommandScheduler.cpp | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 9a5ade1d09f..0292c047aaa 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -199,9 +199,8 @@ void CommandScheduler::Run() { m_impl->inRunLoop = true; bool isDisabled = frc::RobotState::IsDisabled(); - // Run scheduled commands, remove finished commands. - wpi::SmallVector toRemove; - for (Command* command : m_impl->scheduledCommands) { + // create a new set to avoid iterator invalidation. + for (Command* command : wpi::SmallSet(m_impl->scheduledCommands)) { if (isDisabled && !command->RunsWhenDisabled()) { Cancel(command, std::nullopt); continue; @@ -221,10 +220,6 @@ void CommandScheduler::Run() { } m_impl->endingCommands.erase(command); - // do not remove the command from the scheduled commands until the end - // because this causes undefined behavior in the next iteration of the - // loop - toRemove.push_back(command); for (auto&& requirement : command->GetRequirements()) { m_impl->requirements.erase(requirement); } @@ -233,11 +228,6 @@ void CommandScheduler::Run() { } } - // remove the commands that should be removed - for (auto&& command : toRemove) { - m_impl->scheduledCommands.erase(command); - } - m_impl->inRunLoop = false; for (Command* command : m_impl->toSchedule) { From dd60ba064318340f34819c3ac9df029b22eae2dd Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Wed, 22 May 2024 01:19:25 +0300 Subject: [PATCH 06/17] create a copy of the `m_scheduledCommands` this is made to be consistent with C++ due to it not having iterator.Remove() or similar --- .../java/edu/wpi/first/wpilibj2/command/CommandScheduler.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 0e80a74c613..35a2a649cc3 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -277,7 +277,7 @@ public void run() { m_inRunLoop = true; boolean isDisabled = RobotState.isDisabled(); // Run scheduled commands, remove finished commands. - for (Iterator iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) { + for (Iterator iterator = Set.copyOf(m_scheduledCommands).iterator(); iterator.hasNext(); ) { Command command = iterator.next(); if (isDisabled && !command.runsWhenDisabled()) { From 35786d8f7c8e65da19d292ae66ec5a85b1f0ead0 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Wed, 22 May 2024 01:27:02 +0300 Subject: [PATCH 07/17] remove concurrent avoiding modification member variables (C++) --- .../cpp/frc2/command/CommandScheduler.cpp | 47 +++---------------- 1 file changed, 6 insertions(+), 41 deletions(-) diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 0292c047aaa..c2223035b61 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -54,14 +54,6 @@ class CommandScheduler::Impl { wpi::SmallVector executeActions; wpi::SmallVector interruptActions; wpi::SmallVector finishActions; - - // Flag and queues for avoiding concurrent modification if commands are - // scheduled/canceled during run - bool inRunLoop = false; - wpi::SmallVector toSchedule; - wpi::SmallVector toCancelCommands; - wpi::SmallVector, 4> toCancelInterruptors; - wpi::SmallSet endingCommands; }; template @@ -113,11 +105,6 @@ frc::EventLoop* CommandScheduler::GetDefaultButtonLoop() const { } void CommandScheduler::Schedule(Command* command) { - if (m_impl->inRunLoop) { - m_impl->toSchedule.emplace_back(command); - return; - } - RequireUngrouped(command); if (m_impl->disabled || m_impl->scheduledCommands.contains(command) || @@ -197,10 +184,13 @@ void CommandScheduler::Run() { loopCache->Poll(); m_watchdog.AddEpoch("buttons.Run()"); - m_impl->inRunLoop = true; bool isDisabled = frc::RobotState::IsDisabled(); // create a new set to avoid iterator invalidation. for (Command* command : wpi::SmallSet(m_impl->scheduledCommands)) { + if (!IsScheduled(command)) { + continue; // skip as the normal scheduledCommands was modified + } + if (isDisabled && !command->RunsWhenDisabled()) { Cancel(command, std::nullopt); continue; @@ -213,12 +203,11 @@ void CommandScheduler::Run() { m_watchdog.AddEpoch(command->GetName() + ".Execute()"); if (command->IsFinished()) { - m_impl->endingCommands.insert(command); + m_impl->scheduledCommands.erase(command); command->End(false); for (auto&& action : m_impl->finishActions) { action(*command); } - m_impl->endingCommands.erase(command); for (auto&& requirement : command->GetRequirements()) { m_impl->requirements.erase(requirement); @@ -228,20 +217,6 @@ void CommandScheduler::Run() { } } - m_impl->inRunLoop = false; - - for (Command* command : m_impl->toSchedule) { - Schedule(command); - } - - for (size_t i = 0; i < m_impl->toCancelCommands.size(); i++) { - Cancel(m_impl->toCancelCommands[i], m_impl->toCancelInterruptors[i]); - } - - m_impl->toSchedule.clear(); - m_impl->toCancelCommands.clear(); - m_impl->toCancelInterruptors.clear(); - // Add default commands for un-required registered subsystems. for (auto&& subsystem : m_impl->subsystems) { auto s = m_impl->requirements.find(subsystem.getFirst()); @@ -333,24 +308,14 @@ void CommandScheduler::Cancel(Command* command, if (!m_impl) { return; } - if (m_impl->endingCommands.contains(command)) { - return; - } - if (m_impl->inRunLoop) { - m_impl->toCancelCommands.emplace_back(command); - m_impl->toCancelInterruptors.emplace_back(interruptor); - return; - } if (!IsScheduled(command)) { return; } - m_impl->endingCommands.insert(command); + m_impl->scheduledCommands.erase(command); command->End(true); for (auto&& action : m_impl->interruptActions) { action(*command, interruptor); } - m_impl->endingCommands.erase(command); - m_impl->scheduledCommands.erase(command); for (auto&& requirement : m_impl->requirements) { if (requirement.second == command) { m_impl->requirements.erase(requirement.first); From a905f7bd16159d82235716f6edddd19d0056f0a5 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Wed, 22 May 2024 01:28:21 +0300 Subject: [PATCH 08/17] remove concurrent avoiding modification member variables (Java) --- .../wpilibj2/command/CommandScheduler.java | 47 +++---------------- 1 file changed, 7 insertions(+), 40 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 35a2a649cc3..dbcdae25fda 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -89,13 +89,6 @@ public static synchronized CommandScheduler getInstance() { private final List>> m_interruptActions = new ArrayList<>(); private final List> m_finishActions = new ArrayList<>(); - // Flag and queues for avoiding ConcurrentModificationException if commands are - // scheduled/canceled during run - private boolean m_inRunLoop; - private final Set m_toSchedule = new LinkedHashSet<>(); - private final List m_toCancelCommands = new ArrayList<>(); - private final List> m_toCancelInterruptors = new ArrayList<>(); - private final Set m_endingCommands = new LinkedHashSet<>(); private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {}); @@ -187,10 +180,6 @@ private void schedule(Command command) { DriverStation.reportWarning("Tried to schedule a null command", true); return; } - if (m_inRunLoop) { - m_toSchedule.add(command); - return; - } requireNotComposed(command); @@ -274,11 +263,13 @@ public void run() { loopCache.poll(); m_watchdog.addEpoch("buttons.run()"); - m_inRunLoop = true; boolean isDisabled = RobotState.isDisabled(); // Run scheduled commands, remove finished commands. for (Iterator iterator = Set.copyOf(m_scheduledCommands).iterator(); iterator.hasNext(); ) { Command command = iterator.next(); + if (!isScheduled(command)) { + continue; // skip as the normal scheduledCommands was modified and that command was canceled + } if (isDisabled && !command.runsWhenDisabled()) { cancel(command, kNoInterruptor); @@ -291,32 +282,17 @@ public void run() { } m_watchdog.addEpoch(command.getName() + ".execute()"); if (command.isFinished()) { - m_endingCommands.add(command); + // remove command first so the command calling cancel() doesn't crash... + m_scheduledCommands.remove(command); command.end(false); for (Consumer action : m_finishActions) { action.accept(command); } - m_endingCommands.remove(command); - iterator.remove(); m_requirements.keySet().removeAll(command.getRequirements()); m_watchdog.addEpoch(command.getName() + ".end(false)"); } } - m_inRunLoop = false; - - // Schedule/cancel commands from queues populated during loop - for (Command command : m_toSchedule) { - schedule(command); - } - - for (int i = 0; i < m_toCancelCommands.size(); i++) { - cancel(m_toCancelCommands.get(i), m_toCancelInterruptors.get(i)); - } - - m_toSchedule.clear(); - m_toCancelCommands.clear(); - m_toCancelInterruptors.clear(); // Add default commands for un-required registered subsystems. for (Map.Entry subsystemCommand : m_subsystems.entrySet()) { @@ -467,25 +443,16 @@ private void cancel(Command command, Optional interruptor) { DriverStation.reportWarning("Tried to cancel a null command", true); return; } - if (m_endingCommands.contains(command)) { - return; - } - if (m_inRunLoop) { - m_toCancelCommands.add(command); - m_toCancelInterruptors.add(interruptor); - return; - } if (!isScheduled(command)) { return; } - m_endingCommands.add(command); + // remove command first so the command calling cancel() on itself doesn't crash... + m_scheduledCommands.remove(command); command.end(true); for (BiConsumer> action : m_interruptActions) { action.accept(command, interruptor); } - m_endingCommands.remove(command); - m_scheduledCommands.remove(command); m_requirements.keySet().removeAll(command.getRequirements()); m_watchdog.addEpoch(command.getName() + ".end(true)"); } From ede024edcb659858868bc20af56dde5b5c1bb4d0 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 21 May 2024 22:44:22 +0000 Subject: [PATCH 09/17] Formatting fixes --- .../java/edu/wpi/first/wpilibj2/command/CommandScheduler.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index dbcdae25fda..545ab97f7cd 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -89,7 +89,6 @@ public static synchronized CommandScheduler getInstance() { private final List>> m_interruptActions = new ArrayList<>(); private final List> m_finishActions = new ArrayList<>(); - private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {}); CommandScheduler() { @@ -265,7 +264,8 @@ public void run() { boolean isDisabled = RobotState.isDisabled(); // Run scheduled commands, remove finished commands. - for (Iterator iterator = Set.copyOf(m_scheduledCommands).iterator(); iterator.hasNext(); ) { + for (Iterator iterator = Set.copyOf(m_scheduledCommands).iterator(); + iterator.hasNext(); ) { Command command = iterator.next(); if (!isScheduled(command)) { continue; // skip as the normal scheduledCommands was modified and that command was canceled From a7702d06fceae7cb87cf1e54f4c02ac88784ebdb Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Wed, 22 May 2024 02:58:08 +0300 Subject: [PATCH 10/17] remove iterator as we don't need it Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../java/edu/wpi/first/wpilibj2/command/CommandScheduler.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 545ab97f7cd..c570a0d7551 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -264,9 +264,7 @@ public void run() { boolean isDisabled = RobotState.isDisabled(); // Run scheduled commands, remove finished commands. - for (Iterator iterator = Set.copyOf(m_scheduledCommands).iterator(); - iterator.hasNext(); ) { - Command command = iterator.next(); + for (Command command : Set.copyOf(m_scheduledCommands)) { if (!isScheduled(command)) { continue; // skip as the normal scheduledCommands was modified and that command was canceled } From 2f5c5c46dc88f7f9c1c7aca485536ce9cc52f5cc Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Wed, 22 May 2024 00:06:05 +0000 Subject: [PATCH 11/17] Formatting fixes --- .../java/edu/wpi/first/wpilibj2/command/CommandScheduler.java | 1 - 1 file changed, 1 deletion(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index c570a0d7551..88d22f8698d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -26,7 +26,6 @@ import java.util.Arrays; import java.util.Collection; import java.util.Collections; -import java.util.Iterator; import java.util.LinkedHashMap; import java.util.LinkedHashSet; import java.util.List; From 7149e433c15558d21118e4b18bd01dd7152ac0a2 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Fri, 11 Oct 2024 05:53:57 +0300 Subject: [PATCH 12/17] add a test to cancel the next command from the first Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../wpilibj2/command/CommandScheduleTest.java | 45 +++++++++++++++++++ .../cpp/frc2/command/CommandScheduleTest.cpp | 41 +++++++++++++++++ 2 files changed, 86 insertions(+) diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java index 793ad95f1e1..4f9022252d3 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java @@ -5,6 +5,7 @@ package edu.wpi.first.wpilibj2.command; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import static org.mockito.Mockito.never; @@ -13,6 +14,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.concurrent.atomic.AtomicInteger; import org.junit.jupiter.api.Test; class CommandScheduleTest extends CommandTestBase { @@ -109,6 +111,49 @@ void schedulerCancelTest() { } } + @Test + void cancelNextCommandTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + Command[] commands = new Command[2]; + var commandRunCounter = new AtomicInteger(0); + // due to the ordering of sets being non-deterministic, we cancel the other command in the + // first command and check that the second command is not run + Command command1 = + new RunCommand( + () -> { + scheduler.cancel(commands[1]); + commandRunCounter.incrementAndGet(); + }); + Command command2 = + new RunCommand( + () -> { + scheduler.cancel(commands[0]); + commandRunCounter.incrementAndGet(); + }); + + commands[0] = command1; + commands[1] = command2; + + scheduler.schedule(command1, command2); + scheduler.run(); + + assertEquals( + 1, commandRunCounter.get(), "Second command was run when it shouldn't have been"); + + // only one of the commands should be canceled. + assertFalse( + scheduler.isScheduled(command1) && scheduler.isScheduled(command2), + "None of the commands were canceled when one should have been"); + // one of the commands shouldn't be canceled because the other one is canceled first + assertTrue( + scheduler.isScheduled(command1) || scheduler.isScheduled(command2), + "Both commands were canceled when only one should have been"); + + scheduler.run(); + assertEquals(2, commandRunCounter.get()); + } + } + @Test void notScheduledCancelTest() { try (CommandScheduler scheduler = new CommandScheduler()) { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp index 91786770e09..3b5b0a18987 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -2,6 +2,10 @@ // 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 @@ -102,6 +106,43 @@ TEST_F(CommandScheduleTest, NotScheduledCancel) { EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command)); } +TEST_F(CommandScheduleTest, CancelNextCommand) { + CommandScheduler scheduler = GetScheduler(); + + frc2::RunCommand* command1PtrPtr = nullptr; + frc2::RunCommand* command2PtrPtr = nullptr; + auto counterPtr = std::make_shared(0); + + auto command1 = frc2::RunCommand([counterPtr, &command2PtrPtr, &scheduler] { + scheduler.Cancel(command2PtrPtr); + (*counterPtr)++; + }); + auto command2 = frc2::RunCommand([counterPtr, &command1PtrPtr, &scheduler] { + scheduler.Cancel(command1PtrPtr); + (*counterPtr)++; + }); + + command1PtrPtr = &command1; + command2PtrPtr = &command2; + + scheduler.Schedule(&command1); + scheduler.Schedule(&command2); + scheduler.Run(); + + EXPECT_EQ(*counterPtr, 1); + + // only one of the commands should be canceled. + EXPECT_FALSE(scheduler.IsScheduled(&command1) && + scheduler.IsScheduled(&command2)); + // one of the commands shouldn't be canceled because the other one is canceled + // first + EXPECT_TRUE(scheduler.IsScheduled(&command1) || + scheduler.IsScheduled(&command2)); + + scheduler.Run(); + EXPECT_EQ(*counterPtr, 2); +} + TEST_F(CommandScheduleTest, SmartDashboardCancel) { CommandScheduler scheduler = GetScheduler(); frc::SmartDashboard::PutData("Scheduler", &scheduler); From 7c7ca2efd3e1896211ac11e150c67aa49556d5cf Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Fri, 11 Oct 2024 17:33:40 +0300 Subject: [PATCH 13/17] add better error messages to C++ tests --- .../test/native/cpp/frc2/command/CommandScheduleTest.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp index 3b5b0a18987..08dfd64c420 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -133,11 +133,13 @@ TEST_F(CommandScheduleTest, CancelNextCommand) { // only one of the commands should be canceled. EXPECT_FALSE(scheduler.IsScheduled(&command1) && - scheduler.IsScheduled(&command2)); + scheduler.IsScheduled(&command2)) + << "Both commands are running when only one should be"; // one of the commands shouldn't be canceled because the other one is canceled // first EXPECT_TRUE(scheduler.IsScheduled(&command1) || - scheduler.IsScheduled(&command2)); + scheduler.IsScheduled(&command2)) + << "Both commands are canceled when only one should be"; scheduler.Run(); EXPECT_EQ(*counterPtr, 2); From 9154940f952c2775207747f6e28ddde641753709 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Fri, 11 Oct 2024 17:34:15 +0300 Subject: [PATCH 14/17] Optimize copying `m_scheduledCommands` --- .../first/wpilibj2/command/CommandScheduler.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 88d22f8698d..98d11be9214 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -67,6 +67,9 @@ public static synchronized CommandScheduler getInstance() { // A set of the currently-running commands. private final Set m_scheduledCommands = new LinkedHashSet<>(); + // A copy of the currently-running commands, used for iteration stored on class for caching + // purposes. + private Command[] m_scheduledCommandsCopy = new Command[12]; // 12 is arbitrary, it auto-resizes // A map from required subsystems to their requiring commands. Also used as a set of the // currently-required subsystems. @@ -263,9 +266,16 @@ public void run() { boolean isDisabled = RobotState.isDisabled(); // Run scheduled commands, remove finished commands. - for (Command command : Set.copyOf(m_scheduledCommands)) { + m_scheduledCommandsCopy = m_scheduledCommands.toArray(m_scheduledCommandsCopy); + for (Command command : m_scheduledCommandsCopy) { + if (command == null) { + // at least 1 Command was removed before `run` was called (diff between copy and original). + // No more elements to iterate over (see toArray documentation) + break; + } + if (!isScheduled(command)) { - continue; // skip as the normal scheduledCommands was modified and that command was canceled + continue; // Command was canceled in the previous iterations. } if (isDisabled && !command.runsWhenDisabled()) { From 35b02b9fe295029b59ff6f1e62bfccb671a6f3b4 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Sat, 12 Oct 2024 01:37:38 +0300 Subject: [PATCH 15/17] add a test checking if command is scheduled on end() call --- .../wpilibj2/command/CommandScheduleTest.java | 24 +++++++++++++++++++ .../cpp/frc2/command/CommandScheduleTest.cpp | 23 +++++++++++++++++- 2 files changed, 46 insertions(+), 1 deletion(-) diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java index 4f9022252d3..d638dcea3ae 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java @@ -154,6 +154,30 @@ void cancelNextCommandTest() { } } + @Test + void commandKnowsWhenEndedTest() { + try (CommandScheduler scheduler = new CommandScheduler()) { + Command[] commands = new Command[1]; + Command command = + new FunctionalCommand( + () -> {}, + () -> {}, + isForced -> { + assertFalse( + scheduler.isScheduled(commands[0]), + "Command shouldn't be scheduled when its end is called"); + }, + () -> true); + + commands[0] = command; + scheduler.schedule(command); + scheduler.run(); + assertFalse( + scheduler.isScheduled(command), + "Command should be removed from scheduler when its isFinished() returns true"); + } + } + @Test void notScheduledCancelTest() { try (CommandScheduler scheduler = new CommandScheduler()) { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp index 08dfd64c420..010b269f8b3 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -129,7 +129,8 @@ TEST_F(CommandScheduleTest, CancelNextCommand) { scheduler.Schedule(&command2); scheduler.Run(); - EXPECT_EQ(*counterPtr, 1); + EXPECT_EQ(*counterPtr, 1) + << "Second command was run when it shouldn't have been"; // only one of the commands should be canceled. EXPECT_FALSE(scheduler.IsScheduled(&command1) && @@ -145,6 +146,26 @@ TEST_F(CommandScheduleTest, CancelNextCommand) { EXPECT_EQ(*counterPtr, 2); } +TEST_F(CommandScheduleTest, CommandKnowsWhenItEnded) { + CommandScheduler scheduler = GetScheduler(); + + frc2::FunctionalCommand* commandPtr = nullptr; + auto command = frc2::FunctionalCommand( + [] {}, [] {}, + [&](auto isForced) { + EXPECT_FALSE(scheduler.IsScheduled(commandPtr)) + << "Command shouldn't be scheduled when its end is called"; + }, + [] { return true; }); + commandPtr = &command; + + scheduler.Schedule(commandPtr); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(commandPtr)) + << "Command should be removed from scheduler when its isFinished() " + "returns true"; +} + TEST_F(CommandScheduleTest, SmartDashboardCancel) { CommandScheduler scheduler = GetScheduler(); frc::SmartDashboard::PutData("Scheduler", &scheduler); From 0f5c8e691d8b18e632151c7ca81907279ad0057a Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Sat, 12 Oct 2024 04:12:36 +0300 Subject: [PATCH 16/17] Add test for scheduling a command within a command --- .../wpilibj2/command/CommandScheduleTest.java | 29 +++++++++++++ .../cpp/frc2/command/CommandScheduleTest.cpp | 42 +++++++++++++++---- 2 files changed, 64 insertions(+), 7 deletions(-) diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java index d638dcea3ae..70f0be7906b 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java @@ -178,6 +178,35 @@ void commandKnowsWhenEndedTest() { } } + @Test + void scheduleCommandInCommand() { + try (CommandScheduler scheduler = new CommandScheduler()) { + AtomicInteger counter = new AtomicInteger(0); + Command[] commands = new Command[1]; + Command command1 = + new RunCommand( + () -> { + scheduler.schedule(commands[0]); + assertEquals( + 1, + counter.get(), + "Command 2's init was not run immediately after getting scheduled"); + }); + Command command2 = new InstantCommand(counter::incrementAndGet); + + commands[0] = command2; + + scheduler.schedule(command1); + scheduler.run(); + assertEquals(1, counter.get(), "Command 2 was not run when it should have been"); + assertTrue(scheduler.isScheduled(command2)); + + scheduler.run(); + assertEquals(1, counter.get(), "Command 2 was run when it shouldn't have been"); + assertFalse(scheduler.isScheduled(command2), "Command 2 did not end when it should have"); + } + } + @Test void notScheduledCancelTest() { try (CommandScheduler scheduler = new CommandScheduler()) { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp index 010b269f8b3..e046737b53d 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -2,6 +2,8 @@ // 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 @@ -99,13 +101,6 @@ TEST_F(CommandScheduleTest, SchedulerCancel) { EXPECT_FALSE(scheduler.IsScheduled(&command)); } -TEST_F(CommandScheduleTest, NotScheduledCancel) { - CommandScheduler scheduler = GetScheduler(); - MockCommand command; - - EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command)); -} - TEST_F(CommandScheduleTest, CancelNextCommand) { CommandScheduler scheduler = GetScheduler(); @@ -166,6 +161,39 @@ TEST_F(CommandScheduleTest, CommandKnowsWhenItEnded) { "returns true"; } +TEST_F(CommandScheduleTest, ScheduleCommandInCommand) { + CommandScheduler scheduler = GetScheduler(); + int counter = 0; + frc2::InstantCommand* commandPtr = nullptr; + + auto command = frc2::RunCommand([&] { + scheduler.Schedule(commandPtr); + EXPECT_EQ(counter, 1) + << "Command 2's init was not run immediately after getting scheduled"; + }); + auto commandToGetScheduled = frc2::InstantCommand([&counter] { counter++; }); + + commandPtr = &commandToGetScheduled; + + scheduler.Schedule(&command); + scheduler.Run(); + EXPECT_EQ(counter, 1) << "Command 2 was not run when it should have been"; + EXPECT_TRUE(scheduler.IsScheduled(commandPtr)) + << "Command 2 was not added to scheduler"; + + scheduler.Run(); + EXPECT_EQ(counter, 1) << "Command 2 was run when it shouldn't have been"; + EXPECT_FALSE(scheduler.IsScheduled(commandPtr)) + << "Command 2 did not end when it should have"; +} + +TEST_F(CommandScheduleTest, NotScheduledCancel) { + CommandScheduler scheduler = GetScheduler(); + MockCommand command; + + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command)); +} + TEST_F(CommandScheduleTest, SmartDashboardCancel) { CommandScheduler scheduler = GetScheduler(); frc::SmartDashboard::PutData("Scheduler", &scheduler); From 93ad7b8c772dbe6bb82f6fa88e6d4b5bdf48be43 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Sat, 12 Oct 2024 23:46:44 +0300 Subject: [PATCH 17/17] Refactor tests according to reviews Co-Authored-By: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../wpilibj2/command/CommandScheduleTest.java | 22 ++++----- .../cpp/frc2/command/CommandScheduleTest.cpp | 48 +++++++++---------- 2 files changed, 34 insertions(+), 36 deletions(-) diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java index 70f0be7906b..117d694e62b 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java @@ -116,7 +116,8 @@ void cancelNextCommandTest() { try (CommandScheduler scheduler = new CommandScheduler()) { Command[] commands = new Command[2]; var commandRunCounter = new AtomicInteger(0); - // due to the ordering of sets being non-deterministic, we cancel the other command in the + // for parity with C++ where the ordering of sets is non-deterministic, we cancel the other + // command in the // first command and check that the second command is not run Command command1 = new RunCommand( @@ -182,28 +183,27 @@ void commandKnowsWhenEndedTest() { void scheduleCommandInCommand() { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicInteger counter = new AtomicInteger(0); - Command[] commands = new Command[1]; - Command command1 = + Command commandToGetScheduled = new InstantCommand(counter::incrementAndGet); + Command command = new RunCommand( () -> { - scheduler.schedule(commands[0]); + scheduler.schedule(commandToGetScheduled); assertEquals( 1, counter.get(), - "Command 2's init was not run immediately after getting scheduled"); + "Scheduled command's init was not run immediately after getting scheduled"); }); - Command command2 = new InstantCommand(counter::incrementAndGet); - commands[0] = command2; - - scheduler.schedule(command1); + scheduler.schedule(command); scheduler.run(); assertEquals(1, counter.get(), "Command 2 was not run when it should have been"); - assertTrue(scheduler.isScheduled(command2)); + assertTrue(scheduler.isScheduled(commandToGetScheduled)); scheduler.run(); assertEquals(1, counter.get(), "Command 2 was run when it shouldn't have been"); - assertFalse(scheduler.isScheduled(command2), "Command 2 did not end when it should have"); + assertFalse( + scheduler.isScheduled(commandToGetScheduled), + "Command 2 did not end when it should have"); } } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp index e046737b53d..b7d46869530 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -104,28 +104,27 @@ TEST_F(CommandScheduleTest, SchedulerCancel) { TEST_F(CommandScheduleTest, CancelNextCommand) { CommandScheduler scheduler = GetScheduler(); - frc2::RunCommand* command1PtrPtr = nullptr; - frc2::RunCommand* command2PtrPtr = nullptr; - auto counterPtr = std::make_shared(0); + frc2::RunCommand* command1Ptr = nullptr; + frc2::RunCommand* command2Ptr = nullptr; + int counter = 0; - auto command1 = frc2::RunCommand([counterPtr, &command2PtrPtr, &scheduler] { - scheduler.Cancel(command2PtrPtr); - (*counterPtr)++; + auto command1 = frc2::RunCommand([&counter, &command2Ptr, &scheduler] { + scheduler.Cancel(command2Ptr); + counter++; }); - auto command2 = frc2::RunCommand([counterPtr, &command1PtrPtr, &scheduler] { - scheduler.Cancel(command1PtrPtr); - (*counterPtr)++; + auto command2 = frc2::RunCommand([&counter, &command1Ptr, &scheduler] { + scheduler.Cancel(command1Ptr); + counter++; }); - command1PtrPtr = &command1; - command2PtrPtr = &command2; + command1Ptr = &command1; + command2Ptr = &command2; scheduler.Schedule(&command1); scheduler.Schedule(&command2); scheduler.Run(); - EXPECT_EQ(*counterPtr, 1) - << "Second command was run when it shouldn't have been"; + EXPECT_EQ(counter, 1) << "Second command was run when it shouldn't have been"; // only one of the commands should be canceled. EXPECT_FALSE(scheduler.IsScheduled(&command1) && @@ -138,7 +137,7 @@ TEST_F(CommandScheduleTest, CancelNextCommand) { << "Both commands are canceled when only one should be"; scheduler.Run(); - EXPECT_EQ(*counterPtr, 2); + EXPECT_EQ(counter, 2); } TEST_F(CommandScheduleTest, CommandKnowsWhenItEnded) { @@ -164,26 +163,25 @@ TEST_F(CommandScheduleTest, CommandKnowsWhenItEnded) { TEST_F(CommandScheduleTest, ScheduleCommandInCommand) { CommandScheduler scheduler = GetScheduler(); int counter = 0; - frc2::InstantCommand* commandPtr = nullptr; - - auto command = frc2::RunCommand([&] { - scheduler.Schedule(commandPtr); - EXPECT_EQ(counter, 1) - << "Command 2's init was not run immediately after getting scheduled"; - }); - auto commandToGetScheduled = frc2::InstantCommand([&counter] { counter++; }); + frc2::InstantCommand commandToGetScheduled{[&counter] { counter++; }}; - commandPtr = &commandToGetScheduled; + auto command = + frc2::RunCommand([&counter, &scheduler, &commandToGetScheduled] { + scheduler.Schedule(&commandToGetScheduled); + EXPECT_EQ(counter, 1) + << "Scheduled cmmand's init was not run immediately " + "after getting scheduled"; + }); scheduler.Schedule(&command); scheduler.Run(); EXPECT_EQ(counter, 1) << "Command 2 was not run when it should have been"; - EXPECT_TRUE(scheduler.IsScheduled(commandPtr)) + EXPECT_TRUE(scheduler.IsScheduled(&commandToGetScheduled)) << "Command 2 was not added to scheduler"; scheduler.Run(); EXPECT_EQ(counter, 1) << "Command 2 was run when it shouldn't have been"; - EXPECT_FALSE(scheduler.IsScheduled(commandPtr)) + EXPECT_FALSE(scheduler.IsScheduled(&commandToGetScheduled)) << "Command 2 did not end when it should have"; }