diff --git a/grl_snam/clock.py b/grl_snam/clock.py index 40709cf..e97dab1 100644 --- a/grl_snam/clock.py +++ b/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) ────────── diff --git a/grl_snam/demos/_common.py b/grl_snam/demos/_common.py index a53e57e..54699cf 100644 --- a/grl_snam/demos/_common.py +++ b/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: diff --git a/grl_snam/nav_native.py b/grl_snam/nav_native.py index 802b89a..24ffc01 100644 --- a/grl_snam/nav_native.py +++ b/grl_snam/nav_native.py @@ -685,6 +685,8 @@ 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: @@ -692,11 +694,36 @@ class NativeSimThread: ``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) diff --git a/grl_snam/sim_thread.py b/grl_snam/sim_thread.py index c69b679..0128049 100644 --- a/grl_snam/sim_thread.py +++ b/grl_snam/sim_thread.py @@ -8,7 +8,7 @@ │ 1 drain command queue (live edits, top-of-tick) │ │ 2 world.step() (GIL released inside kernels) │ │ 3 publish snapshot (one atomic reference store) │ - │ 4 pace to fixed dt (time.sleep -> releases the GIL) │ + │ 4 wait for the clock (tick wait / sleep: GIL released)│ └───────────────────────┬─────────────────────────────────┘ │ publish (ref swap, no lock) ┌────────▼─────────┐ read latest (ref load) @@ -36,6 +36,11 @@ drained at the *top* of the tick — before sensing — so a moved obstacle or a retargeted goal is sensed and reacted to on that very tick. +Time comes from a :class:`~grl_snam.clock.WorldClock`: the thread runs the +steps its clock's quanta make due (following a shared clock the frame loop +drives, or driving a private one at ``hz``), so the steps a run takes never +depend on how its passes were timed. + Fixed-sim / variable-render decoupling: the sim ticks at a fixed dt; a renderer reads the latest published frame at its own cadence and may interpolate between the two most-recent frames (``read_pair``). Rendering faster never drives the @@ -49,6 +54,7 @@ import time from dataclasses import dataclass +from .clock import ClockFollower, WorldClock, ticks_per_step from .swarm import Snapshot, Swarm @@ -109,34 +115,89 @@ def read_pair(self) -> tuple[Snapshot, Snapshot]: class SimThread(threading.Thread): - """Advance a :class:`~grl_snam.swarm.Swarm` at a fixed rate on a daemon - thread, publishing an immutable frame each tick. + """Advance a :class:`~grl_snam.swarm.Swarm` on a daemon thread, publishing an + immutable frame each step. Time comes from a :class:`~grl_snam.clock.WorldClock`. Usage:: - sim = SimThread(swarm, hz=60.0) + sim = SimThread(swarm, hz=60.0) # paces itself (a private clock) + sim = SimThread(swarm, clock=clock) # follows a shared clock sim.start() ... # render loop, elsewhere: + clock.advance(wall_dt) # (shared clock) the ONE driver, once per frame frame = sim.buffer.read() # lock-free, never blocks the sim sim.send(RetargetGoal(3, (120.0, 40.0))) # reacted to next tick sim.stop() + + Given a shared ``clock``, the thread FOLLOWS it like ``cvc::nav::sim_thread``: + one step every ``world.dt / clock.fixed_dt`` quanta, run as the driver + advances the clock, so speed, pause, single-step and lockstep are the clock's, + shared with every other simulation on it. ``world.dt`` must be a whole number + of the clock's quanta. Given only ``hz`` (the original form), the thread + drives a private clock whose quantum is ``world.dt`` at a speed of + ``hz * world.dt`` (60 Hz at 0.06 s is 3.6x real time). Either way the steps a + run takes are fixed by the clock's ticks, not by how this thread's passes + happen to be timed. """ - def __init__(self, world: Swarm, *, hz: float = 60.0, name: str = "sim"): + def __init__( + self, + world: Swarm, + *, + hz: float = 60.0, + clock: WorldClock | None = None, + drive_clock: bool = False, + phase: int = 0, + name: str = "sim", + ): super().__init__(name=name, daemon=True) self.world = world - self._period = 1.0 / float(hz) + dt = float(world.dt) + if clock is None: + # The hz form: a private clock, one quantum per step, driven here. A + # cap of 8 per advance keeps the old "fell behind -> no spiral of death" + # behaviour, now with the dropped quanta counted. + clock = WorldClock(fixed_dt=dt, scale=float(hz) * dt, max_steps_per_advance=8) + self._private = True + self._drive = True + else: + self._private = False + self._drive = bool(drive_clock) + self.clock = clock + self._follower = ClockFollower(ticks_per_step(dt, clock.fixed_dt), phase, clock.tick()) + self._seen_tick = clock.tick() # the clock tick this thread has caught up to self._run = threading.Event() self._run.set() self._paused = False - self._nudge = 0 self.cmdq: queue.SimpleQueue = queue.SimpleQueue() self.buffer = TripleBuffer(world.snapshot()) # Diagnostics (read-only from other threads; plain attributes). - self.ticks = 0 - self.behind = 0 # ticks that overran their dt budget + self.ticks = 0 # steps run + self.behind = 0 # quanta dropped by the clock this thread drives (0 when following) self._step_ms_ewma = 0.0 + def start(self) -> None: + # Anchor on the caller's thread, before the worker exists: steps that fall + # due from here on are run (none from before start() is replayed as a + # burst), and a driver advancing the clock right after start() cannot race + # the worker's first look at it. + self._follower.reset(self.clock.tick()) + self._seen_tick = self.clock.tick() + super().start() + + def wait_caught_up(self, timeout: float = 5.0) -> bool: + """Block until this thread has run and published every step due up to the + clock's current tick (True), or ``timeout`` seconds pass (False). A + capture calls it after driving the clock and before reading, so each + frame holds exactly the steps its quanta made due.""" + target = self.clock.tick() + deadline = time.perf_counter() + max(0.0, float(timeout)) + while self._seen_tick < target: + if not self.is_alive() or time.perf_counter() >= deadline: + return False + time.sleep(0.0005) + return True + # ── producer API (any thread) ──────────────────────────────────────────── def send(self, cmd) -> None: """Enqueue a live-scene command; applied at the top of the next tick.""" @@ -151,33 +212,54 @@ def stop(self, join: bool = True, timeout: float = 2.0) -> None: # ── the loop ───────────────────────────────────────────────────────────── def run(self) -> None: prev = self.world.snapshot() - next_t = time.perf_counter() + k = self._follower.period + last = time.perf_counter() while self._run.is_set(): self._drain_commands() # top-of-tick: live edits sensed THIS tick - if self._paused and self._nudge <= 0: - time.sleep(0.002) # idle without spinning; GIL free for renderer - next_t = time.perf_counter() - continue - if self._nudge > 0: - self._nudge -= 1 - - t0 = time.perf_counter() - self.world.step() # GIL released across kernels + torch ATen ops - step_ms = (time.perf_counter() - t0) * 1000.0 - self._step_ms_ewma = 0.9 * self._step_ms_ewma + 0.1 * step_ms - - curr = self.world.snapshot() - self.buffer.publish(prev, curr) # one atomic ref store; no lock - prev = curr - self.ticks += 1 - - next_t += self._period - dt = next_t - time.perf_counter() - if dt > 0: - time.sleep(dt) # release the GIL; renderer runs free + + if self._drive: + # The one driver of this clock: advance it by the wall time since + # the last pass. What runs is decided by the clock, never by this + # loop's timing, so a slow pass changes how many steps run now, + # not what they compute. + now = time.perf_counter() + r = self.clock.advance(now - last) + last = now + self.behind += r.dropped_steps + + now_tick = self.clock.tick() + due = self._follower.take(now_tick) + if self._paused: + due = 0 # this simulation alone is paused: due steps are skipped, not banked + for _ in range(due): + if not self._run.is_set(): + break + self._drain_commands() + t0 = time.perf_counter() + self.world.step() # GIL released across kernels + torch ATen ops + step_ms = (time.perf_counter() - t0) * 1000.0 + self._step_ms_ewma = 0.9 * self._step_ms_ewma + 0.1 * step_ms + curr = self.world.snapshot() + self.buffer.publish(prev, curr) # one atomic ref store; no lock + prev = curr + self.ticks += 1 + self._seen_tick = now_tick # everything due up to now_tick is published + + if self._drive: + # Sleep until about when the next step falls due in wall time + # (releasing the GIL for the renderer), bounded both ways so a + # pause or a speed change is noticed promptly. + wait = 0.002 + scale = self.clock.scale + if not self._paused and self.clock.mode == "live" and scale > 0.0: + since = (self.clock.tick() + k - self._follower.phase) % k + world_left = (k - since) * self.clock.fixed_dt - self.clock.pending_seconds() + wait = world_left / scale + time.sleep(min(max(wait, 0.0002), 0.02)) else: - self.behind += 1 - next_t = time.perf_counter() # fell behind -> no spiral of death + # Follow: wake when the driver moves the clock (or after 20 ms, to + # notice stop() and queued commands). + self.clock.wait_for_tick_change(now_tick, 0.02) # ── command handling ───────────────────────────────────────────────────── def _drain_commands(self) -> None: @@ -191,11 +273,23 @@ def _drain_commands(self) -> None: elif isinstance(cmd, MoveObstacle): self.world.add_obstacle(*cmd.rect) elif isinstance(cmd, SetRate): - self._period = 1.0 / max(1e-3, float(cmd.hz)) + # The private clock's speed; a shared clock's speed is its own control. + if self._private: + self.clock.set_scale(max(1e-3, float(cmd.hz)) * self.clock.fixed_dt) elif isinstance(cmd, Pause): - self._paused = bool(cmd.paused) + if self._private: + # Pause the private clock itself, so a Nudge can still step it. + self.clock.set_mode("paused" if cmd.paused else "live") + else: + # A shared clock is left alone (pausing everything is the clock's + # own control); this simulation's due steps are skipped instead. + self._paused = bool(cmd.paused) elif isinstance(cmd, Nudge): - self._nudge += int(cmd.steps) + # Steps through the clock, so world time and the steps run agree. On + # a shared clock the driver owns the steps (world_clock_binding's + # sim.step), so a nudge is ignored there. + if self._private: + self.clock.advance_ticks(max(0, int(cmd.steps)) * self._follower.period) elif isinstance(cmd, Stop): self._run.clear() return diff --git a/tests/test_clock.py b/tests/test_clock.py index 9c67fe3..dd6c266 100644 --- a/tests/test_clock.py +++ b/tests/test_clock.py @@ -9,6 +9,8 @@ from __future__ import annotations import math +import threading +import time import pytest @@ -165,6 +167,20 @@ def step(c): assert trace[:n] == live[:n] +def test_a_follower_thread_wakes_when_the_driver_moves_the_clock(): + c = WorldClock(fixed_dt=0.01) + assert c.wait_for_tick_change(0, 0.01) == 0, "no driver: times out at the same tick" + seen = [] + t = threading.Thread(target=lambda: seen.append(c.wait_for_tick_change(0, 5.0))) + t.start() + time.sleep(0.02) + c.advance_ticks(3) + t.join(5.0) + assert seen == [3] + c.reset() + assert c.wait_for_tick_change(3, 0.0) == 0, "a rewind counts as a change" + + def test_parity_with_the_native_clock(): pycvc = pytest.importorskip("pycvc") if not hasattr(pycvc, "clock_follower"): diff --git a/tests/test_sim_thread.py b/tests/test_sim_thread.py index 865bf85..8ef8ede 100644 --- a/tests/test_sim_thread.py +++ b/tests/test_sim_thread.py @@ -114,3 +114,29 @@ def test_sim_thread_pause_resume(tmp_path): assert sim.ticks > a, "resumed sim must advance" finally: sim.stop() + + +@pytest.mark.skipif( + not nav_native.HAS_SIM_THREAD_CLOCK, reason="pycvc build lacks nav_sim_thread_create_on_clock" +) +def test_sim_thread_follows_a_shared_pycvc_clock(tmp_path): + """The C++ worker follows app.world_clock(): one step per veh.dt / fixed_dt + quanta, as the Python driver advances the clock; a capture that waits for it + sees exactly each frame's steps.""" + import pycvc + + cw, sw = _native_world(tmp_path, n=200) + app = pycvc.App() + clock = app.world_clock() + clock.set_config(sw.dt / 3.0, 1.0, 8, 2, False) # 3 quanta per navigator step + clock.set_mode_name("lockstep") # 2 quanta per advance + sim = nav_native.NativeSimThread(cw, clock=clock) + sim.start() + try: + for frame in range(1, 13): + clock.advance(0.0) + assert sim.wait_caught_up(5.0), f"frame {frame}" + assert sim.ticks == clock.tick() // 3, f"frame {frame}" + assert sim.read()[5] == sim.ticks, f"frame {frame}" + finally: + sim.stop() diff --git a/tests/test_swarm.py b/tests/test_swarm.py index a3dcb10..df5463a 100644 --- a/tests/test_swarm.py +++ b/tests/test_swarm.py @@ -22,6 +22,7 @@ import grl_snam.sdf_nav as sdf_nav # noqa: E402 from grl_snam import planner # noqa: E402 +from grl_snam.clock import WorldClock # noqa: E402 from grl_snam.fog_stories import STORIES, shrunk # noqa: E402 from grl_snam.metrics import NavStats # noqa: E402 from grl_snam.nav import SdfNavigator # noqa: E402 @@ -343,6 +344,34 @@ def test_sim_thread_runs_concurrently_and_reacts_to_commands(): assert not sim.is_alive(), "sim thread must shut down cleanly" +def test_sim_thread_follows_a_shared_clock(): + """Given a shared WorldClock, the thread runs exactly the steps the driver's + quanta make due (one per world.dt / fixed_dt), and a capture that waits for it + sees every frame's steps, however the worker is scheduled.""" + story = _story(96) + truth = story.truth_grid() + specs = _free_specs(story, truth, 16, seed=3) + sw = Swarm(story, specs, model=_model(), truth_occ=truth, prior_occ=truth, sense_every=4) + sw._sense_shared = lambda: None + + clock = WorldClock(fixed_dt=sw.dt / 3.0, mode="lockstep", lockstep_steps=2) + sim = SimThread(sw, clock=clock) + sim.start() + try: + for frame in range(1, 13): # 2 quanta a frame: a step every 1.5 frames + clock.advance(0.0) + assert sim.wait_caught_up(5.0), f"frame {frame}" + assert sim.ticks == clock.tick() // 3, f"frame {frame}" + assert sim.buffer.read().tick == sim.ticks, f"frame {frame}" + clock.set_mode("paused") + clock.advance(1.0) + assert sim.wait_caught_up(5.0) + assert sim.ticks == 8, "a paused clock runs nothing" + finally: + sim.stop() + assert not sim.is_alive() + + # ── grouped belief: shared / clustered / private via map_id ──────────────────