843 lines
36 KiB
Python
843 lines
36 KiB
Python
"""Camera daemon — single producer, in-memory frame cache.
|
|
|
|
Captures frames at fixed FPS from a RealSense (preferred) or any USB
|
|
camera (fallback), JPEG-encodes them, and caches the latest frame in
|
|
memory in two views (matches Marcus's API/camera_api.py):
|
|
|
|
- `_latest_jpeg` raw JPEG bytes — dashboard preview + frame forwarder
|
|
- `_latest_b64` base64 ASCII — frame forwarder → Gemini child stdin
|
|
|
|
Consumers:
|
|
- dashboard preview → `snapshot_jpeg()` (served as an HTTP Response)
|
|
- face enrollment → `get_fresh_frame()` for a guaranteed-current capture
|
|
- GeminiSubprocess → `get_frame_b64()`, pushed over the child's stdin
|
|
|
|
Lifecycle is driven by the Recognition tab toggle. The daemon is idle
|
|
until `start()` is called; failures in start() are non-fatal and
|
|
reported via `is_running()` / `backend`. Once running it auto-reconnects
|
|
on USB unplug / stalled frames (Marcus-style resilience), and supports
|
|
hot `reconfigure()` of resolution/FPS without a full restart.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import base64
|
|
import os
|
|
import select
|
|
import shutil
|
|
import socket
|
|
import subprocess
|
|
import tempfile
|
|
import threading
|
|
import time
|
|
from typing import Optional
|
|
|
|
import numpy as np
|
|
|
|
from Project.Sanad.core.logger import get_logger
|
|
|
|
log = get_logger("camera")
|
|
|
|
# How many /dev/video* indices to scan for a USB-style color camera when
|
|
# RealSense isn't available. A RealSense exposes ~6 V4L2 nodes (depth, IR,
|
|
# color, metadata…) — the color one is rarely index 0, so we probe each
|
|
# and accept the first that yields a real 3-channel BGR frame.
|
|
_USB_SCAN_RANGE = 10
|
|
|
|
# R1 head video streams (RTP/H.264, one UDP port each, published by the
|
|
# "Stereo patch PC1" service). The head has three fisheye eyes pointing
|
|
# different ways plus a depth stream — the dashboard "Camera Source"
|
|
# selector maps these labels to ports.
|
|
_R1_SOURCE_LABELS = {5000: "depth", 5001: "front", 5002: "left", 5003: "right"}
|
|
|
|
|
|
class CameraDaemon:
|
|
"""RealSense → USB fallback camera capture with in-memory frame cache."""
|
|
|
|
def __init__(
|
|
self,
|
|
width: int = 424,
|
|
height: int = 240,
|
|
fps: int = 15,
|
|
jpeg_quality: int = 70,
|
|
stale_threshold_s: float = 10.0,
|
|
reconnect_min_s: float = 2.0,
|
|
reconnect_max_s: float = 10.0,
|
|
capture_timeout_ms: int = 5000,
|
|
) -> None:
|
|
# Active profile — guarded by _reconfig_lock so reconfigure() can
|
|
# hot-swap it from another thread between capture sessions.
|
|
self._reconfig_lock = threading.Lock()
|
|
self._w = int(width)
|
|
self._h = int(height)
|
|
self._fps = int(fps)
|
|
self._q = max(10, min(95, int(jpeg_quality)))
|
|
self._reconfig_pending = False
|
|
# R1 head video source (UDP port / eye) — swappable from the dashboard.
|
|
self._r1_port = int(os.environ.get("SANAD_R1_VIDEO_PORT", "5001"))
|
|
# Fisheye de-warp — hot toggle (applied per-frame, no pipeline rebuild).
|
|
self._dewarp = os.environ.get("SANAD_R1_DEWARP", "0") == "1"
|
|
# Cached undistortion remap tables, keyed by frame (w, h).
|
|
self._dewarp_wh: Optional[tuple] = None
|
|
self._dewarp_map1 = None
|
|
self._dewarp_map2 = None
|
|
|
|
# Resilience knobs (Marcus-style)
|
|
self._stale_s = float(stale_threshold_s)
|
|
self._reconnect_min_s = float(reconnect_min_s)
|
|
self._reconnect_max_s = float(reconnect_max_s)
|
|
self._capture_timeout_ms = int(capture_timeout_ms)
|
|
|
|
self._thread: Optional[threading.Thread] = None
|
|
self._stop = threading.Event()
|
|
self._backend: Optional[str] = None
|
|
self._lock = threading.Lock()
|
|
self._latest_jpeg: Optional[bytes] = None
|
|
self._latest_b64: Optional[str] = None
|
|
self._latest_ts: float = 0.0
|
|
self._frame_seq: int = 0
|
|
self._error: Optional[str] = None
|
|
self._reconnect_count: int = 0
|
|
|
|
# ── public API ──────────────────────────────────────────
|
|
|
|
@property
|
|
def backend(self) -> Optional[str]:
|
|
return self._backend
|
|
|
|
@property
|
|
def error(self) -> Optional[str]:
|
|
return self._error
|
|
|
|
@property
|
|
def frame_seq(self) -> int:
|
|
return self._frame_seq
|
|
|
|
def is_running(self) -> bool:
|
|
return self._thread is not None and self._thread.is_alive()
|
|
|
|
def start(self) -> bool:
|
|
"""Start capture thread. Returns True if a backend was acquired.
|
|
|
|
Initial probe is synchronous; if it fails the thread isn't spawned.
|
|
Once running, the inner loop auto-reconnects on USB unplug or
|
|
stalled frames using exponential backoff (`reconnect_min_s` ..
|
|
`reconnect_max_s`).
|
|
"""
|
|
if self.is_running():
|
|
return True
|
|
self._stop.clear()
|
|
self._error = None
|
|
self._reconnect_count = 0
|
|
|
|
# One-shot USB-2.0 negotiation diagnostic (warns operator if D435I
|
|
# came up on USB 2.0 — frame drops would be likely otherwise).
|
|
self._check_usb_version()
|
|
|
|
backend = self._probe_any()
|
|
if backend is None:
|
|
log.warning("Camera: no backend available (RealSense + USB both failed)")
|
|
self._backend = None
|
|
return False
|
|
|
|
self._backend = backend["name"]
|
|
self._thread = threading.Thread(
|
|
target=self._reconnect_loop, args=(backend,),
|
|
daemon=True, name="camera-daemon",
|
|
)
|
|
self._thread.start()
|
|
with self._reconfig_lock:
|
|
w, h, f = self._w, self._h, self._fps
|
|
log.info("Camera started (backend=%s, %dx%d @ %dfps)",
|
|
self._backend, w, h, f)
|
|
return True
|
|
|
|
def stop(self) -> None:
|
|
"""Stop the capture thread and release the hardware."""
|
|
if not self.is_running():
|
|
self._backend = None
|
|
self._clear_cache()
|
|
return
|
|
self._stop.set()
|
|
t = self._thread
|
|
if t is not None:
|
|
t.join(timeout=2.0)
|
|
self._thread = None
|
|
self._backend = None
|
|
# Drop the last captured frame so snapshot_jpeg()/get_frame_b64()
|
|
# return None once vision is OFF — otherwise the /frame.jpg preview
|
|
# and the enroll path keep serving a frozen image of whoever was
|
|
# last in front of the camera.
|
|
self._clear_cache()
|
|
log.info("Camera stopped")
|
|
|
|
def _clear_cache(self) -> None:
|
|
"""Drop the cached frame views so nothing stale is served."""
|
|
with self._lock:
|
|
self._latest_jpeg = None
|
|
self._latest_b64 = None
|
|
self._latest_ts = 0.0
|
|
|
|
def reconfigure(self, width: Optional[int] = None, height: Optional[int] = None,
|
|
fps: Optional[int] = None, jpeg_quality: Optional[int] = None,
|
|
r1_port: Optional[int] = None, dewarp: Optional[bool] = None) -> dict:
|
|
"""Hot-swap the capture profile without a full stop/start.
|
|
|
|
Resolution / FPS / quality / the R1 source port all require the
|
|
pipeline to be rebuilt (~0.5 s gap): we set a pending flag the
|
|
capture loop notices, tears down, and rebuilds at the new profile.
|
|
`dewarp` is applied per-frame, so toggling it takes effect instantly
|
|
with no rebuild. If the daemon isn't running the new values just
|
|
take effect on the next `start()`. Returns the resulting profile.
|
|
"""
|
|
needs_rebuild = False
|
|
with self._reconfig_lock:
|
|
if width is not None:
|
|
self._w = int(width); needs_rebuild = True
|
|
if height is not None:
|
|
self._h = int(height); needs_rebuild = True
|
|
if fps is not None:
|
|
self._fps = int(fps); needs_rebuild = True
|
|
if jpeg_quality is not None:
|
|
self._q = max(10, min(95, int(jpeg_quality))); needs_rebuild = True
|
|
if r1_port is not None:
|
|
self._r1_port = int(r1_port); needs_rebuild = True
|
|
if dewarp is not None:
|
|
self._dewarp = bool(dewarp) # hot — no rebuild
|
|
if needs_rebuild and self.is_running():
|
|
self._reconfig_pending = True
|
|
profile = {"width": self._w, "height": self._h,
|
|
"fps": self._fps, "jpeg_quality": self._q,
|
|
"r1_port": self._r1_port, "dewarp": self._dewarp,
|
|
"source": _R1_SOURCE_LABELS.get(self._r1_port, str(self._r1_port))}
|
|
log.info("Camera reconfigure → %s", profile)
|
|
return profile
|
|
|
|
def snapshot_jpeg(self) -> Optional[bytes]:
|
|
"""Return the latest JPEG bytes, or None if no frame yet."""
|
|
with self._lock:
|
|
return self._latest_jpeg
|
|
|
|
def get_frame_b64(self) -> Optional[str]:
|
|
"""Return the latest frame as a base64 ASCII string (or None).
|
|
|
|
Used by the frame forwarder to push frames over the Gemini child's
|
|
stdin without re-encoding — base64 is cached alongside the JPEG.
|
|
"""
|
|
with self._lock:
|
|
return self._latest_b64
|
|
|
|
def get_fresh_frame(self, max_age_s: float = 0.5,
|
|
timeout_s: float = 1.5) -> Optional[bytes]:
|
|
"""Return a JPEG frame newer than `max_age_s`, waiting up to `timeout_s`.
|
|
|
|
Used by face enrollment so the captured frame is guaranteed to be
|
|
the *current* scene, not a stale buffer from before the user got
|
|
into position. On timeout, only falls back to the cached frame if
|
|
it is still within the stale threshold — otherwise returns None so
|
|
the enroll route raises 409 rather than capturing an old scene
|
|
(e.g. while the daemon is stuck reconnecting).
|
|
"""
|
|
deadline = time.time() + timeout_s
|
|
while time.time() < deadline:
|
|
with self._lock:
|
|
if (self._latest_jpeg is not None
|
|
and self._latest_ts > 0
|
|
and (time.time() - self._latest_ts) <= max_age_s):
|
|
return self._latest_jpeg
|
|
time.sleep(0.03)
|
|
# Timed out waiting for a fresh frame. Hand back the cached frame
|
|
# only if it isn't dangerously stale; never enrol an arbitrarily
|
|
# old scene.
|
|
with self._lock:
|
|
if (self._latest_jpeg is not None
|
|
and self._latest_ts > 0
|
|
and (time.time() - self._latest_ts) <= self._stale_s):
|
|
return self._latest_jpeg
|
|
return None
|
|
|
|
def latest_age_s(self) -> float:
|
|
"""Seconds since last successful frame; +inf if none."""
|
|
with self._lock:
|
|
if self._latest_ts <= 0:
|
|
return float("inf")
|
|
return time.time() - self._latest_ts
|
|
|
|
def status(self) -> dict:
|
|
with self._reconfig_lock:
|
|
w, h, f, q = self._w, self._h, self._fps, self._q
|
|
r1_port, dewarp = self._r1_port, self._dewarp
|
|
# latest_age_s() is +inf until the first frame lands. inf is NOT
|
|
# JSON-serialisable by Starlette's JSONResponse (allow_nan=False) —
|
|
# leaving it as inf would 500 the /api/recognition/* routes. Map
|
|
# "running but no frame yet" and "not running" both to None.
|
|
age = self.latest_age_s()
|
|
running = self.is_running()
|
|
age_s = round(age, 2) if (running and age != float("inf")) else None
|
|
# Snapshot the report counters under _lock for a consistent view —
|
|
# the capture/reconnect thread mutates these (see _reconnect_loop).
|
|
# Read latest_age_s()/is_running() above (they self-lock) so we
|
|
# don't re-enter this non-reentrant lock.
|
|
with self._lock:
|
|
backend = self._backend
|
|
frame_seq = self._frame_seq
|
|
error = self._error
|
|
reconnect_count = self._reconnect_count
|
|
return {
|
|
"running": running,
|
|
"backend": backend,
|
|
"width": w,
|
|
"height": h,
|
|
"fps": f,
|
|
"jpeg_quality": q,
|
|
"r1_port": r1_port,
|
|
"dewarp": dewarp,
|
|
"source": _R1_SOURCE_LABELS.get(r1_port, str(r1_port)),
|
|
"frame_seq": frame_seq,
|
|
"age_s": age_s,
|
|
"error": error,
|
|
"reconnect_count": reconnect_count,
|
|
}
|
|
|
|
# ── helpers ─────────────────────────────────────────────
|
|
|
|
def _probe_any(self) -> Optional[dict]:
|
|
"""R1 head stream first, then RealSense, then USB. Backend dict or None."""
|
|
b = self._probe_r1_stereo()
|
|
if b is None:
|
|
b = self._probe_realsense()
|
|
if b is None:
|
|
b = self._probe_usb()
|
|
return b
|
|
|
|
def _rtp_relay_port(self, src_port: int) -> int:
|
|
"""Private loopback port ffmpeg reads instead of the raw source port."""
|
|
return 15000 + (src_port - 5000)
|
|
|
|
def _start_rtp_relay(self, src_port: int, dst_port: int):
|
|
"""Forward RTP from `src_port` to 127.0.0.1:`dst_port`, binding the source
|
|
with SO_REUSEADDR so it coexists with the R1 videohub (which holds
|
|
5002/5003). ffmpeg then reads the private loopback port, so its RTP+RTCP
|
|
pair never collides with the videohub's ports — the reason decoding
|
|
straight off 5001 fails ("bind failed: address already in use" for the
|
|
RTCP port 5002). Returns a stop Event, or None if the source bind fails."""
|
|
stop = threading.Event()
|
|
|
|
def run():
|
|
try:
|
|
rx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
rx.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
|
rx.bind(("0.0.0.0", src_port))
|
|
rx.settimeout(0.5)
|
|
except Exception as exc:
|
|
log.warning("RTP relay: bind :%d failed: %s", src_port, exc)
|
|
stop.set()
|
|
return
|
|
tx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
dst = ("127.0.0.1", dst_port)
|
|
while not stop.is_set():
|
|
try:
|
|
data, _ = rx.recvfrom(65535)
|
|
tx.sendto(data, dst)
|
|
except socket.timeout:
|
|
continue
|
|
except Exception:
|
|
break
|
|
for s in (rx, tx):
|
|
try:
|
|
s.close()
|
|
except Exception:
|
|
pass
|
|
|
|
t = threading.Thread(target=run, daemon=True, name="rtp-relay-%d" % src_port)
|
|
t.start()
|
|
return stop
|
|
|
|
def _probe_r1_stereo(self) -> Optional[dict]:
|
|
"""R1 EDU head camera — the RTP/H.264 stereo stream published by the robot's
|
|
"stereo_patch_pc1" video service. We (optionally) START that service via the
|
|
robot_state DDS call (phone-free; SANAD_R1_STREAM_AUTOSTART=0 to disable),
|
|
then decode with a *system* ffmpeg subprocess (the container's cv2-bundled
|
|
ffmpeg lacks RTP/SDP support). Because the R1 videohub owns UDP 5002/5003,
|
|
ffmpeg can't grab the RTCP port for a 5001 decode, so we run a tiny
|
|
SO_REUSEADDR RTP relay (source → private loopback port) and point ffmpeg
|
|
there. Returns None (fails fast) when no stream arrives, so RealSense/USB
|
|
still get a turn. The eye (port) and resolution/FPS follow the live daemon
|
|
profile (dashboard Camera Source + Resolution/FPS controls)."""
|
|
if os.environ.get("SANAD_R1_VIDEO", "1") == "0":
|
|
return None
|
|
ffmpeg = shutil.which("ffmpeg")
|
|
if not ffmpeg:
|
|
log.info("R1 stereo camera: system ffmpeg not installed")
|
|
return None
|
|
with self._reconfig_lock:
|
|
port = self._r1_port
|
|
w, h, fps = self._w, self._h, self._fps
|
|
self._reconfig_pending = False # this build satisfies any pending reconfigure
|
|
# Phone-free stream start: ask the robot_state service to enable the stereo
|
|
# patch (idempotent). Give PC1 a moment to spin up before we read.
|
|
if os.environ.get("SANAD_R1_STREAM_AUTOSTART", "1") != "0":
|
|
try:
|
|
from Project.Sanad.vision.stream_service import get_stream_service
|
|
if get_stream_service().set_stereo(True):
|
|
time.sleep(0.5) # let the DDS call land; the 6 s read below
|
|
# absorbs the rest of PC1's spin-up latency
|
|
except Exception as exc:
|
|
log.info("R1 stereo auto-start skipped: %s", exc)
|
|
# Relay the source port to a private loopback port for ffmpeg.
|
|
relay_port = self._rtp_relay_port(port)
|
|
relay_stop = self._start_rtp_relay(port, relay_port)
|
|
if relay_stop is None:
|
|
return None
|
|
sdp = (
|
|
"v=0\r\no=- 0 0 IN IP4 127.0.0.1\r\ns=R1\r\nc=IN IP4 127.0.0.1\r\n"
|
|
"t=0 0\r\nm=video %d RTP/AVP 96\r\n"
|
|
"a=rtpmap:96 H264/90000\r\na=fmtp:96 packetization-mode=1\r\n" % relay_port
|
|
)
|
|
sdp_path = os.path.join(tempfile.gettempdir(), "r1_front_%d.sdp" % relay_port)
|
|
try:
|
|
with open(sdp_path, "w") as fh:
|
|
fh.write(sdp)
|
|
except Exception:
|
|
relay_stop.set()
|
|
return None
|
|
cmd = [
|
|
ffmpeg, "-hide_banner", "-loglevel", "error",
|
|
"-protocol_whitelist", "file,rtp,udp",
|
|
"-fflags", "nobuffer", "-flags", "low_delay",
|
|
"-analyzeduration", "3M", "-probesize", "3M",
|
|
"-i", sdp_path,
|
|
"-an", "-sn",
|
|
"-vf", "scale=%d:%d" % (w, h), "-r", str(fps),
|
|
"-f", "rawvideo", "-pix_fmt", "bgr24", "-",
|
|
]
|
|
try:
|
|
proc = subprocess.Popen(
|
|
cmd, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL,
|
|
bufsize=0,
|
|
)
|
|
except Exception as exc:
|
|
log.info("R1 stereo camera: ffmpeg spawn failed: %s", exc)
|
|
relay_stop.set()
|
|
return None
|
|
backend = {"name": "r1_stereo", "proc": proc, "wh": (w, h),
|
|
"frame_wh": (w, h), "is_color": True, "sdp_path": sdp_path,
|
|
"port": port, "relay_stop": relay_stop}
|
|
# Confirm the stream is live: try to read one frame within ~6 s (ffmpeg
|
|
# needs to bind the port + wait for the next H.264 keyframe). If nothing
|
|
# arrives, the service is off — tear down and fall through.
|
|
frame = self._read_r1_frame(backend, timeout_s=6.0)
|
|
if frame is None:
|
|
self._teardown(backend)
|
|
return None
|
|
log.info("R1 head camera: %s eye RTP/H.264 udp:%d via ffmpeg (%dx%d @ %dfps)",
|
|
_R1_SOURCE_LABELS.get(port, str(port)), port, w, h, fps)
|
|
return backend
|
|
|
|
def _read_r1_frame(self, backend: dict, timeout_s: float = 2.0):
|
|
"""Read one rawvideo BGR frame (w*h*3 bytes) from the ffmpeg subprocess."""
|
|
proc = backend["proc"]
|
|
w, h = backend["wh"]
|
|
need = w * h * 3
|
|
buf = bytearray()
|
|
deadline = time.time() + timeout_s
|
|
stdout = proc.stdout
|
|
while len(buf) < need:
|
|
if proc.poll() is not None:
|
|
return None
|
|
r, _, _ = select.select([stdout], [], [], 0.5)
|
|
if not r:
|
|
if time.time() > deadline:
|
|
return None
|
|
continue
|
|
chunk = stdout.read(need - len(buf))
|
|
if not chunk:
|
|
return None
|
|
buf.extend(chunk)
|
|
deadline = time.time() + timeout_s
|
|
return np.frombuffer(bytes(buf), dtype=np.uint8).reshape((h, w, 3))
|
|
|
|
def _check_usb_version(self) -> None:
|
|
"""Warn if a connected RealSense negotiated USB 2.0 (needs 3.x).
|
|
|
|
Marcus has this same check — D435I on USB 2.0 can't deliver
|
|
color+depth+IMU and the pipeline silently stalls. Catching it at
|
|
startup lets the operator fix the cable/port instead of chasing a
|
|
"no frames" loop. Diagnostic only; never blocks startup.
|
|
"""
|
|
try:
|
|
import pyrealsense2 as rs # type: ignore
|
|
ctx = rs.context()
|
|
for dev in ctx.query_devices():
|
|
try:
|
|
usb_type = dev.get_info(rs.camera_info.usb_type_descriptor)
|
|
name = dev.get_info(rs.camera_info.name)
|
|
except Exception:
|
|
continue
|
|
if str(usb_type).startswith("2."):
|
|
log.warning(
|
|
"RealSense %s negotiated USB %s — expected 3.x. "
|
|
"Frame drops likely. Try a USB 3 port / shorter cable / "
|
|
"powered hub.", name, usb_type,
|
|
)
|
|
else:
|
|
log.info("RealSense %s on USB %s", name, usb_type)
|
|
except Exception:
|
|
pass
|
|
|
|
# ── backend probing ─────────────────────────────────────
|
|
|
|
def _probe_realsense(self) -> Optional[dict]:
|
|
with self._reconfig_lock:
|
|
w, h, f = self._w, self._h, self._fps
|
|
self._reconfig_pending = False # this build satisfies any pending reconfigure
|
|
try:
|
|
import pyrealsense2 as rs # type: ignore
|
|
pipeline = rs.pipeline()
|
|
cfg = rs.config()
|
|
cfg.enable_stream(rs.stream.color, w, h, rs.format.bgr8, f)
|
|
profile = pipeline.start(cfg)
|
|
return {"name": "realsense", "pipeline": pipeline, "rs": rs,
|
|
"profile": profile}
|
|
except Exception as exc:
|
|
log.info("RealSense unavailable: %s", exc)
|
|
return None
|
|
|
|
def _open_usb_index(self, idx: int, w: int, h: int, f: int,
|
|
cv2) -> Optional[dict]:
|
|
"""Open one /dev/video<idx>, validate it yields a 3-channel frame,
|
|
and classify it as colour vs grayscale/IR.
|
|
|
|
A RealSense IR node delivers Y8 — cv2 replicates that single plane
|
|
across 3 channels, so the planes come back *bit-identical*. A real
|
|
colour sensor never produces bit-identical channels (per-channel
|
|
sensor noise differs even on a flat gray scene). That's the test.
|
|
Returns a backend dict with `is_color`, or None if the node is
|
|
unusable.
|
|
"""
|
|
cap = None
|
|
try:
|
|
cap = cv2.VideoCapture(idx)
|
|
if not cap.isOpened():
|
|
cap.release()
|
|
return None
|
|
cap.set(cv2.CAP_PROP_FRAME_WIDTH, w)
|
|
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, h)
|
|
cap.set(cv2.CAP_PROP_FPS, f)
|
|
good = None
|
|
for _ in range(5):
|
|
ok, frame = cap.read()
|
|
if (ok and frame is not None and frame.ndim == 3
|
|
and frame.shape[2] == 3):
|
|
good = frame
|
|
break
|
|
if good is None:
|
|
cap.release()
|
|
return None
|
|
is_color = not (
|
|
np.array_equal(good[:, :, 0], good[:, :, 1])
|
|
and np.array_equal(good[:, :, 1], good[:, :, 2])
|
|
)
|
|
return {"name": "usb", "cap": cap, "cv2": cv2, "index": idx,
|
|
"is_color": is_color,
|
|
"frame_wh": (good.shape[1], good.shape[0])}
|
|
except Exception as exc:
|
|
log.info("USB camera index %d: %s", idx, exc)
|
|
if cap is not None:
|
|
try:
|
|
cap.release()
|
|
except Exception:
|
|
pass
|
|
return None
|
|
|
|
def _probe_usb(self) -> Optional[dict]:
|
|
"""Scan /dev/video* for a colour camera node, falling back to a
|
|
grayscale/IR node only if no colour node exists.
|
|
|
|
On a RealSense, /dev/video0 is the *depth* stream (Z16, cv2 can't
|
|
open it as a webcam); the IR nodes deliver Y8 (grayscale); the
|
|
*colour* node delivers YUYV/BGR. We can't know the index up front,
|
|
so we probe each and prefer the first genuine colour node — that's
|
|
why the dashboard preview used to come up grayscale. Pin a node
|
|
with SANAD_CAMERA_USB_INDEX=<n> to skip the scan entirely.
|
|
"""
|
|
with self._reconfig_lock:
|
|
w, h, f = self._w, self._h, self._fps
|
|
self._reconfig_pending = False # this build satisfies any pending reconfigure
|
|
try:
|
|
import cv2 # type: ignore
|
|
except Exception as exc:
|
|
log.info("USB camera unavailable: %s", exc)
|
|
return None
|
|
|
|
# Pinned index — accept whatever it is (colour or not).
|
|
explicit = os.environ.get("SANAD_CAMERA_USB_INDEX", "").strip()
|
|
if explicit.isdigit():
|
|
backend = self._open_usb_index(int(explicit), w, h, f, cv2)
|
|
if backend is not None:
|
|
fw, fh = backend["frame_wh"]
|
|
log.info("USB camera: pinned /dev/video%d (%dx%d, %s)",
|
|
backend["index"], fw, fh,
|
|
"colour" if backend["is_color"] else "grayscale/IR")
|
|
return backend
|
|
log.warning("USB camera: pinned index %s unusable", explicit)
|
|
return None
|
|
|
|
# Scan — prefer a real colour node; keep the first grayscale node
|
|
# as a last resort so the camera still works if that's all there is.
|
|
gray_fallback: Optional[dict] = None
|
|
for idx in range(_USB_SCAN_RANGE):
|
|
backend = self._open_usb_index(idx, w, h, f, cv2)
|
|
if backend is None:
|
|
continue
|
|
fw, fh = backend["frame_wh"]
|
|
if backend["is_color"]:
|
|
log.info("USB camera: using /dev/video%d (colour, %dx%d)",
|
|
idx, fw, fh)
|
|
if gray_fallback is not None:
|
|
try:
|
|
gray_fallback["cap"].release()
|
|
except Exception:
|
|
pass
|
|
return backend
|
|
# grayscale/IR — remember the first, release any extras
|
|
if gray_fallback is None:
|
|
gray_fallback = backend
|
|
else:
|
|
try:
|
|
backend["cap"].release()
|
|
except Exception:
|
|
pass
|
|
|
|
if gray_fallback is not None:
|
|
fw, fh = gray_fallback["frame_wh"]
|
|
log.warning("USB camera: no colour node found — falling back to "
|
|
"/dev/video%d (grayscale/IR, %dx%d). For a RealSense, "
|
|
"build pyrealsense2 or pin the colour node with "
|
|
"SANAD_CAMERA_USB_INDEX.", gray_fallback["index"], fw, fh)
|
|
return gray_fallback
|
|
|
|
log.info("USB camera unavailable: no working /dev/video* node found "
|
|
"(scanned %d indices)", _USB_SCAN_RANGE)
|
|
return None
|
|
|
|
# ── main capture loop ───────────────────────────────────
|
|
|
|
def _reconnect_loop(self, initial_backend: dict) -> None:
|
|
"""Outer loop — owns reconnect with exponential backoff.
|
|
|
|
Invariant: a torn-down backend is NEVER reused. After every capture
|
|
session we drop the local `backend` and, at the top of the loop,
|
|
(re)probe a fresh live backend at the *current* profile — so a
|
|
source/resolution switch or a stall always rebuilds cleanly and
|
|
`_capture_session` can never spin on a killed subprocess. On probe
|
|
failure `self._backend` is nulled so status() reports "reconnecting"
|
|
(not a false-green badge on a dead eye) and the MJPEG preview
|
|
detaches rather than freezing. `_reconfig_pending` is cleared inside
|
|
the probe (atomically with reading the profile it builds against),
|
|
so a rebuilt session doesn't immediately re-rebuild.
|
|
"""
|
|
backend = initial_backend # already probed synchronously in start()
|
|
backoff = self._reconnect_min_s
|
|
|
|
while not self._stop.is_set():
|
|
# (Re)acquire a live backend at the current profile if we lack one.
|
|
if backend is None:
|
|
backend = self._probe_any()
|
|
if backend is None:
|
|
self._backend = None # surface "reconnecting", not a false OK
|
|
self._error = "reconnecting"
|
|
log.warning("Camera source down — retrying in %.1fs", backoff)
|
|
if self._stop.wait(backoff):
|
|
break
|
|
backoff = min(backoff * 2, self._reconnect_max_s)
|
|
continue
|
|
self._backend = backend["name"]
|
|
self._error = None
|
|
backoff = self._reconnect_min_s
|
|
log.info("Camera acquired (backend=%s)", self._backend)
|
|
|
|
reconfigured = False
|
|
try:
|
|
reconfigured = self._capture_session(backend)
|
|
except Exception as exc:
|
|
log.exception("Camera capture session crashed: %s", exc)
|
|
self._error = str(exc)
|
|
finally:
|
|
self._teardown(backend)
|
|
backend = None # never reuse a torn-down backend
|
|
|
|
if self._stop.is_set():
|
|
break
|
|
|
|
# reconfigure → re-probe immediately (loop top, no backoff);
|
|
# stall/crash → count it, back off, then re-probe at the top.
|
|
if not reconfigured:
|
|
self._reconnect_count += 1
|
|
self._error = "reconnecting"
|
|
log.warning("Camera disconnected — reconnecting in %.1fs", backoff)
|
|
if self._stop.wait(backoff): # interruptible sleep
|
|
break
|
|
backoff = min(backoff * 2, self._reconnect_max_s)
|
|
|
|
def _capture_session(self, backend: dict) -> bool:
|
|
"""Inner capture loop — runs until stop, stale-frame timeout, or
|
|
a reconfigure request.
|
|
|
|
Returns True if it exited because of a reconfigure (caller rebuilds
|
|
immediately), False on a stall or clean stop.
|
|
"""
|
|
import cv2 # always available — used for JPEG encode
|
|
|
|
with self._reconfig_lock:
|
|
encode_params = [int(cv2.IMWRITE_JPEG_QUALITY), self._q]
|
|
last_frame_time = time.time()
|
|
consecutive_failures = 0
|
|
|
|
while not self._stop.is_set():
|
|
if self._reconfig_pending:
|
|
log.info("Camera reconfigure requested — rebuilding pipeline")
|
|
return True
|
|
|
|
bgr = self._read_frame(backend)
|
|
if bgr is None:
|
|
consecutive_failures += 1
|
|
age = time.time() - last_frame_time
|
|
if age > self._stale_s:
|
|
log.warning(
|
|
"Camera stalled %.1fs (%d consecutive timeouts) — "
|
|
"rebuilding pipeline", age, consecutive_failures,
|
|
)
|
|
return False
|
|
# Intermediate warnings so degradation is visible early
|
|
if consecutive_failures in (3, 10, 30):
|
|
log.warning("Camera slow (%d failures, age %.1fs)",
|
|
consecutive_failures, age)
|
|
time.sleep(0.05)
|
|
continue
|
|
|
|
# Fisheye de-warp (hot toggle, read live so it needs no rebuild).
|
|
if self._dewarp:
|
|
bgr = self._apply_dewarp(bgr, cv2)
|
|
|
|
try:
|
|
ok, buf = cv2.imencode(".jpg", bgr, encode_params)
|
|
except Exception as exc:
|
|
log.warning("JPEG encode failed: %s", exc)
|
|
continue
|
|
if not ok:
|
|
continue
|
|
jpeg = bytes(buf)
|
|
b64 = base64.b64encode(jpeg).decode("ascii")
|
|
now = time.time()
|
|
with self._lock:
|
|
self._latest_jpeg = jpeg
|
|
self._latest_b64 = b64
|
|
self._latest_ts = now
|
|
self._frame_seq += 1
|
|
last_frame_time = now
|
|
consecutive_failures = 0
|
|
|
|
return False
|
|
|
|
def _read_frame(self, backend: dict) -> Optional[np.ndarray]:
|
|
name = backend["name"]
|
|
if name == "realsense":
|
|
try:
|
|
frames = backend["pipeline"].wait_for_frames(
|
|
timeout_ms=self._capture_timeout_ms,
|
|
)
|
|
color = frames.get_color_frame()
|
|
if not color:
|
|
return None
|
|
return np.asanyarray(color.get_data())
|
|
except Exception:
|
|
# Soft path — single timeouts handled by _capture_session's
|
|
# stale-detection logic; don't spam the log per frame.
|
|
return None
|
|
elif name == "r1_stereo":
|
|
return self._read_r1_frame(backend, timeout_s=3.0)
|
|
elif name == "usb":
|
|
cap = backend["cap"]
|
|
ok, frame = cap.read()
|
|
if not ok or frame is None:
|
|
return None
|
|
return frame
|
|
return None
|
|
|
|
# ── fisheye de-warp ─────────────────────────────────────
|
|
|
|
def _get_dewarp_maps(self, w: int, h: int, cv2):
|
|
"""Precompute + cache undistortion remap tables for a (w, h) frame.
|
|
|
|
The R1 head cam is a wide fisheye with no published calibration, so
|
|
the camera matrix K and distortion coeffs are *approximated* (all
|
|
env-tunable) — this straightens lines approximately, not to metric
|
|
accuracy. Maps are cached per resolution; cv2.remap() then applies
|
|
them cheaply per frame.
|
|
"""
|
|
# Gate on resolution alone: _dewarp_wh is set to (w, h) after BOTH a
|
|
# successful build and a failed one, so a match means "already
|
|
# attempted at this size" — return the cached maps (or the cached
|
|
# (None, None) on prior failure) instead of rebuilding every frame.
|
|
if self._dewarp_wh == (w, h):
|
|
return self._dewarp_map1, self._dewarp_map2
|
|
try:
|
|
focal = float(os.environ.get("SANAD_R1_DEWARP_FOCAL", "0.55")) * w
|
|
k1 = float(os.environ.get("SANAD_R1_DEWARP_K1", "-0.35"))
|
|
k2 = float(os.environ.get("SANAD_R1_DEWARP_K2", "0.14"))
|
|
alpha = float(os.environ.get("SANAD_R1_DEWARP_ALPHA", "0.0"))
|
|
K = np.array([[focal, 0, w / 2.0],
|
|
[0, focal, h / 2.0],
|
|
[0, 0, 1.0]], dtype=np.float64)
|
|
dist = np.array([k1, k2, 0.0, 0.0, 0.0], dtype=np.float64)
|
|
newK, _ = cv2.getOptimalNewCameraMatrix(K, dist, (w, h), alpha, (w, h))
|
|
map1, map2 = cv2.initUndistortRectifyMap(
|
|
K, dist, None, newK, (w, h), cv2.CV_16SC2)
|
|
self._dewarp_wh = (w, h)
|
|
self._dewarp_map1, self._dewarp_map2 = map1, map2
|
|
return map1, map2
|
|
except Exception as exc:
|
|
log.warning("de-warp map build failed (%dx%d): %s", w, h, exc)
|
|
self._dewarp_wh = (w, h)
|
|
self._dewarp_map1 = self._dewarp_map2 = None
|
|
return None, None
|
|
|
|
def _apply_dewarp(self, bgr: np.ndarray, cv2) -> np.ndarray:
|
|
"""Remap a BGR frame through the cached undistortion tables."""
|
|
h, w = bgr.shape[:2]
|
|
m1, m2 = self._get_dewarp_maps(w, h, cv2)
|
|
if m1 is None:
|
|
return bgr
|
|
try:
|
|
return cv2.remap(bgr, m1, m2, interpolation=cv2.INTER_LINEAR,
|
|
borderMode=cv2.BORDER_CONSTANT)
|
|
except Exception:
|
|
return bgr
|
|
|
|
def _teardown(self, backend: dict) -> None:
|
|
name = backend.get("name")
|
|
try:
|
|
if name == "realsense":
|
|
backend["pipeline"].stop()
|
|
elif name == "usb":
|
|
backend["cap"].release()
|
|
elif name == "r1_stereo":
|
|
relay_stop = backend.get("relay_stop")
|
|
if relay_stop is not None:
|
|
relay_stop.set() # stop the RTP relay thread + free the port
|
|
proc = backend.get("proc")
|
|
if proc is not None:
|
|
try:
|
|
proc.kill()
|
|
proc.wait(timeout=2)
|
|
except Exception:
|
|
pass
|
|
if backend.get("sdp_path"):
|
|
try:
|
|
os.remove(backend["sdp_path"])
|
|
except Exception:
|
|
pass
|
|
except Exception as exc:
|
|
log.info("Camera teardown: %s", exc)
|