diff --git a/src/backend/services/camera.py b/src/backend/services/camera.py index 54a7fa3..bd1aa4f 100644 --- a/src/backend/services/camera.py +++ b/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() diff --git a/src/backend/services/frame_acquisition.py b/src/backend/services/frame_acquisition.py index a4f0c84..f6cbb10 100644 --- a/src/backend/services/frame_acquisition.py +++ b/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() diff --git a/tests/test_parallel_capture.py b/tests/test_parallel_capture.py index cb9bca6..d542a71 100644 --- a/tests/test_parallel_capture.py +++ b/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)