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.
This commit is contained in:
robot committed 2026-05-19 00:58:23 +02:00
commit 07d95792e3
58 files changed
+6987

No files matched your search

+537
View File
@@ -0,0 +1,537 @@
"""REST API routes for the RealSense D435 camera server."""
import base64
import logging
import cv2
import numpy as np
from fastapi import APIRouter, Depends, HTTPException, Query
from fastapi.responses import Response, JSONResponse
from src.backend.api.dependencies import verify_api_key
from src.backend.models.api import ok, fail
from src.backend.services.camera import RealSenseD435, CameraFrame
from src.backend.services.image_saver import ImageSaver
logger = logging.getLogger(__name__)
router = APIRouter(dependencies=[Depends(verify_api_key)])
_camera: RealSenseD435 | None = None
_saver: ImageSaver | None = None
_frame_count = 0
def set_camera(camera: RealSenseD435) -> None:
global _camera
_camera = camera
def set_image_saver(saver: ImageSaver) -> None:
global _saver
_saver = saver
# ---------------------------------------------------------------------------
# Image encoding helpers
# ---------------------------------------------------------------------------
def _encode_rgb_jpeg(rgb: np.ndarray, quality: int = 85) -> bytes:
_, buf = cv2.imencode(".jpg", rgb, [cv2.IMWRITE_JPEG_QUALITY, quality])
return buf.tobytes()
def _encode_depth_png(depth: np.ndarray) -> bytes:
_, buf = cv2.imencode(".png", depth)
return buf.tobytes()
def _depth_colormap(depth: np.ndarray) -> np.ndarray:
valid = depth[depth > 0]
if len(valid) == 0:
return np.zeros((*depth.shape, 3), dtype=np.uint8)
p5, p95 = np.percentile(valid, [5, 95])
vis = np.zeros_like(depth, dtype=np.uint8)
mask = depth > 0
vis[mask] = np.clip(
(depth[mask].astype(float) - p5) / max(p95 - p5, 1) * 255, 0, 255
).astype(np.uint8)
return cv2.applyColorMap(vis, cv2.COLORMAP_JET)
def _frame_to_dict(frame: CameraFrame, include_images: bool = True) -> dict:
global _frame_count
_frame_count += 1
result = {
"frame_id": _frame_count,
"timestamp": frame.timestamp,
"rgb_shape": list(frame.rgb.shape),
"depth_shape": list(frame.depth.shape),
"depth_min_mm": int(frame.depth[frame.depth > 0].min()) if (frame.depth > 0).any() else 0,
"depth_max_mm": int(frame.depth.max()),
"depth_valid_pixels": int((frame.depth > 0).sum()),
}
if include_images:
result["rgb_jpeg_b64"] = base64.b64encode(_encode_rgb_jpeg(frame.rgb)).decode()
result["depth_png_b64"] = base64.b64encode(_encode_depth_png(frame.depth)).decode()
colormap = _depth_colormap(frame.depth)
result["depth_color_b64"] = base64.b64encode(_encode_rgb_jpeg(colormap, 90)).decode()
return result
# ---------------------------------------------------------------------------
# Endpoints
# ---------------------------------------------------------------------------
# /health is mounted as public (no API key) in main.py
def _calibration_payload() -> dict | None:
"""Full calibration (intrinsics + distortion + extrinsics) or None."""
if not _camera or not _camera.intrinsics:
return None
i = _camera.intrinsics
e = _camera.extrinsics
dir_ = _camera.distortion_ir
drgb = _camera.distortion_rgb
def _d(d):
return None if d is None else {"k1": d.k1, "k2": d.k2, "k3": d.k3, "p1": d.p1, "p2": d.p2}
return {
"ir": {
"fx": i.fx_ir, "fy": i.fy_ir, "cx": i.cx_ir, "cy": i.cy_ir,
"distortion": _d(dir_),
},
"rgb": {
"fx": i.fx_rgb, "fy": i.fy_rgb, "cx": i.cx_rgb, "cy": i.cy_rgb,
"distortion": _d(drgb),
},
"extrinsics_ir_to_rgb": None if e is None else {
"rotation": list(e.rotation),
"translation": list(e.translation),
},
}
@router.get("/status")
async def status():
"""Camera status and intrinsics."""
return ok({
"sdk_initialized": _camera.is_initialized() if _camera else False,
"camera_open": _camera.is_open() if _camera else False,
"color_available": _camera.is_color_available() if (_camera and _camera.is_open()) else False,
"usb_type": _camera.usb_type() if (_camera and _camera.is_open()) else None,
"frame_count": _frame_count,
"resolution": {"width": 640, "height": 480},
"intrinsics": _calibration_payload(),
})
@router.get("/discover")
async def discover(timeout_ms: int = Query(default=5000, ge=1000, le=30000)):
"""Discover available RealSense cameras on USB bus."""
if not _camera or not _camera.is_initialized():
return fail("SDK_NOT_INITIALIZED", "SDK not initialized")
devices = _camera.discover(timeout_ms=timeout_ms)
return ok({
"cameras": [
{
"index": d.index,
"vid": f"{d.vid:04x}",
"pid": f"{d.pid:04x}",
"serial": d.serial,
"bus": d.bus,
"device": d.device,
}
for d in devices
],
"count": len(devices),
})
@router.post("/open")
async def open_camera(device_index: int = Query(default=0, ge=0)):
"""Open a discovered camera by index and start streaming.
Idempotent: if a camera is already open, returns the current open state
instead of failing — the server only supports one open camera at a time.
"""
if not _camera or not _camera.is_initialized():
return fail("SDK_NOT_INITIALIZED", "SDK not initialized")
if _camera.is_open():
return ok({
"status": "open",
"device_index": _camera.device_index,
"intrinsics": _calibration_payload(),
"already_open": True,
})
if _camera.open(device_index):
return ok({
"status": "open",
"device_index": device_index,
"intrinsics": _calibration_payload(),
"already_open": False,
})
else:
return fail("OPEN_FAILED", "Failed to open camera")
@router.post("/close")
async def close_camera():
"""Close the active camera and stop streaming."""
if not _camera or not _camera.is_open():
return fail("CAMERA_NOT_OPEN", "No camera is open")
_camera.close()
return ok({"status": "closed"})
@router.get("/capture")
async def capture():
"""Capture a single RGB+depth frame. Returns JSON with base64-encoded images."""
if not _camera or not _camera.is_open():
return fail("CAMERA_NOT_OPEN", "Camera not open")
frame = _camera.capture()
if frame is None:
return fail("CAPTURE_TIMEOUT", "Frame capture timeout")
return JSONResponse(ok(_frame_to_dict(frame)))
@router.get("/rgb")
async def get_rgb():
"""Get RGB frame as JPEG image."""
if not _camera or not _camera.is_open():
raise HTTPException(503, "Camera not open")
rgb = _camera.get_rgb()
if rgb is None:
raise HTTPException(504, "No RGB frame")
return Response(content=_encode_rgb_jpeg(rgb), media_type="image/jpeg")
@router.get("/depth")
async def get_depth():
"""Get depth frame as 16-bit PNG (values in mm)."""
if not _camera or not _camera.is_open():
raise HTTPException(503, "Camera not open")
depth = _camera.get_depth()
if depth is None:
raise HTTPException(504, "No depth frame")
return Response(content=_encode_depth_png(depth), media_type="image/png")
@router.get("/depth/color")
async def get_depth_colormap():
"""Get depth frame as JET colormap JPEG."""
if not _camera or not _camera.is_open():
raise HTTPException(503, "Camera not open")
depth = _camera.get_depth()
if depth is None:
raise HTTPException(504, "No depth frame")
colormap = _depth_colormap(depth)
return Response(content=_encode_rgb_jpeg(colormap, 90), media_type="image/jpeg")
@router.get("/pointcloud", summary="3D Point cloud", tags=["capture"])
async def get_pointcloud(
step: int = Query(default=4, ge=1, le=16),
max_depth_mm: int = Query(default=4000, ge=100, le=10000),
):
"""Get 3D point cloud as JSON array of [x,y,z,r,g,b] points.
Points are computed server-side using depth + RGB + intrinsics.
step: subsampling factor (4 = every 4th pixel, faster)
max_depth_mm: discard points beyond this depth
"""
if not _camera or not _camera.is_open():
return fail("NO_CAMERA", "Camera not open")
frame = _camera.capture()
if frame is None:
return fail("TIMEOUT", "Frame capture timeout")
depth = frame.depth
rgb = frame.rgb # BGR format
# Use intrinsics if available, otherwise use typical HP60C defaults
if _camera.intrinsics:
fx = _camera.intrinsics.fx_ir
fy = _camera.intrinsics.fy_ir
cx = _camera.intrinsics.cx_ir
cy = _camera.intrinsics.cy_ir
else:
# Default intrinsics for 640x480 HP60C
fx, fy = 445.0, 445.0
cx, cy = 320.0, 240.0
h, w = depth.shape
points = []
for y in range(0, h, step):
for x in range(0, w, step):
z_val = float(depth[y, x])
if z_val <= 0 or z_val > max_depth_mm:
continue
z_m = z_val / 1000.0
x_m = (x - cx) * z_m / fx
y_m = (y - cy) * z_m / fy
b_val, g_val, r_val = rgb[y, x] # BGR to RGB
points.append([
round(x_m, 4), round(-y_m, 4), round(-z_m, 4),
int(r_val), int(g_val), int(b_val)
])
return ok({
"points": points,
"count": len(points),
"step": step,
"intrinsics": {
"fx": fx, "fy": fy, "cx": cx, "cy": cy
}
})
@router.get("/pointcloud/roi", summary="3D Point cloud within ROI", tags=["capture"])
async def get_pointcloud_roi(
u_min: int = Query(..., ge=0, description="ROI left pixel (inclusive)"),
v_min: int = Query(..., ge=0, description="ROI top pixel (inclusive)"),
u_max: int = Query(..., ge=1, description="ROI right pixel (exclusive)"),
v_max: int = Query(..., ge=1, description="ROI bottom pixel (exclusive)"),
step: int = Query(default=1, ge=1, le=16),
max_depth_mm: int = Query(default=4000, ge=100, le=10000),
):
"""Get 3D point cloud restricted to a pixel-space ROI.
Coordinates are RAW image pixels (no flip applied) in the depth/RGB
frame (typically 640x480). Points are returned as [x, -y, -z, r, g, b]
— same convention as /pointcloud so the frontend renderer is compatible.
"""
if not _camera or not _camera.is_open():
return fail("NO_CAMERA", "Camera not open")
frame = _camera.capture()
if frame is None:
return fail("TIMEOUT", "Frame capture timeout")
depth = frame.depth
rgb = frame.rgb # BGR format
h, w = depth.shape
u0 = max(0, min(w, u_min))
v0 = max(0, min(h, v_min))
u1 = max(0, min(w, u_max))
v1 = max(0, min(h, v_max))
if u1 <= u0 or v1 <= v0:
return fail("INVALID_ROI", f"Empty ROI ({u0},{v0})-({u1},{v1}) in {w}x{h}")
if _camera.intrinsics:
fx = _camera.intrinsics.fx_ir
fy = _camera.intrinsics.fy_ir
cx = _camera.intrinsics.cx_ir
cy = _camera.intrinsics.cy_ir
else:
fx, fy = 445.0, 445.0
cx, cy = 320.0, 240.0
# Vectorized: restrict both arrays to the ROI and filter by depth mask.
roi_depth = depth[v0:v1:step, u0:u1:step].astype(np.float32)
roi_rgb = rgb[v0:v1:step, u0:u1:step]
mask = (roi_depth > 0) & (roi_depth <= max_depth_mm)
if not mask.any():
return ok({
"points": [],
"count": 0,
"step": step,
"roi": {"u_min": u0, "v_min": v0, "u_max": u1, "v_max": v1},
"intrinsics": {"fx": fx, "fy": fy, "cx": cx, "cy": cy},
})
ys_local, xs_local = np.where(mask)
zs_mm = roi_depth[ys_local, xs_local]
# Absolute pixel coordinates within the full image (needed for fx/cx back-projection).
xs_abs = u0 + xs_local * step
ys_abs = v0 + ys_local * step
zs_m = zs_mm / 1000.0
xs_m = (xs_abs - cx) * zs_m / fx
ys_m = (ys_abs - cy) * zs_m / fy
bgr = roi_rgb[ys_local, xs_local] # (N, 3) BGR
# Match /pointcloud convention: [x, -y, -z, r, g, b]
points = np.stack([
np.round(xs_m, 4),
np.round(-ys_m, 4),
np.round(-zs_m, 4),
bgr[:, 2].astype(np.float32),
bgr[:, 1].astype(np.float32),
bgr[:, 0].astype(np.float32),
], axis=1).tolist()
return ok({
"points": points,
"count": len(points),
"step": step,
"roi": {"u_min": u0, "v_min": v0, "u_max": u1, "v_max": v1},
"intrinsics": {"fx": fx, "fy": fy, "cx": cx, "cy": cy},
})
@router.get("/measure", summary="3D measurement at pixel", tags=["capture"])
async def measure_point(
u: int = Query(..., ge=0, description="Pixel X in raw image coords (0..W-1)"),
v: int = Query(..., ge=0, description="Pixel Y in raw image coords (0..H-1)"),
radius: int = Query(default=6, ge=0, le=50, description="Search radius (px) for nearest valid depth"),
):
"""Find nearest valid-depth pixel around (u, v) and back-project to 3D.
Useful for testing RGB/depth alignment: click on RGB at a pixel, get the
closest point-cloud point within `radius`. Coordinates are RAW image pixels
(no flip applied) in the depth/RGB frame (typically 640x480).
"""
if not _camera or not _camera.is_open():
return fail("CAMERA_NOT_OPEN", "Camera not open")
frame = _camera.capture()
if frame is None:
return fail("CAPTURE_TIMEOUT", "Frame capture timeout")
depth = frame.depth
h, w = depth.shape
if u >= w or v >= h:
return fail("OUT_OF_RANGE", f"Pixel ({u},{v}) outside image ({w}x{h})")
u0 = max(0, u - radius)
u1 = min(w, u + radius + 1)
v0 = max(0, v - radius)
v1 = min(h, v + radius + 1)
window = depth[v0:v1, u0:u1]
ys, xs = np.where(window > 0)
if len(xs) == 0:
return fail("NO_DEPTH", f"No valid depth within radius {radius}")
abs_xs = xs + u0
abs_ys = ys + v0
d2 = (abs_xs - u) ** 2 + (abs_ys - v) ** 2
idx = int(np.argmin(d2))
u_found = int(abs_xs[idx])
v_found = int(abs_ys[idx])
z_mm = int(depth[v_found, u_found])
if _camera.intrinsics:
fx = _camera.intrinsics.fx_ir
fy = _camera.intrinsics.fy_ir
cx = _camera.intrinsics.cx_ir
cy = _camera.intrinsics.cy_ir
else:
fx, fy = 445.0, 445.0
cx, cy = 320.0, 240.0
z_m = z_mm / 1000.0
x_m = (u_found - cx) * z_m / fx
y_m = (v_found - cy) * z_m / fy
dist_mm = float(np.sqrt(x_m * x_m + y_m * y_m + z_m * z_m) * 1000.0)
return ok({
"u": u,
"v": v,
"u_found": u_found,
"v_found": v_found,
"pixel_offset": round(float(np.sqrt(float(d2[idx]))), 2),
"radius": radius,
"depth_mm": z_mm,
"distance_mm": round(dist_mm, 1),
"point_m": {
"x": round(x_m, 4),
"y": round(y_m, 4),
"z": round(z_m, 4),
},
})
@router.post("/snapshot")
async def snapshot(
flip_h: bool = Query(default=False),
flip_v: bool = Query(default=False),
):
"""Capture a single RGB frame and save it as JPG in images_dir."""
if not _camera or not _camera.is_open():
return fail("CAMERA_NOT_OPEN", "Camera not open")
if _saver is None:
return fail("SAVER_NOT_READY", "Image saver not initialized")
frame = _camera.capture()
if frame is None:
return fail("CAPTURE_TIMEOUT", "Frame capture timeout")
filename = _saver.save_snapshot(frame.rgb, flip_h, flip_v)
return ok({"filename": filename})
@router.post("/recording/start")
async def recording_start(
flip_h: bool = Query(default=False),
flip_v: bool = Query(default=False),
interval: int = Query(default=1, ge=1, le=600),
):
"""Start continuous frame recording (applies to WebSocket stream loop).
interval: save 1 frame every N streamed frames (1 = every frame).
"""
if _saver is None:
return fail("SAVER_NOT_READY", "Image saver not initialized")
if _saver.is_recording:
return fail("ALREADY_RECORDING", "Recording already in progress")
status = _saver.start_recording(flip_h, flip_v, interval)
return ok(status)
@router.post("/recording/stop")
async def recording_stop():
"""Stop continuous frame recording."""
if _saver is None:
return fail("SAVER_NOT_READY", "Image saver not initialized")
if not _saver.is_recording:
return fail("NOT_RECORDING", "No recording in progress")
status = _saver.stop_recording()
return ok(status)
@router.get("/recording/status")
async def recording_status():
"""Current recording state."""
if _saver is None:
return fail("SAVER_NOT_READY", "Image saver not initialized")
return ok(_saver.status())
@router.get("/intrinsics")
async def get_intrinsics():
"""Get camera intrinsic parameters."""
if not _camera:
return fail("INTRINSICS_NOT_AVAILABLE", "Camera driver not ready")
if not _camera.is_open():
return fail("INTRINSICS_NOT_AVAILABLE", "Camera not open")
if not _camera.intrinsics:
return fail(
"INTRINSICS_NOT_AVAILABLE",
"Camera open but SDK did not return calibration (AS_SDK_GetCamParameter). "
"Check docker logs for 'GetCamParameter ... failed'.",
)
return ok(_calibration_payload())