256 lines
8.1 KiB
Python
256 lines
8.1 KiB
Python
from picamera2 import Picamera2
|
|
from threading import Lock, RLock
|
|
import threading
|
|
import time
|
|
import numpy as np
|
|
|
|
|
|
class CameraManager:
|
|
def __init__(self, state):
|
|
self.state = state
|
|
self.initialized = False
|
|
|
|
self.camera_lock = RLock()
|
|
self.frame_lock = Lock()
|
|
|
|
self.cameras_runtime = {}
|
|
self._reconfigure_needed = False
|
|
self._sensor_modes_cache = None
|
|
|
|
def mark_reconfigure_needed(self):
|
|
with self.camera_lock:
|
|
self._reconfigure_needed = True
|
|
|
|
def _init_camera_runtime(self, cam_spec):
|
|
return {
|
|
"picam2": None,
|
|
"last_frame": None,
|
|
"frame_id": 0,
|
|
"frame_ts": None,
|
|
"buffer": None,
|
|
"stop_event": threading.Event(),
|
|
"thread": None
|
|
}
|
|
|
|
def _get_required_camera_ids(self):
|
|
frame_type = self.state.frame_type
|
|
resolved_mode = self.state.resolve_capture_mode()
|
|
|
|
if frame_type == "RGB":
|
|
return ["cam0"]
|
|
|
|
if frame_type == "RGBNIR":
|
|
return ["cam0", "cam1"]
|
|
|
|
if frame_type == "RAW_BRUTO":
|
|
if resolved_mode == "DUAL":
|
|
return ["cam0", "cam1"]
|
|
return ["cam0"]
|
|
|
|
return ["cam0"]
|
|
|
|
def begin(self):
|
|
with self.camera_lock:
|
|
self.stop()
|
|
|
|
required_ids = set(self._get_required_camera_ids())
|
|
|
|
for cam in self.state.cameras:
|
|
if cam.id not in required_ids:
|
|
self.state.set_camera_connected(cam.index, False)
|
|
continue
|
|
|
|
try:
|
|
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 = self._init_camera_runtime(cam)
|
|
runtime["picam2"] = picam2
|
|
self.cameras_runtime[cam.id] = runtime
|
|
|
|
self.state.set_camera_connected(
|
|
cam.index,
|
|
True,
|
|
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}): {e}")
|
|
self.state.set_camera_connected(cam.index, False)
|
|
|
|
for cam_id in self.state.active_camera_ids:
|
|
self._start_thread(cam_id)
|
|
|
|
self.initialized = len(self.cameras_runtime) > 0
|
|
self._reconfigure_needed = False
|
|
|
|
return self.initialized
|
|
|
|
def _start_thread(self, cam_id):
|
|
runtime = self.cameras_runtime[cam_id]
|
|
runtime["stop_event"].clear()
|
|
|
|
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]
|
|
|
|
while not runtime["stop_event"].is_set():
|
|
request = None
|
|
try:
|
|
picam2 = runtime["picam2"]
|
|
request = picam2.capture_request()
|
|
|
|
raw = request.make_array("raw")
|
|
|
|
with self.frame_lock:
|
|
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)
|
|
|
|
runtime["last_frame"] = runtime["buffer"]
|
|
runtime["frame_id"] += 1
|
|
runtime["frame_ts"] = time.perf_counter()
|
|
|
|
except Exception as e:
|
|
print(f"[ERRO LOOP {cam_id}] {e}")
|
|
time.sleep(0.05)
|
|
|
|
finally:
|
|
if request is not None:
|
|
try:
|
|
request.release()
|
|
except Exception:
|
|
pass
|
|
|
|
def capture_raw_frames(self):
|
|
result = {}
|
|
|
|
with self.frame_lock:
|
|
for cam_id, runtime in self.cameras_runtime.items():
|
|
if runtime["last_frame"] is None:
|
|
continue
|
|
|
|
frame = runtime["last_frame"]
|
|
h, w = frame.shape[:2]
|
|
|
|
result[cam_id] = (
|
|
frame,
|
|
w,
|
|
h,
|
|
1,
|
|
runtime["frame_id"],
|
|
runtime["frame_ts"]
|
|
)
|
|
|
|
return result
|
|
|
|
def apply_controls(self):
|
|
with self.camera_lock:
|
|
for runtime in self.cameras_runtime.values():
|
|
picam2 = runtime.get("picam2")
|
|
if picam2 is None:
|
|
continue
|
|
|
|
controls = {}
|
|
frame_us = int(1_000_000 / max(1, self.state.fps or 10))
|
|
controls["FrameDurationLimits"] = (frame_us, frame_us)
|
|
controls["AeEnable"] = bool(self.state.ae_enable)
|
|
controls["AwbEnable"] = bool(self.state.awb_enable)
|
|
|
|
if not self.state.ae_enable:
|
|
if self.state.exposure_time_us is not None:
|
|
controls["ExposureTime"] = int(self.state.exposure_time_us)
|
|
if self.state.analogue_gain is not None:
|
|
controls["AnalogueGain"] = float(self.state.analogue_gain)
|
|
|
|
if not self.state.awb_enable and self.state.colour_gains is not None:
|
|
r_gain, b_gain = self.state.colour_gains
|
|
controls["ColourGains"] = (float(r_gain), float(b_gain))
|
|
|
|
try:
|
|
picam2.set_controls(controls)
|
|
except Exception as e:
|
|
print(f"[ERRO CONTROLS] {e} | controls={controls}")
|
|
|
|
return True
|
|
|
|
def get_sensor_modes(self):
|
|
if self._sensor_modes_cache is not None:
|
|
return self._sensor_modes_cache
|
|
|
|
try:
|
|
temp = Picamera2(camera_num=0)
|
|
modes = temp.sensor_modes
|
|
result = []
|
|
|
|
for i, m in enumerate(modes):
|
|
result.append({
|
|
"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,
|
|
})
|
|
|
|
self._sensor_modes_cache = result
|
|
return result
|
|
|
|
finally:
|
|
try:
|
|
temp.close()
|
|
except Exception:
|
|
pass
|
|
|
|
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)
|
|
|
|
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
|
|
|
|
self.cameras_runtime.clear()
|
|
self.initialized = False
|
|
self._reconfigure_needed = False
|
|
|
|
for cam in self.state.cameras:
|
|
self.state.set_camera_connected(cam.index, False) |