279 lines
8.8 KiB
Python
279 lines
8.8 KiB
Python
|
from pathlib import Path
|
||
|
|
from datetime import datetime
|
||
|
|
import subprocess
|
||
|
|
from picamera2 import Picamera2
|
||
|
|
import base64
|
||
|
|
import io
|
||
|
|
from PIL import Image
|
||
|
|
from threading import Lock, RLock
|
||
|
|
import threading
|
||
|
|
import time
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
|
||
|
|
class CameraManager:
|
||
|
|
def __init__(self, state):
|
||
|
|
self.state = state
|
||
|
|
self.picam2 = None
|
||
|
|
self.initialized = False
|
||
|
|
|
||
|
|
self.output_dir = Path("/tmp/multispec_captures")
|
||
|
|
self.output_dir.mkdir(parents=True, exist_ok=True)
|
||
|
|
|
||
|
|
self.last_raw_frame = None
|
||
|
|
self.last_raw_frame_id = 0
|
||
|
|
self.last_raw_frame_ts = None
|
||
|
|
self._raw_buffer = None
|
||
|
|
self.frame_lock = Lock()
|
||
|
|
self.camera_lock = RLock()
|
||
|
|
|
||
|
|
self._stop_event = threading.Event()
|
||
|
|
self._stream_thread = None
|
||
|
|
self._sensor_modes_cache = None
|
||
|
|
|
||
|
|
self.current_width = None
|
||
|
|
self.current_height = None
|
||
|
|
self.current_fps = None
|
||
|
|
self._reconfigure_needed = False
|
||
|
|
|
||
|
|
def mark_reconfigure_needed(self):
|
||
|
|
with self.camera_lock:
|
||
|
|
self._reconfigure_needed = True
|
||
|
|
|
||
|
|
def needs_reconfigure(self):
|
||
|
|
return (
|
||
|
|
self._reconfigure_needed or
|
||
|
|
not self.initialized or
|
||
|
|
self.current_width != self.state.width or
|
||
|
|
self.current_height != self.state.height or
|
||
|
|
self.current_fps != self.state.fps
|
||
|
|
)
|
||
|
|
|
||
|
|
def begin(self):
|
||
|
|
with self.camera_lock:
|
||
|
|
if self.picam2 is not None and self.needs_reconfigure():
|
||
|
|
self.stop()
|
||
|
|
|
||
|
|
if self.initialized and self.picam2 is not None:
|
||
|
|
return True
|
||
|
|
|
||
|
|
self.picam2 = Picamera2()
|
||
|
|
|
||
|
|
config = self.picam2.create_video_configuration(
|
||
|
|
main={"size": (640, 480), "format": "RGB888"},
|
||
|
|
raw={"size": (self.state.width, self.state.height)},
|
||
|
|
buffer_count=6
|
||
|
|
)
|
||
|
|
|
||
|
|
if config is None:
|
||
|
|
self.picam2 = None
|
||
|
|
raise RuntimeError("Falha ao criar configuração da câmera. Verifique se a resolução é suportada.")
|
||
|
|
|
||
|
|
self.picam2.configure(config)
|
||
|
|
self.picam2.start()
|
||
|
|
|
||
|
|
self.initialized = True
|
||
|
|
self.current_width = self.state.width
|
||
|
|
self.current_height = self.state.height
|
||
|
|
self.current_fps = self.state.fps
|
||
|
|
self._reconfigure_needed = False
|
||
|
|
|
||
|
|
self.apply_controls()
|
||
|
|
|
||
|
|
with self.frame_lock:
|
||
|
|
self.last_raw_frame = None
|
||
|
|
self.last_raw_frame_id = 0
|
||
|
|
self.last_raw_frame_ts = None
|
||
|
|
|
||
|
|
self._stop_event.clear()
|
||
|
|
self._stream_thread = threading.Thread(target=self._update_loop, daemon=True)
|
||
|
|
self._stream_thread.start()
|
||
|
|
|
||
|
|
if not self.wait_first_frame(timeout_s=2.0):
|
||
|
|
self.stop()
|
||
|
|
raise RuntimeError("Câmera inicializada, mas nenhum frame foi recebido a tempo")
|
||
|
|
|
||
|
|
return True
|
||
|
|
|
||
|
|
def _update_loop(self):
|
||
|
|
print("[DEBUG] Loop de atualização de frames iniciado")
|
||
|
|
|
||
|
|
while not self._stop_event.is_set():
|
||
|
|
request = None
|
||
|
|
try:
|
||
|
|
with self.camera_lock:
|
||
|
|
if self.picam2 is None:
|
||
|
|
time.sleep(0.05)
|
||
|
|
continue
|
||
|
|
request = self.picam2.capture_request()
|
||
|
|
|
||
|
|
raw_array = request.make_array("raw")
|
||
|
|
self.last_raw_shape = raw_array.shape
|
||
|
|
self.last_raw_dtype = raw_array.dtype
|
||
|
|
|
||
|
|
with self.frame_lock:
|
||
|
|
if (
|
||
|
|
self._raw_buffer is None or
|
||
|
|
self._raw_buffer.shape != raw_array.shape or
|
||
|
|
self._raw_buffer.dtype != raw_array.dtype
|
||
|
|
):
|
||
|
|
self._raw_buffer = raw_array.copy()
|
||
|
|
else:
|
||
|
|
np.copyto(self._raw_buffer, raw_array)
|
||
|
|
|
||
|
|
self.last_raw_frame = self._raw_buffer
|
||
|
|
self.last_raw_frame_id += 1
|
||
|
|
self.last_raw_frame_ts = time.perf_counter()
|
||
|
|
|
||
|
|
except Exception as e:
|
||
|
|
print(f"[ERRO LOOP] {e}")
|
||
|
|
time.sleep(0.1)
|
||
|
|
finally:
|
||
|
|
if request is not None:
|
||
|
|
try:
|
||
|
|
request.release()
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
|
||
|
|
def stop(self):
|
||
|
|
with self.camera_lock:
|
||
|
|
self._stop_event.set()
|
||
|
|
|
||
|
|
if self._stream_thread is not None:
|
||
|
|
self._stream_thread.join(timeout=1.5)
|
||
|
|
self._stream_thread = None
|
||
|
|
|
||
|
|
if self.picam2 is not None:
|
||
|
|
try:
|
||
|
|
self.picam2.stop()
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
try:
|
||
|
|
self.picam2.close()
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
|
||
|
|
self.picam2 = None
|
||
|
|
self.initialized = False
|
||
|
|
self._reconfigure_needed = False
|
||
|
|
|
||
|
|
self.current_width = None
|
||
|
|
self.current_height = None
|
||
|
|
self.current_fps = None
|
||
|
|
|
||
|
|
with self.frame_lock:
|
||
|
|
self.last_raw_frame = None
|
||
|
|
|
||
|
|
with self.frame_lock:
|
||
|
|
self.last_raw_frame = None
|
||
|
|
self.last_raw_frame_id = 0
|
||
|
|
self.last_raw_frame_ts = None
|
||
|
|
self._raw_buffer = None
|
||
|
|
|
||
|
|
def capture_raw_frame(self):
|
||
|
|
with self.frame_lock:
|
||
|
|
if self.last_raw_frame is None:
|
||
|
|
return None, 0, 0, 0, 0, None
|
||
|
|
|
||
|
|
frame = self.last_raw_frame
|
||
|
|
frame_id = self.last_raw_frame_id
|
||
|
|
frame_ts = self.last_raw_frame_ts
|
||
|
|
h, w = frame.shape[:2]
|
||
|
|
|
||
|
|
return {
|
||
|
|
"frame": frame,
|
||
|
|
"width": w,
|
||
|
|
"height": h,
|
||
|
|
"channels": 1,
|
||
|
|
"frame_id": frame_id,
|
||
|
|
"timestamp": frame_ts,
|
||
|
|
}
|
||
|
|
|
||
|
|
def _build_controls_from_state(self):
|
||
|
|
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))
|
||
|
|
|
||
|
|
return controls
|
||
|
|
|
||
|
|
def apply_controls(self):
|
||
|
|
with self.camera_lock:
|
||
|
|
if self.picam2 is None:
|
||
|
|
return False
|
||
|
|
|
||
|
|
controls = self._build_controls_from_state()
|
||
|
|
try:
|
||
|
|
self.picam2.set_controls(controls)
|
||
|
|
return True
|
||
|
|
except Exception as e:
|
||
|
|
print(f"[ERRO CONTROLS] {e} | controls={controls}")
|
||
|
|
return False
|
||
|
|
|
||
|
|
def get_sensor_modes(self):
|
||
|
|
if self._sensor_modes_cache:
|
||
|
|
return self._sensor_modes_cache
|
||
|
|
|
||
|
|
temp_picam2 = None
|
||
|
|
try:
|
||
|
|
if self.picam2 is not None:
|
||
|
|
cam = self.picam2
|
||
|
|
else:
|
||
|
|
temp_picam2 = Picamera2()
|
||
|
|
cam = temp_picam2
|
||
|
|
|
||
|
|
modes = cam.sensor_modes
|
||
|
|
result = []
|
||
|
|
|
||
|
|
for i, m in enumerate(modes):
|
||
|
|
fmt = str(m.get("format")) if m.get("format") is not None else None
|
||
|
|
size = m.get("size")
|
||
|
|
bit_depth = m.get("bit_depth")
|
||
|
|
fps = m.get("fps")
|
||
|
|
crop_limits = m.get("crop_limits")
|
||
|
|
exposure_limits = m.get("exposure_limits")
|
||
|
|
|
||
|
|
result.append({
|
||
|
|
"index": i,
|
||
|
|
"format": fmt,
|
||
|
|
"size": list(size) if size is not None else None,
|
||
|
|
"bit_depth": bit_depth,
|
||
|
|
"fps": fps,
|
||
|
|
"crop_limits": list(crop_limits) if crop_limits is not None else None,
|
||
|
|
"exposure_limits": list(exposure_limits) if exposure_limits is not None else None,
|
||
|
|
})
|
||
|
|
|
||
|
|
self._sensor_modes_cache = result
|
||
|
|
return result
|
||
|
|
|
||
|
|
finally:
|
||
|
|
if temp_picam2 is not None:
|
||
|
|
try:
|
||
|
|
temp_picam2.close()
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
|
||
|
|
def wait_first_frame(self, timeout_s=2.0):
|
||
|
|
t0 = time.perf_counter()
|
||
|
|
|
||
|
|
while time.perf_counter() - t0 < timeout_s:
|
||
|
|
with self.frame_lock:
|
||
|
|
if self.last_raw_frame is not None:
|
||
|
|
return True
|
||
|
|
time.sleep(0.01)
|
||
|
|
|
||
|
|
return False
|