[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

hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java

@@ -19,5 +19,6 @@ public class SimulatorJNI extends JNIWrapper {
   public static native void resumeTiming();
   public static native boolean isTimingPaused();
   public static native void stepTiming(long delta);
+  public static native void stepTimingAsync(long delta);
   public static native void resetHandles();
 }

hal/src/main/native/athena/mockdata/MockHooks.cpp

@@ -27,6 +27,8 @@ HAL_Bool HALSIM_IsTimingPaused(void) { return false; }
 
 void HALSIM_StepTiming(uint64_t delta) {}
 
+void HALSIM_StepTimingAsync(uint64_t delta) {}
+
 void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {}
 
 void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {}

hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp

@@ -218,6 +218,18 @@ Java_edu_wpi_first_hal_simulation_SimulatorJNI_stepTiming
   HALSIM_StepTiming(delta);
 }
 
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    stepTimingAsync
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_stepTimingAsync
+  (JNIEnv*, jclass, jlong delta)
+{
+  HALSIM_StepTimingAsync(delta);
+}
+
 /*
  * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
  * Method:    resetHandles

hal/src/main/native/include/hal/simulation/MockHooks.h

@@ -20,6 +20,7 @@ void HALSIM_PauseTiming(void);
 void HALSIM_ResumeTiming(void);
 HAL_Bool HALSIM_IsTimingPaused(void);
 void HALSIM_StepTiming(uint64_t delta);
+void HALSIM_StepTimingAsync(uint64_t delta);
 
 typedef int32_t (*HALSIM_SendErrorHandler)(
     HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, const char* details,

hal/src/main/native/sim/MockHooks.cpp

@@ -92,6 +92,11 @@ void HALSIM_ResumeTiming(void) {
 HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); }
 
 void HALSIM_StepTiming(uint64_t delta) {
+  StepTiming(delta);
+  WakeupWaitNotifiers();
+}
+
+void HALSIM_StepTimingAsync(uint64_t delta) {
   StepTiming(delta);
   WakeupNotifiers();
 }

hal/src/main/native/sim/Notifier.cpp

@@ -13,6 +13,7 @@
 #include <cstring>
 #include <string>
 
+#include <wpi/SmallVector.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 #include <wpi/timestamp.h>
@@ -30,6 +31,7 @@ struct Notifier {
   uint64_t waitTime;
   bool active = true;
   bool running = false;
+  uint64_t count = 0;
   wpi::mutex mutex;
   wpi::condition_variable cond;
 };
@@ -37,6 +39,9 @@ struct Notifier {
 
 using namespace hal;
 
+static wpi::mutex notifiersWaiterMutex;
+static wpi::condition_variable notifiersWaiterCond;
+
 class NotifierHandleContainer
     : public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
                                      HAL_HandleEnum::Notifier> {
@@ -50,6 +55,7 @@ class NotifierHandleContainer
       }
       notifier->cond.notify_all();  // wake up any waiting threads
     });
+    notifiersWaiterCond.notify_all();
   }
 };
 
@@ -76,6 +82,42 @@ void WakeupNotifiers() {
     notifier->cond.notify_all();
   });
 }
+
+void WakeupWaitNotifiers() {
+  std::unique_lock ulock(notifiersWaiterMutex);
+  int32_t status = 0;
+  uint64_t curTime = HAL_GetFPGATime(&status);
+  wpi::SmallVector<std::pair<HAL_NotifierHandle, uint64_t>, 8> waiters;
+  notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
+    std::scoped_lock lock(notifier->mutex);
+    // only wait for it if it's going to wake up (either because
+    // the timeout has expired or the alarm hasn't been waited on yet)
+    if (notifier->running &&
+        (notifier->count == 0 || curTime >= notifier->waitTime)) {
+      waiters.emplace_back(handle, notifier->count);
+      notifier->cond.notify_all();
+    }
+  });
+  for (;;) {
+    int count = 0;
+    int end = waiters.size();
+    while (count < end) {
+      auto& it = waiters[count];
+      if (auto notifier = notifierHandles->Get(it.first)) {
+        std::scoped_lock lock(notifier->mutex);
+        if (notifier->active && notifier->count == it.second) {
+          ++count;
+          continue;
+        }
+      }
+      // no longer need to wait for it, put at end so it can be erased
+      it.swap(waiters[--end]);
+    }
+    if (count == 0) break;
+    waiters.resize(count);
+    notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
+  }
+}
 }  // namespace hal
 
 extern "C" {
@@ -132,7 +174,7 @@ void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
   {
     std::scoped_lock lock(notifier->mutex);
     notifier->waitTime = triggerTime;
-    notifier->running = true;
+    notifier->running = (triggerTime != UINT64_MAX);
   }
 
   // We wake up any waiters to change how long they're sleeping for
@@ -155,7 +197,11 @@ uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
   auto notifier = notifierHandles->Get(notifierHandle);
   if (!notifier) return 0;
 
+  std::unique_lock ulock(notifiersWaiterMutex);
   std::unique_lock lock(notifier->mutex);
+  ++notifier->count;
+  ulock.unlock();
+  notifiersWaiterCond.notify_all();
   while (notifier->active) {
     uint64_t curTime = HAL_GetFPGATime(status);
     if (notifier->running && curTime >= notifier->waitTime) {
@@ -165,15 +211,13 @@ uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
 
     double waitTime;
     if (!notifier->running || notifiersPaused) {
-      waitTime = (curTime * 1e-6) + 1000.0;
       // If not running, wait 1000 seconds
+      waitTime = 1000.0;
     } else {
-      waitTime = notifier->waitTime * 1e-6;
+      waitTime = (notifier->waitTime - curTime) * 1e-6;
     }
 
-    auto timeoutTime =
-        hal::fpga_clock::epoch() + std::chrono::duration<double>(waitTime);
-    notifier->cond.wait_until(lock, timeoutTime);
+    notifier->cond.wait_for(lock, std::chrono::duration<double>(waitTime));
   }
   return 0;
 }

hal/src/main/native/sim/NotifierInternal.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* 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.                                                               */
@@ -11,4 +11,5 @@ namespace hal {
 void PauseNotifiers();
 void ResumeNotifiers();
 void WakeupNotifiers();
+void WakeupWaitNotifiers();
 }  // namespace hal

simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp

@@ -32,7 +32,8 @@ static void DisplayTiming() {
   if (ImGui::Button("Step")) {
     HALSIM_PauseTiming();
     uint64_t nextTimeout = HALSIM_GetNextNotifierTimeout();
-    if (nextTimeout != UINT64_MAX) HALSIM_StepTiming(nextTimeout - curTime);
+    if (nextTimeout != UINT64_MAX)
+      HALSIM_StepTimingAsync(nextTimeout - curTime);
   }
   ImGui::PopButtonRepeat();
   ImGui::PushItemWidth(ImGui::GetFontSize() * 4);

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();
 }

wpilibc/src/main/native/cpp/simulation/SimHooks.cpp

@@ -32,5 +32,9 @@ void StepTiming(units::second_t delta) {
   HALSIM_StepTiming(static_cast<uint64_t>(delta.to<double>() * 1e6));
 }
 
+void StepTimingAsync(units::second_t delta) {
+  HALSIM_StepTimingAsync(static_cast<uint64_t>(delta.to<double>() * 1e6));
+}
+
 }  // namespace sim
 }  // namespace frc

wpilibc/src/main/native/include/frc/simulation/SimHooks.h

@@ -33,5 +33,7 @@ bool IsTimingPaused();
 
 void StepTiming(units::second_t delta);
 
+void StepTimingAsync(units::second_t delta);
+
 }  // namespace sim
 }  // namespace frc

wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp

@@ -12,12 +12,13 @@
 #include <units/velocity.h>
 
 #include "frc/SlewRateLimiter.h"
+#include "frc/simulation/SimHooks.h"
 #include "gtest/gtest.h"
 
 TEST(SlewRateLimiterTest, SlewRateLimitTest) {
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1.0_s);
 
   EXPECT_TRUE(limiter.Calculate(2_m) < 2_m);
 }
@@ -25,7 +26,7 @@ TEST(SlewRateLimiterTest, SlewRateLimitTest) {
 TEST(SlewRateLimiterTest, SlewRateNoLimitTest) {
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1.0_s);
 
   EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
 }

wpilibc/src/test/native/cpp/WatchdogTest.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-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.                                                               */
@@ -9,26 +9,31 @@
 
 #include <stdint.h>
 
-#include <thread>
-
 #include <wpi/raw_ostream.h>
 
