From a148ee5da5d9992d609d734c7bfb26d8ffdf37b6 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Thu, 23 Apr 2026 09:16:58 -0300 Subject: [PATCH] Ajustes finais no modulo multiespectral --- .../datasets/multiespec_module/_0_capture.py | 4 +- .../multiespec_module/_9_test_segformer.py | 58 +++++++++--- .../multispec_segformer_service.py | 15 +--- .../multispectral_service.py | 55 ++++++------ .../multiespec_module/stream_receiver.py | 19 ++-- Python/raspi/cam_3/multispectral_service.py | 55 ++++++------ Python/raspi/cam_3/pi/camera_manager.py | 63 +++++++++++-- Python/raspi/cam_3/pi/state.py | 88 +++++++++++++++---- Python/raspi/cam_3/stream_receiver.py | 19 ++-- 9 files changed, 250 insertions(+), 126 deletions(-) diff --git a/Python/OAK/datasets/multiespec_module/_0_capture.py b/Python/OAK/datasets/multiespec_module/_0_capture.py index 425abe06b..b2edcf7e1 100644 --- a/Python/OAK/datasets/multiespec_module/_0_capture.py +++ b/Python/OAK/datasets/multiespec_module/_0_capture.py @@ -376,9 +376,7 @@ def main(): print(f"[INFO] Verificando conexão com o módulo em {args.pi_host}:{args.server_port}...") - svc.ensure_alive() - - if not svc.is_alive(): + if not svc.check_connection(2): raise RuntimeError("Módulo não encontrado ou não respondeu ao ping.") print("[OK] Módulo conectado e respondendo.") diff --git a/Python/OAK/datasets/multiespec_module/_9_test_segformer.py b/Python/OAK/datasets/multiespec_module/_9_test_segformer.py index 2c8760b23..83863bade 100644 --- a/Python/OAK/datasets/multiespec_module/_9_test_segformer.py +++ b/Python/OAK/datasets/multiespec_module/_9_test_segformer.py @@ -212,6 +212,9 @@ def main(): receiver = StreamReceiver(host="0.0.0.0", port=STREAM_PORT) svc = MultiSpectralService(host=PI_HOST, port=5000, timeout=10) + if not svc.check_connection(2): + raise RuntimeError("Módulo não encontrado ou não respondeu ao ping.") + receiver.start() time.sleep(0.5) @@ -239,6 +242,11 @@ def main(): last_frame_id = -1 + fps_pc = 0.0 + fps_pi = 0.0 + last_pc_frame_ts = None + fps_smooth = 0.15 + try: while True: meta = receiver.last_meta @@ -252,6 +260,27 @@ def main(): continue last_frame_id = frame_id + now_pc = time.perf_counter() + + # FPS real percebido no PC + if last_pc_frame_ts is not None: + dt_pc = now_pc - last_pc_frame_ts + if dt_pc > 0: + inst_fps_pc = 1.0 / dt_pc + if fps_pc <= 0: + fps_pc = inst_fps_pc + else: + fps_pc = (1.0 - fps_smooth) * fps_pc + fps_smooth * inst_fps_pc + last_pc_frame_ts = now_pc + + # FPS reportado pelo módulo/Pi + dt_frame_period = meta.get("dt_frame_period") + if dt_frame_period is not None and dt_frame_period > 0: + inst_fps_pi = 1.0 / float(dt_frame_period) + if fps_pi <= 0: + fps_pi = inst_fps_pi + else: + fps_pi = (1.0 - fps_smooth) * fps_pi + fps_smooth * inst_fps_pi try: raw_np = core.build_infer_tensor_from_stream(frame, meta, channels_expected=CHANNELS) @@ -264,17 +293,26 @@ def main(): # ========================================================= # HUD # ========================================================= - txt = f"C={CHANNELS} | inf={t_inf:.1f}ms | pvw={t_pvw:.1f}ms" + lines = [ + f"C={CHANNELS}", + f"fps_pc={fps_pc:.1f} | fps_pi={fps_pi:.1f}", + f"inf={t_inf:.1f}ms | pvw={t_pvw:.1f}ms" + ] - cv2.putText( - overlay, - txt, - (10, 24), - cv2.FONT_HERSHEY_SIMPLEX, - 0.7, - (0, 255, 0), - 2, - ) + y0 = 24 + dy = 28 # espaçamento entre linhas + + for i, line in enumerate(lines): + y = y0 + i * dy + cv2.putText( + overlay, + line, + (10, y), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 255, 0), + 2, + ) cv2.imshow(win, overlay) diff --git a/Python/OAK/datasets/multiespec_module/multispec_segformer_service.py b/Python/OAK/datasets/multiespec_module/multispec_segformer_service.py index 787d5290d..223094252 100644 --- a/Python/OAK/datasets/multiespec_module/multispec_segformer_service.py +++ b/Python/OAK/datasets/multiespec_module/multispec_segformer_service.py @@ -563,7 +563,10 @@ class MultispecSegformerService: if self._buf_shape == shape: return self._buf_shape = shape - self._cpu_pinned = torch.empty(shape, dtype=torch.float32, pin_memory=True) + if self.device.type == "cuda": + self._cpu_pinned = torch.empty(shape, dtype=torch.float32, pin_memory=True) + else: + self._cpu_pinned = torch.empty(shape, dtype=torch.float32) self._gpu_input = torch.empty(shape, dtype=torch.float32, device=self.device) def _load_model(self, backbone: str) -> torch.nn.Module: @@ -589,16 +592,6 @@ class MultispecSegformerService: raise ValueError(f"fusion_mode inválido: {self.fusion_mode}") return model - def _preprocess_tensor(self, raw_np: np.ndarray) -> torch.Tensor: - if raw_np.dtype != np.float32: - raw_np = raw_np.astype(np.float32) - x = torch.from_numpy(raw_np).unsqueeze(0).to(self.device) - if self._norm_mean is not None and self._norm_std is not None: - x = (x - self._norm_mean) / self._norm_std - else: - x = normalize_per_batch(x) - return x - def _build_preview(self, raw_np, pred_ids, alpha, fast): t0 = time.time() bgr = make_bgr_preview_from_tensor(raw_np, preview_fast=fast) diff --git a/Python/OAK/datasets/multiespec_module/multispectral_service.py b/Python/OAK/datasets/multiespec_module/multispectral_service.py index 18578fefc..2492e22b8 100644 --- a/Python/OAK/datasets/multiespec_module/multispectral_service.py +++ b/Python/OAK/datasets/multiespec_module/multispectral_service.py @@ -3,7 +3,7 @@ import json import numpy as np import base64 import time -from typing import Optional, Any +from typing import Optional class MultiSpectralService: @@ -50,40 +50,35 @@ class MultiSpectralService: self.sock = None def check_connection(self, timeout: float = None) -> bool: + old_timeout = self.timeout + old_sock_timeout = None + try: + if timeout is not None: + self.timeout = timeout + self.connect() + + if timeout is not None and self.sock is not None: + old_sock_timeout = self.sock.gettimeout() + self.sock.settimeout(timeout) + resp = self._send_command({"cmd": "ping"}) return resp.get("ok") and resp.get("reply") == "pong" + except Exception: return False - def is_alive(self) -> bool: - try: - self.connect() - resp = self._send_command({"cmd": "ping"}) - return resp.get("ok") and resp.get("reply") == "pong" - except Exception: - return False + finally: + # restaura timeout do socket + if old_sock_timeout is not None and self.sock is not None: + try: + self.sock.settimeout(old_sock_timeout) + except Exception: + pass - def ensure_alive(self): - try: - self.connect() - except Exception as e: - raise RuntimeError( - f"Não foi possível conectar ao módulo em {self.host}:{self.port}. " - f"Verifique rede, IP e se o Pi está ligado. Erro: {e}" - ) from e - - try: - resp = self._send_command({"cmd": "ping"}) - except Exception as e: - raise RuntimeError( - f"Conectou ao endereço {self.host}:{self.port}, mas o módulo não respondeu ao ping. " - f"Verifique se o serviço está rodando no Pi. Erro: {e}" - ) from e - - if not resp.get("ok") or resp.get("reply") != "pong": - raise RuntimeError(f"Resposta inválida do módulo ao ping: {resp}") + # restaura timeout do serviço + self.timeout = old_timeout def _send_command(self, payload: dict) -> dict: if self.sock is None: @@ -176,8 +171,10 @@ class MultiSpectralService: shape = part_meta.get("shape") if dtype_str == "multi" or dtype_str is None: - # fallback conservador - dtype_str = "uint8" if int(cam_meta.get("channels", 1)) > 1 else "uint16" + # fallback conservador: + # no protocolo atual do Pi, payload multi nativo trafega bytes crus, + # inclusive mono packed, então o mais seguro é assumir uint8. + dtype_str = "uint8" if shape: arr = self._reshape_array_from_shape(raw, dtype_str, shape) diff --git a/Python/OAK/datasets/multiespec_module/stream_receiver.py b/Python/OAK/datasets/multiespec_module/stream_receiver.py index e43253c21..d5996026b 100644 --- a/Python/OAK/datasets/multiespec_module/stream_receiver.py +++ b/Python/OAK/datasets/multiespec_module/stream_receiver.py @@ -54,6 +54,10 @@ class StreamReceiver: except Exception: pass + if self._thread is not None: + self._thread.join(timeout=2.0) + self._thread = None + self._client_sock = None self._server_sock = None @@ -156,15 +160,6 @@ class StreamReceiver: return mapping[dtype_str] - def _numpy_dtype_from_header(self, header: dict): - dtype_str = header.get("dtype") or header.get("output_dtype") or "uint8" - - if dtype_str == "multi": - # fallback conservador para protocolos mais antigos - return np.uint16 - - return self._numpy_dtype_from_string(dtype_str) - def _reshape_from_shape(self, payload: bytes, dtype_str: str, shape): dtype = self._numpy_dtype_from_string(dtype_str) arr = np.frombuffer(payload, dtype=dtype) @@ -239,8 +234,10 @@ class StreamReceiver: shape = part.get("shape") if dtype_str == "multi" or dtype_str is None: - channels = int(part.get("channels", cam_meta.get("channels", 1))) - dtype_str = "uint8" if channels > 1 else "uint16" + # fallback conservador: + # no protocolo atual do Pi, payload multi nativo trafega bytes crus, + # inclusive mono packed, então o mais seguro é assumir uint8. + dtype_str = "uint8" if shape: frame = self._reshape_from_shape(part_bytes, dtype_str, shape) diff --git a/Python/raspi/cam_3/multispectral_service.py b/Python/raspi/cam_3/multispectral_service.py index 18578fefc..2492e22b8 100644 --- a/Python/raspi/cam_3/multispectral_service.py +++ b/Python/raspi/cam_3/multispectral_service.py @@ -3,7 +3,7 @@ import json import numpy as np import base64 import time -from typing import Optional, Any +from typing import Optional class MultiSpectralService: @@ -50,40 +50,35 @@ class MultiSpectralService: self.sock = None def check_connection(self, timeout: float = None) -> bool: + old_timeout = self.timeout + old_sock_timeout = None + try: + if timeout is not None: + self.timeout = timeout + self.connect() + + if timeout is not None and self.sock is not None: + old_sock_timeout = self.sock.gettimeout() + self.sock.settimeout(timeout) + resp = self._send_command({"cmd": "ping"}) return resp.get("ok") and resp.get("reply") == "pong" + except Exception: return False - def is_alive(self) -> bool: - try: - self.connect() - resp = self._send_command({"cmd": "ping"}) - return resp.get("ok") and resp.get("reply") == "pong" - except Exception: - return False + finally: + # restaura timeout do socket + if old_sock_timeout is not None and self.sock is not None: + try: + self.sock.settimeout(old_sock_timeout) + except Exception: + pass - def ensure_alive(self): - try: - self.connect() - except Exception as e: - raise RuntimeError( - f"Não foi possível conectar ao módulo em {self.host}:{self.port}. " - f"Verifique rede, IP e se o Pi está ligado. Erro: {e}" - ) from e - - try: - resp = self._send_command({"cmd": "ping"}) - except Exception as e: - raise RuntimeError( - f"Conectou ao endereço {self.host}:{self.port}, mas o módulo não respondeu ao ping. " - f"Verifique se o serviço está rodando no Pi. Erro: {e}" - ) from e - - if not resp.get("ok") or resp.get("reply") != "pong": - raise RuntimeError(f"Resposta inválida do módulo ao ping: {resp}") + # restaura timeout do serviço + self.timeout = old_timeout def _send_command(self, payload: dict) -> dict: if self.sock is None: @@ -176,8 +171,10 @@ class MultiSpectralService: shape = part_meta.get("shape") if dtype_str == "multi" or dtype_str is None: - # fallback conservador - dtype_str = "uint8" if int(cam_meta.get("channels", 1)) > 1 else "uint16" + # fallback conservador: + # no protocolo atual do Pi, payload multi nativo trafega bytes crus, + # inclusive mono packed, então o mais seguro é assumir uint8. + dtype_str = "uint8" if shape: arr = self._reshape_array_from_shape(raw, dtype_str, shape) diff --git a/Python/raspi/cam_3/pi/camera_manager.py b/Python/raspi/cam_3/pi/camera_manager.py index 314bdcf8a..2b2b49d92 100644 --- a/Python/raspi/cam_3/pi/camera_manager.py +++ b/Python/raspi/cam_3/pi/camera_manager.py @@ -50,6 +50,7 @@ class CameraManager: "last_new_frame_ts": None, "frame_lock": Lock(), + "consecutive_failures": 0, } def _make_frame_signature(self, frame: np.ndarray): @@ -81,11 +82,17 @@ class CameraManager: with self.camera_lock: self.stop() - required_ids = set(self.state.get_required_camera_ids_for_frame_type()) + 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: + 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: if cam.id not in required_ids: self.state.set_camera_connected(cam.index, False) continue @@ -96,7 +103,7 @@ class CameraManager: self.state.set_camera_connected( cam.index, - True, + False, width=cam.width, height=cam.height, bayer_pattern=cam.bayer_pattern, @@ -117,6 +124,12 @@ class CameraManager: 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 > 0: + break + time.sleep(0.02) + self.initialized = len(self.cameras_runtime) > 0 self._reconfigure_needed = False @@ -216,12 +229,16 @@ class CameraManager: 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"] = 1 - runtime["frame_ts"] = time.perf_counter() + 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} " @@ -261,6 +278,7 @@ class CameraManager: 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, @@ -283,8 +301,18 @@ class CameraManager: 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): @@ -296,6 +324,8 @@ class CameraManager: 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 @@ -305,9 +335,20 @@ class CameraManager: else: np.copyto(runtime["buffer"], raw) + read_ts = time.perf_counter() runtime["last_frame"] = runtime["buffer"] runtime["frame_id"] += 1 - runtime["frame_ts"] = time.perf_counter() + 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: @@ -332,6 +373,7 @@ class CameraManager: 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"): @@ -350,6 +392,15 @@ class CameraManager: 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: diff --git a/Python/raspi/cam_3/pi/state.py b/Python/raspi/cam_3/pi/state.py index 232d1bb39..a2bbd1ae8 100644 --- a/Python/raspi/cam_3/pi/state.py +++ b/Python/raspi/cam_3/pi/state.py @@ -142,6 +142,7 @@ class ModuleState: self.stream_port = None self.stream_fps = None self.stream_frame_id = 0 + self.stream_frame_id_sent = 0 # Codec self.codec_family = "none" # numcodecs @@ -200,11 +201,11 @@ class ModuleState: def get_enabled_camera_by_role(self, role: str) -> Optional[CameraSpec]: for cam in self.cameras: - if cam.role == role and cam.enabled: - return cam + if cam.role == role and cam.enabled: + return cam return None - def get_required_camera_ids_for_frame_type(self) -> List[str]: + def get_bootstrap_camera_ids_for_frame_type(self) -> List[str]: def enabled(role: str): return self.get_enabled_camera_by_role(role) @@ -212,6 +213,63 @@ class ModuleState: re_cam = enabled("re") nir_cam = enabled("nir") + if self.frame_type == "RGB": + return [rgb_cam.id] if rgb_cam else [] + + if self.frame_type in ("RAW_BRUTO", "MULTISPEC"): + if self.capture_mode == "TRIPLE": + ids = [] + if rgb_cam: + ids.append(rgb_cam.id) + if re_cam: + ids.append(re_cam.id) + if nir_cam: + ids.append(nir_cam.id) + return ids + + if self.capture_mode == "DOUBLE": + ids = [] + if rgb_cam: + ids.append(rgb_cam.id) + if re_cam: + ids.append(re_cam.id) + elif nir_cam: + ids.append(nir_cam.id) + return ids + + if self.capture_mode == "SINGLE": + for cam in (rgb_cam, re_cam, nir_cam): + if cam: + return [cam.id] + return [] + + if self.capture_mode == "AUTO": + ids = [] + if rgb_cam: + ids.append(rgb_cam.id) + if re_cam: + ids.append(re_cam.id) + if nir_cam: + ids.append(nir_cam.id) + if ids: + return ids + + return [] + + return [] + + return [] + + def get_required_camera_ids_for_frame_type(self) -> List[str]: + def activated(role: str): + return self.get_active_camera_by_role(role) + def enabled(role: str): + return self.get_enabled_camera_by_role(role) + + rgb_cam = activated("rgb") + re_cam = activated("re") + nir_cam = activated("nir") + resolved = self.resolve_capture_mode() if self.frame_type == "RGB": @@ -353,24 +411,17 @@ class ModuleState: self.detected_mode = "NONE" def resolve_capture_mode(self) -> str: - connected_re = self.get_enabled_camera_by_role("re") is not None - connected_nir = self.get_enabled_camera_by_role("nir") is not None - connected_rgb = self.get_enabled_camera_by_role("rgb") is not None + connected_re = self.get_active_camera_by_role("re") is not None + connected_nir = self.get_active_camera_by_role("nir") is not None + connected_rgb = self.get_active_camera_by_role("rgb") is not None if self.capture_mode == "AUTO": - # MULTISPEC e RAW_BRUTO: preferir DOUBLE no automático - if self.frame_type in ("MULTISPEC", "RAW_BRUTO"): - if connected_rgb and (connected_re or connected_nir): - return "DOUBLE" - if self.frame_type == "RAW_BRUTO": - if connected_rgb or connected_re or connected_nir: - return "SINGLE" - return "NONE" - if connected_rgb and connected_re and connected_nir and self.multi_camera_enabled: return "TRIPLE" if connected_rgb and (connected_re or connected_nir): return "DOUBLE" + if connected_re and connected_nir: + return "DOUBLE" if self.frame_type == "RAW_BRUTO" else "NONE" if connected_rgb or connected_re or connected_nir: return "SINGLE" return "NONE" @@ -379,7 +430,11 @@ class ModuleState: return "TRIPLE" if (connected_rgb and connected_re and connected_nir and self.multi_camera_enabled) else "NONE" if self.capture_mode == "DOUBLE": - return "DOUBLE" if (connected_rgb and (connected_re or connected_nir)) else "NONE" + if connected_rgb and (connected_re or connected_nir): + return "DOUBLE" + if self.frame_type == "RAW_BRUTO" and connected_re and connected_nir: + return "DOUBLE" + return "NONE" if self.capture_mode == "SINGLE": return "SINGLE" if (connected_rgb or connected_re or connected_nir) else "NONE" @@ -608,6 +663,7 @@ class ModuleState: "stream_port": self.stream_port, "stream_fps": self.stream_fps, "stream_frame_id": self.stream_frame_id, + "stream_frame_id_sent": self.stream_frame_id_sent, "codec_family": self.codec_family, "codec_name": self.codec_name, diff --git a/Python/raspi/cam_3/stream_receiver.py b/Python/raspi/cam_3/stream_receiver.py index e43253c21..d5996026b 100644 --- a/Python/raspi/cam_3/stream_receiver.py +++ b/Python/raspi/cam_3/stream_receiver.py @@ -54,6 +54,10 @@ class StreamReceiver: except Exception: pass + if self._thread is not None: + self._thread.join(timeout=2.0) + self._thread = None + self._client_sock = None self._server_sock = None @@ -156,15 +160,6 @@ class StreamReceiver: return mapping[dtype_str] - def _numpy_dtype_from_header(self, header: dict): - dtype_str = header.get("dtype") or header.get("output_dtype") or "uint8" - - if dtype_str == "multi": - # fallback conservador para protocolos mais antigos - return np.uint16 - - return self._numpy_dtype_from_string(dtype_str) - def _reshape_from_shape(self, payload: bytes, dtype_str: str, shape): dtype = self._numpy_dtype_from_string(dtype_str) arr = np.frombuffer(payload, dtype=dtype) @@ -239,8 +234,10 @@ class StreamReceiver: shape = part.get("shape") if dtype_str == "multi" or dtype_str is None: - channels = int(part.get("channels", cam_meta.get("channels", 1))) - dtype_str = "uint8" if channels > 1 else "uint16" + # fallback conservador: + # no protocolo atual do Pi, payload multi nativo trafega bytes crus, + # inclusive mono packed, então o mais seguro é assumir uint8. + dtype_str = "uint8" if shape: frame = self._reshape_from_shape(part_bytes, dtype_str, shape)