115 lines
3.8 KiB
Python
115 lines
3.8 KiB
Python
"""
|
|
camera_api.py — RealSense D435I camera thread
|
|
"""
|
|
import base64
|
|
import io
|
|
import time
|
|
import threading
|
|
import numpy as np
|
|
from PIL import Image
|
|
from Core.config_loader import load_config
|
|
from Core.logger import log
|
|
|
|
_cfg = load_config("Camera")
|
|
|
|
CAM_WIDTH = int(_cfg.get("width", 424))
|
|
CAM_HEIGHT = int(_cfg.get("height", 240))
|
|
CAM_FPS = int(_cfg.get("fps", 15))
|
|
CAM_QUALITY = int(_cfg.get("jpeg_quality", 70))
|
|
CAM_TIMEOUT_MS = int(_cfg.get("timeout_ms", 5000)) # pipeline.wait_for_frames timeout
|
|
CAM_STALE_THRESHOLD = float(_cfg.get("stale_threshold_s", 10.0)) # trip reconnect after this long without a frame
|
|
CAM_RECONNECT_DELAY = float(_cfg.get("reconnect_delay_s", 2.0)) # initial backoff; doubles up to 10 s
|
|
|
|
# Shared state
|
|
latest_frame_b64 = [None]
|
|
_raw_frame = [None]
|
|
camera_lock = threading.Lock()
|
|
_raw_lock = threading.Lock()
|
|
camera_alive = [True]
|
|
_cam_last_frame_time = [0.0]
|
|
_cam_connected = [False]
|
|
|
|
|
|
def get_raw_refs():
|
|
"""Return (raw_frame_ref, raw_lock) for YOLO to share."""
|
|
return _raw_frame, _raw_lock
|
|
|
|
|
|
def camera_loop():
|
|
"""Capture RealSense frames continuously with auto-reconnect."""
|
|
import pyrealsense2 as rs
|
|
|
|
backoff = CAM_RECONNECT_DELAY
|
|
while camera_alive[0]:
|
|
pipeline = None
|
|
try:
|
|
pipeline = rs.pipeline()
|
|
cfg = rs.config()
|
|
cfg.enable_stream(rs.stream.color, CAM_WIDTH, CAM_HEIGHT, rs.format.bgr8, CAM_FPS)
|
|
pipeline.start(cfg)
|
|
backoff = CAM_RECONNECT_DELAY
|
|
_cam_connected[0] = True
|
|
print("Camera connected")
|
|
log(f"Camera connected {CAM_WIDTH}x{CAM_HEIGHT}@{CAM_FPS}", "info", "camera")
|
|
|
|
while camera_alive[0]:
|
|
try:
|
|
frames = pipeline.wait_for_frames(timeout_ms=CAM_TIMEOUT_MS)
|
|
color_frame = frames.get_color_frame()
|
|
if not color_frame:
|
|
continue
|
|
|
|
frame = np.asanyarray(color_frame.get_data())
|
|
if frame is None or frame.size == 0:
|
|
continue
|
|
|
|
with _raw_lock:
|
|
_raw_frame[0] = frame.copy()
|
|
|
|
img = Image.fromarray(frame[:, :, ::-1])
|
|
buf = io.BytesIO()
|
|
img.save(buf, format="JPEG", quality=CAM_QUALITY)
|
|
with camera_lock:
|
|
latest_frame_b64[0] = base64.b64encode(buf.getvalue()).decode()
|
|
|
|
_cam_last_frame_time[0] = time.time()
|
|
|
|
except Exception:
|
|
if time.time() - _cam_last_frame_time[0] > CAM_STALE_THRESHOLD:
|
|
print(f" [Camera] No frame for {CAM_STALE_THRESHOLD:.0f}s — reconnecting...")
|
|
break
|
|
|
|
except Exception as e:
|
|
if _cam_connected[0]:
|
|
print(f" [Camera] Disconnected ({type(e).__name__}) — retrying in {backoff:.0f}s...")
|
|
_cam_connected[0] = False
|
|
try:
|
|
pipeline.stop()
|
|
except Exception:
|
|
pass
|
|
time.sleep(backoff)
|
|
backoff = min(backoff * 2, 10.0)
|
|
|
|
|
|
def start_camera():
|
|
"""Start camera thread. Returns (raw_frame_ref, raw_lock)."""
|
|
threading.Thread(target=camera_loop, daemon=True).start()
|
|
time.sleep(3.0)
|
|
return _raw_frame, _raw_lock
|
|
|
|
|
|
def stop_camera():
|
|
"""Stop camera thread."""
|
|
camera_alive[0] = False
|
|
|
|
|
|
def get_frame():
|
|
"""Return latest base64 JPEG frame for LLaVA. None if not ready."""
|
|
with camera_lock:
|
|
return latest_frame_b64[0]
|
|
|
|
|
|
def get_frame_age() -> float:
|
|
"""Return seconds since last camera frame."""
|
|
return time.time() - _cam_last_frame_time[0] if _cam_last_frame_time[0] > 0 else 999.0
|