diff --git a/SimLoop/README.md b/SimLoop/README.md index e61b8c9..931a4ec 100644 --- a/SimLoop/README.md +++ b/SimLoop/README.md @@ -376,6 +376,8 @@ RLOG has one frame per tick and the usual completeness checks. the first `loop()` on the first ACTIVE tick, `loop()` per tick, `stop()` - with `telemetry.update()` after each, as the SDK does. Each call runs on the OpMode's own thread while the sim waits (never both at once); breakpoints still work, and a debugger that suspends all threads pauses the watchdog too. + Under `forTeamClass` a wait in `init()` holds it for ticks of sim time, as the SDK lets `init()` block; + waits in the other calls are refused. - **`LinearOpMode`**: `runOpMode()` runs on its own thread in strict lockstep with the sim (never both at once). A tick ends when the OpMode repeats a lifecycle check (`opModeIsActive()`, `isStopRequested()`, `opModeInInit()`, `isStarted()`) it already made this tick at the same place, so one pass of diff --git a/SimLoop/docs/packages/opmode.md b/SimLoop/docs/packages/opmode.md index 36941a5..9dd4676 100644 --- a/SimLoop/docs/packages/opmode.md +++ b/SimLoop/docs/packages/opmode.md @@ -56,7 +56,10 @@ One INIT tick, `initLoopTicks` INIT_LOOP ticks, `activeTicks` ACTIVE ticks, then 4. every fake device updates, then the `eachTick` step runs. An iterative `OpMode` gets the SDK's own call order: `init()`, `init_loop()` each INIT_LOOP tick, `start()` -and the first `loop()` on the first ACTIVE tick, `loop()` each tick after, `stop()` on STOP. +and the first `loop()` on the first ACTIVE tick, `loop()` each tick after, `stop()` on STOP. Under +`forTeamClass`, `init()` may wait, as the SDK lets it: a `Thread.sleep` there holds it for `ceil(ms / tick)` +ticks, and the OpMode's next call comes on the tick after it returns. See +[Running your timer code unchanged](../unchanged-team-code.md). A `LinearOpMode`'s `runOpMode()` runs on its own thread in lockstep with the sim. One pass of `while (opModeIsActive()) { ... }` is one tick. `waitForStart()` holds until PLAY. `sleep(ms)` holds for diff --git a/SimLoop/docs/packages/simclock.md b/SimLoop/docs/packages/simclock.md index 0c6f50a..d5e827e 100644 --- a/SimLoop/docs/packages/simclock.md +++ b/SimLoop/docs/packages/simclock.md @@ -72,7 +72,8 @@ Your classes are loaded a second time for each run. So: can see are listed in `result.warnings()`. - **It will not move `Object.wait`, `Thread.join`, latches, futures or executors.** - **It refuses some things outright**, and fails the run even if your code catches the refusal: a - `Thread.sleep` on any thread but a `LinearOpMode`'s own, and a method reference `this::sleep` (write + `Thread.sleep` on any thread but a `LinearOpMode`'s own or an iterative OpMode's own during `init()`, + and a method reference `this::sleep` (write `ms -> sleep(ms)` instead). - **It will not rewrite an OpMode instance you built yourself.** Only `forTeamClass` loads classes through it. diff --git a/SimLoop/docs/unchanged-team-code.md b/SimLoop/docs/unchanged-team-code.md index 9cfa8b5..322cd12 100644 --- a/SimLoop/docs/unchanged-team-code.md +++ b/SimLoop/docs/unchanged-team-code.md @@ -128,6 +128,7 @@ its top-level package (see [Pedro Pathing](#pedro-pathing-and-other-libraries)). | `System.currentTimeMillis()` | 1 January 2026, 00:00 UTC, plus sim time. The same date every run, so a file name made from it repeats. | | `sleep(ms)` in a `LinearOpMode` | Waits that many ms of **sim** time: `sleep(1000)` is exactly 50 ticks. The duration can be anything: `sleep(500)`, `sleep(ms * 2)`, `sleep(fast ? 100 : 1000)`, or `sleep(millis)` inside a helper method. | | `Thread.sleep(ms)`, `Thread.sleep(ms, nanos)`, `TimeUnit.MILLISECONDS.sleep(ms)` | The same as `sleep(ms)`, but like the real ones they throw `InterruptedException` if STOP is pressed while they wait. `Thread.sleep(0)` returns at once, as on the robot, even in an iterative OpMode. | +| `Thread.sleep(ms)` and the other waits above in an iterative OpMode's `init()`, your own or a library's (Pedro Pathing's Pinpoint localizer sleeps 500 ms in its constructor) | Holds `init()` for that many ms of **sim** time, as the SDK lets `init()` block on the robot: `Thread.sleep(500)` is exactly 25 ticks. Meanwhile motors keep their power and no other OpMode method runs; `init_loop()` starts on the tick after `init()` returns, or `start()` does if PLAY came first. STOP does not interrupt the wait (the SDK does not interrupt an iterative OpMode): sim time ends there, so the rest of it takes no time, then `stop()` runs. Waits in `init_loop()`, `start()`, `loop()` and `stop()` are still refused. | | Android's `SystemClock.uptimeMillis()`, `elapsedRealtime()`, `elapsedRealtimeNanos()`, `SystemClock.sleep(ms)` | Sim time in ms (or ns), and a sim-time wait. (In a JVM test Android's own `SystemClock` reads 0 and waits for nothing; rewritten, it is never used.) | | `idle()` in a `LinearOpMode` | Like the SDK's, it takes no time by itself; the tick ends when the same `idle()` runs a second time in it. So `while (timer.seconds() < 2) { idle(); }` waits 2 s of sim time, one pass per tick after the first, and `sleep(50); idle();` takes exactly the 3 ticks of the sleep. | | `System::nanoTime`, `ElapsedTime::new`, `() -> System.nanoTime()` | Method references and lambdas follow along. | @@ -192,6 +193,9 @@ OpModeRunner.forTeamClass("org.firstinspires.ftc.teamcode.MyAuto", hardwareMap) .run(); ``` +With Pedro listed, its own waits move too: the Pinpoint localizer's 500 ms sleep while the device +calibrates takes 25 ticks of sim time, even when your iterative OpMode builds the follower in `init()`. + What that costs: Pedro's classes are then loaded afresh for each run, like yours, so a Pedro `Pose` or `Path` cannot be handed from the test into the run, or from one run to the next through the blackboard (see the trade-off below). Hand over numbers instead. @@ -229,9 +233,10 @@ changes that let a test hand one in, which is the better fix when you can make i ## Error messages, and what to do **"SimLoop cannot run Thread.sleep(10) at ...loop(MyTeleOp.java:42) on sim time: MyTeleOp is an iterative -OpMode, whose init(), init_loop(), start(), loop() and stop() must each return promptly"** -You called `Thread.sleep` inside an iterative `OpMode`. On a robot this freezes the whole loop, gamepads -included. Instead, start an `ElapsedTime` and check it on each `loop()` call, or make it a `LinearOpMode`. +OpMode, whose init_loop(), start(), loop() and stop() must each return promptly"** +You called `Thread.sleep` inside an iterative `OpMode`, outside `init()`. On a robot this freezes the whole +loop, gamepads included. Instead, start an `ElapsedTime` and check it on each `loop()` call, or make it a +`LinearOpMode`. A wait in `init()` is allowed, as it is on the robot. **"SimLoop refused a call in MyAuto's code, so the run fails even though that code carried on ...: SimLoop cannot run Thread.sleep(50) at ... on sim time: it was called on thread "arm-helper", and only MyAuto's own diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/IterativeOpModeStepper.java b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/IterativeOpModeStepper.java index e580fae..645032d 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/IterativeOpModeStepper.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/IterativeOpModeStepper.java @@ -19,14 +19,41 @@ * *

