feat: continuous color acquisition without align, align only on request

Two threads while the camera is open:
- realsense-raw (continuous, FrameAcquisition(on_demand=False)): the only
  caller of wait_for_frames. For every frameset it publishes the color image
  (own BGR copy) with the camera timestamp, its domain and the host arrival
  time, and keeps the SDK frameset. No align and no depth conversion at the
  camera rate: per frame it costs wait + one color copy (YUYV decode on USB 2).
- realsense-align (on demand, as in R01): when someone asks for color+depth
  it aligns the newest frameset not aligned yet, converts the depth, copies.
  Concurrent callers share one align. capture(), the 100 ms cache of
  get_rgb/get_depth/cached_frame and every existing endpoint read this
  aligned frame, so /api/rgb stays the color of the same frame as /api/depth.
The aligned color reuses the raw color pixels (align maps depth onto color).
At most 3 SDK framesets are referenced at a time (latest, receiving,
aligning); ColorFrame and CameraFrame hold no SDK memory.
New timing phase color_convert. One commit for items 1 and 2 of the request:
once the continuous thread owns the pipeline, the aligned path must take
its framesets, so they cannot be split without a broken intermediate state.
color_frame(after_seq, timeout_ms) gives the fast channel to the API.

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

src/backend/services/camera.py

@@ -36,7 +36,8 @@ 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
+ALIGN_WAIT_S = GRAB_TIMEOUT_MS / 1000.0        # how long the align thread waits for a frameset it has not aligned yet
+STOP_JOIN_S = GRAB_TIMEOUT_MS / 1000.0 + 1.0   # close() waits this long for a thread to leave a grab
 
 
 @dataclass
@@ -47,6 +48,31 @@ class CameraFrame:
     timestamp: float
 
 
+@dataclass
+class ColorFrame:
+    """The color image of one frameset on its own, without align: the fast channel.
+
+    Holds no SDK memory, so clients may keep it as long as they like.
+    """
+    rgb: np.ndarray               # (H, W, 3) uint8 BGR, own memory
+    camera_timestamp_ms: float    # color_frame.get_timestamp(): the camera's clock
+    timestamp_domain: str         # its domain, e.g. "hardware_clock", "global_time"
+    host_timestamp: float         # time.time() when the frameset reached this process
+
+
+@dataclass
+class RawFrameset:
+    """What the continuous thread publishes: the color frame, plus the SDK frameset
+    kept for an align on request (its depth is not converted until then)."""
+    color: ColorFrame
+    frameset: object
+
+
+def _domain_name(domain) -> str:
+    """'timestamp_domain.hardware_clock' -> 'hardware_clock'."""
+    return str(domain).split(".")[-1]
+
+
 def _owned(array: np.ndarray) -> np.ndarray:
     """`array` if it owns its memory, else a copy (a view on an SDK buffer has a `base`)."""
     return array if array.base is None else array.copy()
@@ -119,8 +145,12 @@ class RealSenseD435:
         self._initialized = False
         self._open = False
         self._discovered: list[DiscoveredDevice] = []
-        # The only caller of wait_for_frames once the camera is open (see capture()).
-        self._acquisition: Optional[FrameAcquisition[CameraFrame]] = None
+        # Two threads while the camera is open (see _start_acquisition()):
+        # _raw, the only caller of wait_for_frames, publishes every frameset (fast color channel);
+        # _aligned aligns one of those framesets only when someone asks (capture(), cache).
+        self._raw: Optional[FrameAcquisition[RawFrameset]] = None
+        self._aligned: Optional[FrameAcquisition[CameraFrame]] = None
+        self._last_aligned_raw_seq = 0
         self._color_available: bool = True
         self._color_format = rs.format.bgr8
         self._usb_type: str = "unknown"
@@ -326,18 +356,31 @@ class RealSenseD435:
     def close(self) -> None:
         if not self._open:
             return
-        acquisition, self._acquisition = self._acquisition, None
-        if acquisition is not None:
-            acquisition.request_stop()
+        threads = [t for t in (self._aligned, self._raw) if t is not None]
+        self._aligned = self._raw = None
+        for thread in threads:
+            thread.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)
+        for thread in threads:
+            thread.join(STOP_JOIN_S)
         self.device_index = 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()
