world clock C5: SimThread, NativeSimThread and the demo pacer follow a WorldClock

grl_snam/clock.py

@@ -46,6 +46,7 @@
 from __future__ import annotations
 
 import math
+import threading
 from dataclasses import dataclass
 
 # Ceiling on the world delta one advance() may bank; past this a caller passed
@@ -115,6 +116,9 @@ def __init__(
         if mode not in MODES:
             raise ValueError(f"world clock: mode must be one of {MODES}")
         self._mode = mode
+        # Notified whenever the tick moves, so a follower thread can sleep until
+        # the driver advances the clock (wait_for_tick_change).
+        self._tick_changed = threading.Condition()
         self._apply_config(
             fixed_dt, scale, max_steps_per_advance, lockstep_steps, cap_scales_with_rate
         )
@@ -144,6 +148,7 @@ def _apply_config(
         self._accumulator = 0.0
         self._dropped = 0
         self._clear_measurement()
+        self._ticked()
 
     def configure(
         self,
@@ -183,11 +188,18 @@ def _note_wall(self, wall_dt: float) -> None:
             self._win_wall = 0.0
             self._win_world = 0.0
 
+    def _ticked(self) -> None:
+        """Wake every thread in :meth:`wait_for_tick_change`."""
+        with self._tick_changed:
+            self._tick_changed.notify_all()
+
     def _run(self, n: int) -> StepResult:
         """Run n quanta now: no cap, no bank."""
         first = self._tick
         self._tick += n
         self._win_world += n * self.fixed_dt
+        if n:
+            self._ticked()
         return StepResult(
             steps=n, alpha=_clamp_alpha(self._accumulator / self.fixed_dt), first_tick=first
         )
@@ -234,6 +246,8 @@ def _advance_live(self, world_dt: float) -> StepResult:
         self._accumulator = remainder
         self._dropped += dropped
         self._win_world += run * self.fixed_dt
+        if run:
+            self._ticked()
         return StepResult(
             steps=run,
             alpha=_clamp_alpha(self._accumulator / self.fixed_dt),
@@ -267,6 +281,15 @@ def seek_tick(self, tick: int) -> None:
         if tick < 0:
             raise ValueError("world clock: tick must be >= 0")
         self._tick = int(tick)
+        self._ticked()
+
+    def wait_for_tick_change(self, tick: int, timeout: float) -> int:
+        """Block until the tick differs from ``tick`` or ``timeout`` seconds
+        pass, and return the current tick. A follower thread waits here for the
+        driver to move the clock, instead of polling."""
+        with self._tick_changed:
+            self._tick_changed.wait_for(lambda: self._tick != tick, timeout=max(0.0, timeout))
+            return self._tick
 
     def seek_time(self, t: float) -> None:
         """Seek to the quantum containing world time ``t``."""
@@ -340,6 +363,7 @@ def reset(self) -> None:
         self._accumulator = 0.0
         self._dropped = 0
         self._clear_measurement()
+        self._ticked()
 
 
 # ── following one clock at several rates (cvc/core/clock_follower.h) ──────────

grl_snam/demos/_common.py

@@ -18,6 +18,7 @@
 import os
 import sys
 
+from grl_snam.clock import ClockFollower, WorldClock, ticks_per_step
 from grl_snam.metrics import NavMetrics, hud_lines
 
 
@@ -186,30 +187,51 @@ def vehicle_box_mesh(length=4.6, width=2.0, height=1.6):
 
 
 class SimPacer:
-    """Wall-clock -> fixed-dt sim pacing — the Python twin of the native demos'
-    ``SimPacer`` (libcvc ``src/cvcGL/examples/nav_common.h``).
+    """Wall-clock -> fixed-dt sim pacing, on the shared world-clock mechanism.
 
-    Accumulate real seconds (times an optional ``speed``) and hand back the whole
-    number of fixed ``sim_dt`` ticks to run this frame, so the sim advances at a
-    fixed rate on ANY display rate. Stepping the navigator a fixed N times per
-    render frame instead made the same drive run faster the faster the window drew
-    (and much faster than real time on an unthrottled display) — this keeps world
-    time honest, exactly like the native cvc::nav demos' world clock."""
+    The demo's frame loop calls :meth:`ticks` once per frame with the wall delta.
+    That drives :attr:`clock` (a :class:`~grl_snam.clock.WorldClock`, the twin of
+    ``cvc::world_clock``) as its ONE driver and returns the navigator steps that
+    fell due: one per ``sim_dt``, through a :class:`~grl_snam.clock.ClockFollower`.
+    So the sim advances at a fixed rate on ANY display rate. (Stepping the
+    navigator a fixed N times per render frame instead made the same drive run
+    faster the faster the window drew.) It replaces a private accumulator that the
+    libcvc demos also used; they now follow ``app.world_clock()`` the same way.
+
+    The clock's quantum is the suite's 0.01 s when ``sim_dt`` is a whole number of
+    them (a 0.06 s navigator is 6 quanta), else ``sim_dt`` itself. A stall is
+    capped at ``cap`` navigator steps; the clock drops and counts the rest
+    (``clock.total_dropped()``) rather than bursting."""
+
+    SUITE_QUANTUM = 0.01
 
     def __init__(self):
-        self.carry = 0.0
+        self.clock: WorldClock | None = None
+        self._nav: ClockFollower | None = None
+        self._key = None
+
+    def _configure(self, sim_dt: float, cap: int) -> None:
+        try:
+            k = ticks_per_step(sim_dt, self.SUITE_QUANTUM)
+            quantum = self.SUITE_QUANTUM
+        except ValueError:
+            k, quantum = 1, sim_dt
+        self.clock = WorldClock(
+            fixed_dt=quantum,
+            max_steps_per_advance=max(1, int(cap)) * k,
+            cap_scales_with_rate=True,
+        )
+        self._nav = ClockFollower(k, 0, self.clock.tick())
+        self._key = (sim_dt, cap)
 
     def ticks(self, wall_dt: float, sim_dt: float, speed: float = 1.0, cap: int = 8) -> int:
-        if not (wall_dt > 0.0) or not (sim_dt > 0.0):
+        if not (sim_dt > 0.0):
             return 0
-        self.carry += wall_dt * speed
-        n = int(self.carry / sim_dt)
-        if n > cap:
-            n = cap  # a stall drops the backlog, never a tick burst that fast-forwards
-            self.carry = 0.0
-        else:
-            self.carry -= n * sim_dt
-        return n
+        if self._key != (sim_dt, cap):
+            self._configure(sim_dt, cap)
+        self.clock.set_scale(speed)
+        self.clock.advance(wall_dt)  # the one driver; a non-positive delta runs nothing
+        return self._nav.take(self.clock)
 
 
 class CameraDriver:

grl_snam/nav_native.py

@@ -685,18 +685,45 @@ def sim_world_from_swarm(sw, weights_path, *, truth, freeze_sense=False, sense_e
 
 
 HAS_SIM_THREAD = AVAILABLE and hasattr(_pycvc, "nav_sim_thread_create")
+# The pycvc build can make the thread FOLLOW a shared pycvc.world_clock.
+HAS_SIM_THREAD_CLOCK = AVAILABLE and hasattr(_pycvc, "nav_sim_thread_create_on_clock")
 
 
 class NativeSimThread:
     """Run a :class:`NativeSimWorld` off the render thread — the C++
     ``cvc::nav::sim_thread``. The worker is a real thread that never touches the
     GIL (it runs only cvc::nav kernels), so it advances genuinely concurrently
     with Python; ``read()`` is a lock-free snapshot the renderer never blocks
-    the sim to take."""
-
-    def __init__(self, sim_world, hz=60.0):
+    the sim to take.
+
+    Two forms. ``NativeSimThread(world, hz=60.0)`` paces itself: ``hz`` steps per
+    wall second on a private clock (60 Hz at 0.06 s is 3.6x real time).
+    ``NativeSimThread(world, clock=app.world_clock())`` FOLLOWS a shared
+    ``pycvc.world_clock``: one navigator step every ``veh.dt / fixed_dt`` quanta,
+    run as the caller's driver advances the clock (``world_clock_binding.drive``),
+    so speed, pause, single-step and lockstep are the clock's, shared with every
+    other simulation on it. ``veh.dt`` must be a whole number of the clock's
+    quanta (configure the clock at 0.01 s for a 0.06 s navigator)."""
+
+    def __init__(self, sim_world, hz=60.0, *, clock=None):
         self._sw = sim_world  # keep the NativeSimWorld (and its capsule) alive
-        self._h = _pycvc.nav_sim_thread_create(sim_world._h, float(hz))
+        self._clock = clock  # the C++ thread borrows it; keep it alive too
+        if clock is None:
+            self._h = _pycvc.nav_sim_thread_create(sim_world._h, float(hz))
+        else:
+            if not HAS_SIM_THREAD_CLOCK:
+                raise RuntimeError(
+                    "this pycvc build cannot follow a shared clock "
+                    "(it lacks nav_sim_thread_create_on_clock; libcvc #630)"
+                )
+            self._h = _pycvc.nav_sim_thread_create_on_clock(sim_world._h, clock)
+
+    def wait_caught_up(self, timeout=5.0):
+        """Block until the worker has run every step due up to the clock's
+        current tick (True), or ``timeout`` seconds pass (False). A capture calls
+        this after driving the clock and before :meth:`read`, so each frame holds
+        exactly the steps its quanta made due."""
+        return bool(_pycvc.nav_sim_thread_wait_caught_up(self._h, float(timeout)))
 
     def start(self):
         _pycvc.nav_sim_thread_start(self._h)