feat: one acquisition thread owns the pipeline, requests wait for its frames

FrameAcquisition: a single thread calls wait_for_frames + align (the old
capture() body, now _grab_once) and keeps the latest frame, numbered.
capture() no longer touches the pipeline: it waits for the next frame of
that thread; requests waiting at the same moment get the same frame, so N
callers cost one grab. get_rgb()/get_depth() keep their 100 ms cache,
now on the thread's latest frame (_last_frame is gone).

The thread grabs on demand, only while someone waits for a newer frame:
grabbing continuously at 30 fps would align frames nobody reads, and align
on the Jetson CPU is the main suspect of the capture cost. A grab error is
logged and retried, the thread never dies. close() stops it before the
pipeline and joins it after (pipeline.stop() ends a wait in progress).
open/close and camera_open are unchanged, so the healthcheck + autoheal
recovery of ede70a2 sees the same state as before.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

src/backend/services/camera.py

@@ -30,10 +30,14 @@ import cv2
 import numpy as np
 import pyrealsense2 as rs
 
+from src.backend.services.frame_acquisition import FrameAcquisition
 from src.backend.services.phase_timing import timer
 
 logger = logging.getLogger(__name__)
 
+GRAB_TIMEOUT_MS = 5000   # wait_for_frames timeout of the acquisition thread (the old capture() default)
+STOP_JOIN_S = GRAB_TIMEOUT_MS / 1000.0 + 1.0   # close() waits this long for the thread to leave a grab
+
 
 @dataclass
 class CameraFrame:
@@ -110,7 +114,8 @@ class RealSenseD435:
         self._initialized = False
         self._open = False
         self._discovered: list[DiscoveredDevice] = []
-        self._last_frame: Optional[CameraFrame] = None
+        # The only caller of wait_for_frames once the camera is open (see capture()).
+        self._acquisition: Optional[FrameAcquisition[CameraFrame]] = None
         self._color_available: bool = True
         self._color_format = rs.format.bgr8
         self._usb_type: str = "unknown"
@@ -301,6 +306,7 @@ class RealSenseD435:
 
             self.device_index = device_index
             self._open = True
