feat(icp): LiDAR TCP socket reader with reconnect

data_only_viz/lidar_receiver.py

@@ -42,3 +42,89 @@ def decode_frame(body: bytes) -> LidarFrame:
     raw = body[_HEADER.size : expected]
     pts = np.frombuffer(raw, dtype="<f4").reshape(vertex_count, 3).astype(np.float32, copy=True)
     return LidarFrame(timestamp_ns=int(timestamp_ns), points=pts)
+
+
+import logging
+import socket
+import threading
+from typing import Optional
+
+_LOG = logging.getLogger(__name__)
+_LEN_PREFIX = struct.Struct(">I")
+
+
+class LidarTCPReader:
+    """Background TCP reader producing a single-slot latest-frame mailbox.
+
+    Reconnects on transient failures with linear backoff up to 5s.
+    """
+
+    def __init__(self, host: str, port: int, connect_timeout_s: float = 2.0) -> None:
+        self._host = host
+        self._port = port
+        self._connect_timeout_s = connect_timeout_s
+        self._stop = threading.Event()
+        self._lock = threading.Lock()
+        self._latest: Optional[LidarFrame] = None
+        self._thread: Optional[threading.Thread] = None
+
+    def start(self) -> None:
+        if self._thread is not None:
+            return
+        self._thread = threading.Thread(target=self._run, name="lidar-tcp", daemon=True)
+        self._thread.start()
+
+    def stop(self) -> None:
+        self._stop.set()
+        if self._thread is not None:
+            self._thread.join(timeout=2.0)
+            self._thread = None
+
+    def latest(self) -> Optional[LidarFrame]:
+        with self._lock:
+            return self._latest
+
+    def _run(self) -> None:
+        backoff_s = 0.5
+        while not self._stop.is_set():
+            try:
+                with socket.create_connection((self._host, self._port), timeout=self._connect_timeout_s) as sock:
+                    sock.settimeout(1.0)
+                    backoff_s = 0.5
+                    self._read_loop(sock)
+            except (OSError, ValueError) as exc:
+                _LOG.warning("lidar reader: %s; reconnecting in %.1fs", exc, backoff_s)
+                if self._stop.wait(backoff_s):
+                    return
+                backoff_s = min(backoff_s * 2.0, 5.0)
+
+    def _read_loop(self, sock: socket.socket) -> None:
+        while not self._stop.is_set():
+            header = self._recv_exact(sock, _LEN_PREFIX.size)
+            if header is None:
+                return
+            (length,) = _LEN_PREFIX.unpack(header)
+            if length <= 0 or length > 8_000_000:  # sanity cap: 8 MB per frame
+                raise ValueError(f"implausible frame length {length}")
+            body = self._recv_exact(sock, length)
+            if body is None:
+                return
+            frame = decode_frame(body)
+            with self._lock:
+                self._latest = frame
+
+    def _recv_exact(self, sock: socket.socket, n: int) -> Optional[bytes]:
+        buf = bytearray(n)
+        view = memoryview(buf)
+        got = 0
+        while got < n:
+            if self._stop.is_set():
+                return None
+            try:
+                k = sock.recv_into(view[got:])
+            except socket.timeout:
+                continue
+            if k == 0:
+                return None
+            got += k
+        return bytes(buf)

data_only_viz/tests/test_lidar_receiver.py

@@ -48,3 +48,64 @@ def test_decode_lidar_frame_rejects_zero_vertex_count() -> None:
     body = struct.pack(">Q", 0) + struct.pack(">I", 0)
     with pytest.raises(ValueError, match="vertex_count"):
         decode_frame(body)
+
+
+import socket
+import threading
+import time
+
+
+@pytest.fixture
+def unused_tcp_port() -> int:
+    """Bind to port 0 to grab a free port from the OS, then release it."""
+    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    s.bind(("127.0.0.1", 0))
+    port = s.getsockname()[1]
+    s.close()
+    return port
+
+
+def _serve_one_frame(port: int, frame_bytes: bytes) -> None:
+    srv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    srv.bind(("127.0.0.1", port))
+    srv.listen(1)
+    conn, _ = srv.accept()
+    conn.sendall(frame_bytes)
+    time.sleep(0.1)
+    conn.close()
+    srv.close()
+
+
+def test_reader_grabs_latest_frame(unused_tcp_port: int) -> None:
+    from data_only_viz.lidar_receiver import LidarTCPReader
+
+    pts = np.array([[1.0, 2.0, 3.0]], dtype=np.float32)
+    frame = _encode_frame(pts, timestamp_ns=42)
+    t = threading.Thread(target=_serve_one_frame, args=(unused_tcp_port, frame), daemon=True)
+    t.start()
+    time.sleep(0.05)
+
+    reader = LidarTCPReader(host="127.0.0.1", port=unused_tcp_port, connect_timeout_s=2.0)
+    reader.start()
+    deadline = time.monotonic() + 2.0
+    latest = None
+    while time.monotonic() < deadline:
+        latest = reader.latest()
+        if latest is not None:
+            break
+        time.sleep(0.02)
+    reader.stop()
+    t.join(timeout=1.0)
+
+    assert latest is not None
+    assert latest.timestamp_ns == 42
+    np.testing.assert_allclose(latest.points, pts, atol=1e-6)
+
+
+def test_reader_returns_none_before_first_frame(unused_tcp_port: int) -> None:
+    from data_only_viz.lidar_receiver import LidarTCPReader
+
+    reader = LidarTCPReader(host="127.0.0.1", port=unused_tcp_port, connect_timeout_s=0.05)
+    # Do not start it; latest() must be None.
+    assert reader.latest() is None