Breakpoints in an iterative OpMode still work: a debugger stops at them on any thread, and one that * suspends every thread (the default in IntelliJ and Android Studio) pauses the watchdog too. + * + *

A wait in {@code init()}

+ * The SDK calls {@code init()} on the OpMode's own thread and starts {@code init_loop()} only once it returns, + * so on a robot {@code init()} may block: a library that sleeps while a sensor calibrates (Pedro Pathing's + * Pinpoint localizer sleeps 500 ms in its constructor) just makes INIT take longer. Under + * {@link OpModeRunner#forTeamClass} such a rewritten wait holds {@code init()} for whole ticks of sim time, + * rounded up, through {@link #holdForSimSeconds}. While it holds: + * + * A wait in {@code init_loop()}, {@code start()}, {@code loop()} or {@code stop()} is still refused (see + * {@link SimClockBridge}). */ final class IterativeOpModeStepper implements OpModeStepper { /** The SDK's own force-stop exception, thrown by {@code terminateOpModeNow()}; matched by name. */ static final String FORCE_STOP = "com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl$ForceStopException"; + /** The name {@link #callInProgress} has while {@code init()} runs: the one call that may wait. */ + private static final String INIT = "init()"; + private final OpMode opMode; private final OpModeContext context; + private final double tickSeconds; private final int maxWaits; private final CallThread thread; @@ -39,6 +66,9 @@ final class IterativeOpModeStepper implements OpModeStepper { /** True once the call last handed over has returned or thrown. Guarded by {@link #lock}. */ private boolean callDone; + /** True while that call is held in a wait, until the sim resumes it on the next tick. Guarded by {@link #lock}. */ + private boolean parked; + /** What that call threw, or null. Guarded by {@link #lock}. */ private Throwable thrown; @@ -48,15 +78,36 @@ final class IterativeOpModeStepper implements OpModeStepper { /** Set on the OpMode thread by {@code requestOpModeStop()}; read on the sim thread after the call. */ private volatile boolean askedToStop; + /** The call running on the OpMode thread, such as {@code "init()"}, or null between calls. */ + private volatile String callInProgress; + + /** STOP has come: a wait in progress, or one started after it, takes no more ticks. */ + private volatile boolean stopping; + + /** Sim thread only: the call held in a wait across ticks, or null. */ + private String heldCall; + + /** Sim thread only: PLAY has been pressed. */ + private boolean played; + + /** Sim thread only: {@code start()} has been called. */ + private boolean startCalled; + /** Sim thread only: the OpMode has finished. */ private boolean ended; /** Sim thread only: the OpMode thread got stuck and was left behind. */ private boolean abandoned; - IterativeOpModeStepper(OpMode opMode, OpModeContext context, int maxWaits, ThreadGroup group) { + /** + * @param tickSeconds the length of one tick, in seconds of sim time, for rounding a wait in {@code init()} + * @param maxWaits the stuck timeout, in one-millisecond waits + */ + IterativeOpModeStepper(OpMode opMode, OpModeContext context, double tickSeconds, int maxWaits, + ThreadGroup group) { this.opMode = opMode; this.context = context; + this.tickSeconds = tickSeconds; this.maxWaits = maxWaits; this.thread = new CallThread(group); } @@ -74,14 +125,23 @@ public void runTick(OpModePhase phase, int tickInPhase, int tick) { } switch (phase) { case INIT: - call("init()", phase, tick, opMode::init); + call(INIT, phase, tick, opMode::init); break; case INIT_LOOP: - call("init_loop()", phase, tick, opMode::init_loop); + if (!resumeHeldCall(phase, tick)) { + call("init_loop()", phase, tick, opMode::init_loop); + } break; case ACTIVE: if (tickInPhase == 0) { context.markStarted(); + played = true; + } + if (resumeHeldCall(phase, tick)) { + break; + } + if (!startCalled) { + startCalled = true; call("start()", phase, tick, opMode::start); } if (!ended) { @@ -89,13 +149,86 @@ public void runTick(OpModePhase phase, int tickInPhase, int tick) { } break; case STOP: - stop(phase, tick); + // Before the held call resumes, so its wait ends now: there is no sim time after STOP. + stopping = true; + context.markStopRequested(); + resumeHeldCall(phase, tick); + if (!ended && played && !startCalled) { + // PLAY came while init() waited: internalRunOpMode() still calls start() before stop(). + startCalled = true; + call("start()", phase, tick, opMode::start); + } + if (!ended) { + stop(phase, tick); + } break; default: throw new IllegalStateException("unknown phase " + phase); } } + /** {@return true while {@code init()} is running on the OpMode thread, where a wait holds for sim time} */ + boolean inInit() { + return INIT.equals(callInProgress); + } + + /** + * On the OpMode's own thread, in {@code init()}: holds it for enough ticks to cover {@code seconds} of sim + * time, the way the sim-clock hooks ask for a rewritten {@code Thread.sleep}. Each tick the sim resumes it + * instead of calling another OpMode method. + * + * @param seconds how long, in seconds of sim time + * @return true if the wait returns normally: always, unless the run is being torn down, because the SDK + * does not interrupt an iterative OpMode at STOP (after STOP the rest of the wait takes no time) + */ + boolean holdForSimSeconds(double seconds) { + long ticks = LinearOpModeStepper.ticksFor(seconds, tickSeconds); + for (long t = 0; t < ticks && !stopping; t++) { + if (!park()) { + return false; + } + } + return true; + } + + /** + * On the OpMode thread: ends this tick for the call in progress and blocks until the sim resumes it. + * + * @return false if the OpMode thread must end instead (the run is being torn down) + */ + private boolean park() { + synchronized (lock) { + parked = true; + lock.notifyAll(); + while (parked && !exit) { + try { + lock.wait(); + } catch (InterruptedException ignored) { + // Only the sim resumes a held call; an interrupt is not that. + } + } + return !exit; + } + } + + /** + * Resumes the call held in a wait, if there is one, for this tick. + * + * @return true if there was one: this tick belonged to it, whether or not it has now returned + */ + private boolean resumeHeldCall(OpModePhase phase, int tick) { + if (heldCall == null) { + return false; + } + context.resumeUserCode(); + synchronized (lock) { + parked = false; + lock.notifyAll(); + } + finishCall(heldCall, phase, tick); + return true; + } + @Override public boolean hasEnded() { return ended; @@ -148,10 +281,31 @@ private void stop(OpModePhase phase, int tick) { private void call(String method, OpModePhase phase, int tick, Runnable userCode) { context.beforeUserCode(); - Throwable failure = runOnOpModeThread(method, phase, tick, () -> { + callInProgress = method; + handOver(() -> { userCode.run(); opMode.telemetry.update(); }); + finishCall(method, phase, tick); + } + + /** + * Waits for the call on the OpMode thread to return or to be held in a wait, and once it has returned does + * what follows a call: surface what it threw, and run {@code stop()} if it asked to stop. + */ + private void finishCall(String method, OpModePhase phase, int tick) { + Throwable failure; + synchronized (lock) { + awaitCallOrPark(method, phase, tick); + if (parked) { + heldCall = method; + context.afterUserCode(); // a resetRuntime() before the wait counts from this tick, not the next + return; + } + failure = thrown; + } + heldCall = null; + callInProgress = null; if (failure != null) { if (FORCE_STOP.equals(failure.getClass().getName())) { ended = true; // terminateOpModeNow(): the SDK ends the OpMode on the spot, without stop() @@ -168,13 +322,8 @@ private void call(String method, OpModePhase phase, int tick, Runnable userCode) } } - /** - * Hands {@code work} to the OpMode thread and waits, bounded, for it to finish. - * - * @return what it threw, or null - * @throws OpModeException if it did not finish within {@link #maxWaits} one-millisecond waits - */ - private Throwable runOnOpModeThread(String method, OpModePhase phase, int tick, Runnable work) { + /** Hands {@code work} to the OpMode thread, starting the thread on the first call. */ + private void handOver(Runnable work) { if (thread.getState() == Thread.State.NEW) { thread.start(); } @@ -183,21 +332,29 @@ private Throwable runOnOpModeThread(String method, OpModePhase phase, int tick, callDone = false; thrown = null; lock.notifyAll(); - int waits = 0; - while (!callDone) { - if (waits++ >= maxWaits) { - throw abandon(method, phase, tick); - } - try { - lock.wait(1); - } catch (InterruptedException interrupted) { - Thread.currentThread().interrupt(); - throw new OpModeException("The thread running the sim was interrupted while " - + opMode.getClass().getSimpleName() + "." + method + " was running.", phase, tick, - interrupted); - } + } + } + + /** + * Waits, bounded, until the call on the OpMode thread has returned or is held in a wait. Caller holds the + * lock. + * + * @throws OpModeException if neither happened within {@link #maxWaits} one-millisecond waits + */ + private void awaitCallOrPark(String method, OpModePhase phase, int tick) { + int waits = 0; + while (!callDone && !parked) { + if (waits++ >= maxWaits) { + throw abandon(method, phase, tick); + } + try { + lock.wait(1); + } catch (InterruptedException interrupted) { + Thread.currentThread().interrupt(); + throw new OpModeException("The thread running the sim was interrupted while " + + opMode.getClass().getSimpleName() + "." + method + " was running.", phase, tick, + interrupted); } - return thrown; } } diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/LinearOpModeStepper.java b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/LinearOpModeStepper.java index 5dc9ce9..b801b34 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/LinearOpModeStepper.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/LinearOpModeStepper.java @@ -267,8 +267,13 @@ private OpModeException notFinishing(String what, OpModePhase phase, int tick) { + " thread is a daemon and is left running; the cause shows where it was.", phase, tick, whereItIs); } - /** {@return how many ticks a wait of {@code seconds} holds the OpMode for: enough to cover it, never fewer} */ - private long ticksFor(double seconds) { + /** + * {@return how many ticks a wait of {@code seconds} holds the OpMode for: enough to cover it, never fewer} + * + * @param seconds the wait, in seconds of sim time + * @param tickSeconds the length of one tick, in seconds of sim time + */ + static long ticksFor(double seconds, double tickSeconds) { if (!(seconds > 0)) { return 0; } @@ -291,7 +296,7 @@ Thread opModeThread() { * @return true if the wait ran its full length, false if STOP came before or during it */ boolean holdForSimSeconds(double seconds) { - long ticks = ticksFor(seconds); + long ticks = ticksFor(seconds, tickSeconds); for (long t = 0; t < ticks && !stopping; t++) { thread.park(); } diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/OpModeContext.java b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/OpModeContext.java index 32352be..c17c0c7 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/OpModeContext.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/OpModeContext.java @@ -98,11 +98,31 @@ void markStopRequested() { void beforeUserCode() { opMode.gamepad1.copy(driverStationGamepad1); opMode.gamepad2.copy(driverStationGamepad2); + opMode.time = anchorRuntime(); + } + + /** + * Runs when user code held in a wait across ticks carries on, as an iterative OpMode's {@code init()} does + * after a sleep: {@code getRuntime()} reads the sim runtime again, as it keeps following the real clock on + * the robot. Gamepads and {@code time} stay as they were, because the SDK updates those only before a call. + * The stepper calls {@link #afterUserCode} when the code starts to wait, so a {@code resetRuntime()} made + * before it is already counted. + */ + void resumeUserCode() { + anchorRuntime(); + } + + /** + * Moves the SDK's private {@code startTime} so that {@code getRuntime()} reads the sim runtime now. + * + * @return the sim runtime, in seconds since INIT or the OpMode's last {@code resetRuntime()} + */ + private double anchorRuntime() { double runtimeSeconds = timer.time() - runtimeOriginSeconds; - opMode.time = runtimeSeconds; opMode.resetRuntime(); anchoredStartTime = SdkOpModeAccess.startTime(opMode) - Math.round(runtimeSeconds * 1e9); SdkOpModeAccess.setStartTime(opMode, anchoredStartTime); + return runtimeSeconds; } /** Runs after every piece of user code: an OpMode that called {@code resetRuntime()} restarts the count. */ diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/OpModeRunner.java b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/OpModeRunner.java index 389e10f..d24cd06 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/OpModeRunner.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/OpModeRunner.java @@ -521,7 +521,8 @@ private final class Run { stepper = opMode instanceof LinearOpMode ? new LinearOpModeStepper((LinearOpMode) opMode, context, tickSeconds, stuckTimeoutMillis, threads.group()) - : new IterativeOpModeStepper(opMode, context, stuckTimeoutMillis, threads.group()); + : new IterativeOpModeStepper(opMode, context, tickSeconds, stuckTimeoutMillis, + threads.group()); rewiring = context.inject(hardwareMap, telemetry, services()); simClockBridge = simClock != null ? new SimClockBridge(opModeName, stepper) : null; if (simClock != null) { diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/SimClockBridge.java b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/SimClockBridge.java index 505a535..5342c6d 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/opmode/SimClockBridge.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/opmode/SimClockBridge.java @@ -6,47 +6,58 @@ * Connects a run's {@link org.horizon36596.simloop.simclock.SimClockLoader} to the stepper driving the * OpMode, so that a rewritten {@code Thread.sleep}, {@code sleep(ms)} or {@code idle()} waits in sim time. * - *

Only a {@code LinearOpMode}'s own thread can wait: it is the one thread the sim hands turns to. A wait on - * any other thread is refused, loudly, with a reason the hook puts after the call and its place in the code: - * an iterative OpMode's methods must return every tick, and a thread the OpMode starts runs in real time - * beside the sim, so there is no sim-time answer to give it. The loader keeps each refusal, and the run fails - * with it, because a thread the OpMode started dies of it with no one watching and team code can catch it. + *

Only the OpMode's own thread can wait, because it is the one thread the sim steps: a {@code LinearOpMode}'s + * anywhere in {@code runOpMode()}, and an iterative OpMode's only in {@code init()}, which the SDK lets block + * (see {@link IterativeOpModeStepper}). A wait anywhere else is refused, loudly, with a reason the hook puts + * after the call and its place in the code: an iterative OpMode's other methods must return every tick, and a + * thread the OpMode starts runs in real time beside the sim, so there is no sim-time answer to give it. The + * loader keeps each refusal, and the run fails with it, because a thread the OpMode started dies of it with no + * one watching and team code can catch it. */ final class SimClockBridge implements SimClockScheduler { private final String opModeName; - /** The stepper of a {@code LinearOpMode} run, or null for an iterative OpMode, which cannot wait. */ + /** The stepper of a {@code LinearOpMode} run, or null. */ private final LinearOpModeStepper linear; + /** The stepper of an iterative OpMode run, or null. */ + private final IterativeOpModeStepper iterative; + SimClockBridge(String opModeName, OpModeStepper stepper) { this.opModeName = opModeName; this.linear = stepper instanceof LinearOpModeStepper ? (LinearOpModeStepper) stepper : null; + this.iterative = stepper instanceof IterativeOpModeStepper ? (IterativeOpModeStepper) stepper : null; } @Override public String whyThreadCannotWait(Thread thread) { - if (linear == null) { - return opModeName + " is an iterative OpMode, whose init(), init_loop(), start(), loop() and stop() must" - + " each return promptly (on a robot a sleep there freezes the whole loop, gamepads and all)." - + " Wait with an ElapsedTime check across loop() calls instead, or make it a LinearOpMode."; - } - if (thread != linear.opModeThread()) { + Thread opModeThread = linear != null ? linear.opModeThread() : iterative.thread(); + if (thread != opModeThread) { return "it was called on thread \"" + thread.getName() + "\", and only " + opModeName + "'s own thread" - + " (\"" + linear.opModeThread().getName() + "\") waits in sim time. A thread the OpMode starts" - + " runs in real time beside the sim, so SimLoop cannot step its waits; do that work in" - + " runOpMode()'s loop instead."; + + " (\"" + opModeThread.getName() + "\") waits in sim time. A thread the OpMode starts" + + " runs in real time beside the sim, so SimLoop cannot step its waits; do that work in " + + (linear != null ? "runOpMode()'s loop" : "loop()") + " instead."; + } + if (iterative != null && !iterative.inInit()) { + return opModeName + " is an iterative OpMode, whose init_loop(), start(), loop() and stop() must each" + + " return promptly (on a robot a sleep there freezes the whole loop, gamepads and all; only" + + " init() may wait). Wait with an ElapsedTime check across loop() calls instead, or make it a" + + " LinearOpMode."; } return null; } @Override public boolean holdFor(double seconds) { - return linear.holdForSimSeconds(seconds); + return linear != null ? linear.holdForSimSeconds(seconds) : iterative.holdForSimSeconds(seconds); } @Override public void idleAt(String callSite) { - linear.idleAt(callSite); + // idle() is LinearOpMode's; reached from an iterative OpMode's init() it is the SDK's yield, no time. + if (linear != null) { + linear.idleAt(callSite); + } } } diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/simclock/SimClockScheduler.java b/SimLoop/src/main/java/org/horizon36596/simloop/simclock/SimClockScheduler.java index cb1ec47..1392c38 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/simclock/SimClockScheduler.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/simclock/SimClockScheduler.java @@ -13,7 +13,8 @@ public interface SimClockScheduler { /** * {@return null if {@code thread} may wait in sim time, or else a sentence saying why it may not}, to go * after "SimLoop cannot run Thread.sleep(500) at MyAuto.java:12 on sim time: ". Only the thread the sim - * steps can wait: a {@code LinearOpMode}'s own thread. Any other thread runs in real time beside the sim. + * steps can wait: a {@code LinearOpMode}'s own thread, or an iterative OpMode's own thread while it is in + * {@code init()}. Any other thread runs in real time beside the sim. * * @param thread the thread that wants to wait */ @@ -24,7 +25,9 @@ public interface SimClockScheduler { * takes to cover {@code seconds} of sim time; no real time passes. * * @param seconds how long to wait, in seconds of sim time; zero or less waits no ticks - * @return true if the wait ran its full length, false if the OpMode was stopped before or during it + * @return true if the wait returns normally, false if it ends as interrupted: a {@code LinearOpMode} was + * stopped before or during it. An iterative OpMode is not interrupted at STOP, as on the robot, so + * its wait returns true, cut short because sim time ends at STOP */ boolean holdFor(double seconds); diff --git a/SimLoop/src/test/java/com/pedropathing/simclockfixture/PedroStyleLocalizer.java b/SimLoop/src/test/java/com/pedropathing/simclockfixture/PedroStyleLocalizer.java new file mode 100644 index 0000000..f99e8ba --- /dev/null +++ b/SimLoop/src/test/java/com/pedropathing/simclockfixture/PedroStyleLocalizer.java @@ -0,0 +1,21 @@ +package com.pedropathing.simclockfixture; + +/** + * Stands in for a Pedro Pathing localizer that sleeps in its constructor while the odometry computer + * calibrates (as Pedro's {@code PinpointLocalizer} does, 500 ms), wrapping the {@code InterruptedException} + * the way that library does. Teams build it in an iterative OpMode's {@code init()}. Test sources only. + */ +public final class PedroStyleLocalizer { + + /** How long the constructor sleeps, in milliseconds. */ + public static final long CALIBRATION_MILLIS = 500; + + /** Sleeps {@link #CALIBRATION_MILLIS}. */ + public PedroStyleLocalizer() { + try { + Thread.sleep(CALIBRATION_MILLIS); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } +} diff --git a/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/InitHelperThreadTeleOp.java b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/InitHelperThreadTeleOp.java new file mode 100644 index 0000000..c5e9ea7 --- /dev/null +++ b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/InitHelperThreadTeleOp.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode.simclockfixture; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/** An iterative OpMode whose {@code init()} hands a sleep to a helper thread and waits for it to finish. */ +@TeleOp(name = "Init Helper Thread TeleOp") +public class InitHelperThreadTeleOp extends OpMode { + + @Override + public void init() { + Thread helper = new Thread(() -> { + try { + Thread.sleep(50); + blackboard.put("helperSlept", true); + } catch (Throwable refused) { + blackboard.put("helperFailure", refused); + } + }, "init-helper"); + helper.start(); + try { + helper.join(); // real time, briefly: the refusal ends the helper at once + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } + + @Override + public void loop() { + } +} diff --git a/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/PedroLocalizerTeleOp.java b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/PedroLocalizerTeleOp.java new file mode 100644 index 0000000..a39875c --- /dev/null +++ b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/PedroLocalizerTeleOp.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.simclockfixture; + +import com.pedropathing.simclockfixture.PedroStyleLocalizer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/** An iterative OpMode that builds its path follower's localizer in {@code init()}, as Pedro teams do. */ +@TeleOp(name = "Pedro Localizer TeleOp") +public class PedroLocalizerTeleOp extends OpMode { + + @Override + public void init() { + long before = System.nanoTime(); + new PedroStyleLocalizer(); + blackboard.put("buildNanos", System.nanoTime() - before); + } + + @Override + public void loop() { + } +} diff --git a/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/ResetThenSleepInitTeleOp.java b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/ResetThenSleepInitTeleOp.java new file mode 100644 index 0000000..3646998 --- /dev/null +++ b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/ResetThenSleepInitTeleOp.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.simclockfixture; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/** + * An iterative OpMode whose {@code init()} calls {@code resetRuntime()} and then sleeps 500 ms. It reports + * {@code getRuntime()} after the sleep, in seconds, through the blackboard. + */ +@TeleOp(name = "Reset Then Sleep Init TeleOp") +public class ResetThenSleepInitTeleOp extends OpMode { + + @Override + public void init() { + resetRuntime(); + try { + Thread.sleep(500); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + blackboard.put("runtimeAfterSleep", getRuntime()); + } + + @Override + public void loop() { + } +} diff --git a/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/SleepyInitLoopTeleOp.java b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/SleepyInitLoopTeleOp.java new file mode 100644 index 0000000..94044e2 --- /dev/null +++ b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/SleepyInitLoopTeleOp.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.simclockfixture; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/** An iterative OpMode that sleeps in {@code init_loop()}, which on a robot stalls every pass before PLAY. */ +@TeleOp(name = "Sleepy Init Loop TeleOp") +public class SleepyInitLoopTeleOp extends OpMode { + + @Override + public void init() { + } + + @Override + public void init_loop() { + try { + Thread.sleep(10); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } + + @Override + public void loop() { + } +} diff --git a/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/SleepyInitTeleOp.java b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/SleepyInitTeleOp.java new file mode 100644 index 0000000..c692f9b --- /dev/null +++ b/SimLoop/src/test/java/org/firstinspires/ftc/teamcode/simclockfixture/SleepyInitTeleOp.java @@ -0,0 +1,61 @@ +package org.firstinspires.ftc.teamcode.simclockfixture; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +/** + * An iterative OpMode whose {@code init()} starts a motor and then sleeps 500 ms, as a team waits for a sensor + * to calibrate. It reports what it saw through the blackboard: the sim clock ({@code System.nanoTime()}) at + * each lifecycle call, how the sleep ended, and the motor's encoder after it. + */ +@TeleOp(name = "Sleepy Init TeleOp") +public class SleepyInitTeleOp extends OpMode { + + private int initLoops; + private boolean looped; + + @Override + public void init() { + blackboard.put("initNanos", System.nanoTime()); + DcMotor motor = hardwareMap.get(DcMotor.class, "motor"); + motor.setPower(1.0); + try { + Thread.sleep(500); + blackboard.put("slept", "normally"); + } catch (InterruptedException e) { + blackboard.put("slept", "interrupted"); + } + blackboard.put("initReturnNanos", System.nanoTime()); + blackboard.put("runtimeAfterSleep", getRuntime()); + blackboard.put("encoderAfterSleep", motor.getCurrentPosition()); + } + + @Override + public void init_loop() { + if (initLoops == 0) { + blackboard.put("firstInitLoopNanos", System.nanoTime()); + } + initLoops++; + blackboard.put("initLoops", initLoops); + telemetry.addData("initLoops", initLoops); + } + + @Override + public void start() { + blackboard.put("startNanos", System.nanoTime()); + } + + @Override + public void loop() { + if (!looped) { + blackboard.put("firstLoopNanos", System.nanoTime()); + looped = true; + } + } + + @Override + public void stop() { + blackboard.put("stopNanos", System.nanoTime()); + } +} diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/opmode/IterativeInitWaitTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/opmode/IterativeInitWaitTest.java new file mode 100644 index 0000000..79abb65 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/opmode/IterativeInitWaitTest.java @@ -0,0 +1,165 @@ +package org.horizon36596.simloop.opmode; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.simclockfixture.InitHelperThreadTeleOp; +import org.firstinspires.ftc.teamcode.simclockfixture.PedroLocalizerTeleOp; +import org.firstinspires.ftc.teamcode.simclockfixture.ResetThenSleepInitTeleOp; +import org.firstinspires.ftc.teamcode.simclockfixture.SleepyInitLoopTeleOp; +import org.firstinspires.ftc.teamcode.simclockfixture.SleepyInitTeleOp; +import org.horizon36596.simloop.fakehardware.FakeHardwareMap; +import org.horizon36596.simloop.fakehardware.FakeMotor; +import org.horizon36596.simloop.simclock.SimClockLoader; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import java.util.List; + +/** + * Pins a wait in an iterative OpMode's {@code init()} under {@link OpModeRunner#forTeamClass}. The SDK + * (RobotCore 12.0.0, {@code OpMode.internalRunOpMode()}) calls {@code init()} on the OpMode's own thread and + * starts {@code init_loop()} only once it returns, so on a robot {@code init()} may block, as a library does + * when it sleeps while a sensor calibrates. In the sim the wait holds {@code init()} for whole ticks of sim + * time; every other iterative method still may not wait. The fixtures are in + * {@code org.firstinspires.ftc.teamcode.simclockfixture} and report through {@code OpMode.blackboard}. + * + *

Invariant tier: each test states what the robot does with the same code. + */ +class IterativeInitWaitTest { + + /** One tick, in nanoseconds of sim time. */ + private static final long TICK_NANOS = Math.round(OpModeRunner.DEFAULT_TICK_SECONDS * 1e9); + + /** The 500 ms sleep in the fixtures' {@code init()}, in ticks: 500 ms / 20 ms. */ + private static final int SLEEP_TICKS = 25; + + @BeforeEach + void emptyBlackboard() { + OpMode.blackboard.clear(); + } + + private final OpModeThreadLeakCheck leaks = new OpModeThreadLeakCheck(OpModeThreadLeakCheck.ITERATIVE); + + @AfterEach + void noOpModeThreadOutlivesItsRun() throws InterruptedException { + OpMode.blackboard.clear(); + leaks.verify(); + } + + private static OpModeRunner runner(Class teamClass) { + FakeHardwareMap hardwareMap = new FakeHardwareMap(); + hardwareMap.register("motor", new FakeMotor(2787, 10)); + return OpModeRunner.forTeamClass(teamClass, hardwareMap); + } + + /** {@return the sim clock's nanoseconds from {@code init()} starting to the reading under {@code key}} */ + private static long sinceInit(String key) { + return (Long) OpMode.blackboard.get(key) - (Long) OpMode.blackboard.get("initNanos"); + } + + @Test + void aWaitInInitHoldsItForSimTimeAndInitLoopStartsTheTickAfterItReturns() { + runner(SleepyInitTeleOp.class).initLoopTicks(30).activeTicks(2).run(); + + assertEquals("normally", OpMode.blackboard.get("slept")); + assertEquals(SLEEP_TICKS * TICK_NANOS, sinceInit("initReturnNanos"), "500 ms of sim time, 25 ticks"); + assertEquals((SLEEP_TICKS + 1) * TICK_NANOS, sinceInit("firstInitLoopNanos"), + "the tick init() returns on is its own; init_loop() starts on the next"); + assertEquals(30 - SLEEP_TICKS, OpMode.blackboard.get("initLoops"), "ticks 26 to 30"); + assertEquals(0.5, (Double) OpMode.blackboard.get("runtimeAfterSleep"), 0.01, + "getRuntime() followed the sim clock through the wait, as it follows the real one on a robot"); + assertTrue((Integer) OpMode.blackboard.get("encoderAfterSleep") > 0, + "the motor init() started kept turning while init() waited"); + } + + @Test + void aResetRuntimeJustBeforeTheWaitCountsTheWholeWait() { + runner(ResetThenSleepInitTeleOp.class).initLoopTicks(30).activeTicks(1).run(); + + assertEquals(0.5, (Double) OpMode.blackboard.get("runtimeAfterSleep"), 0.005, + "getRuntime() counts from the resetRuntime() call, as on a robot, not from the tick after it"); + } + + @Test + void playDuringTheWaitSkipsInitLoopAndStartsOnTheTickAfterInitReturns() { + runner(SleepyInitTeleOp.class).initLoopTicks(5).activeTicks(30).run(); + + assertFalse(OpMode.blackboard.containsKey("initLoops"), "internalRunOpMode() goes straight to start()"); + assertEquals((SLEEP_TICKS + 1) * TICK_NANOS, sinceInit("startNanos")); + assertEquals(OpMode.blackboard.get("startNanos"), OpMode.blackboard.get("firstLoopNanos"), + "the first loop() comes straight after start(), as always"); + } + + @Test + void stopDuringTheWaitEndsItWithoutAnInterruptAndStopRuns() { + runner(SleepyInitTeleOp.class).initLoopTicks(5).activeTicks(0).run(); + + assertEquals("normally", OpMode.blackboard.get("slept"), + "the SDK does not interrupt an iterative OpMode at STOP; it waits for init() to return"); + assertEquals(6 * TICK_NANOS, sinceInit("initReturnNanos"), "sim time ends at STOP, on tick 6"); + assertFalse(OpMode.blackboard.containsKey("startNanos"), "PLAY never came"); + assertEquals(6 * TICK_NANOS, sinceInit("stopNanos")); + } + + @Test + void stopAfterPlayDuringTheWaitRunsStartThenStopButNoLoop() { + runner(SleepyInitTeleOp.class).initLoopTicks(2).activeTicks(3).run(); + + assertEquals(6 * TICK_NANOS, sinceInit("startNanos"), "start() on the STOP tick, once init() returned"); + assertFalse(OpMode.blackboard.containsKey("firstLoopNanos"), "stopRequested is checked before loop()"); + assertEquals(6 * TICK_NANOS, sinceInit("stopNanos")); + } + + @Test + void aLibraryThatSleepsWhileItCalibratesCanBeBuiltInInit() { + runner(PedroLocalizerTeleOp.class) + .teamPackages("org.firstinspires.ftc.teamcode.", "com.seattlesolvers.solverslib.", + SimClockLoader.PEDRO_PATHING) + .initLoopTicks(30).activeTicks(1).run(); + + assertEquals(SLEEP_TICKS * TICK_NANOS, (long) (Long) OpMode.blackboard.get("buildNanos"), + "the library's 500 ms sleep took 500 ms of sim time"); + } + + @Test + void aWaitInInitIsTheSameEveryRun() { + List> first = runner(SleepyInitTeleOp.class).initLoopTicks(30).activeTicks(2).run() + .telemetry().transmissions(); + Object firstEncoder = OpMode.blackboard.get("encoderAfterSleep"); + OpMode.blackboard.clear(); + List> second = runner(SleepyInitTeleOp.class).initLoopTicks(30).activeTicks(2).run() + .telemetry().transmissions(); + + assertEquals(first, second); + assertEquals(firstEncoder, OpMode.blackboard.get("encoderAfterSleep")); + } + + @Test + void aWaitInInitLoopIsStillRefused() { + OpModeException failure = assertThrows(OpModeException.class, + () -> runner(SleepyInitLoopTeleOp.class).initLoopTicks(3).run()); + + assertEquals(OpModePhase.INIT_LOOP, failure.phase()); + String message = failure.getCause().getMessage(); + assertTrue(message.startsWith("SimLoop cannot run Thread.sleep(10) at " + + SleepyInitLoopTeleOp.class.getName() + ".init_loop(SleepyInitLoopTeleOp.java:"), message); + assertTrue(message.contains("SleepyInitLoopTeleOp is an iterative OpMode"), message); + assertTrue(message.contains("only init() may wait"), message); + } + + @Test + void aWaitOnAThreadStartedInInitIsStillRefused() { + OpModeException failure = assertThrows(OpModeException.class, + () -> runner(InitHelperThreadTeleOp.class).run()); + + assertEquals(OpModePhase.INIT, failure.phase()); + assertTrue(failure.getMessage().contains("it was called on thread \"init-helper\""), failure.getMessage()); + assertFalse(OpMode.blackboard.containsKey("helperSlept")); + } +}