112 lines
3.3 KiB
Python
112 lines
3.3 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 = _cfg["width"]
|
|
CAM_HEIGHT = _cfg["height"]
|
|
CAM_FPS = _cfg["fps"]
|
|
CAM_QUALITY = _cfg["jpeg_quality"]
|
|
|
|
# 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 = 2.0
|
|
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 = 2.0
|
|
_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=5000)
|
|
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] > 10.0:
|
|
print(" [Camera] No frame for 10s — 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
|