Spin through the tick where sleeps wake late

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

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]

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