From cc28127c9c9e8bc122ef611330cd17e2cd390f0e Mon Sep 17 00:00:00 2001 From: Olivia Casadei Date: Tue, 29 Sep 2026 12:59:13 +0200 Subject: [PATCH] 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 --- src/backend/services/camera.py | 58 ++++++++---- src/backend/services/frame_acquisition.py | 105 ++++++++++++++++++++++ tests/test_frame_acquisition.py | 102 +++++++++++++++++++++ 3 files changed, 250 insertions(+), 15 deletions(-) create mode 100644 src/backend/services/frame_acquisition.py create mode 100644 tests/test_frame_acquisition.py diff --git a/src/backend/services/camera.py b/src/backend/services/camera.py index 480cc59..46f1508 100644 --- a/src/backend/services/camera.py +++ b/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) - self._last_frame = frame - return frame + return self._frame_from(color_frame, depth_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 ( - self._last_frame is not None - and (time.time() - self._last_frame.timestamp) * 1000.0 < self._frame_cache_ms - ): - return self._last_frame + """The latest frame if younger than frame_cache_ms, else the next one.""" + acquisition = self._acquisition + if acquisition is not None: + _, last = acquisition.latest() + 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]: diff --git a/src/backend/services/frame_acquisition.py b/src/backend/services/frame_acquisition.py new file mode 100644 index 0000000..a4f0c84 --- /dev/null +++ b/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 diff --git a/tests/test_frame_acquisition.py b/tests/test_frame_acquisition.py new file mode 100644 index 0000000..be294e0 --- /dev/null +++ b/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)