Initial commit: RealSense D435 API server

Drop-in replacement for the Nuwa-HP60C camera container, exposing RGB +
aligned depth + intrinsics via REST/WebSocket so the existing
pestello-pose-detector pipeline can consume frames without changes.

Key adaptations for D4xx on USB 2.0 (this robot has no USB 3 port exposed
externally — see STATUS.md):
- pyrealsense2 built from source; the librealsense v2.55.1 install rule
  doesn't ship the main .so, so we copy it from the build dir and add an
  __init__.py that re-exports the submodule.
- Color stream uses YUYV format on USB 2.x (BGR8/RGB8 time out on this
  bus); converted to BGR via cv2.cvtColor in capture().
- rs.align is skipped on USB 2 (incompatible with YUYV); color is
  cv2-resized to depth shape for approximate per-pixel alignment.
- Driver auto-detects usb_type_descriptor and falls back to the
  full-resolution BGR8 + rs.align pipeline when on USB 3.

Tested: /api/{health,status,discover,rgb,depth,depth/color,capture,
intrinsics} all return 200 with valid payloads at 640x480.

src/backend/services/camera.py

@@ -0,0 +1,413 @@
+"""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
+
+logger = logging.getLogger(__name__)
+
+
+@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 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] = []
+        self._last_frame: Optional[CameraFrame] = None
+        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 usb_type(self) -> str:
+        return self._usb_type
+
+    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
+            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
+        self._teardown_pipeline()
+        self.device_index = None
+        self._last_frame = None
+        logger.info("RealSense camera closed")
+
+    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]:
+        if not self._open:
+            return None
+        try:
+            frames = self._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:
+            frames = self._align.process(frames)
+
+        color_frame = frames.get_color_frame()
+        depth_frame = frames.get_depth_frame()
+        if not color_frame or not depth_frame:
+            return None
+
+        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())
+
+        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)
+
+        frame = CameraFrame(rgb=rgb, depth=depth_mm, timestamp=time.time())
+        self._last_frame = frame
+        return frame
+
+    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
+        return self.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