+            self._start_acquisition()
             logger.info(
                 f"RealSense D4xx opened: serial={serial} usb={usb_type} "
                 f"color={color_w}x{color_h}@{color_fps}({color_fmt}) "
@@ -315,11 +321,19 @@ class RealSenseD435:
     def close(self) -> None:
         if not self._open:
             return
-        self._teardown_pipeline()
+        acquisition, self._acquisition = self._acquisition, None
+        if acquisition is not None:
+            acquisition.request_stop()
+        self._teardown_pipeline()   # pipeline.stop() also ends a wait_for_frames in progress
+        if acquisition is not None:
+            acquisition.join(STOP_JOIN_S)
         self.device_index = None
-        self._last_frame = None
         logger.info("RealSense camera closed")
 
+    def _start_acquisition(self) -> None:
+        self._acquisition = FrameAcquisition(lambda: self._grab_once(GRAB_TIMEOUT_MS), name="realsense-acquisition")
+        self._acquisition.start()
+
     def _teardown_pipeline(self) -> None:
         if self._pipeline is not None:
             try:
@@ -375,18 +389,33 @@ class RealSenseD435:
     # Frame capture
     # ------------------------------------------------------------------
     def capture(self, timeout_ms: int = 5000) -> Optional[CameraFrame]:
-        if not self._open:
+        """The next frame acquired after this call (None on timeout or when closed).
+
+        Does not touch the pipeline: it waits for the acquisition thread.
+        Requests waiting at the same moment get the same frame.
+        """
+        acquisition = self._acquisition
+        if not self._open or acquisition is None:
+            return None
+        seq, _ = acquisition.latest()
+        _, frame = acquisition.wait_newer(seq, timeout_ms / 1000.0)
+        return frame
+
+    def _grab_once(self, timeout_ms: int) -> Optional[CameraFrame]:
+        """wait_for_frames + align + numpy conversion. Called by the acquisition thread only."""
+        pipeline, align = self._pipeline, self._align
+        if pipeline is None:
             return None
         try:
             with timer.phase("wait_frames"):
-                frames = self._pipeline.wait_for_frames(timeout_ms)
+                frames = pipeline.wait_for_frames(timeout_ms)
         except Exception as e:
             logger.warning(f"wait_for_frames failed: {e}")
             return None
 
-        if self._align is not None:
+        if align is not None:
             with timer.phase("align"):
-                frames = self._align.process(frames)
+                frames = align.process(frames)
 
         color_frame = frames.get_color_frame()
         depth_frame = frames.get_depth_frame()
@@ -394,9 +423,7 @@ class RealSenseD435:
             return None
 
         with timer.phase("convert"):
-            frame = self._frame_from(color_frame, depth_frame)
+            return self._frame_from(color_frame, depth_frame)
-        self._last_frame = frame
-        return frame
 
     def _frame_from(self, color_frame, depth_frame) -> CameraFrame:
         """numpy BGR + depth in mm from the SDK frames (YUYV decode, USB 2 resize, depth scale)."""
@@ -431,11 +458,12 @@ class RealSenseD435:
         return CameraFrame(rgb=rgb, depth=depth_mm, timestamp=time.time())
 
     def _cached_or_capture(self) -> Optional[CameraFrame]:
-        if (
+        """The latest frame if younger than frame_cache_ms, else the next one."""
-            self._last_frame is not None
+        acquisition = self._acquisition
-            and (time.time() - self._last_frame.timestamp) * 1000.0 < self._frame_cache_ms
+        if acquisition is not None:
-        ):
+            _, last = acquisition.latest()
-            return self._last_frame
+            if last is not None and (time.time() - last.timestamp) * 1000.0 < self._frame_cache_ms:
+                return last
         return self.capture()
 
     def get_rgb(self) -> Optional[np.ndarray]:

src/backend/services/frame_acquisition.py

@@ -0,0 +1,105 @@
+"""One thread owns the camera pipeline: it grabs frames on demand and keeps the latest one.
+
+Requests never call the camera. A request that needs a new frame waits for
+`seq > n`; the thread grabs ONE frame (wait_for_frames + align) and every
+request waiting at that moment gets the same frame. The thread grabs only
+when someone waits: grabbing continuously at the camera rate would align
+frames nobody reads, and align on the Jetson CPU is the main suspect of the
+capture cost (optimize-streams R01).
+
+The thread never dies: an exception from `grab` is logged and it goes on.
+"""
+
+import logging
+import threading
+from typing import Callable, Generic, Optional, TypeVar
+
+logger = logging.getLogger(__name__)
+
+T = TypeVar("T")
+
+ERROR_BACKOFF_S = 0.5   # pause after an unexpected grab error, so a broken SDK does not spin
+EMPTY_BACKOFF_S = 0.05  # pause after a grab that returned nothing (closed pipeline, missing stream)
+
+
+class FrameAcquisition(Generic[T]):
+    """Latest frame, numbered, produced by one background thread calling `grab`."""
+
+    def __init__(self, grab: Callable[[], Optional[T]], name: str = "frame-acquisition"):
+        self._grab = grab
+        self._name = name
+        self._cond = threading.Condition()
+        self._seq = 0                 # number of the latest frame (0 = none yet)
+        self._latest: Optional[T] = None
+        self._wanted = 0              # highest frame number someone is waiting for
+        self._stopping = False
+        self._thread: Optional[threading.Thread] = None
+
+    # Lifecycle -----------------------------------------------------------
+
+    def start(self) -> None:
+        self._stopping = False
+        self._thread = threading.Thread(target=self._run, name=self._name, daemon=True)
+        self._thread.start()
+
+    def request_stop(self) -> None:
+        """Ask the thread to end; waiters wake up with nothing."""
+        with self._cond:
+            self._stopping = True
+            self._cond.notify_all()
+
+    def join(self, timeout_s: float) -> bool:
+        """Wait for the thread to end; False if it is still running (stuck in `grab`)."""
+        if self._thread is None:
+            return True
+        self._thread.join(timeout_s)
+        alive = self._thread.is_alive()
+        if alive:
+            logger.warning(f"{self._name}: thread still running {timeout_s:.1f} s after stop")
+        return not alive
+
+    # Read side -------------------------------------------------------------
+
+    def latest(self) -> tuple[int, Optional[T]]:
+        """(number, frame) of the latest frame; (0, None) before the first one."""
+        with self._cond:
+            return self._seq, self._latest
+
+    def wait_newer(self, after_seq: int, timeout_s: float) -> tuple[int, Optional[T]]:
+        """Wait for a frame numbered above `after_seq`; (seq, None) on timeout or stop."""
+        with self._cond:
+            if self._seq <= after_seq:
+                self._wanted = max(self._wanted, after_seq + 1)
+                self._cond.notify_all()
+                self._cond.wait_for(lambda: self._seq > after_seq or self._stopping, timeout_s)
+            if self._seq > after_seq:
+                return self._seq, self._latest
+            return self._seq, None
+
+    # Thread ----------------------------------------------------------------
+
+    def _run(self) -> None:
+        while True:
+            with self._cond:
+                self._cond.wait_for(lambda: self._wanted > self._seq or self._stopping)
+                if self._stopping:
+                    return
+            frame = self._grab_safely()
+            if frame is None:
+                with self._cond:
+                    self._cond.wait_for(lambda: self._stopping, EMPTY_BACKOFF_S)
+                continue
+            with self._cond:
+                self._seq += 1
+                self._latest = frame
+                self._cond.notify_all()
+
+    def _grab_safely(self) -> Optional[T]:
+        try:
+            return self._grab()
+        except Exception:
+            # Logged with its traceback, then retried: the acquisition must outlive any SDK error.
+            logger.exception(f"{self._name}: grab failed")
+            with self._cond:
+                self._cond.wait_for(lambda: self._stopping, ERROR_BACKOFF_S)
+            return None

tests/test_frame_acquisition.py

@@ -0,0 +1,102 @@
+"""FrameAcquisition: one thread grabs on demand; waiters share frames; errors never kill it."""
+
+import threading
+import time
+
+import pytest
+
+from src.backend.services.frame_acquisition import FrameAcquisition
+
+LATENCY_S = 0.1
+
+
+class SlowGrab:
+    """Returns 1, 2, 3, ... after LATENCY_S each; optionally raises on chosen calls."""
+
+    def __init__(self, fail_on=()):
+        self.calls = 0
+        self.fail_on = set(fail_on)
+
+    def __call__(self):
+        self.calls += 1
+        time.sleep(LATENCY_S)
+        if self.calls in self.fail_on:
+            raise RuntimeError("SDK error")
+        return self.calls
+
+
+@pytest.fixture
+def acquisition():
+    made = []
+
+    def make(grab):
+        acq = FrameAcquisition(grab, name="test-acquisition")
+        acq.start()
+        made.append(acq)
+        return acq
+
+    yield make
+    for acq in made:
+        acq.request_stop()
+        assert acq.join(2.0)
+
+
+def test_three_waiters_share_one_grab(acquisition):
+    grab = SlowGrab()
+    acq = acquisition(grab)
+    results = []
+
+    def wait():
+        results.append(acq.wait_newer(0, timeout_s=2.0))
+
+    threads = [threading.Thread(target=wait) for _ in range(3)]
+    t0 = time.perf_counter()
+    for t in threads:
+        t.start()
+    for t in threads:
+        t.join()
+    elapsed = time.perf_counter() - t0
+    assert results == [(1, 1)] * 3
+    assert grab.calls == 1
+    assert elapsed < 2 * LATENCY_S
+
+
+def test_no_grab_without_demand(acquisition):
+    grab = SlowGrab()
+    acquisition(grab)
+    time.sleep(3 * LATENCY_S)
+    assert grab.calls == 0
+
+
+def test_an_exception_in_grab_does_not_stop_the_thread(acquisition):
+    grab = SlowGrab(fail_on={1})
+    acq = acquisition(grab)
+    seq, frame = acq.wait_newer(0, timeout_s=3.0)
+    assert (seq, frame) == (1, 2)   # call 1 raised, call 2 delivered frame number 1
+
+
+def test_timeout_returns_no_frame():
+    grab = SlowGrab()
+    acq = FrameAcquisition(grab)   # never started: nothing will arrive
+    assert acq.wait_newer(0, timeout_s=0.05) == (0, None)
+
+
+def test_stop_wakes_waiters_and_ends_the_thread():
+    acq = FrameAcquisition(lambda: None)   # a grab that never delivers
+    acq.start()
+    got = []
+    waiter = threading.Thread(target=lambda: got.append(acq.wait_newer(0, timeout_s=5.0)))
+    waiter.start()
+    time.sleep(0.1)
+    acq.request_stop()
+    waiter.join(1.0)
+    assert got == [(0, None)]
+    assert acq.join(1.0)
+
+
+def test_latest_is_the_last_delivered_frame(acquisition):
+    acq = acquisition(SlowGrab())
+    assert acq.latest() == (0, None)
+    acq.wait_newer(0, timeout_s=2.0)
+    acq.wait_newer(1, timeout_s=2.0)
+    assert acq.latest() == (2, 2)