+#include "frc/simulation/SimHooks.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_EnableDisable) {
-#else
-TEST(WatchdogTest, EnableDisable) {
-#endif
+namespace {
+class WatchdogTest : public ::testing::Test {
+ protected:
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+}  // namespace
+
+TEST_F(WatchdogTest, EnableDisable) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
 
   wpi::outs() << "Run 1\n";
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
@@ -36,7 +41,7 @@ TEST(WatchdogTest, EnableDisable) {
   wpi::outs() << "Run 2\n";
   watchdogCounter = 0;
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(600));
+  frc::sim::StepTiming(0.6_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
@@ -45,65 +50,53 @@ TEST(WatchdogTest, EnableDisable) {
   wpi::outs() << "Run 3\n";
   watchdogCounter = 0;
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
       << "Watchdog either didn't trigger or triggered more than once";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_Reset) {
-#else
-TEST(WatchdogTest, Reset) {
-#endif
+TEST_F(WatchdogTest, Reset) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
 
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Reset();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_SetTimeout) {
-#else
-TEST(WatchdogTest, SetTimeout) {
-#endif
+TEST_F(WatchdogTest, SetTimeout) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(1.0_s, [&] { watchdogCounter++; });
 
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.SetTimeout(0.2_s);
 
   EXPECT_EQ(0.2, watchdog.GetTimeout());
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.3_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
       << "Watchdog either didn't trigger or triggered more than once";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_IsExpired) {
-#else
-TEST(WatchdogTest, IsExpired) {
-#endif
+TEST_F(WatchdogTest, IsExpired) {
   Watchdog watchdog(0.2_s, [] {});
   EXPECT_FALSE(watchdog.IsExpired());
   watchdog.Enable();
 
   EXPECT_FALSE(watchdog.IsExpired());
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.3_s);
   EXPECT_TRUE(watchdog.IsExpired());
 
   watchdog.Disable();
@@ -113,11 +106,7 @@ TEST(WatchdogTest, IsExpired) {
   EXPECT_FALSE(watchdog.IsExpired());
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_Epochs) {
-#else
-TEST(WatchdogTest, Epochs) {
-#endif
+TEST_F(WatchdogTest, Epochs) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
@@ -125,9 +114,9 @@ TEST(WatchdogTest, Epochs) {
   wpi::outs() << "Run 1\n";
   watchdog.Enable();
   watchdog.AddEpoch("Epoch 1");
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  frc::sim::StepTiming(0.1_s);
   watchdog.AddEpoch("Epoch 2");
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  frc::sim::StepTiming(0.1_s);
   watchdog.AddEpoch("Epoch 3");
   watchdog.Disable();
 
@@ -136,20 +125,16 @@ TEST(WatchdogTest, Epochs) {
   wpi::outs() << "Run 2\n";
   watchdog.Enable();
   watchdog.AddEpoch("Epoch 1");
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Reset();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.AddEpoch("Epoch 2");
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_MultiWatchdog) {
-#else
-TEST(WatchdogTest, MultiWatchdog) {
-#endif
+TEST_F(WatchdogTest, MultiWatchdog) {
   uint32_t watchdogCounter1 = 0;
   uint32_t watchdogCounter2 = 0;
 
@@ -157,13 +142,13 @@ TEST(WatchdogTest, MultiWatchdog) {
   Watchdog watchdog2(0.6_s, [&] { watchdogCounter2++; });
 
   watchdog2.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.25_s);
   EXPECT_EQ(0u, watchdogCounter1) << "Watchdog triggered early";
   EXPECT_EQ(0u, watchdogCounter2) << "Watchdog triggered early";
 
   // Sleep enough such that only the watchdog enabled later times out first
   watchdog1.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.25_s);
   watchdog1.Disable();
   watchdog2.Disable();
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java

@@ -48,4 +48,8 @@ public final class SimHooks {
   public static void stepTiming(double deltaSeconds) {
     SimulatorJNI.stepTiming((long) (deltaSeconds * 1e6));
   }
+
+  public static void stepTimingAsync(double deltaSeconds) {
+    SimulatorJNI.stepTimingAsync((long) (deltaSeconds * 1e6));
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java

@@ -9,28 +9,41 @@ package edu.wpi.first.wpilibj;
 
 import java.util.concurrent.atomic.AtomicInteger;
 
+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.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-@DisabledOnOs(OS.MAC)
 class WatchdogTest {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
   @Test
+  @ResourceLock("timing")
   void enableDisableTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
+    try (Watchdog watchdog = new Watchdog(0.4, () -> {
+      watchdogCounter.addAndGet(1);
+    })) {
       System.out.println("Run 1");
       watchdog.enable();
-      try {
-        Thread.sleep(200);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.2);
       watchdog.disable();
 
       assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
@@ -38,11 +51,7 @@ class WatchdogTest {
       System.out.println("Run 2");
       watchdogCounter.set(0);
       watchdog.enable();
-      try {
-        Thread.sleep(600);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.6);
       watchdog.disable();
 
       assertEquals(1, watchdogCounter.get(),
@@ -51,11 +60,7 @@ class WatchdogTest {
       // Run 3
       watchdogCounter.set(0);
       watchdog.enable();
-      try {
-        Thread.sleep(1000);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(1);
       watchdog.disable();
 
       assertEquals(1, watchdogCounter.get(),
@@ -64,22 +69,17 @@ class WatchdogTest {
   }
 
   @Test
+  @ResourceLock("timing")
   void resetTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
+    try (Watchdog watchdog = new Watchdog(0.4, () -> {
+      watchdogCounter.addAndGet(1);
+    })) {
       watchdog.enable();
-      try {
-        Thread.sleep(200);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.2);
       watchdog.reset();
-      try {
-        Thread.sleep(200);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.2);
       watchdog.disable();
 
       assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
@@ -87,26 +87,21 @@ class WatchdogTest {
   }
 
   @Test
+  @ResourceLock("timing")
   void setTimeoutTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog = new Watchdog(1.0, () -> watchdogCounter.addAndGet(1))) {
+    try (Watchdog watchdog = new Watchdog(1.0, () -> {
+      watchdogCounter.addAndGet(1);
+    })) {
       watchdog.enable();
-      try {
-        Thread.sleep(200);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.2);
       watchdog.setTimeout(0.2);
 
       assertEquals(0.2, watchdog.getTimeout());
       assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
 
-      try {
-        Thread.sleep(300);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.3);
       watchdog.disable();
 
       assertEquals(1, watchdogCounter.get(),
@@ -115,6 +110,7 @@ class WatchdogTest {
   }
 
   @Test
+  @ResourceLock("timing")
   void isExpiredTest() {
     try (Watchdog watchdog = new Watchdog(0.2, () -> {
     })) {
@@ -122,11 +118,7 @@ class WatchdogTest {
       watchdog.enable();
 
       assertFalse(watchdog.isExpired());
-      try {
-        Thread.sleep(300);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.3);
       assertTrue(watchdog.isExpired());
 
       watchdog.disable();
@@ -138,24 +130,19 @@ class WatchdogTest {
   }
 
   @Test
+  @ResourceLock("timing")
   void epochsTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) {
+    try (Watchdog watchdog = new Watchdog(0.4, () -> {
+      watchdogCounter.addAndGet(1);
+    })) {
       System.out.println("Run 1");
       watchdog.enable();
       watchdog.addEpoch("Epoch 1");
-      try {
-        Thread.sleep(100);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.1);
       watchdog.addEpoch("Epoch 2");
-      try {
-        Thread.sleep(100);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.1);
       watchdog.addEpoch("Epoch 3");
       watchdog.disable();
 
@@ -164,17 +151,9 @@ class WatchdogTest {
       System.out.println("Run 2");
       watchdog.enable();
       watchdog.addEpoch("Epoch 1");
-      try {
-        Thread.sleep(200);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.2);
       watchdog.reset();
-      try {
-        Thread.sleep(200);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.2);
       watchdog.addEpoch("Epoch 2");
       watchdog.disable();
 
@@ -183,28 +162,25 @@ class WatchdogTest {
   }
 
   @Test
+  @ResourceLock("timing")
   void multiWatchdogTest() {
     final AtomicInteger watchdogCounter1 = new AtomicInteger(0);
     final AtomicInteger watchdogCounter2 = new AtomicInteger(0);
 
-    try (Watchdog watchdog1 = new Watchdog(0.2, () -> watchdogCounter1.addAndGet(1));
-        Watchdog watchdog2 = new Watchdog(0.6, () -> watchdogCounter2.addAndGet(1))) {
+    try (Watchdog watchdog1 = new Watchdog(0.2, () -> {
+      watchdogCounter1.addAndGet(1);
+    });
+        Watchdog watchdog2 = new Watchdog(0.6, () -> {
+          watchdogCounter2.addAndGet(1);
+        })) {
       watchdog2.enable();
-      try {
-        Thread.sleep(200);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.25);
       assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early");
       assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
 
       // Sleep enough such that only the watchdog enabled later times out first
       watchdog1.enable();
-      try {
-        Thread.sleep(300);
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
-      }
+      SimHooks.stepTiming(0.25);
       watchdog1.disable();
       watchdog2.disable();