"""Intel RealSense D4xx depth camera driver via pyrealsense2. Provides RGB (BGR8) and aligned Depth (uint16 mm) frames plus intrinsics/extrinsics through the same public surface as the Nuwa HP60C driver, so the routes/websocket/frontend stay unchanged. In this driver the "ir_*" intrinsic fields hold the **depth** sensor intrinsics (the D4xx depth stream is rectified from the IR stereo pair), and "rgb_*" hold the **color** sensor intrinsics. After alignment both streams share the color intrinsics, but we still expose the raw depth intrinsics so client code that wants to back-project the unaligned depth can do so. Usage: cam = RealSenseD435() cam.init() devs = cam.discover() cam.open(0) frame = cam.capture() cam.close() cam.deinit() """ import logging import time from dataclasses import dataclass from typing import Optional 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) 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 class CameraFrame: """Container for a single capture: aligned color + depth + timestamp.""" rgb: np.ndarray # (H, W, 3) uint8 BGR depth: np.ndarray # (H, W) uint16 millimeters 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() @dataclass class CameraIntrinsics: """Per-sensor intrinsics. `*_ir` = depth, `*_rgb` = color.""" fx_ir: float fy_ir: float cx_ir: float cy_ir: float fx_rgb: float fy_rgb: float cx_rgb: float cy_rgb: float @dataclass class CameraExtrinsics: """Rigid transform depth → color. Row-major 3x3 rotation, translation in mm.""" rotation: tuple translation: tuple @dataclass class Distortion: """Brown-Conrady distortion (k1, k2, k3, p1, p2).""" k1: float k2: float k3: float p1: float p2: float @dataclass class DiscoveredDevice: index: int vid: int pid: int serial: str bus: int device: int class RealSenseD435: """Driver for Intel RealSense D4xx (tested on D435) via pyrealsense2. Lifecycle: init() → discover() → open(index) → capture/get_rgb/get_depth → close() → deinit() """ def __init__( self, width: int = 640, height: int = 480, fps: int = 30, frame_cache_ms: float = 100.0, ): self._width = width self._height = height self._fps = fps self._frame_cache_ms = frame_cache_ms self._ctx: Optional[rs.context] = None self._pipeline: Optional[rs.pipeline] = None self._profile = None self._align: Optional[rs.align] = None self._depth_scale: float = 0.001 self._initialized = False self._open = False self._discovered: list[DiscoveredDevice] = [] # 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" self.device_index: Optional[int] = None self.intrinsics: Optional[CameraIntrinsics] = None self.extrinsics: Optional[CameraExtrinsics] = None self.distortion_ir: Optional[Distortion] = None self.distortion_rgb: Optional[Distortion] = None # ------------------------------------------------------------------ # Lifecycle # ------------------------------------------------------------------ def is_initialized(self) -> bool: return self._initialized def is_open(self) -> bool: return self._open def is_color_available(self) -> bool: """False when the pipeline runs depth-only (USB 2.x fallback).""" return self._color_available def is_depth_aligned_to_color(self) -> bool: """True when depth is hardware-aligned to the color frame. When aligned (USB 3 default), the depth map lives on the **color** pixel grid, so back-projecting a pixel must use the color (rgb) intrinsics. On USB 2 the align step is skipped and color is resized onto the depth grid instead, so the depth (ir) intrinsics apply. """ return self._align is not None def usb_type(self) -> str: return self._usb_type def resolution(self) -> Optional[dict]: """Negotiated color stream resolution, or None if not open. Reads from the live pipeline profile so the value reflects what the firmware actually accepted — on USB 2.x this is the forced 640x480@15 fallback, not the configured `_width/_height`. """ if not self._open or self._profile is None: return None try: stream = self._profile.get_stream(rs.stream.color).as_video_stream_profile() return {"width": stream.width(), "height": stream.height(), "fps": stream.fps()} except Exception: return None def init(self) -> bool: try: self._ctx = rs.context() self._initialized = True logger.info("RealSense SDK initialized") return True except Exception as e: logger.error(f"RealSense init failed: {e}") return False def deinit(self) -> None: if self._open: self.close() self._ctx = None self._initialized = False logger.info("RealSense SDK deinitialized") # ------------------------------------------------------------------ # Discovery # ------------------------------------------------------------------ def discover(self, timeout_ms: int = 5000) -> list[DiscoveredDevice]: """Enumerate RealSense devices on USB. `timeout_ms` kept for API parity.""" if not self._initialized: raise RuntimeError("SDK not initialized, call init() first") devices = self._ctx.query_devices() self._discovered = [] for i, dev in enumerate(devices): try: serial = dev.get_info(rs.camera_info.serial_number) try: pid_hex = dev.get_info(rs.camera_info.product_id) pid = int(pid_hex, 16) if pid_hex else 0 except Exception: pid = 0 bus = 0 addr = 0 try: phys = dev.get_info(rs.camera_info.physical_port) parts = phys.rsplit("/", 1)[-1].split("-") if len(parts) >= 2: try: bus = int(parts[0]) except ValueError: pass except Exception: pass self._discovered.append(DiscoveredDevice( index=i, vid=0x8086, pid=pid, serial=serial, bus=bus, device=addr, )) except Exception as e: logger.warning(f"Failed to read device {i} info: {e}") logger.info(f"Discovered {len(self._discovered)} RealSense device(s)") return self._discovered # ------------------------------------------------------------------ # Stream open/close # ------------------------------------------------------------------ def open(self, device_index: int = 0) -> bool: if not self._initialized: logger.error("Cannot open: SDK not initialized") return False if self._open: logger.warning("Camera already open") return True if not self._discovered: self.discover() if device_index < 0 or device_index >= len(self._discovered): logger.error( f"Invalid device_index {device_index} ({len(self._discovered)} devices found)" ) return False serial = self._discovered[device_index].serial # Probe the USB type via the SDK. D4xx firmware can't sustain both # depth and color streams simultaneously on USB 2.x — wait_for_frames # times out indefinitely. We fall back to depth-only there. usb_type = "unknown" for dev in self._ctx.query_devices(): try: if dev.get_info(rs.camera_info.serial_number) == serial: if dev.supports(rs.camera_info.usb_type_descriptor): usb_type = dev.get_info(rs.camera_info.usb_type_descriptor) break except Exception: continue self._usb_type = usb_type is_usb2 = usb_type.startswith("2.") if is_usb2: # On USB 2.x the D4xx firmware only sustains the *native* YUYV # color format — BGR8/RGB8 trigger a wait_for_frames timeout. We # convert YUYV → BGR in capture() so callers stay format-agnostic. # 640x480@15 is the highest tested combination that holds without # dropping frames on this Jetson; actual delivery rate is ~4 fps # due to USB 2 bandwidth, but wait_for_frames stays under 300 ms. color_w, color_h, color_fps = 640, 480, 15 depth_w, depth_h, depth_fps = 640, 480, 15 color_fmt = rs.format.yuyv logger.warning( f"D4xx on USB {usb_type} — using USB2 profile " f"(color YUYV {color_w}x{color_h}@{color_fps}, " f"depth {depth_w}x{depth_h}@{depth_fps}, ~4 fps actual). " f"Move to a USB 3 port for full framerate." ) else: color_w, color_h, color_fps = self._width, self._height, self._fps depth_w, depth_h, depth_fps = self._width, self._height, self._fps color_fmt = rs.format.bgr8 try: self._pipeline = rs.pipeline(self._ctx) config = rs.config() config.enable_device(serial) config.enable_stream( rs.stream.depth, depth_w, depth_h, rs.format.z16, depth_fps ) config.enable_stream( rs.stream.color, color_w, color_h, color_fmt, color_fps ) self._profile = self._pipeline.start(config) self._color_available = True self._color_format = color_fmt # rs.align fails when the color stream is YUYV (the only color # format the D4xx can sustain on USB 2). Skip hardware alignment # there; we fall back to a numpy resize in capture() so the rgb # and depth arrays share the same shape for downstream consumers. self._align = None if is_usb2 else rs.align(rs.stream.color) depth_sensor = self._profile.get_device().first_depth_sensor() self._depth_scale = depth_sensor.get_depth_scale() self._read_calibration() 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}) " f"depth={depth_w}x{depth_h}@{depth_fps} depth_scale={self._depth_scale}" ) return True except Exception as e: logger.error(f"Open failed: {e}") self._teardown_pipeline() return False def close(self) -> None: if not self._open: return 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 for thread in threads: thread.join(STOP_JOIN_S) self.device_index = None logger.info("RealSense camera closed") def _start_acquisition(self) -> None: """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: try: self._pipeline.stop() except Exception as e: logger.warning(f"pipeline.stop failed: {e}") self._pipeline = None self._profile = None self._align = None self._open = False # ------------------------------------------------------------------ # Calibration # ------------------------------------------------------------------ def _read_calibration(self) -> None: color_stream = self._profile.get_stream(rs.stream.color).as_video_stream_profile() depth_stream = self._profile.get_stream(rs.stream.depth).as_video_stream_profile() ci = color_stream.get_intrinsics() di = depth_stream.get_intrinsics() self.intrinsics = CameraIntrinsics( fx_ir=di.fx, fy_ir=di.fy, cx_ir=di.ppx, cy_ir=di.ppy, fx_rgb=ci.fx, fy_rgb=ci.fy, cx_rgb=ci.ppx, cy_rgb=ci.ppy, ) try: ext = depth_stream.get_extrinsics_to(color_stream) r = list(ext.rotation) rotation = ( r[0], r[3], r[6], r[1], r[4], r[7], r[2], r[5], r[8], ) t = list(ext.translation) translation = (t[0] * 1000.0, t[1] * 1000.0, t[2] * 1000.0) self.extrinsics = CameraExtrinsics(rotation=rotation, translation=translation) except Exception as e: logger.warning(f"Extrinsics read failed: {e}") self.extrinsics = None # rs intrinsics.coeffs order is [k1, k2, p1, p2, k3]. def _to_distortion(coeffs) -> Distortion: if not coeffs or len(coeffs) < 5: return Distortion(0.0, 0.0, 0.0, 0.0, 0.0) return Distortion( k1=coeffs[0], k2=coeffs[1], k3=coeffs[4], p1=coeffs[2], p2=coeffs[3], ) self.distortion_ir = _to_distortion(list(di.coeffs)) self.distortion_rgb = _to_distortion(list(ci.coeffs)) # ------------------------------------------------------------------ # Frame capture # ------------------------------------------------------------------ def capture(self, timeout_ms: int = 5000) -> Optional[CameraFrame]: """The next aligned color+depth frame COMPLETED after this call (None on timeout or when closed). 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. """ aligned = self._aligned if not self._open or aligned is None: return None seq, _ = aligned.latest() _, frame = aligned.wait_newer(seq, timeout_ms / 1000.0) return frame 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; without, returns the latest one (waiting only for the very first). """ raw = self._raw if not self._open or raw is None: return 0, None seq, published = raw.latest() if after_seq is not None or published is None: seq, published = raw.wait_newer(after_seq or 0, 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: with timer.phase("wait_frames"): frames = pipeline.wait_for_frames(timeout_ms) 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) depth_frame = frames.get_depth_frame() if not depth_frame: return None with timer.phase("convert"): 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 _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) 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 # downstream endpoints (/pointcloud, /measure) that index rgb[y, x] # and depth[y, x] in lockstep continue to work. This is an # *approximate* alignment — depth↔color extrinsics are ignored. if rgb.shape[:2] != depth_raw.shape[:2]: rgb = cv2.resize( rgb, (depth_raw.shape[1], depth_raw.shape[0]), interpolation=cv2.INTER_LINEAR, ) # depth_scale ~ 0.001 m/unit on D4xx → raw uint16 is already millimeters. if abs(self._depth_scale - 0.001) < 1e-6: depth_mm = depth_raw else: depth_mm = (depth_raw.astype(np.float32) * self._depth_scale * 1000.0).astype(np.uint16) return CameraFrame(rgb=rgb, depth=depth_mm, timestamp=time.time()) def _cached_or_capture(self) -> Optional[CameraFrame]: """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() def cached_frame(self) -> Optional[CameraFrame]: """Like get_rgb()/get_depth(), but the whole frame (so its encodings can be shared).""" return self._cached_or_capture() def get_rgb(self) -> Optional[np.ndarray]: f = self._cached_or_capture() return f.rgb if f else None def get_depth(self) -> Optional[np.ndarray]: f = self._cached_or_capture() return f.depth if f else None