676 lines
22 KiB
Python
676 lines
22 KiB
Python
from picamera2 import Picamera2
|
|
from threading import Lock, RLock
|
|
import threading
|
|
import time
|
|
import numpy as np
|
|
import cv2
|
|
from pathlib import Path
|
|
from typing import Optional
|
|
import subprocess
|
|
|
|
|
|
class CameraManager:
|
|
def __init__(self, state):
|
|
self.state = state
|
|
self.initialized = False
|
|
|
|
self.camera_lock = RLock()
|
|
|
|
self.cameras_runtime = {}
|
|
self._reconfigure_needed = False
|
|
self._sensor_modes_cache = None
|
|
|
|
# =========================================================
|
|
# Estado interno
|
|
# =========================================================
|
|
|
|
def mark_reconfigure_needed(self):
|
|
with self.camera_lock:
|
|
self._reconfigure_needed = True
|
|
|
|
def _init_camera_runtime(self, cam_spec):
|
|
return {
|
|
"camera_id": cam_spec.id,
|
|
"camera_index": cam_spec.index,
|
|
"role": cam_spec.role,
|
|
"interface": getattr(cam_spec, "interface", "CSI"),
|
|
|
|
"backend": None, # picamera2 | opencv
|
|
"picam2": None,
|
|
"cap": None,
|
|
|
|
"last_frame": None,
|
|
"frame_id": 0,
|
|
"frame_ts": None,
|
|
"buffer": None,
|
|
|
|
"stop_event": threading.Event(),
|
|
"thread": None,
|
|
|
|
"last_signature": None,
|
|
"last_read_ts": None,
|
|
"last_new_frame_ts": None,
|
|
|
|
"frame_lock": Lock(),
|
|
"consecutive_failures": 0,
|
|
}
|
|
|
|
def _make_frame_signature(self, frame: np.ndarray):
|
|
if frame is None:
|
|
return None
|
|
|
|
h, w = frame.shape[:2]
|
|
|
|
# pega uma grade simples e barata
|
|
step_y = max(1, h // 16)
|
|
step_x = max(1, w // 16)
|
|
|
|
sample = frame[::step_y, ::step_x]
|
|
|
|
# reduz para assinatura curtinha
|
|
return (
|
|
sample.shape,
|
|
int(sample.mean()),
|
|
int(sample.std()),
|
|
int(sample[0, 0, 0]) if sample.ndim == 3 else int(sample[0, 0]),
|
|
int(sample[-1, -1, 1]) if sample.ndim == 3 and sample.shape[2] > 1 else 0,
|
|
)
|
|
|
|
# =========================================================
|
|
# Inicialização
|
|
# =========================================================
|
|
|
|
def begin(self):
|
|
with self.camera_lock:
|
|
self.stop()
|
|
|
|
required_ids = set(self.state.get_bootstrap_camera_ids_for_frame_type())
|
|
if not required_ids:
|
|
print("[WARN] Nenhuma câmera requerida para o frame_type/capture_mode atual")
|
|
|
|
for cam in self.state.cameras:
|
|
cam.attempted = False
|
|
|
|
cams_to_open = [cam for cam in self.state.cameras if cam.id in required_ids]
|
|
|
|
cams_to_open.sort(
|
|
key=lambda cam: 0 if getattr(cam, "interface", "CSI").upper() == "USB" else 1
|
|
)
|
|
|
|
for cam in cams_to_open:
|
|
cam.attempted = True
|
|
|
|
if cam.id not in required_ids:
|
|
self.state.set_camera_connected(cam.index, False)
|
|
continue
|
|
|
|
try:
|
|
runtime = self._open_camera(cam)
|
|
self.cameras_runtime[cam.id] = runtime
|
|
|
|
connected_now = False #(getattr(cam, "interface", "CSI").upper() == "USB")
|
|
self.state.set_camera_connected(
|
|
cam.index,
|
|
connected_now,
|
|
width=cam.width,
|
|
height=cam.height,
|
|
bayer_pattern=cam.bayer_pattern,
|
|
bit_depth=cam.bit_depth,
|
|
)
|
|
|
|
except Exception as e:
|
|
print(f"[WARN] Falha ao abrir {cam.id} (index={cam.index}, role={cam.role}): {e}")
|
|
self.state.set_camera_connected(cam.index, False)
|
|
|
|
# Aplica controles logo após abrir
|
|
if self.cameras_runtime:
|
|
try:
|
|
self.apply_controls()
|
|
except Exception as e:
|
|
print(f"[WARN] Falha ao aplicar controles iniciais: {e}")
|
|
|
|
for cam_id in list(self.cameras_runtime.keys()):
|
|
self._start_thread(cam_id)
|
|
|
|
deadline = time.perf_counter() + 1.0
|
|
while time.perf_counter() < deadline:
|
|
if self.state.camera_count_active > 2:
|
|
break
|
|
time.sleep(0.02)
|
|
|
|
self.initialized = len(self.cameras_runtime) > 0
|
|
self._reconfigure_needed = False
|
|
|
|
return self.initialized
|
|
|
|
def _open_camera(self, cam):
|
|
interface = getattr(cam, "interface", "CSI").upper()
|
|
|
|
if interface == "CSI":
|
|
return self._open_csi_camera(cam)
|
|
|
|
if interface == "USB":
|
|
return self._open_usb_camera(cam)
|
|
|
|
raise RuntimeError(f"Interface de câmera não suportada: {interface}")
|
|
|
|
def _open_csi_camera(self, cam):
|
|
runtime = self._init_camera_runtime(cam)
|
|
|
|
picam2 = Picamera2(camera_num=cam.index)
|
|
|
|
config = picam2.create_video_configuration(
|
|
#main={"size": (640, 480), "format": "RGB888"},
|
|
raw={"size": (cam.width, cam.height)},
|
|
buffer_count=6
|
|
)
|
|
|
|
picam2.configure(config)
|
|
picam2.start()
|
|
|
|
runtime["backend"] = "picamera2"
|
|
runtime["picam2"] = picam2
|
|
return runtime
|
|
|
|
def _open_usb_camera(self, cam):
|
|
runtime = self._init_camera_runtime(cam)
|
|
|
|
backend_name = str(getattr(cam, "usb_backend", "V4L2")).upper()
|
|
api_preference = cv2.CAP_V4L2 if backend_name == "V4L2" else cv2.CAP_ANY
|
|
|
|
source = self._resolve_usb_video_path(cam)
|
|
device = source or f"/dev/video{cam.index}"
|
|
runtime["device"] = device
|
|
|
|
if source:
|
|
cap = cv2.VideoCapture(source, api_preference)
|
|
else:
|
|
cap = cv2.VideoCapture(cam.index, api_preference)
|
|
|
|
if not cap.isOpened():
|
|
# fallback no índice se o path falhar
|
|
if source:
|
|
cap.release()
|
|
cap = cv2.VideoCapture(cam.index, api_preference)
|
|
|
|
if not cap.isOpened():
|
|
raise RuntimeError(
|
|
f"Falha ao abrir câmera USB. device_path={getattr(cam, 'device_path', None)} index={cam.index}"
|
|
)
|
|
|
|
device = source or f"/dev/video{cam.index}"
|
|
self._v4l2_set_ctrls(device, {
|
|
"exposure_dynamic_framerate": 0,
|
|
"auto_exposure": 3,
|
|
"white_balance_automatic": 1,
|
|
"gain": 0,
|
|
})
|
|
|
|
# Configura formato primeiro
|
|
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
|
|
cap.set(cv2.CAP_PROP_FRAME_WIDTH, cam.width)
|
|
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, cam.height)
|
|
cap.set(cv2.CAP_PROP_FPS, float(self.state.fps))
|
|
|
|
# Pequeno warmup
|
|
time.sleep(0.25)
|
|
|
|
frame = None
|
|
ok = False
|
|
for i in range(15):
|
|
ok, frame = cap.read()
|
|
if ok and frame is not None:
|
|
break
|
|
time.sleep(0.05)
|
|
|
|
if not ok or frame is None:
|
|
cap.release()
|
|
raise RuntimeError(
|
|
f"Falha ao capturar frame inicial da câmera USB "
|
|
f"(device_path={getattr(cam, 'device_path', None)}, index={cam.index})"
|
|
)
|
|
|
|
actual_h, actual_w = frame.shape[:2]
|
|
channels = frame.shape[2] if frame.ndim == 3 else 1
|
|
|
|
cam.width = int(actual_w)
|
|
cam.height = int(actual_h)
|
|
|
|
initial_ts = time.perf_counter()
|
|
runtime["backend"] = "opencv"
|
|
runtime["cap"] = cap
|
|
runtime["buffer"] = frame.copy()
|
|
runtime["last_frame"] = runtime["buffer"]
|
|
runtime["frame_id"] = 0
|
|
runtime["last_signature"] = self._make_frame_signature(frame)
|
|
runtime["last_read_ts"] = initial_ts
|
|
runtime["last_new_frame_ts"] = initial_ts
|
|
runtime["frame_ts"] = initial_ts
|
|
|
|
print(
|
|
f"[INFO] USB {cam.id} aberta: src={getattr(cam, 'device_path', None) or cam.index} "
|
|
f"{actual_w}x{actual_h}, channels={channels}, backend={backend_name}"
|
|
)
|
|
|
|
return runtime
|
|
|
|
def _resolve_usb_video_path(self, cam):
|
|
# 1. Se já veio um caminho explícito e ele existe, usa
|
|
device_path = getattr(cam, "device_path", None)
|
|
if device_path and Path(device_path).exists():
|
|
return str(Path(device_path).resolve()) if Path(device_path).is_symlink() else device_path
|
|
|
|
# 2. Tenta by-id
|
|
by_id_dir = Path("/dev/v4l/by-id")
|
|
if by_id_dir.exists():
|
|
candidates = sorted(by_id_dir.glob("*video-index0"))
|
|
if candidates:
|
|
# Para 1 webcam USB, o primeiro já costuma resolver
|
|
return str(candidates[0])
|
|
|
|
# 3. Tenta by-path
|
|
by_path_dir = Path("/dev/v4l/by-path")
|
|
if by_path_dir.exists():
|
|
candidates = sorted(by_path_dir.glob("*video-index0"))
|
|
if candidates:
|
|
return str(candidates[0])
|
|
|
|
# 4. Fallback bruto
|
|
return None
|
|
|
|
# =========================================================
|
|
# Threads de captura
|
|
# =========================================================
|
|
|
|
def _start_thread(self, cam_id):
|
|
runtime = self.cameras_runtime[cam_id]
|
|
runtime["stop_event"].clear()
|
|
runtime["consecutive_failures"] = 0
|
|
|
|
t = threading.Thread(
|
|
target=self._update_loop,
|
|
args=(cam_id,),
|
|
daemon=True
|
|
)
|
|
runtime["thread"] = t
|
|
t.start()
|
|
|
|
def _update_loop(self, cam_id):
|
|
runtime = self.cameras_runtime[cam_id]
|
|
backend = runtime["backend"]
|
|
|
|
while not runtime["stop_event"].is_set():
|
|
try:
|
|
if backend == "picamera2":
|
|
self._update_loop_picamera2(runtime)
|
|
elif backend == "opencv":
|
|
self._update_loop_opencv(runtime)
|
|
else:
|
|
raise RuntimeError(f"Backend desconhecido: {backend}")
|
|
|
|
runtime["consecutive_failures"] = 0
|
|
|
|
except Exception as e:
|
|
runtime["consecutive_failures"] += 1
|
|
print(f"[ERRO LOOP {cam_id}/{backend}] {e}")
|
|
|
|
if runtime["consecutive_failures"] >= 5:
|
|
self.state.set_camera_connected(runtime["camera_index"], False)
|
|
runtime["stop_event"].set()
|
|
print(f"[WARN] Desativando {cam_id} após falhas consecutivas no loop")
|
|
break
|
|
|
|
time.sleep(0.05)
|
|
|
|
def _update_loop_picamera2(self, runtime):
|
|
request = None
|
|
try:
|
|
picam2 = runtime["picam2"]
|
|
request = picam2.capture_request()
|
|
|
|
raw = request.make_array("raw")
|
|
|
|
with runtime["frame_lock"]:
|
|
first_valid_frame = runtime["frame_id"] == 0
|
|
|
|
if (
|
|
runtime["buffer"] is None or
|
|
runtime["buffer"].shape != raw.shape or
|
|
runtime["buffer"].dtype != raw.dtype
|
|
):
|
|
runtime["buffer"] = raw.copy()
|
|
else:
|
|
np.copyto(runtime["buffer"], raw)
|
|
|
|
read_ts = time.perf_counter()
|
|
runtime["last_frame"] = runtime["buffer"]
|
|
runtime["frame_id"] += 1
|
|
runtime["frame_ts"] = read_ts
|
|
runtime["last_read_ts"] = read_ts
|
|
runtime["last_new_frame_ts"] = read_ts
|
|
|
|
if first_valid_frame:
|
|
self.state.set_camera_connected(
|
|
runtime["camera_index"],
|
|
True,
|
|
width=raw.shape[1],
|
|
height=raw.shape[0],
|
|
)
|
|
|
|
finally:
|
|
if request is not None:
|
|
try:
|
|
request.release()
|
|
except Exception:
|
|
pass
|
|
|
|
def _update_loop_opencv(self, runtime):
|
|
cap = runtime["cap"]
|
|
|
|
target_fps = max(1.0, float(self.state.fps or 10))
|
|
min_period = 1.0 / target_fps
|
|
t_start = time.perf_counter()
|
|
|
|
ok, frame = cap.read()
|
|
read_ts = time.perf_counter()
|
|
|
|
if not ok or frame is None:
|
|
raise RuntimeError("Falha ao ler frame da câmera USB")
|
|
|
|
signature = self._make_frame_signature(frame)
|
|
|
|
with runtime["frame_lock"]:
|
|
first_valid_frame = runtime["frame_id"] == 0
|
|
runtime["last_read_ts"] = read_ts
|
|
|
|
if signature != runtime.get("last_signature"):
|
|
if (
|
|
runtime["buffer"] is None or
|
|
runtime["buffer"].shape != frame.shape or
|
|
runtime["buffer"].dtype != frame.dtype
|
|
):
|
|
runtime["buffer"] = frame.copy()
|
|
else:
|
|
np.copyto(runtime["buffer"], frame)
|
|
|
|
runtime["last_frame"] = runtime["buffer"]
|
|
runtime["frame_id"] += 1
|
|
runtime["frame_ts"] = read_ts
|
|
runtime["last_new_frame_ts"] = read_ts
|
|
runtime["last_signature"] = signature
|
|
|
|
if first_valid_frame:
|
|
self.state.set_camera_connected(
|
|
runtime["camera_index"],
|
|
True,
|
|
width=frame.shape[1],
|
|
height=frame.shape[0],
|
|
bit_depth=8,
|
|
)
|
|
|
|
dt = time.perf_counter() - t_start
|
|
sleep_s = min_period - dt
|
|
if sleep_s > 0:
|
|
time.sleep(sleep_s)
|
|
|
|
# =========================================================
|
|
# Leitura consolidada
|
|
# =========================================================
|
|
|
|
def capture_raw_frames(self):
|
|
result = {}
|
|
|
|
for cam_id, runtime in self.cameras_runtime.items():
|
|
with runtime["frame_lock"]:
|
|
if runtime["last_frame"] is None:
|
|
continue
|
|
|
|
frame = runtime["last_frame"].copy()
|
|
h, w = frame.shape[:2]
|
|
channels = frame.shape[2] if frame.ndim == 3 else 1
|
|
|
|
result[cam_id] = (
|
|
frame,
|
|
w,
|
|
h,
|
|
channels,
|
|
runtime["frame_id"],
|
|
runtime["frame_ts"],
|
|
runtime["last_read_ts"]
|
|
)
|
|
|
|
return result
|
|
|
|
# =========================================================
|
|
# Controles
|
|
# =========================================================
|
|
|
|
def apply_controls(self, camera_id: Optional[str] = None):
|
|
with self.camera_lock:
|
|
if camera_id is not None:
|
|
runtime = self.cameras_runtime.get(camera_id)
|
|
if runtime is None:
|
|
raise ValueError(f"Runtime da câmera {camera_id} não encontrado")
|
|
|
|
backend = runtime.get("backend")
|
|
|
|
if backend == "picamera2":
|
|
self._apply_controls_picamera2(camera_id, runtime)
|
|
elif backend == "opencv":
|
|
self._apply_controls_opencv(camera_id, runtime)
|
|
|
|
return True
|
|
|
|
for cam_id, runtime in self.cameras_runtime.items():
|
|
backend = runtime.get("backend")
|
|
|
|
if backend == "picamera2":
|
|
self._apply_controls_picamera2(cam_id, runtime)
|
|
elif backend == "opencv":
|
|
self._apply_controls_opencv(cam_id, runtime)
|
|
|
|
return True
|
|
|
|
def _apply_controls_picamera2(self, camera_id: str, runtime):
|
|
picam2 = runtime.get("picam2")
|
|
if picam2 is None:
|
|
return
|
|
|
|
cam = self.state.get_camera(camera_id)
|
|
ctrl = self.state.get_camera_controls(camera_id)
|
|
|
|
controls = {}
|
|
frame_us = int(1_000_000 / max(1, self.state.fps or 10))
|
|
|
|
controls["FrameDurationLimits"] = (frame_us, frame_us)
|
|
controls["AeEnable"] = bool(ctrl.ae_enable)
|
|
|
|
# Só RGB deve realmente usar AWB/ColourGains
|
|
if cam is not None and cam.role == "rgb":
|
|
controls["AwbEnable"] = bool(ctrl.awb_enable)
|
|
|
|
if not ctrl.ae_enable:
|
|
if ctrl.exposure_time_us is not None:
|
|
controls["ExposureTime"] = int(ctrl.exposure_time_us)
|
|
if ctrl.analogue_gain is not None:
|
|
controls["AnalogueGain"] = float(ctrl.analogue_gain)
|
|
|
|
if (
|
|
cam is not None
|
|
and cam.role == "rgb"
|
|
and not ctrl.awb_enable
|
|
and ctrl.colour_gains is not None
|
|
):
|
|
r_gain, b_gain = ctrl.colour_gains
|
|
controls["ColourGains"] = (float(r_gain), float(b_gain))
|
|
|
|
try:
|
|
picam2.set_controls(controls)
|
|
except Exception as e:
|
|
print(f"[ERRO CONTROLS PICAM2] cam={camera_id} err={e} | controls={controls}")
|
|
|
|
def _apply_controls_opencv(self, camera_id: str, runtime):
|
|
cap = runtime.get("cap")
|
|
if cap is None:
|
|
return
|
|
|
|
cam = self.state.get_camera(camera_id)
|
|
ctrl = self.state.get_camera_controls(camera_id)
|
|
|
|
device = runtime.get("device")
|
|
if not device:
|
|
device = getattr(cam, "device_path", None) if cam is not None else None
|
|
if not device:
|
|
device = f"/dev/video{runtime['camera_index']}"
|
|
|
|
# Sempre protege FPS
|
|
self._v4l2_set_ctrls(device, {
|
|
"exposure_dynamic_framerate": 0,
|
|
})
|
|
|
|
# AE
|
|
if ctrl.ae_enable:
|
|
self._v4l2_set_ctrls(device, {
|
|
"auto_exposure": 3,
|
|
})
|
|
else:
|
|
self._v4l2_set_ctrls(device, {
|
|
"auto_exposure": 1,
|
|
})
|
|
|
|
if ctrl.exposure_time_us is not None:
|
|
# V4L2 exposure_time_absolute é em unidades de 100 us
|
|
exp_abs = int(ctrl.exposure_time_us / 100)
|
|
exp_abs = max(1, min(5000, exp_abs))
|
|
|
|
self._v4l2_set_ctrls(device, {
|
|
"exposure_time_absolute": exp_abs,
|
|
})
|
|
|
|
# Ganho
|
|
if ctrl.analogue_gain is not None:
|
|
gain = int(max(0, min(100, ctrl.analogue_gain)))
|
|
self._v4l2_set_ctrls(device, {
|
|
"gain": gain,
|
|
})
|
|
|
|
# AWB
|
|
if cam is not None and cam.role == "rgb":
|
|
self._v4l2_set_ctrls(device, {
|
|
"white_balance_automatic": 1 if ctrl.awb_enable else 0,
|
|
})
|
|
|
|
if not ctrl.awb_enable and ctrl.colour_gains is not None:
|
|
# Aqui não dá para aplicar r_gain/b_gain diretamente nessa webcam.
|
|
# Ela só tem white_balance_temperature.
|
|
pass
|
|
|
|
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))
|
|
cap.set(cv2.CAP_PROP_FPS, float(self.state.fps))
|
|
|
|
def _v4l2_set_ctrls(self, device, controls: dict):
|
|
args = ["v4l2-ctl", "-d", device]
|
|
for k, v in controls.items():
|
|
args += ["-c", f"{k}={v}"]
|
|
|
|
try:
|
|
subprocess.run(args, check=False, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
|
|
except Exception:
|
|
pass
|
|
|
|
# =========================================================
|
|
# Sensor modes
|
|
# =========================================================
|
|
|
|
def get_sensor_modes(self):
|
|
if self._sensor_modes_cache is not None:
|
|
return self._sensor_modes_cache
|
|
|
|
result = []
|
|
|
|
for cam in self.state.cameras:
|
|
if getattr(cam, "interface", "CSI").upper() != "CSI":
|
|
continue
|
|
|
|
temp = None
|
|
try:
|
|
temp = Picamera2(camera_num=cam.index)
|
|
modes = temp.sensor_modes
|
|
|
|
cam_modes = []
|
|
for i, m in enumerate(modes):
|
|
cam_modes.append({
|
|
"camera_id": cam.id,
|
|
"camera_index": cam.index,
|
|
"role": cam.role,
|
|
"interface": cam.interface,
|
|
"mode_index": i,
|
|
"format": str(m.get("format")) if m.get("format") is not None else None,
|
|
"size": list(m.get("size")) if m.get("size") is not None else None,
|
|
"bit_depth": m.get("bit_depth"),
|
|
"fps": m.get("fps"),
|
|
"crop_limits": list(m.get("crop_limits")) if m.get("crop_limits") is not None else None,
|
|
"exposure_limits": list(m.get("exposure_limits")) if m.get("exposure_limits") is not None else None,
|
|
})
|
|
|
|
result.extend(cam_modes)
|
|
|
|
except Exception as e:
|
|
result.append({
|
|
"camera_id": cam.id,
|
|
"camera_index": cam.index,
|
|
"role": cam.role,
|
|
"interface": cam.interface,
|
|
"error": str(e),
|
|
})
|
|
|
|
finally:
|
|
if temp is not None:
|
|
try:
|
|
temp.close()
|
|
except Exception:
|
|
pass
|
|
|
|
self._sensor_modes_cache = result
|
|
return result
|
|
|
|
# =========================================================
|
|
# Encerramento
|
|
# =========================================================
|
|
|
|
def stop(self):
|
|
with self.camera_lock:
|
|
for runtime in self.cameras_runtime.values():
|
|
runtime["stop_event"].set()
|
|
|
|
for runtime in self.cameras_runtime.values():
|
|
t = runtime.get("thread")
|
|
if t:
|
|
t.join(timeout=1.5)
|
|
|
|
for runtime in self.cameras_runtime.values():
|
|
cam = runtime.get("picam2")
|
|
if cam:
|
|
try:
|
|
cam.stop()
|
|
except Exception:
|
|
pass
|
|
try:
|
|
cam.close()
|
|
except Exception:
|
|
pass
|
|
|
|
cap = runtime.get("cap")
|
|
if cap:
|
|
try:
|
|
cap.release()
|
|
except Exception:
|
|
pass
|
|
|
|
self.cameras_runtime.clear()
|
|
self.initialized = False
|
|
self._reconfigure_needed = False
|
|
self._sensor_modes_cache = None
|
|
|
|
for cam in self.state.cameras:
|
|
self.state.set_camera_connected(cam.index, False) |