From 9adffd356d25263a82e2159ba8ba3588cc67eb0c Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 29 May 2026 23:09:25 -0700 Subject: [PATCH] [hal] Update waitForProgramStart to optionally wait for first notifier (#8932) This addresses a race condition caused by TimedRobot and other frameworks not creating their first notifier alarm until after signaling program start. Default this to true because it's the most common desired use case when combined with TimedRobot. --- .../hardware/hal/simulation/SimulatorJNI.java | 2 +- .../main/native/cpp/jni/simulation/SimulatorJNI.cpp | 6 +++--- .../native/include/wpi/hal/simulation/MockHooks.h | 2 +- hal/src/main/native/sim/MockHooks.cpp | 13 ++++++++++++- .../main/native/systemcore/mockdata/MockHooks.cpp | 2 +- hal/src/main/python/hal/simulation/mockhooks.py | 13 ++++++++++--- wpilibc/src/main/native/cpp/simulation/SimHooks.cpp | 4 ++-- .../main/native/include/wpi/simulation/SimHooks.hpp | 4 +++- .../main/java/org/wpilib/simulation/SimHooks.java | 13 +++++++++++-- 9 files changed, 44 insertions(+), 15 deletions(-) diff --git a/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java index 3ca71ce596..929829169c 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java @@ -11,7 +11,7 @@ import org.wpilib.hardware.hal.JNIWrapper; public class SimulatorJNI extends JNIWrapper { public static native void setRuntimeType(int type); - public static native void waitForProgramStart(); + public static native void waitForProgramStart(boolean waitForFirstNotifier); public static native void setProgramStarted(boolean started); diff --git a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp index 2aab0b293d..79b34922fb 100644 --- a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp +++ b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp @@ -141,13 +141,13 @@ Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_setRuntimeType /* * Class: org_wpilib_hardware_hal_simulation_SimulatorJNI * Method: waitForProgramStart - * Signature: ()V + * Signature: (Z)V */ JNIEXPORT void JNICALL Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_waitForProgramStart - (JNIEnv*, jclass) + (JNIEnv*, jclass, jboolean waitForFirstNotifier) { - HALSIM_WaitForProgramStart(); + HALSIM_WaitForProgramStart(waitForFirstNotifier); } /* diff --git a/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h b/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h index e5ac98c5d2..8f389c5d69 100644 --- a/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h +++ b/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h @@ -13,7 +13,7 @@ extern "C" { #endif void HALSIM_SetRuntimeType(HAL_RuntimeType type); -void HALSIM_WaitForProgramStart(void); +void HALSIM_WaitForProgramStart(HAL_Bool waitForFirstNotifier); void HALSIM_SetProgramStarted(HAL_Bool started); HAL_Bool HALSIM_GetProgramStarted(void); void HALSIM_SetProgramState(HAL_ControlWord controlWord); diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp index 97d574a61f..34600effb4 100644 --- a/hal/src/main/native/sim/MockHooks.cpp +++ b/hal/src/main/native/sim/MockHooks.cpp @@ -77,7 +77,7 @@ bool GetProgramStarted() { using namespace wpi::hal; extern "C" { -void HALSIM_WaitForProgramStart(void) { +void HALSIM_WaitForProgramStart(HAL_Bool waitForFirstNotifier) { int count = 0; while (!programStarted) { count++; @@ -86,6 +86,17 @@ void HALSIM_WaitForProgramStart(void) { } std::this_thread::sleep_for(std::chrono::milliseconds(1)); } + + // Frameworks observe program start before arming their first notifier alarm. + // Wait for that alarm so a following StepTiming() can see and service it. + while (waitForFirstNotifier && + HALSIM_GetNextNotifierTimeout() == UINT64_MAX) { + count++; + if (count % 10 == 0) { + wpi::util::print("Waiting for first notifier alarm: {}\n", count); + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } } void HALSIM_SetProgramStarted(HAL_Bool started) { diff --git a/hal/src/main/native/systemcore/mockdata/MockHooks.cpp b/hal/src/main/native/systemcore/mockdata/MockHooks.cpp index a619cfb439..f17d5b87b0 100644 --- a/hal/src/main/native/systemcore/mockdata/MockHooks.cpp +++ b/hal/src/main/native/systemcore/mockdata/MockHooks.cpp @@ -8,7 +8,7 @@ extern "C" { void HALSIM_SetRuntimeType(HAL_RuntimeType type) {} -void HALSIM_WaitForProgramStart(void) {} +void HALSIM_WaitForProgramStart(HAL_Bool waitForFirstNotifier) {} void HALSIM_SetProgramStarted(HAL_Bool started) {} diff --git a/hal/src/main/python/hal/simulation/mockhooks.py b/hal/src/main/python/hal/simulation/mockhooks.py index fc4ef11b22..eae82ee8b9 100644 --- a/hal/src/main/python/hal/simulation/mockhooks.py +++ b/hal/src/main/python/hal/simulation/mockhooks.py @@ -1,15 +1,20 @@ import time import typing as T -from ._simulation import getProgramStarted +from ._simulation import getNextNotifierTimeout, getProgramStarted -def waitForProgramStart(timeout: T.Optional[float] = None, delta: float = 0.001): +def waitForProgramStart( + timeout: T.Optional[float] = None, + delta: float = 0.001, + waitForFirstNotifier: bool = True, +): """ Polls robot program and returns when it has reported that it started :param timeout: Amount of time to wait :param delta: Amount of time to sleep between checks + :param waitForFirstNotifier: Wait for the first notifier alarm to be armed """ # This is basically the same thing that the C version of this function @@ -18,7 +23,9 @@ def waitForProgramStart(timeout: T.Optional[float] = None, delta: float = 0.001) until = None if timeout and timeout > 0: until = time.monotonic() + timeout - while not getProgramStarted(): + while not getProgramStarted() or ( + waitForFirstNotifier and getNextNotifierTimeout() == 0xFFFFFFFFFFFFFFFF + ): if until is not None and time.monotonic() > until: raise TimeoutError("Program did not start") time.sleep(delta) diff --git a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp index 803836558c..5e498564f5 100644 --- a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp @@ -12,8 +12,8 @@ void SetRuntimeType(HAL_RuntimeType type) { HALSIM_SetRuntimeType(type); } -void WaitForProgramStart() { - HALSIM_WaitForProgramStart(); +void WaitForProgramStart(bool waitForFirstNotifier) { + HALSIM_WaitForProgramStart(waitForFirstNotifier); } void SetProgramStarted(bool started) { diff --git a/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp b/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp index 45a51271f8..3888b74255 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp @@ -21,8 +21,10 @@ void SetRuntimeType(HAL_RuntimeType type); /** * Waits until the user program has started. + * + * @param waitForFirstNotifier wait for the first notifier alarm to be armed */ -void WaitForProgramStart(); +void WaitForProgramStart(bool waitForFirstNotifier = true); /** * Sets flag that indicates if the user program has started. diff --git a/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java b/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java index ba3c51d556..d6b09e0e0d 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java @@ -20,9 +20,18 @@ public final class SimHooks { SimulatorJNI.setRuntimeType(type); } - /** Waits until the user program has started. */ + /** Waits until the user program has started and the first notifier alarm has been armed. */ public static void waitForProgramStart() { - SimulatorJNI.waitForProgramStart(); + waitForProgramStart(true); + } + + /** + * Waits until the user program has started. + * + * @param waitForFirstNotifier wait for the first notifier alarm to be armed + */ + public static void waitForProgramStart(boolean waitForFirstNotifier) { + SimulatorJNI.waitForProgramStart(waitForFirstNotifier); } /**