+        """Start the continuous color thread and the on-demand align thread.
+
+        SDK framesets referenced by this driver at any moment: at most 3 — the
+        latest published one, the one the continuous thread is receiving, and
+        the one being aligned (often the same as the latest). Older ones are
+        dropped as soon as a newer one is published, and nothing else keeps
+        SDK memory (ColorFrame and CameraFrame own their arrays), so the
+        SDK's frame pools are not exhausted by clients holding frames.
+        """
+        self._last_aligned_raw_seq = 0
+        self._raw = FrameAcquisition(lambda: self._grab_raw(GRAB_TIMEOUT_MS), name="realsense-raw", on_demand=False)
+        self._aligned = FrameAcquisition(self._align_newest, name="realsense-align")
+        self._raw.start()
+        self._aligned.start()
 
     def _teardown_pipeline(self) -> None:
         if self._pipeline is not None:
@@ -394,22 +437,40 @@ class RealSenseD435:
     # Frame capture
     # ------------------------------------------------------------------
     def capture(self, timeout_ms: int = 5000) -> Optional[CameraFrame]:
-        """The next frame COMPLETED after this call (None on timeout or when closed).
+        """The next aligned color+depth frame COMPLETED after this call (None on timeout or when closed).
 
-        A grab already in progress when the call arrives may have started
-        before it. Does not touch the pipeline: it waits for the acquisition
-        thread. Requests waiting at the same moment get the same frame.
+        Its frameset may have arrived shortly before the call, and an align
+        already in progress may have started before it. Does not touch the
+        pipeline: it waits for the align thread. Requests waiting at the same
+        moment get the same frame.
         """
-        acquisition = self._acquisition
-        if not self._open or acquisition is None:
+        aligned = self._aligned
+        if not self._open or aligned is None:
             return None
-        seq, _ = acquisition.latest()
-        _, frame = acquisition.wait_newer(seq, timeout_ms / 1000.0)
+        seq, _ = aligned.latest()
+        _, frame = aligned.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
+    def color_frame(self, after_seq: Optional[int] = None, timeout_ms: int = 1000) -> tuple[int, Optional[ColorFrame]]:
+        """(number, image) of the fast color channel: the latest color frame, no align.
+
+        With `after_seq`, waits up to `timeout_ms` for a frame numbered above it.
+        """
+        raw = self._raw
+        if not self._open or raw is None:
+            return 0, None
+        if after_seq is None:
+            seq, published = raw.latest()
+        else:
+            seq, published = raw.wait_newer(after_seq, timeout_ms / 1000.0)
+        return seq, (published.color if published is not None else None)
+
+    def _grab_raw(self, timeout_ms: int) -> Optional[RawFrameset]:
+        """wait_for_frames, and the color image as an owned BGR array. Continuous thread only.
+
+        Runs at the camera rate: no align, no depth conversion here.
+        """
+        pipeline = self._pipeline
         if pipeline is None:
             return None
         try:
@@ -418,35 +479,60 @@ class RealSenseD435:
         except Exception as e:
             logger.warning(f"wait_for_frames failed: {e}")
             return None
+        host_timestamp = time.time()
+        color_frame = frames.get_color_frame()
+        if not color_frame:
+            return None
+        with timer.phase("color_convert"):
+            rgb = _owned(self._color_bgr(color_frame))
+        color = ColorFrame(
+            rgb=rgb,
+            camera_timestamp_ms=float(color_frame.get_timestamp()),
+            timestamp_domain=_domain_name(color_frame.get_frame_timestamp_domain()),
+            host_timestamp=host_timestamp,
+        )
+        return RawFrameset(color=color, frameset=frames)
 
+    def _align_newest(self) -> Optional[CameraFrame]:
+        """Align the newest frameset not aligned yet, and convert its depth. Align thread only."""
+        raw = self._raw
+        if raw is None:
+            return None
+        seq, published = raw.wait_newer(self._last_aligned_raw_seq, ALIGN_WAIT_S)
+        if published is None:
+            return None
+        self._last_aligned_raw_seq = seq
+        frames, align = published.frameset, self._align
         if align is not None:
             with timer.phase("align"):
                 frames = align.process(frames)
-
-        color_frame = frames.get_color_frame()
         depth_frame = frames.get_depth_frame()
-        if not color_frame or not depth_frame:
+        if not depth_frame:
             return None
-
         with timer.phase("convert"):
-            frame = self._frame_from(color_frame, depth_frame)
-        # Own copies: frames now live on after the grab (latest frame, 100 ms cache,
+            frame = self._aligned_frame(published.color.rgb, depth_frame)
+        # Own copies: frames live on after the align (latest frame, 100 ms cache,
         # shared encodings), and views would keep the SDK's frame pool busy.
         with timer.phase("copy"):
             return CameraFrame(rgb=_owned(frame.rgb), depth=_owned(frame.depth), timestamp=frame.timestamp)
 
-    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)."""
+    def _color_bgr(self, color_frame) -> np.ndarray:
+        """BGR image of an SDK color frame (YUYV decoded on USB 2; else a view on the SDK buffer)."""
         if self._color_format == rs.format.yuyv:
             # SDK exposes YUYV as a 1-channel view of the packed buffer;
             # rebuild it as (H, W, 2) uint8 so cv2 can decode it to BGR.
             h = color_frame.get_height()
             w = color_frame.get_width()
             buf = np.frombuffer(color_frame.get_data(), dtype=np.uint8)
