[sim] Change StepTiming to wait for notifiers (#2603)

Old behavior is available via StepTimingAsync.

This makes it significantly easier to use simulation timing with notifiers.

Also update tests to use simulation framework.  This also speeds up the
timing-dependent tests by using simulation timing.  ResourceLock is used
in the Java tests to prevent parallel execution.

While we're here, tweak HAL Notifier implementation:
- Use wait_for instead of wait_until in WaitForNotifierAlarm
- Check for triggerTime = UINT64_MAX in UpdateNotifierAlarm

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java

@@ -8,15 +8,20 @@
 package edu.wpi.first.wpilibj2.command;
 
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 class CommandDecoratorTest extends CommandTestBase {
   @Test
+  @ResourceLock("timing")
   void withTimeoutTest() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
     try (CommandScheduler scheduler = new CommandScheduler()) {
       Command command1 = new WaitCommand(10);
 
@@ -28,10 +33,12 @@ class CommandDecoratorTest extends CommandTestBase {
       assertFalse(scheduler.isScheduled(command1));
       assertTrue(scheduler.isScheduled(timeout));
 
-      Timer.delay(3);
+      SimHooks.stepTiming(3);
       scheduler.run();
 
       assertFalse(scheduler.isScheduled(timeout));
+    } finally {
+      SimHooks.resumeTiming();
     }
   }
 

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java

@@ -9,10 +9,12 @@ package edu.wpi.first.wpilibj2.command;
 
 import java.util.ArrayList;
 
-import org.junit.jupiter.api.AfterAll;
-import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
+import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
@@ -31,13 +33,14 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 class MecanumControllerCommandTest {
-  @BeforeAll
-  static void setupAll() {
+  @BeforeEach
+  void setupAll() {
+    HAL.initialize(500, 0);
     SimHooks.pauseTiming();
   }
 
-  @AfterAll
-  static void cleanupAll() {
+  @AfterEach
+  void cleanupAll() {
     SimHooks.resumeTiming();
   }
 
@@ -86,6 +89,7 @@ class MecanumControllerCommandTest {
   }
 
   @Test
+  @ResourceLock("timing")
   @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
     final var subsystem = new Subsystem() {};

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java

@@ -7,17 +7,30 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.condition.DisabledOnOs;
-import org.junit.jupiter.api.condition.OS;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 
-import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 
-@DisabledOnOs(OS.MAC)
 class NotifierCommandTest extends CommandTestBase {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
   @Test
+  @ResourceLock("timing")
   void notifierCommandScheduleTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
       Counter counter = new Counter();
@@ -25,11 +38,12 @@ class NotifierCommandTest extends CommandTestBase {
       NotifierCommand command = new NotifierCommand(counter::increment, 0.01);
 
       scheduler.schedule(command);
-      Timer.delay(0.25);
+      for (int i = 0; i < 5; ++i) {
+        SimHooks.stepTiming(0.005);
+      }
       scheduler.cancel(command);
 
-      assertTrue(counter.m_counter > 10, "Should have hit at least 10 triggers");
-      assertTrue(counter.m_counter < 100, "Shouldn't hit more then 100 triggers");
+      assertEquals(2, counter.m_counter);
     }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java

@@ -9,10 +9,12 @@ package edu.wpi.first.wpilibj2.command;
 
 import java.util.ArrayList;
 
-import org.junit.jupiter.api.AfterAll;
-import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
+import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
@@ -31,13 +33,14 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 class SwerveControllerCommandTest {
-  @BeforeAll
-  static void setupAll() {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
     SimHooks.pauseTiming();
   }
 
-  @AfterAll
-  static void cleanupAll() {
+  @AfterEach
+  void cleanup() {
     SimHooks.resumeTiming();
   }
 
@@ -80,6 +83,7 @@ class SwerveControllerCommandTest {
   }
 
   @Test
+  @ResourceLock("timing")
   @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
     final var subsystem = new Subsystem() {};

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java

@@ -7,9 +7,13 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -19,19 +23,31 @@ import static org.mockito.Mockito.verify;
 import static org.mockito.Mockito.when;
 
 class WaitCommandTest extends CommandTestBase {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
   @Test
+  @ResourceLock("timing")
   void waitCommandTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
       WaitCommand waitCommand = new WaitCommand(2);
 
       scheduler.schedule(waitCommand);
       scheduler.run();
-      Timer.delay(1);
+      SimHooks.stepTiming(1);
       scheduler.run();
 
       assertTrue(scheduler.isScheduled(waitCommand));
 
-      Timer.delay(2);
+      SimHooks.stepTiming(2);
 
       scheduler.run();
 
@@ -40,6 +56,7 @@ class WaitCommandTest extends CommandTestBase {
   }
 
   @Test
+  @ResourceLock("timing")
   void withTimeoutTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
       MockCommandHolder command1Holder = new MockCommandHolder(true);
@@ -56,7 +73,7 @@ class WaitCommandTest extends CommandTestBase {
       assertFalse(scheduler.isScheduled(command1));
       assertTrue(scheduler.isScheduled(timeout));
 
-      Timer.delay(3);
+      SimHooks.stepTiming(3);
       scheduler.run();
 
       verify(command1).end(true);

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp

@@ -1,10 +1,12 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/simulation/SimHooks.h>
+
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/ParallelRaceGroup.h"
@@ -18,6 +20,8 @@ class CommandDecoratorTest : public CommandTestBase {};
 TEST_F(CommandDecoratorTest, WithTimeoutTest) {
   CommandScheduler scheduler = GetScheduler();
 
+  frc::sim::PauseTiming();
+
   auto command = RunCommand([] {}, {}).WithTimeout(100_ms);
 
   scheduler.Schedule(&command);
@@ -25,10 +29,12 @@ TEST_F(CommandDecoratorTest, WithTimeoutTest) {
   scheduler.Run();
   EXPECT_TRUE(scheduler.IsScheduled(&command));
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  frc::sim::StepTiming(150_ms);
 
   scheduler.Run();
   EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  frc::sim::ResumeTiming();
 }
 
 TEST_F(CommandDecoratorTest, WithInterruptTest) {

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp

@@ -18,12 +18,7 @@ CommandTestBase::CommandTestBase() {
 
 CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
 
-void CommandTestBase::SetUp() {
-  frc::sim::DriverStationSim::SetEnabled(true);
-  while (!frc::sim::DriverStationSim::GetEnabled()) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-  }
-}
+void CommandTestBase::SetUp() { frc::sim::DriverStationSim::SetEnabled(true); }
 
 void CommandTestBase::TearDown() {
   CommandScheduler::GetInstance().ClearButtons();
@@ -31,7 +26,4 @@ void CommandTestBase::TearDown() {
 
 void CommandTestBase::SetDSEnabled(bool enabled) {
   frc::sim::DriverStationSim::SetEnabled(enabled);
-  while (frc::sim::DriverStationSim::GetEnabled() != enabled) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-  }
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp

@@ -1,31 +1,35 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/simulation/SimHooks.h>
+
 #include "CommandTestBase.h"
 #include "frc2/command/NotifierCommand.h"
 
 using namespace frc2;
+using namespace std::chrono_literals;
+
 class NotifierCommandTest : public CommandTestBase {};
 
-#ifdef __APPLE__
-TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) {
-#else
 TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
-#endif
   CommandScheduler scheduler = GetScheduler();
 
-  int counter = 0;
+  frc::sim::PauseTiming();
 
-  NotifierCommand command([&counter] { counter++; }, 0.01_s, {});
+  int counter = 0;
+  NotifierCommand command([&] { counter++; }, 0.01_s, {});
 
   scheduler.Schedule(&command);
-  std::this_thread::sleep_for(std::chrono::milliseconds(250));
+  for (int i = 0; i < 5; ++i) {
+    frc::sim::StepTiming(0.005_s);
+  }
   scheduler.Cancel(&command);
 
-  EXPECT_GT(counter, 10);
-  EXPECT_LT(counter, 100);
+  frc::sim::ResumeTiming();
+
+  EXPECT_EQ(counter, 2);
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp

@@ -1,10 +1,12 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/simulation/SimHooks.h>
+
 #include "CommandTestBase.h"
 #include "frc2/command/WaitCommand.h"
 #include "frc2/command/WaitUntilCommand.h"
@@ -13,6 +15,8 @@ using namespace frc2;
 class WaitCommandTest : public CommandTestBase {};
 
 TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
+  frc::sim::PauseTiming();
+
   CommandScheduler scheduler = GetScheduler();
 
   WaitCommand command(100_ms);
@@ -20,7 +24,9 @@ TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
   scheduler.Schedule(&command);
   scheduler.Run();
   EXPECT_TRUE(scheduler.IsScheduled(&command));
-  std::this_thread::sleep_for(std::chrono::milliseconds(110));
+  frc::sim::StepTiming(110_ms);
   scheduler.Run();
   EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  frc::sim::ResumeTiming();
 }