diff --git a/parol6/server/loop_timer.py b/parol6/server/loop_timer.py index f022b5c..f0315dd 100644 --- a/parol6/server/loop_timer.py +++ b/parol6/server/loop_timer.py @@ -702,12 +702,23 @@ def format_hz_summary(m: LoopMetrics) -> str: ) +# How far one late wake moves the estimate of how late sleeps wake, and how +# fast that estimate fades on ticks that spin instead of sleeping. +_LATE_GAIN = 0.25 +_LATE_FADE = 0.995 + + class LoopTimer: """Deadline-based loop timing with hybrid sleep + busy-loop. Uses time.sleep() for most of the wait time to reduce CPU usage, then switches to a busy-loop for the final portion to achieve precise timing without OS scheduling jitter. + + The busy window adapts to how late this machine's sleeps wake: where + a sleep reliably overshoots (an oversubscribed VM), the tick is spun + rather than slept past; where sleep is accurate, the window stays at + the configured threshold. """ def __init__( @@ -725,11 +736,13 @@ def __init__( stats_interval: Compute stats every N loops (default 50 = 5Hz at 250Hz loop). """ self._interval = interval_s - self._busy_threshold = ( + self._base_threshold = ( busy_threshold_s if busy_threshold_s is not None else cfg.BUSY_THRESHOLD_MS / 1000.0 ) + self._busy_threshold = self._base_threshold + self._sleep_late = 0.0 self._stats_interval = stats_interval self._next_deadline = 0.0 self._prev_t = 0.0 @@ -771,7 +784,16 @@ def wait_for_next_tick(self) -> None: if sleep_time > self._busy_threshold: # leave headroom for the busy-loop so the sleep can't overshoot - time.sleep(sleep_time - self._busy_threshold) + requested = sleep_time - self._busy_threshold + slept_from = time.perf_counter() + time.sleep(requested) + late = time.perf_counter() - slept_from - requested + self._sleep_late += _LATE_GAIN * (max(late, 0.0) - self._sleep_late) + else: + self._sleep_late *= _LATE_FADE + self._busy_threshold = min( + self._interval, self._base_threshold + 2.0 * self._sleep_late + ) if sleep_time > 0: # busy-loop the remainder for precise timing without OS jitter diff --git a/tests/integration/test_planned_paths.py b/tests/integration/test_planned_paths.py index b8d6f69..e7414ab 100644 --- a/tests/integration/test_planned_paths.py +++ b/tests/integration/test_planned_paths.py @@ -11,7 +11,6 @@ import math import threading -import time import numpy as np import pytest @@ -65,9 +64,8 @@ def _wire_rotation(pose: list[float]) -> np.ndarray: class _TcpSampler: - """Samples the TCP transform from ``status()`` on a background thread; - ``positions`` drops the repeats the status cache serves between its - updates.""" + """Records the TCP transform of every published status frame on a + background thread; ``positions`` drops repeats.""" def __init__(self, client): self._client = client @@ -83,14 +81,15 @@ def __exit__(self, *exc): self._done.set() self._thread.join(timeout=2.0) + def _record(self, status) -> bool: + self.frames.append(np.array(status.pose, dtype=np.float64).reshape(4, 4)) + return self._done.is_set() + def _run(self): + # Woken by each frame's arrival: a polling sleep can wake late enough + # on a loaded machine to skip whole corners of a fast path. while not self._done.is_set(): - status = self._client.status() - if status is not None: - self.frames.append( - np.asarray(status.pose, dtype=np.float64).reshape(4, 4) - ) - time.sleep(0.02) + self._client.wait_status(self._record, timeout=1.0) def positions(self) -> np.ndarray: pts = [f[:3, 3] for f in self.frames] diff --git a/tests/unit/test_loop_timer_late_sleep.py b/tests/unit/test_loop_timer_late_sleep.py new file mode 100644 index 0000000..de99b02 --- /dev/null +++ b/tests/unit/test_loop_timer_late_sleep.py @@ -0,0 +1,43 @@ +"""The control loop keeps its ticks on a machine whose sleeps wake late.""" + +from parol6.server import loop_timer +from parol6.server.loop_timer import LoopTimer + + +class _Clock: + """A clock the timer reads and sleeps on: each read moves it a + microsecond, and a sleep lasts what it was asked plus ``late``.""" + + def __init__(self, late: float) -> None: + self.now = 0.0 + self.late = late + + def perf_counter(self) -> float: + self.now += 1e-6 + return self.now + + def sleep(self, seconds: float) -> None: + self.now += seconds + self.late + + +def _run(monkeypatch, late: float, ticks: int) -> LoopTimer: + clock = _Clock(late) + monkeypatch.setattr(loop_timer.time, "perf_counter", clock.perf_counter) + monkeypatch.setattr(loop_timer.time, "sleep", clock.sleep) + timer = LoopTimer(0.01, busy_threshold_s=0.001) + timer.start() + for _ in range(ticks): + timer.wait_for_next_tick() + return timer + + +def test_sleeps_that_wake_late_are_spun_through(monkeypatch): + # An oversubscribed VM: every sleep wakes 20 ms after it was asked to. + timer = _run(monkeypatch, late=0.02, ticks=500) + assert timer.metrics.overrun_count <= 25, timer.metrics.overrun_count + + +def test_accurate_sleep_keeps_the_configured_busy_window(monkeypatch): + timer = _run(monkeypatch, late=0.0, ticks=500) + assert timer.metrics.overrun_count == 0 + assert timer._busy_threshold < 0.0011, timer._busy_threshold