haptic_skin.bridge

  1"""Bridge between a fix source (Street View browser, GPS, replay) and the necklace.
  2
  3Entry points:
  4  * :func:`serve` — async WebSocket server. The browser streams JSON fixes
  5    ``{lat, lon, heading}``; we feed them to a :class:`~haptic_skin.nav.Navigator`
  6    and drive the necklace, echoing the live motor vector + state back for the
  7    on-screen mirror. A 30 Hz applier loop decouples fix rate from pulse cadence.
  8  * :func:`replay` — synchronous, no deps: feed a route's own trace through the
  9    navigator with real timing. The indoor "no internet / no browser" fallback.
 10
 11The per-connection logic (:func:`handle_connection`) is module-level so it can
 12be tested with a fake websocket, without the ``websockets`` dependency.
 13"""
 14
 15from __future__ import annotations
 16
 17import asyncio
 18import json
 19import time
 20from typing import Optional
 21
 22from . import protocol
 23from .nav import Navigator
 24from .necklace import Necklace
 25from .routing import Route, route_from_waypoints, route_to_trace
 26
 27APPLIER_HZ = 30.0
 28TEST_STEP_S = 0.18      # dwell per motor during the self-test
 29
 30
 31# --------------------------------------------------------------------------
 32# Offline replay (sync)
 33# --------------------------------------------------------------------------
 34def replay(necklace: Necklace, route: Route, *, speed: float = 1.5,
 35           step_m: float = 3.0, sleep=time.sleep, clock=time.monotonic,
 36           on_state=None, on_frame=None) -> None:
 37    """Walk the route at ``speed`` m/s, driving the necklace. Blocking.
 38
 39    ``on_state(NavState)`` is called whenever the navigation *band* changes.
 40    ``on_frame(vec, NavState, t)`` is called on every applier tick with the live
 41    motor vector — used by the demo to render a per-frame compass view.
 42    """
 43    nav = Navigator(route)
 44    trace = route_to_trace(route, step_m=step_m)
 45    if not trace:
 46        return
 47    seg_time = step_m / max(0.1, speed)
 48    start = clock()
 49    last_band = None
 50    fix_i = 0
 51
 52    def _drive(now: float) -> None:
 53        vec = nav.vector(now)
 54        necklace.set_vector(vec)
 55        if on_frame:
 56            on_frame(vec, nav.state, now)
 57
 58    try:
 59        while fix_i < len(trace):
 60            now = clock()
 61            target_i = min(len(trace) - 1, int((now - start) / seg_time))
 62            while fix_i <= target_i:
 63                st = nav.update(trace[fix_i])
 64                if on_state and st.band != last_band:
 65                    on_state(st)
 66                    last_band = st.band
 67                fix_i += 1
 68            _drive(now)
 69            if nav.state.finished and fix_i >= len(trace):
 70                break
 71            sleep(1.0 / APPLIER_HZ)
 72        end = clock() + 1.5
 73        while clock() < end:
 74            _drive(clock())
 75            sleep(1.0 / APPLIER_HZ)
 76    finally:
 77        necklace.stop()
 78
 79
 80# --------------------------------------------------------------------------
 81# WebSocket connection logic (async, testable with a fake ws)
 82# --------------------------------------------------------------------------
 83def state_frame(state, vec) -> dict:
 84    """The JSON frame streamed to the browser (live motor mirror + nav state)."""
 85    return {
 86        "motors": list(vec),
 87        "band": state.band,
 88        "bucket": state.bucket,
 89        "distance_m": round(state.distance_m, 1),
 90        "instruction": state.instruction,
 91        "step": state.step_index,
 92        "total": state.total_steps,
 93        "finished": state.finished,
 94    }
 95
 96
 97async def send_json(ws, obj) -> bool:
 98    try:
 99        await ws.send(json.dumps(obj))
100        return True
101    except Exception:
102        return False
103
104
105async def motor_self_test(necklace: Necklace, ws, st: dict) -> None:
106    """Light each motor in turn — verifies the page -> bridge -> necklace chain."""
107    st["testing"] = True
108    n = protocol.NUM_MOTORS
109    for i in range(n):
110        vec = [0] * n
111        vec[i] = 200
112        necklace.set_vector(vec)
113        await send_json(ws, {"motors": vec, "band": "test", "bucket": i,
114                             "distance_m": 0, "instruction": f"Test moteur {i}",
115                             "step": 0, "total": 0, "finished": False})
116        await asyncio.sleep(TEST_STEP_S)
117    necklace.set_vector([0] * n)
118    await send_json(ws, {"motors": [0] * n, "band": "silent", "bucket": 0,
119                         "distance_m": 0, "instruction": "Test OK",
120                         "step": 0, "total": 0, "finished": False})
121    st["testing"] = False
122
123
124async def _applier(necklace: Necklace, ws, st: dict, stop: asyncio.Event) -> None:
125    """Single sender: drive the necklace AND stream the live frame to the browser."""
126    loop = asyncio.get_running_loop()
127    t0 = loop.time()
128    while not stop.is_set():
129        nav = st["nav"]
130        if nav is not None and not st["testing"]:
131            vec = nav.vector(loop.time() - t0)
132            necklace.set_vector(vec)
133            if not await send_json(ws, state_frame(nav.state, vec)):
134                break
135        await asyncio.sleep(1.0 / APPLIER_HZ)
136
137
138async def handle_connection(necklace: Necklace, ws, route: Optional[Route] = None) -> None:
139    """Handle one client: messages drive the navigator, the applier streams back."""
140    st = {"nav": Navigator(route) if route else None, "testing": False}
141    stop = asyncio.Event()
142    applier_task = asyncio.create_task(_applier(necklace, ws, st, stop))
143    try:
144        async for message in ws:
145            msg = json.loads(message)
146            if msg.get("test"):                        # motor self-test
147                await motor_self_test(necklace, ws, st)
148            elif "waypoints" in msg:                   # route handshake
149                st["nav"] = Navigator(route_from_waypoints(msg))
150            elif st["nav"] is not None:                # a fix
151                st["nav"].update(msg)
152    finally:
153        stop.set()
154        applier_task.cancel()
155        necklace.stop()
156
157
158async def serve(necklace: Necklace, route: Optional[Route] = None, *,
159                host: str = "localhost", port: int = 8765) -> None:
160    """Run the WebSocket bridge until cancelled (Ctrl-C)."""
161    import logging
162
163    try:
164        import websockets  # type: ignore
165    except ImportError as exc:  # pragma: no cover
166        raise RuntimeError("`pip install websockets` to run the bridge") from exc
167
168    # hush noisy tracebacks from non-WebSocket clients hitting the port
169    logging.getLogger("websockets.server").setLevel(logging.CRITICAL)
170
171    async def handler(ws):
172        await handle_connection(necklace, ws, route)
173
174    where = f"{len(route)} waypoints" if route else "route supplied by browser"
175    print(f"bridge listening on ws://{host}:{port}  ({where})")
176    async with websockets.serve(handler, host, port):
177        await asyncio.Future()  # run forever