feat: SLAM streaming, recording, and playback (issues #78, #82)

.gitignore

@@ -57,6 +57,11 @@ Thumbs.db
 credentials.json
 id_rsa
 
-
-mavlink-router/
-log
+
+mavlink-router/
+log
+
+*.swp
+
+# Local deep-dive docs (personal study notes)
+docs_deep_dive/

CLAUDE.md

@@ -0,0 +1,72 @@
+# CLAUDE.md
+
+This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
+
+## Setup
+
+```bash
+git submodule update --init --recursive   # clones src/modules/mavctl only
+pip install -r requirements.txt
+```
+
+`dep/labeller` is NOT a registered submodule — that directory is empty. Do not import from `dep.labeller`; use `src.modules.imaging.detector` instead.
+
+## Commands
+
+```bash
+./scripts/lint.sh          # flake8, max-line-length 140
+./scripts/fmt.sh           # yapf formatter
+./scripts/type.sh          # mypy on src/modules/imaging/ and test/
+./scripts/test.sh          # pytest (uses PYTHONPATH=".:dep/labeller")
+
+# Run tests directly (preferred locally — avoids test_battery.py crash):
+python -m pytest test/ --ignore=test/test_battery.py -v
+
+# Run a single test:
+python -m pytest test/test_analysis.py::test_analysis_subscriber -v
+
+# Run a sample script:
+samples/run.sh aruco_stream
+```
+
+CI runs lint and typecheck on every push; tests only run on push/PR to `main`.
+
+## Architecture
+
+Shepard runs on a Raspberry Pi or ODroid connected to a PixHawk via MAVLink. It serves an aiohttp web server that the Emu ground station connects to.
+
+### Data flow
+
+```
+Camera (RPiCamera / OakdCamera / DebugCamera)
+  └─► SharedFrameCamera          # single capture thread, shared frame
+        ├─► VideoEmuStreamer      # encodes JPEG, pushes to Emu /video endpoint
+        └─► ImageAnalysisDelegate # runs detector in background thread
+              └─► BaseDetector.predict() → BoundingBox
+                    └─► get_object_location() → (x, y) direction vector
+                          └─► subscribers(image, bounding_box, pos)
+```
+
+### Key abstractions
+
+- **`CameraProvider`** (`camera.py`): base class for all cameras. `capture()` returns `PIL.Image`. `SharedFrameCamera` wraps any provider and serves the latest frame to multiple consumers thread-safely.
+- **`BaseDetector`** (`detector.py`): single method `predict(image) → Optional[BoundingBox]`. Implementations: `ArucoDetector` (cv2 DICT_4X4_50), `IrDetector` (brightness threshold), `BucketDetector` (YOLO via ultralytics).
+- **`ImageAnalysisDelegate`** (`analysis.py`): runs `camera.capture() → detector.predict()` in a loop in a background thread. Subscribers receive `(image, bounding_box, pos)` — `pos` is currently a raw direction vector in meters (not lon/lat; `XY_To_LonLat` call is commented out in `inference_georeference.py`).
+- **`Emu`** (`emu/emu.py`): aiohttp server. Routes: `/ws` (WebSocket for telemetry/commands), `/images/<path>` (static), `/video` (latest JPEG frame). `send_video_frame(jpeg_bytes)` updates the frame; `VideoEmuStreamer` calls this from a background thread.
+- **`VideoEmuStreamer`** (`video_emu_stream.py`): pulls frames from `SharedFrameCamera.capture()`, encodes to JPEG, calls `emu.send_video_frame()`. Frame rate set by `fps` param.
+- **`Navigator`** (`autopilot/navigator.py`): wraps DroneKit vehicle for flight control via MAVLink.
+- **`LocationProvider`** / **`MAVLinkDelegate`** (`location.py`, `mavlink.py`): provide GPS, heading, altitude, orientation from MAVLink messages.
+
+### Subscriber pattern
+
+`ImageAnalysisDelegate.subscribe(callback)` — callback signature:
+```python
+def on_detection(image: Image, bounding_box: Optional[BoundingBox], pos: Optional[tuple[float, float]]):
+    ...
+```
+
+### Testing without hardware
+
+Use `DebugCamera("res/test-image.jpeg")` and `DebugLocationProvider()`. Run `samples/aruco_stream.py` then open `http://localhost:8080/video` to verify streaming.
+
+`test_battery.py` is not a real test — it connects to a real drone at import time. Always `--ignore=test/test_battery.py` when running locally.

pytest.ini

@@ -0,0 +1,2 @@
+[pytest]
+testpaths = test

samples/aruco_stream.py

@@ -0,0 +1,52 @@
+import time
+
+from src.modules.emu import Emu
+from src.modules.imaging.detector import ArucoDetector
+from src.modules.imaging.camera import DebugCamera
+from src.modules.imaging.location import DebugLocationProvider
+from src.modules.imaging.analysis import ImageAnalysisDelegate
+from src.modules.imaging.camera import SharedFrameCamera
+from src.modules.imaging.video_emu_stream import VideoEmuStreamer
+
+
+def on_detection(image, bounding_box, position):
+    if bounding_box is not None:
+        print(f"ArUco detected at position {position}")
+
+
+def main():
+    emu = Emu("tmp")
+    emu.start_comms()
+    time.sleep(1)
+
+    # Base camera — swap DebugCamera for RPiCamera/OakdCamera on real hardware
+    base_camera = DebugCamera("res/test-image.jpeg")
+
+    # SharedFrameCamera captures at 15fps; both video stream and analysis read from it
+    shared_cam = SharedFrameCamera(base_camera, fps=15)
+    shared_cam.start()
+
+    # Video stream → EMU /video endpoint (browser polls: <img src="http://HOST:8080/video">)
+    video_streamer = VideoEmuStreamer(emu, shared_cam, fps=15, quality=70)
+    video_streamer.start()
+
+    detector = ArucoDetector()
+    location_provider = DebugLocationProvider()
+
+    analysis = ImageAnalysisDelegate(detector, shared_cam, location_provider)
+    analysis.subscribe(on_detection)
+    analysis.start()
+
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        analysis.stop()
+        video_streamer.stop()
+        shared_cam.stop()
+
+
+if __name__ == "__main__":
+    main()

samples/slam_stream.py

@@ -0,0 +1,30 @@
+"""
+Test SLAM streaming without any hardware.
+Run this, open Emu, go to SLAM tab — you'll see a fake growing point cloud.
+"""
+import time
+
+from src.modules.emu import Emu
+from src.modules.slam.debug_slam_provider import DebugSLAMProvider
+from src.modules.slam.slam_emu_streamer import SLAMEmuStreamer
+
+
+def main():
+    emu = Emu("tmp")
+    emu.start_comms()
+    time.sleep(1)
+
+    streamer = SLAMEmuStreamer(emu, DebugSLAMProvider())
+    emu.register_slam_streamer(streamer)
+
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        streamer.stop()
+
+
+if __name__ == "__main__":
+    main()

samples/slam_stream_real.py

@@ -0,0 +1,57 @@
+"""
+Real SLAM stream using OAK-D depth camera + XM125 altimeter + MAVLink pose.
+Run on the Pi/ODroid with hardware connected.
+"""
+import time
+
+from src.modules.autopilot.altimeter_xm125 import XM125
+from src.modules.emu import Emu
+from src.modules.imaging.camera import OakdCamera
+from src.modules.imaging.location import MAVLinkLocationProvider
+from src.modules.imaging.mavlink import MAVLinkDelegate
+from src.modules.slam.oakd_altimeter_slam_provider import OakdAltimeterSLAMProvider
+from src.modules.slam.slam_emu_streamer import SLAMEmuStreamer
+
+MAVLINK_CONNECTION = "udp:127.0.0.1:14550"
+
+
+def main():
+    # MAVLink for drone pose
+    mavlink = MAVLinkDelegate(MAVLINK_CONNECTION)
+    location_provider = MAVLinkLocationProvider(mavlink)
+
+    # XM125 radar altimeter (I2C bus 1, default address 0x52)
+    altimeter = XM125()
+    if not altimeter.begin():
+        print("XM125 init failed — check wiring")
+        return
+
+    # OAK-D depth camera
+    oakd = OakdCamera(fps=10)
+    oakd.start()
+
+    # SLAM provider fuses the two
+    slam_provider = OakdAltimeterSLAMProvider(oakd, altimeter, location_provider)
+
+    # Emu server + streamer
+    emu = Emu("tmp")
+    emu.start_comms()
+    time.sleep(1)
+
+    streamer = SLAMEmuStreamer(emu, slam_provider)
+    emu.register_slam_streamer(streamer)
+
+    print("Ready — open Emu and go to the SLAM tab")
+
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        streamer.stop()
+        oakd.stop()
+
+
+if __name__ == "__main__":
+    main()

src/modules/emu/emu.py

@@ -2,9 +2,8 @@
 import queue
 
 import asyncio
-from typing import Callable, List
+from typing import Callable, List, Optional
 
-from aiohttp import web
 from aiohttp import web
 import aiohttp
 
@@ -28,6 +27,9 @@ def __init__(self, img_dir: str):
         self._on_connect = lambda: None
         self._is_connected = False
 
+        self._latest_video_frame: Optional[bytes] = None
+        self._video_lock = threading.Lock()
+
     def start_comms(self):
         self._comms_thread = threading.Thread(target=self._start_comms_loop, daemon=True)
         self._comms_thread.start()
@@ -38,16 +40,14 @@ def send_image(self, path: str):
         the path sent should be accessable from within self.img_dir so it can be accessed through
         /images/{filename}
         """
-        print(path)
         img_url = "/images/" + path
-        print(img_url)
         content = {
             "type": "img",
             "value": img_url
         }
         self._send_queue.put(json.dumps(content))
 
-    def send_log(self, message: str, severity: str="normal"):
+    def send_log(self, message: str, severity: str = "normal"):
         """
         sends a log message to Emu
         message: string of flog
@@ -66,6 +66,14 @@ def send_msg(self, message: str):
         """
         self._send_queue.put(message)
 
+    def send_video_frame(self, jpeg_bytes: bytes):
+        """
+        Update the latest video frame served at /video.
+        Call this from a background thread with raw JPEG bytes.
+        """
+        with self._video_lock:
+            self._latest_video_frame = jpeg_bytes
+
     def set_on_connect(self, func: Callable):
         self._on_connect = func
 
@@ -75,19 +83,34 @@ def _start_comms_loop(self):
         """
         print("start_comms loop")
         self.app = web.Application()
-        self.app.add_routes([web.static('/images', self.img_dir),
-                             web.get('/ws', self.handle_websocket)])
+        self.app.add_routes([
+            web.static('/images', self.img_dir),
+            web.get('/ws', self.handle_websocket),
+            web.get('/video', self.handle_video_stream),
+        ])
 
         web.run_app(self.app, handle_signals=False)
 
     def subscribe(self, subscriber: Callable):
         self._subscribers.append(subscriber)
 
+    def register_slam_streamer(self, streamer) -> None:
+        def _handle(raw_msg: str) -> None:
+            try:
+                msg = json.loads(raw_msg)
+            except json.JSONDecodeError:
+                return
+            if msg.get("type") == "slam":
+                if msg.get("command") == "start":
+                    streamer.start()
+                elif msg.get("command") == "stop":
+                    streamer.stop()
+        self.subscribe(_handle)
+
     async def producer_handler(self, ws):
         """
         handles sending messages to the client
         """
-        event_loop = asyncio.get_running_loop()
         while not ws.closed:
             message = await asyncio.to_thread(self._send_queue.get)
 
@@ -101,7 +124,20 @@ async def consumer_handler(self, ws):
 
             elif msg.type == aiohttp.WSMsgType.ERROR:
                 print("WebSocket error:", ws.exception())
-
+
+    async def handle_video_stream(self, request):
+        """
+        Returns latest video frame as JPEG. Frame rate depends on how often
+        send_video_frame() is called (controlled by VideoEmuStreamer).
+        """
+        with self._video_lock:
+            frame = self._latest_video_frame
+
+        if frame is None:
+            raise web.HTTPNoContent()
+
+        return web.Response(body=frame, content_type='image/jpeg')
+
     async def handle_websocket(self, request):
         ws = web.WebSocketResponse()
         await ws.prepare(request)
@@ -122,5 +158,5 @@ async def handle_websocket(self, request):
 
         print('websocket connection closed')
         self._is_connected = False
-        
+
         return ws