OliviaCandClaude Opus 5.5
5c97d166be
The 100 ms cache of get_rgb/get_depth/cached_frame served the latest aligned frame, so an /api/capture from another client between /api/rgb and /api/depth made the depth come from a newer frame than the color (with two clients on PC2, the multi-camera service and the pestle detector, this can happen). Older than R01: capture() already refreshed _last_frame then. The cache is now its own slot, renewed only by the cached readers. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
585 lines
23 KiB
Python
585 lines
23 KiB
Python
"""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
|
|
# The frame get_rgb/get_depth/cached_frame share for frame_cache_ms (see _cached_or_capture()).
|
|
self._cached: 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 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
|
|
self._cached = 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 frame the cached readers share if younger than frame_cache_ms, else the next aligned one.
|
|
|
|
Only these readers renew it: a capture() in between (e.g. /api/capture
|
|
from another client) does not move it, so /api/rgb then /api/depth
|
|
within frame_cache_ms return color and depth of ONE frame.
|
|
"""
|
|
cached = self._cached
|
|
if cached is not None and (time.time() - cached.timestamp) * 1000.0 < self._frame_cache_ms:
|
|
return cached
|
|
frame = self.capture()
|
|
if frame is not None:
|
|
self._cached = frame
|
|
return frame
|
|
|
|
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
|