-            rgb = cv2.cvtColor(buf.reshape(h, w, 2), cv2.COLOR_YUV2BGR_YUYV)
-        else:
-            rgb = np.asanyarray(color_frame.get_data())
+            return cv2.cvtColor(buf.reshape(h, w, 2), cv2.COLOR_YUV2BGR_YUYV)
+        return np.asanyarray(color_frame.get_data())
 
+    def _aligned_frame(self, rgb: np.ndarray, depth_frame) -> CameraFrame:
+        """Color + depth in mm on one grid (USB 2 resize, depth scale).
+
+        `rgb` is the frameset's color image: align maps the depth onto the
+        color, so the color pixels are the same as the raw ones.
+        """
         depth_raw = np.asanyarray(depth_frame.get_data())
         # On USB 2 (no hardware alignment) the color and depth streams have
         # different native resolutions. Resize color to depth shape so
@@ -468,10 +554,10 @@ class RealSenseD435:
         return CameraFrame(rgb=rgb, depth=depth_mm, timestamp=time.time())
 
     def _cached_or_capture(self) -> Optional[CameraFrame]:
-        """The latest frame if younger than frame_cache_ms, else the next one."""
-        acquisition = self._acquisition
-        if acquisition is not None:
-            _, last = acquisition.latest()
+        """The latest ALIGNED frame if younger than frame_cache_ms, else the next one."""
+        aligned = self._aligned
+        if aligned is not None:
+            _, last = aligned.latest()
             if last is not None and (time.time() - last.timestamp) * 1000.0 < self._frame_cache_ms:
                 return last
         return self.capture()

src/backend/services/frame_acquisition.py

@@ -1,4 +1,4 @@
-"""One thread owns the camera pipeline: it grabs frames on demand and keeps the latest one.
+"""A thread that grabs frames and keeps the latest one, numbered: on demand or continuously.
 
 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
@@ -7,6 +7,9 @@ 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).
 
+With `on_demand=False` the thread grabs continuously instead (for a cheap
+grab that should follow the camera rate, like the color-only channel).
+
 The thread never dies: an exception from `grab` is logged and it goes on.
 """
 
@@ -25,9 +28,10 @@ EMPTY_BACKOFF_S = 0.05  # pause after a grab that returned nothing (closed pipel
 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"):
+    def __init__(self, grab: Callable[[], Optional[T]], name: str = "frame-acquisition", on_demand: bool = True):
         self._grab = grab
         self._name = name
+        self._on_demand = on_demand
         self._cond = threading.Condition()
         self._seq = 0                 # number of the latest frame (0 = none yet)
         self._latest: Optional[T] = None
@@ -81,7 +85,7 @@ class FrameAcquisition(Generic[T]):
     def _run(self) -> None:
         while True:
             with self._cond:
-                self._cond.wait_for(lambda: self._wanted > self._seq or self._stopping)
+                self._cond.wait_for(lambda: not self._on_demand or self._wanted > self._seq or self._stopping)
                 if self._stopping:
                     return
             frame = self._grab_safely()

tests/test_parallel_capture.py

@@ -93,14 +93,15 @@ def test_websocket_streams_frames(fake_camera):
     assert second["timestamp"] > first["timestamp"]
 
 
-def test_close_stops_the_acquisition_thread():
+def test_close_stops_the_acquisition_threads():
     cam = RealSenseD435()
     pipe = FakePipeline()
     open_fake_camera(cam, pipe)
     assert cam.capture() is not None
-    acquisition = cam._acquisition
+    threads = [cam._raw, cam._aligned]
     cam.close()
     assert not cam.is_open()
     assert pipe.stopped
-    assert not acquisition._thread.is_alive()
+    assert not any(t._thread.is_alive() for t in threads)
     assert cam.capture() is None
+    assert cam.color_frame() == (0, None)