From b74555301cc96725b19ec4ccfaf670f827a2ae2d Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Fri, 22 May 2026 19:31:00 -0300 Subject: [PATCH] Visual e Weed Workers refatorados V1 --- .../x64/Debug/Parametros/version_files.vsf | 69 +- .../oak_fcc3_core/segformer_service.py | 1184 ++----- .../workers/camera_worker/segformer_runner.py | 908 ++++-- .../workers/shared/gpu_priority_controller.py | 57 +- .../workers/visual_worker/camera_manager.py | 2814 ++++++----------- .../Scripts/workers/visual_worker/config.py | 721 ++++- .../processamento/costmap_fuser.py | 1994 ++++++------ .../processamento/segmentacao_semantica.py | 1598 +++++----- .../processamento/visual_debug_renderer.py | 285 ++ .../processamento/visual_grid_builder.py | 456 +++ .../workers/weed_worker/camera_manager.py | 1804 ++++++----- .../Scripts/workers/weed_worker/config.py | 340 +- .../workers/weed_worker/weed_detector.py | 1021 +++--- 13 files changed, 6570 insertions(+), 6681 deletions(-) create mode 100644 AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_debug_renderer.py create mode 100644 AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_grid_builder.py diff --git a/AgroBase/AgroBase/bin/x64/Debug/Parametros/version_files.vsf b/AgroBase/AgroBase/bin/x64/Debug/Parametros/version_files.vsf index 516cfb9ac..521b80ff2 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Parametros/version_files.vsf +++ b/AgroBase/AgroBase/bin/x64/Debug/Parametros/version_files.vsf @@ -3,58 +3,67 @@ "id": 1, "Arquivo": "model", "Diretorio": "C:\\AgroBaseModels\\Ervas\\", - "Extensao": ".pt", - "Versao": "3_1", + "Extensao": ".onnx", + "Versao": "4_0", "TipoArquivo": 1, - "ArquivoDownload": "models/weed_detector_model-3_1.pt" + "ArquivoDownload": "models/weed_detector_model-4_0.onnx" }, { "id": 2, "Arquivo": "model", "Diretorio": "C:\\AgroBaseModels\\Ervas\\", "Extensao": ".txt", - "Versao": "3_1", + "Versao": "4_0", "TipoArquivo": 1, - "ArquivoDownload": "models/weed_detector_labelmap-3_1.txt" + "ArquivoDownload": "models/weed_detector_labelmap-4_0.txt" }, { "id": 3, "Arquivo": "model", "Diretorio": "C:\\AgroBaseModels\\Ervas\\", "Extensao": ".json", - "Versao": "1_1", + "Versao": "4_0", "TipoArquivo": 1, - "ArquivoDownload": "models/weed_detector_normstats-1_1.json" + "ArquivoDownload": "models/weed_detector_normstats-4_0.json" }, { "id": 4, - "Arquivo": "modelseg", - "Diretorio": "C:\\AgroBaseModels\\Ruas\\", - "Extensao": ".pt", - "Versao": "1_3", - "TipoArquivo": 0, - "ArquivoDownload": "models/street_detector_model_seg-1_3.pt", + "Arquivo": "modelmp", + "Diretorio": "C:\\AgroBaseModels\\Ervas\\", + "Extensao": ".json", + "Versao": "4_0", + "TipoArquivo": 1, + "ArquivoDownload": "models/weed_detector_moduleparams-4_0.json" }, { "id": 5, "Arquivo": "modelseg", "Diretorio": "C:\\AgroBaseModels\\Ruas\\", - "Extensao": ".txt", - "Versao": "1_3", + "Extensao": ".onnx", + "Versao": "2_0", "TipoArquivo": 0, - "ArquivoDownload": "models/street_detector_labelmap_seg-1_3.txt" + "ArquivoDownload": "models/street_detector_model_seg-2_0.onnx", }, { "id": 6, "Arquivo": "modelseg", "Diretorio": "C:\\AgroBaseModels\\Ruas\\", - "Extensao": ".json", - "Versao": "1_1", + "Extensao": ".txt", + "Versao": "2_0", "TipoArquivo": 0, - "ArquivoDownload": "models/street_detector_normstats-1_1.json" + "ArquivoDownload": "models/street_detector_labelmap_seg-2_0.txt" }, { "id": 7, + "Arquivo": "modelseg", + "Diretorio": "C:\\AgroBaseModels\\Ruas\\", + "Extensao": ".json", + "Versao": "1_4", + "TipoArquivo": 0, + "ArquivoDownload": "models/street_detector_normstats-1_4.json" + }, + { + "id": 8, "Arquivo": "modeldet", "Diretorio": "C:\\AgroBaseModels\\Ruas\\", "Extensao": ".blob", @@ -63,7 +72,7 @@ "ArquivoDownload": "models/street_detector_model_det-1_0.blob", }, { - "id": 8, + "id": 9, "Arquivo": "parametersAtu", "Diretorio": "Parametros/", "Extensao": ".par", @@ -72,7 +81,7 @@ "ArquivoDownload": "parameters/parametersAtu-2_0.par", }, { - "id": 9, + "id": 10, "Arquivo": "parametersMvd", "Diretorio": "Parametros/", "Extensao": ".par", @@ -81,7 +90,7 @@ "ArquivoDownload": "parameters/parametersMvd-2_0.par" }, { - "id": 10, + "id": 11, "Arquivo": "parametersSen", "Diretorio": "Parametros/", "Extensao": ".par", @@ -90,7 +99,7 @@ "ArquivoDownload": "parameters/parametersSen-2_0.par" }, { - "id": 11, + "id": 12, "Arquivo": "pinoutAtu", "Diretorio": "Parametros/", "Extensao": ".pin", @@ -99,7 +108,7 @@ "ArquivoDownload": "parameters/pinoutAtu-2_0.pin" }, { - "id": 12, + "id": 13, "Arquivo": "pinoutSen", "Diretorio": "Parametros/", "Extensao": ".pin", @@ -108,7 +117,7 @@ "ArquivoDownload": "parameters/pinoutSen-2_0.pin" }, { - "id": 13, + "id": 14, "Arquivo": "weed_detector_oak", "Diretorio": "Python\\Scripts\\", "Extensao": ".py", @@ -117,7 +126,7 @@ "ArquivoDownload": "weed_detector_oak-1_0.py" }, { - "id": 14, + "id": 15, "Arquivo": "map_load", "Diretorio": "Python\\Scripts\\", "Extensao": ".py", @@ -126,7 +135,7 @@ "ArquivoDownload": "scripts/map_load-1_0.py" }, { - "id": 15, + "id": 16, "Arquivo": "map_follow", "Diretorio": "Python\\Scripts\\", "Extensao": ".py", @@ -135,7 +144,7 @@ "ArquivoDownload": "scripts/map_follow-1_0.py" }, { - "id": 16, + "id": 17, "Arquivo": "gps_viewer", "Diretorio": "Python\\Scripts\\", "Extensao": ".py", @@ -144,7 +153,7 @@ "ArquivoDownload": "scripts/gps_viewer-1_0.py" }, { - "id": 17, + "id": 18, "Arquivo": "modelo_3d", "Diretorio": "Python\\Output\\", "Extensao": ".obj", @@ -153,7 +162,7 @@ "ArquivoDownload": "modelo_3d-1_0.obj" }, { - "id": 18, + "id": 19, "Arquivo": "modelo_3d", "Diretorio": "Python\\Output\\", "Extensao": ".mtl", diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/segformer_service.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/segformer_service.py index 514d92c35..5f706254a 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/segformer_service.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/segformer_service.py @@ -1,88 +1,45 @@ -# camera_worker/multispec_segformer_service.py +# camera_worker/oak_fcc3_core/segformer_service.py # -*- coding: utf-8 -*- from __future__ import annotations -import copy -import json import time from pathlib import Path -from typing import Dict, Optional, Sequence, Tuple, List +from typing import Dict, List, Tuple import cv2 import numpy as np -import torch -import torch.nn as nn -import torch.nn.functional as F -from transformers import SegformerConfig, SegformerForSemanticSegmentation +try: + import onnxruntime as ort +except Exception: + ort = None -DEFAULT_HEADS = { - "semantic": { - "enabled": True, - "type": "multiclass", - "num_classes": 3, - "classes": {"chao": 0, "cana": 1, "erva": 2}, - "ignore_index": 255, - }, - "vegetation": { - "enabled": True, - "type": "binary", - "num_classes": 2, - "classes": {"background": 0, "vegetation": 1}, - "ignore_index": 255, - }, - "cana": { - "enabled": True, - "type": "binary", - "num_classes": 2, - "classes": {"not_cana": 0, "cana": 1}, - "ignore_index": 255, - }, - "target": { - "enabled": True, - "type": "binary", - "num_classes": 2, - "classes": {"background": 0, "target": 1}, - "ignore_index": 255, - }, -} - -SEMANTIC_COLORS_RGB = { - 0: (85, 85, 85), # chao - 1: (0, 190, 0), # cana - 2: (230, 55, 55), # erva -} - -BINARY_COLORS_RGB = { - 0: (30, 30, 30), - 1: (0, 220, 80), -} - -CANA_COLORS_RGB = { - 0: (30, 30, 30), - 1: (40, 210, 255), -} - -TARGET_COLORS_RGB = { - 0: (30, 30, 30), - 1: (255, 70, 30), -} DEFAULT_CHANNEL_ORDER = ["R", "G", "B", "RE", "NIR"] +TARGET_COLORS_RGB = { + 0: (30, 30, 30), # background + 1: (255, 70, 30), # target / alvo pulverizável + 255: (0, 0, 0), # ignore/fallback +} + def get_input_channel_names(config: dict) -> List[str]: - if "input_channels" in config: - names = [str(c).upper() for c in config["input_channels"]] + names = config.get("input_channels", DEFAULT_CHANNEL_ORDER) + + if isinstance(names, str): + names = [c.strip().upper() for c in names.split(",") if c.strip()] else: - n = int(config.get("channels", 5)) - names = DEFAULT_CHANNEL_ORDER[:n] + names = [str(c).upper() for c in names] invalid = [c for c in names if c not in DEFAULT_CHANNEL_ORDER] if invalid: raise RuntimeError(f"Canais inválidos em input_channels: {invalid}") + if not names: + raise RuntimeError("input_channels vazio.") + return names @@ -91,169 +48,35 @@ def get_input_channel_indices(config: dict) -> List[int]: return [DEFAULT_CHANNEL_ORDER.index(c) for c in names] -def load_json(path: str | Path) -> dict: - with open(path, "r", encoding="utf-8") as f: - return json.load(f) - - -def merge_dict(dst: dict, src: dict) -> dict: - out = copy.deepcopy(dst) - - def rec(a, b): - for k, v in b.items(): - if isinstance(v, dict) and isinstance(a.get(k), dict): - rec(a[k], v) - else: - a[k] = v - - if isinstance(src, dict): - rec(out, src) - - return out - - -def build_heads_config(config: dict, ignore_index: int = 255) -> Dict[str, dict]: - cfg = merge_dict(DEFAULT_HEADS, config.get("heads", {}) or {}) - active = {} - - for name, hcfg in cfg.items(): - if not bool(hcfg.get("enabled", True)): - continue - - hcfg.setdefault("ignore_index", ignore_index) - hcfg["ignore_index"] = int(hcfg.get("ignore_index", ignore_index)) - hcfg["num_classes"] = int(hcfg.get("num_classes", 2)) - active[name] = hcfg - - for required in ("semantic", "vegetation", "cana"): - if required not in active: - raise RuntimeError(f"Head obrigatória ausente no config: {required}") - - return active - - -def patch_segformer_encoder_input_channels(segformer_encoder: nn.Module, in_ch: int): - if in_ch == 3: - return segformer_encoder - - proj = segformer_encoder.encoder.patch_embeddings[0].proj - - if proj.in_channels == in_ch: - return segformer_encoder - - old_weight = proj.weight.data.clone() - old_bias = proj.bias.data.clone() if proj.bias is not None else None - - new_proj = nn.Conv2d( - in_channels=in_ch, - out_channels=proj.out_channels, - kernel_size=proj.kernel_size, - stride=proj.stride, - padding=proj.padding, - dilation=proj.dilation, - groups=proj.groups, - bias=proj.bias is not None, - padding_mode=proj.padding_mode, - ) - - with torch.no_grad(): - if in_ch <= old_weight.shape[1]: - new_proj.weight.copy_(old_weight[:, :in_ch, :, :]) - else: - new_proj.weight[:, :old_weight.shape[1], :, :].copy_(old_weight) - extra = in_ch - old_weight.shape[1] - mean_w = old_weight.mean(dim=1, keepdim=True) - new_proj.weight[:, old_weight.shape[1]:, :, :].copy_(mean_w.repeat(1, extra, 1, 1)) - - if old_bias is not None: - new_proj.bias.copy_(old_bias) - - segformer_encoder.encoder.patch_embeddings[0].proj = new_proj - print(f"[MODEL] patch input channels: 3 -> {in_ch}") - return segformer_encoder - - -def replace_segformer_decode_classifier(decode_head: nn.Module, num_classes: int): - old = decode_head.classifier - if not isinstance(old, nn.Conv2d): - raise RuntimeError(f"decode_head.classifier não é Conv2d: {type(old)}") - - new = nn.Conv2d( - in_channels=old.in_channels, - out_channels=int(num_classes), - kernel_size=old.kernel_size, - stride=old.stride, - padding=old.padding, - dilation=old.dilation, - groups=old.groups, - bias=old.bias is not None, - padding_mode=old.padding_mode, - ) - decode_head.classifier = new - return decode_head - - -class MultiHeadSegFormer(nn.Module): - def __init__( - self, - backbone: str, - channels: int, - heads_config: Dict[str, dict], - semantic_id2label: Dict[int, str], - semantic_label2id: Dict[str, int], - ): - super().__init__() - - semantic_classes = int(heads_config["semantic"].get("num_classes", len(semantic_id2label))) - - base_config = SegformerConfig.from_pretrained( - backbone, - local_files_only=True - ) - - base_config.num_labels = semantic_classes - base_config.id2label = {int(k): str(v) for k, v in semantic_id2label.items()} - base_config.label2id = {str(k): int(v) for k, v in semantic_label2id.items()} - - base = SegformerForSemanticSegmentation(base_config) - - patch_segformer_encoder_input_channels(base.segformer, channels) - base.config.num_channels = int(channels) - - self.segformer = base.segformer - self.decode_heads = nn.ModuleDict() - self.heads_config = heads_config - - for head_name, hcfg in heads_config.items(): - h = copy.deepcopy(base.decode_head) - h = replace_segformer_decode_classifier(h, int(hcfg["num_classes"])) - self.decode_heads[head_name] = h - - self.config = base.config - - def forward(self, pixel_values: torch.Tensor, head_names=None) -> Dict[str, torch.Tensor]: - outputs = self.segformer( - pixel_values=pixel_values, - output_hidden_states=True, - return_dict=True, - ) - hidden_states = outputs.hidden_states - - if head_names is None: - selected = list(self.decode_heads.keys()) - else: - selected = [str(h) for h in head_names if str(h) in self.decode_heads] - - return {head_name: self.decode_heads[head_name](hidden_states) for head_name in selected} - - class MultiSpecSegformerService: """ - Service de inferência para tensor multiespectral: - input : CHW float32 [R,G,B,RE,NIR] 0..1 - output: dict de predictions multi-head + Runtime oficial Weed Worker v1. + + Contrato: + input: + tensor CHW float32 0..1 + normalmente [R,G,B,RE,NIR] + + backend: + ONNX Runtime + TensorRT + + modelo ONNX: + já contém: + - normalização + - SegFormer multi-head + - resize fullres + - argmax + e expõe saída: + - target_mask + + output: + np.ndarray uint8 HxW + 0 = fundo / não pulverizar + 1 = alvo pulverizável """ + TARGET_MODES = ("target_direct", "direct_target", "target_head", "target", "spray", "operational") + def __init__(self, model_config: dict, mostrar_log=print): self.config = model_config or {} self.mostrar_log = mostrar_log @@ -262,258 +85,293 @@ class MultiSpecSegformerService: self.input_channel_indices = get_input_channel_indices(self.config) self.channels = len(self.input_channel_names) - self.mostrar_log( - f"[MULTIHEAD][INPUT] selected_channels={self.input_channel_names} " - f"idx={self.input_channel_indices}" - ) + self.runtime_backend = str(self.config.get("runtime_backend", "onnx")).lower() + self.onnx_provider = str(self.config.get("onnx_provider", "tensorrt")).lower() + self.runtime_mode = str(self.config.get("runtime_mode", "target_direct")).lower() + self.onnx_output_mode = str(self.config.get("onnx_output_mode", self.runtime_mode)).lower() + self.onnx_output_kind = str(self.config.get("onnx_output_kind", "mask")).lower() - self.ignore_id = int(self.config.get("ignore_index", 255)) - self.heads_config = build_heads_config(self.config, ignore_index=self.ignore_id) + # No contrato v1, a normalização está dentro do ONNX. + # Manter False evita normalização dupla. + self.onnx_preprocess_norm = bool(self.config.get("onnx_preprocess_norm", False)) - self.semantic_id2label = { - 0: "chao", - 1: "cana", - 2: "erva", - } - - self.semantic_label2id = { - "chao": 0, - "cana": 1, - "erva": 2, - } - - self.classes = dict(self.semantic_label2id) - - # Dict interno, bom para ids_to_rgb - self.colormap_rgb = { - 0: (85, 85, 85), - 1: (0, 190, 0), - 2: (230, 55, 55), - } - - # Lista externa, compatível com WeedDetector - self.colormap_list_rgb = [ - self.colormap_rgb[0], - self.colormap_rgb[1], - self.colormap_rgb[2], - ] - - self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - self.use_amp = bool(self.config.get("amp", True)) and self.device.type == "cuda" - self.sync_for_timing = bool(self.config.get("sync_for_timing", False)) and self.device.type == "cuda" - - self.runtime_mode = str(self.config.get("runtime_mode", "semantic")).lower() - self.lowres_argmax = bool(self.config.get("lowres_argmax", True)) self.trust_input = bool(self.config.get("trust_input", True)) - self.channels_last = bool(self.config.get("channels_last", True)) and self.device.type == "cuda" - self.model_half = bool(self.config.get("model_half", False)) and self.device.type == "cuda" - - if self.device.type == "cuda": - torch.backends.cudnn.benchmark = True - try: - torch.set_float32_matmul_precision("high") - except Exception: - pass + self.sync_for_timing = bool(self.config.get("sync_for_timing", False)) - self.mean, self.std = self._load_norm_stats_from_config() - - backbone = self.config.get("backbone", self.config.get("pretrained_model", "nvidia/mit-b1")) - ckpt_path = self._resolve_checkpoint_path() - - self.mostrar_log(f"[MULTIHEAD] device={self.device}") - self.mostrar_log(f"[MULTIHEAD] backbone={backbone}") - self.mostrar_log(f"[MULTIHEAD] ckpt={ckpt_path}") - - self.model = MultiHeadSegFormer( - backbone=backbone, - channels=self.channels, - heads_config=self.heads_config, - semantic_id2label=self.semantic_id2label, - semantic_label2id=self.semantic_label2id, - ) - - self._load_checkpoint(ckpt_path) - self.model.to(self.device) - self.model.eval() - - if self.model_half: - self.model.half() - if self.mean is not None: - self.mean = self.mean.half() - if self.std is not None: - self.std = self.std.half() - - if self.channels_last: - try: - self.model.to(memory_format=torch.channels_last) - except Exception: - self.channels_last = False - - if bool(self.config.get("fold_input_norm", False)): - self._fold_input_normalization_into_first_conv() - - if bool(self.config.get("torch_compile", False)): - try: - self.model = torch.compile( - self.model, - mode=str(self.config.get("torch_compile_mode", "reduce-overhead")), - fullgraph=False, - ) - self.mostrar_log("[MULTIHEAD][OPT] torch.compile habilitado") - except Exception as e: - self.mostrar_log(f"[MULTIHEAD][OPT] torch.compile falhou: {type(e).__name__}: {e}") - - self.mostrar_log( - f"[MULTIHEAD][FAST] runtime_mode={self.runtime_mode} " - f"lowres_argmax={self.lowres_argmax} trust_input={self.trust_input} " - f"channels_last={self.channels_last} model_half={self.model_half} amp={self.use_amp}" - ) + self.onnx_session = None + self.onnx_input_name = None + self.onnx_output_names = [] + self.onnx_run_output_names = [] self._ultimo_tensor = None self._ultimo_predictions = None self._ultimo_probs = None + self._ultimo_predictions_full = {} + + self._validar_contrato_runtime() + + self.mostrar_log( + f"[WEED_ONNX][INPUT] selected_channels={self.input_channel_names} " + f"idx={self.input_channel_indices}" + ) + + self._init_onnx_runtime() + + self.mostrar_log( + f"[WEED_ONNX] backend=onnx " + f"provider={self.onnx_provider} " + f"runtime_mode={self.runtime_mode} " + f"output_kind={self.onnx_output_kind} " + f"preprocess_norm={self.onnx_preprocess_norm}" + ) # ============================================================ - # Config/load + # Inicialização / contrato # ============================================================ - def _resolve_checkpoint_path(self) -> Path: + def _validar_contrato_runtime(self): + if self.runtime_backend not in ("onnx", "tensorrt", "trt"): + raise RuntimeError( + f"runtime_backend inválido para Weed Worker v1: {self.runtime_backend}. " + "Use runtime_backend='onnx'." + ) + + if self.runtime_mode not in self.TARGET_MODES: + raise RuntimeError( + f"runtime_mode inválido para Weed Worker v1: {self.runtime_mode}. " + "Use runtime_mode='target_direct'." + ) + + if self.onnx_output_mode not in self.TARGET_MODES: + raise RuntimeError( + f"onnx_output_mode inválido para Weed Worker v1: {self.onnx_output_mode}. " + "Use onnx_output_mode='target_direct'." + ) + + if self.onnx_output_kind != "mask": + raise RuntimeError( + f"onnx_output_kind inválido para Weed Worker v1: {self.onnx_output_kind}. " + "O ONNX oficial deve devolver target_mask pronto." + ) + + if self.onnx_preprocess_norm: + raise RuntimeError( + "onnx_preprocess_norm=True não é permitido na v1. " + "O ONNX oficial já inclui normalização interna." + ) + + def _resolve_onnx_path(self) -> Path: candidates = [] - for key in ("ckpt", "checkpoint", "ia_model_path", "model_path"): + for key in ("onnx_model_path", "ia_model_path", "model_path", "onnx_path"): value = self.config.get(key) if value: candidates.append(Path(value)) - backup_root = self.config.get("backup_root") - modelo = self.config.get("modelo", "segformer_b1") - model_name = self.config.get("model_name", "target_teached") - fusion_mode = self.config.get("fusion_mode", "stacked") - - exp_tags = [ - f"{fusion_mode}_raw{self.channels}", - f"{fusion_mode}_raw{self.channels}_multihead", - ] - - ckpt_names = [ - "best_score.pt", - "best_target.pt", - "best_cana_head.pt", - "best_semantic_miou.pt", - "last.pt", - ] - - if backup_root: - for exp_tag in exp_tags: - for ckpt_name in ckpt_names: - candidates.append(Path(backup_root) / modelo / model_name / exp_tag / ckpt_name) - - for exp_tag in exp_tags: - for ckpt_name in ckpt_names: - candidates.append(Path("backup") / modelo / model_name / exp_tag / ckpt_name) - for p in candidates: p = p.resolve() if not p.is_absolute() else p if p.is_file(): return p raise FileNotFoundError( - "Checkpoint multi-head não encontrado. Procurei:\n" + + "Modelo ONNX não encontrado. Procurei:\n" + "\n".join(str(p) for p in candidates) ) - def _load_norm_stats_from_config(self): - path = None - - for key in ("norm_stats", "norm_stats_path", "ia_norm_stats_path"): - if self.config.get(key): - path = Path(self.config[key]) - break - - if path is None: - dataset_root = self.config.get("dataset_root") - ia_resolution = self.config.get("ia_resolution", [1024, 640]) - if dataset_root: - w, h = int(ia_resolution[0]), int(ia_resolution[1]) - path = Path(dataset_root) / f"{w}x{h}" / "group" / "norm_stats.json" - - if path is None or not path.is_file(): - self.mostrar_log("[MULTIHEAD][NORM] norm_stats não encontrado. Usando tensor 0..1 sem padronização.") - return None, None - - js = load_json(path) - mean = js.get("mean") - std = js.get("std") - names = js.get("channels", []) - - if mean is None or std is None: - raise RuntimeError(f"norm_stats inválido, faltando mean/std: {path}") - - max_idx = max(self.input_channel_indices) - - if len(mean) <= max_idx or len(std) <= max_idx: + def _init_onnx_runtime(self): + if ort is None: raise RuntimeError( - f"norm_stats incompatível: precisa índices={self.input_channel_indices}, " - f"mean={len(mean)} std={len(std)} path={path}" + "onnxruntime não está instalado. Instale onnxruntime-gpu " + "para usar TensorRT/CUDA." ) - mean = [mean[i] for i in self.input_channel_indices] - std = [std[i] for i in self.input_channel_indices] + onnx_path = self._resolve_onnx_path() - if names: - names = [names[i] for i in self.input_channel_indices] - else: - names = self.input_channel_names + sess_options = ort.SessionOptions() + sess_options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL - self.mostrar_log(f"[MULTIHEAD][NORM] usando {path}") - self.mostrar_log(f"[MULTIHEAD][NORM] selected_channels={names}") - self.mostrar_log(f"[MULTIHEAD][NORM] mean={mean}") - self.mostrar_log(f"[MULTIHEAD][NORM] std ={std}") + trt_cache_dir = str(self.config.get("trt_cache_dir", "trt_engine_cache")) + trt_fp16 = bool(self.config.get("trt_fp16", True)) - mean_t = torch.tensor(mean, dtype=torch.float32).view(1, self.channels, 1, 1).to(self.device) - std_t = torch.tensor(std, dtype=torch.float32).view(1, self.channels, 1, 1).to(self.device) + available = ort.get_available_providers() + providers = [] - return mean_t, std_t + self.mostrar_log(f"[WEED_ONNX] providers disponíveis: {available}") - def _load_checkpoint(self, ckpt_path: Path): - ckpt = torch.load(str(ckpt_path), map_location="cpu", weights_only=False) - - if isinstance(ckpt, dict): - for key in ("model", "model_state", "model_state_dict", "state_dict"): - if key in ckpt and isinstance(ckpt[key], dict): - state = ckpt[key] - break + if self.onnx_provider in ("tensorrt", "trt"): + if "TensorrtExecutionProvider" in available: + providers.append(( + "TensorrtExecutionProvider", + { + "trt_engine_cache_enable": True, + "trt_engine_cache_path": trt_cache_dir, + "trt_fp16_enable": trt_fp16, + } + )) else: - state = ckpt - else: - raise RuntimeError(f"Checkpoint em formato inesperado: {type(ckpt)}") + self.mostrar_log( + "[WEED_ONNX][WARN] TensorRT provider não disponível. " + "Caindo para CUDA/CPU." + ) - clean = {} - for k, v in state.items(): - nk = k - for prefix in ("module.", "model."): - if nk.startswith(prefix): - nk = nk[len(prefix):] - clean[nk] = v + if self.onnx_provider in ("tensorrt", "trt", "cuda"): + if "CUDAExecutionProvider" in available: + providers.append("CUDAExecutionProvider") - missing, unexpected = self.model.load_state_dict(clean, strict=False) + providers.append("CPUExecutionProvider") - self.mostrar_log( - f"[MULTIHEAD] load_state_dict strict=False | " - f"missing={len(missing)} unexpected={len(unexpected)}" + self.onnx_session = ort.InferenceSession( + str(onnx_path), + sess_options=sess_options, + providers=providers, ) - if missing: - self.mostrar_log(f"[MULTIHEAD] primeiros missing: {missing[:8]}") - if unexpected: - self.mostrar_log(f"[MULTIHEAD] primeiros unexpected: {unexpected[:8]}") + self.onnx_input_name = self.onnx_session.get_inputs()[0].name + self.onnx_output_names = [o.name for o in self.onnx_session.get_outputs()] + self.onnx_run_output_names = self._selecionar_outputs_runtime(self.onnx_output_names) + + self.mostrar_log(f"[WEED_ONNX] modelo={onnx_path}") + self.mostrar_log(f"[WEED_ONNX] input={self.onnx_input_name}") + self.mostrar_log(f"[WEED_ONNX] outputs={self.onnx_output_names}") + self.mostrar_log(f"[WEED_ONNX] outputs executados={self.onnx_run_output_names}") + self.mostrar_log(f"[WEED_ONNX] providers ativos={self.onnx_session.get_providers()}") + + def _selecionar_outputs_runtime(self, output_names: List[str]) -> List[str]: + target_outputs = [ + n for n in output_names + if "target" in str(n).lower() + ] + + if target_outputs: + return target_outputs + + # Fallback seguro: se o ONNX tiver só uma saída, usa ela. + if len(output_names) == 1: + return list(output_names) + + raise RuntimeError( + "Não encontrei saída target no ONNX. " + f"Outputs disponíveis: {output_names}" + ) + + # ============================================================ + # Inferência + # ============================================================ + + def infer_tensor_fast(self, tensor5_chw: np.ndarray, keep_probs: bool = False): + # keep_probs mantido só para compatibilidade de chamada. + return self.infer_tensor_onnx( + tensor5_chw, + return_full=bool(self.config.get("return_full_fast", False)), + ) + + def infer_tensor_onnx(self, tensor5_chw: np.ndarray, return_full: bool = False): + if tensor5_chw is None: + return None + + if self.onnx_session is None: + raise RuntimeError("ONNX Runtime não inicializado.") + + t_total0 = time.perf_counter() + + t_prepare0 = time.perf_counter() + chw, x, out_hw = self._prepare_input_numpy_onnx(tensor5_chw) + prepare_ms = (time.perf_counter() - t_prepare0) * 1000.0 + + if x is None: + return None + + t_forward0 = time.perf_counter() + + outputs = self.onnx_session.run( + self.onnx_run_output_names, + {self.onnx_input_name: x}, + ) + + forward_ms = (time.perf_counter() - t_forward0) * 1000.0 + + t_post0 = time.perf_counter() + + if len(outputs) < 1: + raise RuntimeError("ONNX não retornou nenhuma saída.") + + target_mask = self._onnx_output_to_mask(outputs[0], out_hw) + + post_ms = (time.perf_counter() - t_post0) * 1000.0 + total_ms = (time.perf_counter() - t_total0) * 1000.0 + + self._ultimo_tensor = chw + self._ultimo_predictions = target_mask + self._ultimo_probs = None + + self._ultimo_predictions_full = { + "target": target_mask, + "target_head": target_mask, + "target_op": target_mask, + "semantic": None, + "vegetation": None, + "cana": None, + "probs": None, + + "infer_ms": total_ms, + "prepare_ms": prepare_ms, + "forward_ms": forward_ms, + "post_ms": post_ms, + + "runtime_mode": self.runtime_mode, + "backend": "onnx", + "providers": self.onnx_session.get_providers(), + "outputs": self.onnx_output_names, + "outputs_executados": self.onnx_run_output_names, + } + + if return_full: + return dict(self._ultimo_predictions_full) + + return target_mask + + def _prepare_input_numpy_onnx(self, tensor5_chw: np.ndarray): + if tensor5_chw is None: + return None, None, None + + if self.trust_input: + chw = tensor5_chw + + if not isinstance(chw, np.ndarray): + chw = np.asarray(chw, dtype=np.float32) + + if chw.dtype != np.float32 or not chw.flags.c_contiguous: + chw = np.ascontiguousarray(chw, dtype=np.float32) + + else: + chw = np.asarray(tensor5_chw, dtype=np.float32) + chw = np.nan_to_num(chw, nan=0.0, posinf=1.0, neginf=0.0) + chw = np.clip(chw, 0.0, 1.0).astype(np.float32, copy=False) + chw = np.ascontiguousarray(chw) + + chw = self._select_input_channels(chw) + + if chw.ndim != 3: + raise RuntimeError(f"Tensor inválido ONNX: esperado CHW 3D, veio shape={chw.shape}") + + if chw.shape[0] != self.channels: + raise RuntimeError( + f"Tensor inválido ONNX: esperado C={self.channels}, veio shape={chw.shape}" + ) + + h, w = int(chw.shape[1]), int(chw.shape[2]) + + # No contrato v1 não normaliza aqui. + # O ONNX oficial já possui normalização interna. + x = chw[None, :, :, :].astype(np.float32, copy=False) + x = np.ascontiguousarray(x, dtype=np.float32) + + return chw, x, (h, w) def _select_input_channels(self, chw: np.ndarray) -> np.ndarray: if chw.ndim != 3: raise RuntimeError(f"Tensor inválido: esperado CHW 3D, veio shape={chw.shape}") - # Se já veio no formato exato do modelo, mantém. - # Ex: modelo raw3 recebendo tensor [R,G,B] já fatiado. if chw.shape[0] == self.channels: return chw @@ -527,128 +385,45 @@ class MultiSpecSegformerService: return chw[self.input_channel_indices, :, :] - # ============================================================ - # Inferência - # ============================================================ + @staticmethod + def _onnx_output_to_mask(arr: np.ndarray, out_hw) -> np.ndarray: + h, w = int(out_hw[0]), int(out_hw[1]) - def _normalize(self, x: torch.Tensor) -> torch.Tensor: - if self.mean is not None and self.std is not None: - return (x - self.mean) / torch.clamp(self.std, min=1e-6) - return x + arr = np.asarray(arr) - @torch.inference_mode() - def infer_tensor(self, tensor5_chw: np.ndarray): - """ - Retorna predictions compatível com o WeedDetector. + # Formatos esperados para ONNX full-runtime: + # HxW + # 1xHxW + # BxHxW + if arr.ndim == 2: + mask = arr.astype(np.uint8, copy=False) - Por enquanto: - predictions = semantic mask uint8 HxW + elif arr.ndim == 3: + mask = arr[0].astype(np.uint8, copy=False) - Também mantém: - self._ultimo_predictions_full = dict com semantic/vegetation/cana/target/probs. - """ - if tensor5_chw is None: - return None - - chw = np.asarray(tensor5_chw, dtype=np.float32) - chw = self._select_input_channels(chw) - - if chw.ndim != 3: - raise RuntimeError(f"Tensor inválido: esperado CHW 3D, veio shape={chw.shape}") - - if chw.shape[0] != self.channels: - raise RuntimeError(f"Tensor inválido: esperado C={self.channels}, veio shape={chw.shape}") - - chw = np.nan_to_num(chw, nan=0.0, posinf=1.0, neginf=0.0) - chw = np.clip(chw, 0.0, 1.0).astype(np.float32, copy=False) - chw = np.ascontiguousarray(chw) - - h, w = int(chw.shape[1]), int(chw.shape[2]) - - x = torch.from_numpy(chw).unsqueeze(0).to(self.device, non_blocking=True) - x = self._normalize(x) - - if self.device.type == "cuda": - torch.cuda.synchronize() - - t0 = time.perf_counter() - - with torch.autocast(device_type="cuda", dtype=torch.float16, enabled=self.use_amp): - logits_by_head = self.model(pixel_values=x) - - preds = {} - probs = {} - - for head_name, logits in logits_by_head.items(): - logits = F.interpolate(logits, size=(h, w), mode="bilinear", align_corners=False) - prob = torch.softmax(logits, dim=1)[0] - pred = torch.argmax(prob, dim=0) - - preds[head_name] = pred.detach().cpu().numpy().astype(np.uint8) - probs[head_name] = prob.detach().cpu().numpy().astype(np.float32) - - if self.device.type == "cuda": - torch.cuda.synchronize() - - t_ms = (time.perf_counter() - t0) * 1000.0 - - semantic = preds["semantic"] - vegetation = preds["vegetation"] - cana = preds["cana"] - - self._ultimo_tensor = chw - target_op = self.operational_target_mask(vegetation, cana, ignore_id=self.ignore_id) - target_head = preds.get("target") - - mode = str(getattr(self, "runtime_mode", "semantic")).lower() - - if mode in ("target_direct", "direct_target", "target_head") and target_head is not None: - output = target_head - elif mode in ("target", "spray", "operational"): - output = target_op else: - output = semantic + raise RuntimeError( + f"Saída ONNX inválida para contrato mask. " + f"Esperado HxW ou 1xHxW, veio shape={arr.shape}" + ) - self._ultimo_predictions_full = { - "semantic": semantic, - "vegetation": vegetation, - "cana": cana, - "target": output if mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational") else target_op, - "target_head": target_head, - "target_op": target_op, - "probs": probs, - "infer_ms": t_ms, - "runtime_mode": mode, - } + if mask.shape[0] != h or mask.shape[1] != w: + mask = cv2.resize(mask, (w, h), interpolation=cv2.INTER_NEAREST) - return output - - def infer_tensor_full(self, tensor5_chw: np.ndarray): - semantic = self.infer_tensor(tensor5_chw) - if semantic is None: - return None - return dict(self._ultimo_predictions_full) - - @torch.inference_mode() - def infer_tensor_fast(self, tensor5_chw: np.ndarray, keep_probs: bool = False): - if keep_probs: - return self.infer_tensor(tensor5_chw) - - return self.infer_tensor_ultrafast( - tensor5_chw, - return_full=bool(self.config.get("return_full_fast", False)), - ) + return mask.astype(np.uint8, copy=False) # ============================================================ - # Preview/debug + # Preview / stream # ============================================================ def preview_infer_cached(self, tensor5_chw=None, predictions=None, alpha=0.5): """ - Compatível com o uso atual do weed_worker: - rgb_frame, seg_frame, overlay_frame, _, _ = preview_infer_cached(...) + Gera frames BGR para TCP/C#/debug. - Retorna BGR para OpenCV/TCP. + Não roda inferência aqui. + Usa: + - tensor multiespectral cacheado + - target_mask cacheada """ tensor = tensor5_chw if tensor5_chw is not None else self._ultimo_tensor pred = predictions if predictions is not None else self._ultimo_predictions @@ -662,7 +437,7 @@ class MultiSpecSegformerService: if pred is None: return rgb_bgr, None, rgb_bgr, None, None - seg_rgb = self.ids_to_rgb(pred, self._colormap_for_current_output(), ignore_id=self.ignore_id) + seg_rgb = self.ids_to_rgb(pred, TARGET_COLORS_RGB) overlay_rgb = cv2.addWeighted(rgb, 1.0 - alpha, seg_rgb, alpha, 0.0) seg_bgr = cv2.cvtColor(seg_rgb, cv2.COLOR_RGB2BGR) @@ -670,43 +445,16 @@ class MultiSpecSegformerService: return rgb_bgr, seg_bgr, overlay_bgr, None, None - def get_classes(self): - mode = str(getattr(self, "runtime_mode", "semantic")).lower() - - if mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): - return {"background": 0, "target": 1} - - return self.classes - - def get_colormap(self): - mode = str(getattr(self, "runtime_mode", "semantic")).lower() - - if mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): - return [ - TARGET_COLORS_RGB[0], - TARGET_COLORS_RGB[1], - ] - - return self.colormap_list_rgb - - def _colormap_for_current_output(self): - mode = str(getattr(self, "runtime_mode", "semantic")).lower() - - if mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): - return TARGET_COLORS_RGB - - return self.colormap_rgb - - @staticmethod - def operational_target_mask(veg_mask: np.ndarray, cana_mask: np.ndarray, ignore_id: int = 255) -> np.ndarray: - out = np.zeros_like(veg_mask, dtype=np.uint8) - ignore = (veg_mask == ignore_id) | (cana_mask == ignore_id) - out[(veg_mask == 1) & (cana_mask == 0)] = 1 - out[ignore] = ignore_id - return out - @staticmethod def tensor_to_preview_rgb(chw: np.ndarray, gamma: float = 0.85) -> np.ndarray: + if chw is None: + return None + + chw = np.asarray(chw) + + if chw.ndim != 3: + raise RuntimeError(f"Tensor inválido para preview: esperado CHW, veio shape={chw.shape}") + c, h, w = chw.shape if c >= 3: @@ -731,292 +479,16 @@ class MultiSpecSegformerService: return (rgb * 255.0).astype(np.uint8) @staticmethod - def ids_to_rgb(mask: np.ndarray, colormap_rgb: Dict[int, Tuple[int, int, int]], ignore_id: int = 255) -> np.ndarray: + def ids_to_rgb(mask: np.ndarray, colormap_rgb: Dict[int, Tuple[int, int, int]]) -> np.ndarray: + mask = np.asarray(mask) + + if mask.ndim != 2: + raise RuntimeError(f"Máscara inválida para preview: esperado HxW, veio shape={mask.shape}") + h, w = mask.shape[:2] out = np.zeros((h, w, 3), dtype=np.uint8) for cid, color in colormap_rgb.items(): - out[mask == cid] = color - - out[mask == ignore_id] = (0, 0, 0) - return out - - - - def _runtime_head_names(self): - mode = str(getattr(self, "runtime_mode", "semantic")).lower() - - if mode in ("semantic", "sem", "mask"): - return ["semantic"] - - if mode in ("target", "spray", "operational"): - return ["vegetation", "cana"] - - if mode in ("target_direct", "direct_target", "target_head"): - return ["target"] - - if mode in ("all", "full", "debug"): - return ["semantic", "vegetation", "cana", "target"] - - return ["semantic"] - - def _prepare_input_tensor_fast(self, tensor5_chw: np.ndarray): - if tensor5_chw is None: - return None, None, None - - if bool(getattr(self, "trust_input", True)): - chw = tensor5_chw - if not isinstance(chw, np.ndarray): - chw = np.asarray(chw, dtype=np.float32) - if chw.dtype != np.float32 or not chw.flags.c_contiguous: - chw = np.ascontiguousarray(chw, dtype=np.float32) - else: - chw = np.asarray(tensor5_chw, dtype=np.float32) - chw = np.nan_to_num(chw, nan=0.0, posinf=1.0, neginf=0.0) - chw = np.clip(chw, 0.0, 1.0).astype(np.float32, copy=False) - chw = np.ascontiguousarray(chw) - - # Sempre selecionar canais, independentemente de trust_input. - chw = self._select_input_channels(chw) - - if chw.ndim != 3: - raise RuntimeError(f"Tensor inválido: esperado CHW 3D, veio shape={chw.shape}") - - if chw.shape[0] != self.channels: - raise RuntimeError(f"Tensor inválido: esperado C={self.channels}, veio shape={chw.shape}") - - h, w = int(chw.shape[1]), int(chw.shape[2]) - - x = torch.from_numpy(chw).unsqueeze(0).to(self.device, non_blocking=True) - - if bool(getattr(self, "channels_last", False)): - try: - x = x.contiguous(memory_format=torch.channels_last) - except Exception: - pass - - x = self._normalize(x) - - if bool(getattr(self, "model_half", False)): - x = x.half() - - return chw, x, (h, w) - - def _logits_to_pred_numpy_fast(self, logits: torch.Tensor, out_hw, lowres_argmax: bool): - h, w = int(out_hw[0]), int(out_hw[1]) - - if lowres_argmax: - # Argmax no mapa pequeno, depois resize da máscara uint8. - # Bem mais barato que interpolar logits CxHxW em float. - pred_small = torch.argmax(logits, dim=1)[0] - pred_np = pred_small.detach().to("cpu", non_blocking=False).numpy().astype(np.uint8) - - if pred_np.shape[0] != h or pred_np.shape[1] != w: - pred_np = cv2.resize(pred_np, (w, h), interpolation=cv2.INTER_NEAREST) - - return pred_np - - # Caminho equivalente ao atual, mas sem softmax. - logits = F.interpolate(logits, size=(h, w), mode="bilinear", align_corners=False) - pred = torch.argmax(logits, dim=1)[0] - return pred.detach().to("cpu", non_blocking=False).numpy().astype(np.uint8) - - @torch.inference_mode() - def infer_tensor_ultrafast(self, tensor5_chw: np.ndarray, return_full: bool = False): - if tensor5_chw is None: - return None - - t_total0 = time.perf_counter() - - # ------------------------------------------------------------ - # Prepare: numpy -> torch/cuda + layout + normalização - # ------------------------------------------------------------ - t_prepare0 = time.perf_counter() - - chw, x, out_hw = self._prepare_input_tensor_fast(tensor5_chw) - - prepare_ms = (time.perf_counter() - t_prepare0) * 1000.0 - - if x is None: - return None - - head_names = self._runtime_head_names() - lowres_argmax = bool(getattr(self, "lowres_argmax", True)) - runtime_mode = str(getattr(self, "runtime_mode", "semantic")).lower() - output_mask_fullres = bool(self.config.get("output_mask_fullres", True)) - - if bool(getattr(self, "sync_for_timing", False)) and self.device.type == "cuda": - torch.cuda.synchronize() - - # ------------------------------------------------------------ - # Forward: modelo UMA vez só - # ------------------------------------------------------------ - t_forward0 = time.perf_counter() - - with torch.autocast( - device_type="cuda", - dtype=torch.float16, - enabled=bool(getattr(self, "use_amp", True)) and self.device.type == "cuda", - ): - logits_by_head = self.model(pixel_values=x, head_names=head_names) - - if self.device.type == "cuda": - torch.cuda.synchronize() - - forward_ms = (time.perf_counter() - t_forward0) * 1000.0 - - # ------------------------------------------------------------ - # Post: argmax / target / cópia CPU - # ------------------------------------------------------------ - t_post0 = time.perf_counter() - - semantic = None - vegetation = None - cana = None - target = None - - if runtime_mode in ("target", "spray", "operational"): - logits_veg = logits_by_head.get("vegetation") - logits_cana = logits_by_head.get("cana") - - if logits_veg is None or logits_cana is None: - raise RuntimeError("runtime_mode=target requer heads vegetation e cana") - - target = self._target_from_logits_gpu_fast( - logits_veg, - logits_cana, - out_hw=out_hw, - fullres=output_mask_fullres, - ) - - output = target - - else: - preds = {} - - if "target" in logits_by_head: - target = self._logits_to_pred_numpy_fast( - logits_by_head["target"], - out_hw=out_hw, - lowres_argmax=lowres_argmax, - ) - - semantic = None - vegetation = None - cana = None - output = target - else: - for head_name, logits in logits_by_head.items(): - preds[head_name] = self._logits_to_pred_numpy_fast( - logits, - out_hw=out_hw, - lowres_argmax=lowres_argmax, - ) - - semantic = preds.get("semantic") - vegetation = preds.get("vegetation") - cana = preds.get("cana") - - if vegetation is not None and cana is not None: - target = self.operational_target_mask( - vegetation, - cana, - ignore_id=self.ignore_id, - ) - - output = semantic if semantic is not None else target - - post_ms = (time.perf_counter() - t_post0) * 1000.0 - total_ms = (time.perf_counter() - t_total0) * 1000.0 - - self._ultimo_tensor = chw - self._ultimo_predictions = output - self._ultimo_probs = None - self._ultimo_predictions_full = { - "semantic": semantic, - "vegetation": vegetation, - "cana": cana, - "target": target, - "probs": None, - "infer_ms": total_ms, - "prepare_ms": prepare_ms, - "forward_ms": forward_ms, - "post_ms": post_ms, - "runtime_mode": runtime_mode, - "lowres_argmax": lowres_argmax, - "heads": list(head_names), - "output_mask_fullres": output_mask_fullres, - } - - if return_full: - return dict(self._ultimo_predictions_full) - - return output - - - def _target_from_logits_gpu_fast(self, logits_veg, logits_cana, out_hw, fullres: bool = True): - h, w = int(out_hw[0]), int(out_hw[1]) - - veg = torch.argmax(logits_veg, dim=1)[0] - cana = torch.argmax(logits_cana, dim=1)[0] - - target = ((veg == 1) & (cana == 0)).to(torch.uint8) - - target_np = target.detach().to("cpu", non_blocking=False).numpy() - - if fullres and (target_np.shape[0] != h or target_np.shape[1] != w): - target_np = cv2.resize( - target_np, - (w, h), - interpolation=cv2.INTER_NEAREST, - ) - - return target_np.astype(np.uint8, copy=False) - - def _fold_input_normalization_into_first_conv(self): - if self.mean is None or self.std is None: - return False - - try: - proj = self.model.segformer.encoder.patch_embeddings[0].proj - except Exception: - return False - - if not isinstance(proj, nn.Conv2d): - return False - - with torch.no_grad(): - device = proj.weight.device - dtype = proj.weight.dtype - - mean = self.mean.detach().to(device=device, dtype=dtype).view(-1) - std = self.std.detach().to(device=device, dtype=dtype).view(-1) - std = torch.clamp(std, min=1e-6) - - w_old = proj.weight.data.clone() - b_old = proj.bias.data.clone() if proj.bias is not None else torch.zeros( - proj.out_channels, - device=device, - dtype=dtype, - ) - - # W'[:, c] = W[:, c] / std[c] - w_new = w_old / std.view(1, -1, 1, 1) - - # bias' = bias - sum(W[:,c,:,:] * mean[c] / std[c]) - offset = (w_old * (mean / std).view(1, -1, 1, 1)).sum(dim=(1, 2, 3)) - b_new = b_old - offset - - proj.weight.data.copy_(w_new) - - if proj.bias is None: - proj.bias = nn.Parameter(b_new) - else: - proj.bias.data.copy_(b_new) - - self.mean = None - self.std = None - - self.mostrar_log("[MULTIHEAD][OPT] normalização foldada na primeira conv") - return True + out[mask == int(cid)] = color + return out \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py index e6a07982d..77465b841 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py @@ -1,184 +1,575 @@ import json import os import time +from typing import Any, Dict, List, Optional, Sequence, Tuple + import cv2 import numpy as np -import torch -import torch.nn as nn -import torch.nn.functional as F -from transformers import SegformerConfig, SegformerForSemanticSegmentation - - -class LabelHead(nn.Module): - def __init__(self, feat_ch: int, num_seg_classes: int, num_label_classes: int, hidden: int = 256, dropout: float = 0.2): - super().__init__() - in_ch = feat_ch + num_seg_classes - self.pool = nn.AdaptiveAvgPool2d((1, 1)) - self.net = nn.Sequential( - nn.Linear(in_ch, hidden), - nn.ReLU(inplace=True), - nn.Dropout(dropout), - nn.Linear(hidden, num_label_classes), - ) - - def forward(self, feat: torch.Tensor, logits_seg: torch.Tensor) -> torch.Tensor: - if feat.shape[-2:] != logits_seg.shape[-2:]: - feat = F.interpolate(feat, size=logits_seg.shape[-2:], mode="bilinear", align_corners=False) - - x = torch.cat([feat, logits_seg], dim=1) - x = self.pool(x).flatten(1) - return self.net(x) +import onnxruntime as ort class SegformerNavRunner: - IMAGENET_MEAN = [0.485, 0.456, 0.406] - IMAGENET_STD = [0.229, 0.224, 0.225] + """ + Runner ONNX/TensorRT para segmentação navegável do Visual Worker. - def __init__(self, seg_config, device="cuda"): + Contrato novo: + - Não carrega .pt. + - Não usa Torch. + - Não usa HuggingFace. + - Não lê norm_stats para normalizar fora. + - Assume que o ONNX já contém resize/normalização quando onnx_has_preprocess=True. + + Entrada principal: + frame_rgb: np.ndarray HxWx3, RGB, uint8 + + Retorno de infer_ids: + pred_ids_np, ts, roi_resized, roi_info, aux_result + + Onde: + pred_ids_np : máscara HxW com ids de classe + ts : timestamp da inferência + roi_resized : ROI RGB redimensionada para debug/visualização + roi_info : (y_fim, y_inicio) no frame original + aux_result : dict de label/status, se o ONNX exportar saída auxiliar + """ + + def __init__(self, seg_config: Dict[str, Any]): from shared.utils import carregar_labelmap_completo - self.device = torch.device(device if torch.cuda.is_available() else "cpu") + self.config = dict(seg_config) - self.use_amp = bool(seg_config.get("use_amp", True)) - self.use_channels_last = bool(seg_config.get("use_channels_last", True)) - self.use_compact_aux = bool(seg_config.get("use_compact_aux", True)) self.debug_timing = bool(seg_config.get("debug_timing", False)) + self.debug_session = bool(seg_config.get("debug_session", True)) + self.use_compact_aux = bool(seg_config.get("use_compact_aux", True)) + + self.resolucao = self._read_resolution(seg_config) + self.roi_inicio = float(seg_config.get("ia_roi_begin", 0.0)) + self.roi_tamanho = float(seg_config.get("ia_roi_size", 1.0)) + + self.onnx_path = ( + seg_config.get("ia_onnx_path") + or seg_config.get("onnx_path") + or seg_config.get("ia_model_path") + ) + + if not self.onnx_path: + raise ValueError( + "SegformerNavRunner ONNX precisa de 'ia_onnx_path', 'onnx_path' " + "ou 'ia_model_path' apontando para o arquivo .onnx." + ) + + if not os.path.isfile(self.onnx_path): + raise FileNotFoundError(f"Arquivo ONNX não encontrado: {self.onnx_path}") + + self.provider_mode = str(seg_config.get("onnx_provider", "tensorrt")).lower() + self.onnx_has_preprocess = bool(seg_config.get("onnx_has_preprocess", True)) + + # Formatos aceitos: + # - auto: tenta inferir pela saída + # - ids: saída já é máscara de ids + # - logits: saída é [N,C,H,W] ou [C,H,W] + self.seg_output_format = str(seg_config.get("onnx_seg_output_format", "auto")).lower() + + # Formato de entrada quando o ONNX já tem preprocess embutido. + # Recomendado para seu contrato novo: nhwc_uint8. + # + # Opções: + # - nhwc_uint8 -> [1,H,W,3] uint8 + # - nhwc_float -> [1,H,W,3] float32, 0..255 ou 0..1 conforme input_scale + # - nchw_float -> [1,3,H,W] float32 + self.input_layout = str(seg_config.get("onnx_input_layout", "nhwc_uint8")).lower() + self.input_scale = float(seg_config.get("onnx_input_scale", 1.0)) + + self.input_name_forced = seg_config.get("onnx_input_name") + self.seg_output_name_forced = seg_config.get("onnx_seg_output_name") + self.aux_output_name_forced = seg_config.get("onnx_aux_output_name") + + self.label_names = self._load_label_names(seg_config) + self._last_aux_result = None self._last_aux_ts = 0.0 + self.last_infer = None self.cor_para_id, self.colormap_rgb, self.classes, self.ignore_rgb = carregar_labelmap_completo( seg_config["ia_labelmap_path"] ) - self.resolucao = tuple(seg_config["ia_resolution"]) # [W,H] - self.roi_inicio = seg_config["ia_roi_begin"] - self.roi_tamanho = seg_config["ia_roi_size"] - self.last_infer = None + self.session = self._create_session() + self.input_name = self._resolve_input_name() + self.output_names = [o.name for o in self.session.get_outputs()] - self.mode = seg_config.get("ia_mode", "dual_label") - self.model = None - self.aux_head = None - self.label_names = {} + self.input_meta = next(i for i in self.session.get_inputs() if i.name == self.input_name) + self.input_type = str(self.input_meta.type) + self.input_shape = self.input_meta.shape - self._load_dual_checkpoint(seg_config) + self.seg_output_name = self._resolve_seg_output_name() + self.aux_output_name = self._resolve_aux_output_name() - norm_mean = self.IMAGENET_MEAN - norm_std = self.IMAGENET_STD + if self.debug_session: + self._print_session_summary() - norm_stats_path = seg_config.get("ia_norm_stats_path") - if norm_stats_path and os.path.isfile(norm_stats_path): - with open(norm_stats_path, "r", encoding="utf-8") as f: - norm_stats = json.load(f) + # ------------------------------------------------------------------------- + # Inicialização ONNX + # ------------------------------------------------------------------------- - stats_channels = norm_stats.get("channels", []) - stats_mean = norm_stats.get("mean", []) - stats_std = norm_stats.get("std", []) + def _create_session(self) -> ort.InferenceSession: + sess_options = ort.SessionOptions() + sess_options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL - idx_by_name = {name: i for i, name in enumerate(stats_channels)} + intra_threads = int(self.config.get("onnx_intra_op_num_threads", 1)) + inter_threads = int(self.config.get("onnx_inter_op_num_threads", 1)) - if all(ch in idx_by_name for ch in ["R", "G", "B"]): - norm_mean = [ - stats_mean[idx_by_name["R"]], - stats_mean[idx_by_name["G"]], - stats_mean[idx_by_name["B"]], - ] - norm_std = [ - stats_std[idx_by_name["R"]], - stats_std[idx_by_name["G"]], - stats_std[idx_by_name["B"]], - ] - print(f"[NORM] usando stats fixos de: {norm_stats_path}") - else: - print("[NORM] norm_stats não contém R,G,B. Usando ImageNet.") - else: - print(f"[NORM] norm_stats não encontrado em {norm_stats_path}. Usando ImageNet.") + if intra_threads > 0: + sess_options.intra_op_num_threads = intra_threads + if inter_threads > 0: + sess_options.inter_op_num_threads = inter_threads - self.set_norm_stats(norm_mean, norm_std) + providers = self._build_providers() - def set_norm_stats(self, mean, std): - self._norm_mean = torch.tensor(mean, dtype=torch.float32, device=self.device).view(3, 1, 1) - self._norm_std = torch.tensor(std, dtype=torch.float32, device=self.device).view(3, 1, 1).clamp_min(1e-6) + try: + return ort.InferenceSession( + self.onnx_path, + sess_options=sess_options, + providers=providers, + ) + except Exception as e: + raise RuntimeError( + f"Falha ao carregar ONNX: {self.onnx_path} | " + f"provider_mode={self.provider_mode} | erro={e}" + ) from e - def normalize_img(self, img): - return (img - self._norm_mean) / self._norm_std + def _build_providers(self) -> List[Any]: + available = set(ort.get_available_providers()) - def _build_base_model(self, backbone: str, num_classes: int): - config = SegformerConfig.from_pretrained( - backbone, - local_files_only=True + if self.provider_mode in ("tensorrt", "trt"): + providers: List[Any] = [] + + if "TensorrtExecutionProvider" in available: + trt_options = { + "trt_fp16_enable": bool(self.config.get("trt_fp16_enable", True)), + "trt_engine_cache_enable": bool(self.config.get("trt_engine_cache_enable", True)), + "trt_engine_cache_path": str( + self.config.get("trt_engine_cache_path", "./trt_cache_visual_worker") + ), + } + + max_workspace = self.config.get("trt_max_workspace_size") + if max_workspace is not None: + trt_options["trt_max_workspace_size"] = int(max_workspace) + + providers.append(("TensorrtExecutionProvider", trt_options)) + + if "CUDAExecutionProvider" in available: + providers.append("CUDAExecutionProvider") + + providers.append("CPUExecutionProvider") + return providers + + if self.provider_mode in ("cuda", "gpu"): + providers = [] + if "CUDAExecutionProvider" in available: + providers.append("CUDAExecutionProvider") + providers.append("CPUExecutionProvider") + return providers + + return ["CPUExecutionProvider"] + + def _resolve_input_name(self) -> str: + inputs = self.session.get_inputs() + + if not inputs: + raise RuntimeError("ONNX não possui entradas.") + + if self.input_name_forced: + names = [i.name for i in inputs] + if self.input_name_forced not in names: + raise RuntimeError( + f"onnx_input_name='{self.input_name_forced}' não encontrado. " + f"Entradas disponíveis: {names}" + ) + return str(self.input_name_forced) + + return inputs[0].name + + def _resolve_seg_output_name(self) -> str: + if self.seg_output_name_forced: + if self.seg_output_name_forced not in self.output_names: + raise RuntimeError( + f"onnx_seg_output_name='{self.seg_output_name_forced}' não encontrado. " + f"Saídas disponíveis: {self.output_names}" + ) + return str(self.seg_output_name_forced) + + # Preferências comuns. + preferred = [ + "seg_ids", + "pred_ids", + "mask_ids", + "segmentation", + "seg", + "logits", + "seg_logits", + "output", + ] + + lower_by_name = {name.lower(): name for name in self.output_names} + + for p in preferred: + if p in lower_by_name: + return lower_by_name[p] + + return self.output_names[0] + + def _resolve_aux_output_name(self) -> Optional[str]: + if self.aux_output_name_forced: + if self.aux_output_name_forced not in self.output_names: + raise RuntimeError( + f"onnx_aux_output_name='{self.aux_output_name_forced}' não encontrado. " + f"Saídas disponíveis: {self.output_names}" + ) + return str(self.aux_output_name_forced) + + if len(self.output_names) <= 1: + return None + + preferred = [ + "label_logits", + "aux_logits", + "status_logits", + "label", + "aux", + "status", + ] + + lower_by_name = {name.lower(): name for name in self.output_names} + + for p in preferred: + if p in lower_by_name: + return lower_by_name[p] + + for name in self.output_names: + if name != self.seg_output_name: + return name + + return None + + def _print_session_summary(self): + print("[SEG_ONNX] Runner carregado") + print(f"[SEG_ONNX] modelo : {self.onnx_path}") + print(f"[SEG_ONNX] provider_mode : {self.provider_mode}") + print(f"[SEG_ONNX] providers ativos: {self.session.get_providers()}") + print(f"[SEG_ONNX] input : {self.input_name}") + print(f"[SEG_ONNX] outputs : {self.output_names}") + print(f"[SEG_ONNX] seg_output : {self.seg_output_name}") + print(f"[SEG_ONNX] aux_output : {self.aux_output_name}") + print(f"[SEG_ONNX] resolução : {self.resolucao}") + print(f"[SEG_ONNX] input_layout : {self.input_layout}") + print(f"[SEG_ONNX] has_preprocess : {self.onnx_has_preprocess}") + input_meta = self.session.get_inputs()[0] + print(f"[SEG_ONNX] input type : {input_meta.type}") + print(f"[SEG_ONNX] input shape : {input_meta.shape}") + + # ------------------------------------------------------------------------- + # API principal + # ------------------------------------------------------------------------- + + def infer_ids(self, frame_rgb: np.ndarray): + if frame_rgb is None or not hasattr(frame_rgb, "shape") or frame_rgb.size == 0: + return None, None, None, None, None + + t0 = time.perf_counter() + + H, W = frame_rgb.shape[:2] + y_fim, y_inicio = self.compute_roi_indices( + H, + self.roi_inicio, + self.roi_tamanho, ) - config.num_labels = int(num_classes) - config.output_hidden_states = True + roi_rgb = frame_rgb[y_fim:y_inicio, 0:W] + t_crop = time.perf_counter() - model = SegformerForSemanticSegmentation(config) - return model + if roi_rgb is None or roi_rgb.size == 0: + return None, None, None, (y_fim, y_inicio), None - def _load_dual_checkpoint(self, seg_config): - pt_path = seg_config["ia_model_path"] - backbone = seg_config["ia_backbone"] - num_classes = len(self.classes) + input_tensor, roi_resized = self._prepare_input(roi_rgb) + t_pre = time.perf_counter() - ckpt = torch.load(pt_path, map_location="cpu", weights_only=False) - self.model = self._build_base_model(backbone, num_classes) - self.model.load_state_dict(ckpt["model"], strict=True) - self.model.to(self.device).eval() + requested_outputs = [self.seg_output_name] + if self.aux_output_name: + requested_outputs.append(self.aux_output_name) - if self.use_channels_last and self.device.type == "cuda": - self.model.to(memory_format=torch.channels_last) + outputs = self.session.run( + requested_outputs, + {self.input_name: input_tensor}, + ) + t_model = time.perf_counter() - torch.backends.cudnn.benchmark = True + seg_out = outputs[0] + pred_ids_np = self._decode_seg_output(seg_out) - extra = ckpt.get("extra", {}) or {} - label_names_raw = extra.get("label_name_by_id", {}) or {} - self.label_names = {int(k): str(v) for k, v in label_names_raw.items()} if label_names_raw else {} - - if self.mode == "dual_label": - aux_sd = ckpt.get("aux_head") - if aux_sd is None: - raise RuntimeError("Checkpoint não possui aux_head. Este arquivo parece não ser dual_head_label.") - - max_label_id = -1 - - for k, v in aux_sd.items(): - if k.endswith("net.3.weight") or k.endswith("net.3.bias"): - max_label_id = int(v.shape[0]) - 1 - break - - if max_label_id < 0: - raise RuntimeError("Não consegui inferir número de classes da LabelHead.") - - num_label_classes = max_label_id + 1 - - for i in range(num_label_classes): - self.label_names.setdefault(i, f"label_{i}") - - feat_ch = int(self.model.config.hidden_sizes[-1]) - - self.aux_head = LabelHead( - feat_ch=feat_ch, - num_seg_classes=num_classes, - num_label_classes=num_label_classes, - hidden=256, - dropout=0.2, + expected_w, expected_h = self.resolucao + if pred_ids_np.shape[:2] != (expected_h, expected_w): + pred_ids_np = cv2.resize( + pred_ids_np, + (expected_w, expected_h), + interpolation=cv2.INTER_NEAREST, ) - self.aux_head.load_state_dict(aux_sd, strict=True) - self.aux_head.to(self.device).eval() - if self.use_channels_last and self.device.type == "cuda": - # Linear não usa channels_last, mas manter aqui não atrapalha. - pass + pred_ids_np = np.ascontiguousarray(pred_ids_np.astype(np.uint8, copy=False)) - print(f"[DUAL] LabelHead carregada: classes={num_label_classes} names={self.label_names}") + aux_result = None + if self.aux_output_name and len(outputs) > 1: + aux_result = self._decode_aux_output(outputs[1]) + self.last_infer = time.time() + t_end = time.perf_counter() + + if self.debug_timing: + print( + f"RUNNER_ONNX | " + f"crop={(t_crop - t0) * 1000:.1f}ms | " + f"pre={(t_pre - t_crop) * 1000:.1f}ms | " + f"model={(t_model - t_pre) * 1000:.1f}ms | " + f"post={(t_end - t_model) * 1000:.1f}ms | " + f"total={(t_end - t0) * 1000:.1f}ms" + ) + + return pred_ids_np, self.last_infer, roi_resized, (y_fim, y_inicio), aux_result + + def infer_ids_seg_only(self, frame_rgb: np.ndarray): + """ + Mantido como alias para o novo contrato. + Como este runner já é ONNX puro, a segmentação é o caminho principal. + """ + pred_ids_np, ts, roi_resized, roi_info, _aux = self.infer_ids(frame_rgb) + + aux_result = self._last_aux_result + + if aux_result is not None: + aux_result = dict(aux_result) + aux_result["stale"] = True + aux_result["age_ms"] = (time.time() - self._last_aux_ts) * 1000.0 + + return pred_ids_np, ts, roi_resized, roi_info, aux_result + + # ------------------------------------------------------------------------- + # Preprocess + # ------------------------------------------------------------------------- + + def _prepare_input(self, roi_rgb: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + roi_rgb = np.ascontiguousarray(roi_rgb) + + # Mesmo quando o ONNX tem preprocess embutido, mantemos uma ROI redimensionada + # para debug/visualização e compatibilidade com o retorno. + roi_resized = cv2.resize( + roi_rgb, + self.resolucao, + interpolation=cv2.INTER_AREA, + ) + roi_resized = np.ascontiguousarray(roi_resized) + + if self.onnx_has_preprocess: + base = roi_rgb else: - self.aux_head = None - print("[SEG] Modo single carregado.") + base = roi_resized - print(f"[MODEL] ckpt={pt_path}") - print(f"[MODEL] epoch={ckpt.get('epoch')} bests={ckpt.get('bests')}") + if self.input_layout == "nhwc_uint8": + inp = base[None, ...] + if inp.dtype != np.uint8: + inp = inp.astype(np.uint8, copy=False) + return np.ascontiguousarray(inp), roi_resized + + if self.input_layout == "nhwc_float": + inp = base.astype(np.float32, copy=False) + + if self.input_scale != 1.0: + inp = inp * self.input_scale + + inp = inp[None, ...] + return np.ascontiguousarray(inp), roi_resized + + if self.input_layout == "nchw_float": + inp = base.astype(np.float32, copy=False) + + if self.input_scale != 1.0: + inp = inp * self.input_scale + + inp = np.transpose(inp, (2, 0, 1))[None, ...] + return np.ascontiguousarray(inp), roi_resized + + raise ValueError( + f"onnx_input_layout inválido: {self.input_layout}. " + "Use 'nhwc_uint8', 'nhwc_float' ou 'nchw_float'." + ) + + # ------------------------------------------------------------------------- + # Pós-processamento + # ------------------------------------------------------------------------- + + def _decode_seg_output(self, out: np.ndarray) -> np.ndarray: + arr = np.asarray(out) + + fmt = self.seg_output_format + + if fmt == "ids": + return self._squeeze_ids(arr) + + if fmt == "logits": + return self._logits_to_ids(arr) + + if fmt != "auto": + raise ValueError( + f"onnx_seg_output_format inválido: {fmt}. " + "Use 'auto', 'ids' ou 'logits'." + ) + + # AUTO: + # [N,C,H,W] com C pequeno normalmente é logits. + if arr.ndim == 4: + if arr.shape[1] > 1: + return self._logits_to_ids(arr) + return self._squeeze_ids(arr) + + # [C,H,W] com C pequeno também pode ser logits. + if arr.ndim == 3: + num_classes = len(getattr(self, "classes", []) or []) + if num_classes > 1 and arr.shape[0] == num_classes: + return self._logits_to_ids(arr) + return self._squeeze_ids(arr) + + if arr.ndim == 2: + return self._squeeze_ids(arr) + + raise RuntimeError(f"Formato de saída de segmentação não suportado: shape={arr.shape}") + + def _squeeze_ids(self, arr: np.ndarray) -> np.ndarray: + arr = np.asarray(arr) + + # [1,1,H,W] -> [H,W] + if arr.ndim == 4 and arr.shape[0] == 1 and arr.shape[1] == 1: + return arr[0, 0] + + # [1,H,W] -> [H,W] + if arr.ndim == 3 and arr.shape[0] == 1: + return arr[0] + + # [H,W,1] -> [H,W] + if arr.ndim == 3 and arr.shape[-1] == 1: + return arr[:, :, 0] + + # [H,W] + if arr.ndim == 2: + return arr + + # fallback seguro + return np.squeeze(arr) + + def _logits_to_ids(self, arr: np.ndarray) -> np.ndarray: + arr = np.asarray(arr) + + # [N,C,H,W] -> [H,W] + if arr.ndim == 4: + return np.argmax(arr, axis=1)[0] + + # [C,H,W] -> [H,W] + if arr.ndim == 3: + return np.argmax(arr, axis=0) + + raise RuntimeError(f"Formato de logits não suportado: shape={arr.shape}") + + def _decode_aux_output(self, out: np.ndarray) -> Optional[Dict[str, Any]]: + arr = np.asarray(out) + + if arr.size == 0: + return None + + values = np.squeeze(arr).astype(np.float32) + + if values.ndim != 1: + values = values.reshape(-1).astype(np.float32) + + aux_format = str( + self.config.get("onnx_aux_output_format", "") + ).lower() + + if not aux_format: + name = str(getattr(self, "aux_output_name", "") or "").lower() + aux_format = "probs" if "prob" in name else "logits" + + if aux_format == "probs": + probs = values + s = float(np.sum(probs)) + if s > 1e-6: + probs = probs / s + else: + probs = self._softmax_np(values) + + label_id = int(np.argmax(probs)) + label_conf = float(probs[label_id]) + label_name = self.label_names.get(label_id, f"label_{label_id}") + + if self.use_compact_aux: + aux_result = { + "type": "label", + "label_id": label_id, + "label_name": label_name, + "label_conf": label_conf, + } + else: + aux_result = { + "type": "label", + "label_id": label_id, + "label_name": label_name, + "label_conf": label_conf, + "label_probs": probs.astype(float).tolist(), + "label_names": self.label_names, + } + + self._last_aux_result = aux_result + self._last_aux_ts = time.time() + + return aux_result + + @staticmethod + def _softmax_np(x: np.ndarray) -> np.ndarray: + x = x.astype(np.float32, copy=False) + x = x - np.max(x) + exp = np.exp(x) + denom = float(np.sum(exp)) + + if denom <= 1e-12: + return np.zeros_like(exp, dtype=np.float32) + + return exp / denom + + # ------------------------------------------------------------------------- + # Utilitários + # ------------------------------------------------------------------------- + + @staticmethod + def _read_resolution(seg_config: Dict[str, Any]) -> Tuple[int, int]: + res = seg_config.get("ia_resolution", [1024, 576]) + + if not isinstance(res, Sequence) or len(res) != 2: + raise ValueError(f"ia_resolution inválida: {res}") + + w = int(res[0]) + h = int(res[1]) + + if w <= 0 or h <= 0: + raise ValueError(f"ia_resolution precisa ser positiva: {res}") + + return w, h + + @staticmethod + def compute_roi_indices(H: int, zona_inicio: float, faixa_atuacao: float) -> Tuple[int, int]: + zona_inicio = float(zona_inicio) + faixa_atuacao = float(faixa_atuacao) + + zona_inicio = max(0.0, min(1.0, zona_inicio)) + faixa_atuacao = max(0.0, min(1.0, faixa_atuacao)) - def compute_roi_indices(self, H: int, zona_inicio: float, faixa_atuacao: float): y_inicio = int((1.0 - zona_inicio) * H) y_fim = int((1.0 - (zona_inicio + faixa_atuacao)) * H) @@ -190,226 +581,49 @@ class SegformerNavRunner: return y_fim, y_inicio - def resize_keep_width(self, img: np.ndarray, new_w: int, min_h: int, interpolation: int) -> np.ndarray: - h, w = img.shape[:2] - new_h = int(round(new_w * (h / max(1, w)))) + def _load_label_names(self, seg_config: Dict[str, Any]) -> Dict[int, str]: + """ + Carrega nomes do label/status auxiliar, se existir. - if min_h is not None and new_h < min_h: - new_h = min_h + Aceita: + - label_name_by_id direto no config + - ia_label_names_path apontando para JSON + - onnx_label_names_path apontando para JSON - return cv2.resize(img, (new_w, new_h), interpolation=interpolation) + O JSON pode ser: + - {"0": "Parado", "1": "CaminhandoRua"} + - {"label_name_by_id": {"0": "Parado", "1": "CaminhandoRua"}} + - ["Parado", "CaminhandoRua"] + """ + raw = seg_config.get("label_name_by_id") - def _preprocess_roi(self, roi_rgb: np.ndarray): - roi_resized = self.resize_keep_width( - roi_rgb, - self.resolucao[0], - self.resolucao[1], - cv2.INTER_AREA, + path = ( + seg_config.get("ia_label_names_path") + or seg_config.get("onnx_label_names_path") ) - # Garante array contínuo para reduzir cópia torta no torch.from_numpy - roi_resized = np.ascontiguousarray(roi_resized) + if raw is None and path and os.path.isfile(path): + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) - img_tensor = torch.from_numpy(roi_resized).to( - device=self.device, - dtype=torch.float32, - non_blocking=True - ) - - # HWC -> NCHW - img_tensor = img_tensor.permute(2, 0, 1).unsqueeze(0) - img_tensor = img_tensor.div_(255.0) - - img_tensor = self.normalize_img(img_tensor) - - if self.use_channels_last and self.device.type == "cuda": - img_tensor = img_tensor.contiguous(memory_format=torch.channels_last) - - return img_tensor, roi_resized - - @torch.inference_mode() - def infer_ids(self, frame_rgb): - if frame_rgb is None or not hasattr(frame_rgb, "shape") or frame_rgb.size == 0: - return None, None, None, None, None - t0 = time.perf_counter() - H, W = frame_rgb.shape[:2] - y_fim, y_inicio = self.compute_roi_indices(H, self.roi_inicio, self.roi_tamanho) - - roi_rgb = frame_rgb[y_fim:y_inicio, 0:W] - t_crop = time.perf_counter() - if roi_rgb is None or roi_rgb.size == 0: - return None, None, None, (y_fim, y_inicio), None - img_tensor, roi_resized = self._preprocess_roi(roi_rgb) - t_pre = time.perf_counter() - - use_amp_now = self.use_amp and self.device.type == "cuda" - - with torch.autocast(device_type="cuda", dtype=torch.float16, enabled=use_amp_now): - out = self.model(pixel_values=img_tensor, output_hidden_states=True) - - t_model = time.perf_counter() - - logits_seg = out.logits - - t_arg0 = time.perf_counter() - pred_low = torch.argmax(logits_seg, dim=1)[0] - pred_low_np = pred_low.detach().cpu().numpy().astype(np.uint8) - pred_ids_np = cv2.resize( - pred_low_np, - (roi_resized.shape[1], roi_resized.shape[0]), - interpolation=cv2.INTER_NEAREST - ) - t_arg1 = time.perf_counter() - - aux_result = None - - t_aux0 = time.perf_counter() - if self.mode == "dual_label" and self.aux_head is not None: - feat = out.hidden_states[-1] - - if feat.shape[-2:] != logits_seg.shape[-2:]: - feat = F.interpolate( - feat, - size=logits_seg.shape[-2:], - mode="bilinear", - align_corners=False, - ) - - with torch.autocast(device_type="cuda", dtype=torch.float16, enabled=use_amp_now): - logits_label = self.aux_head(feat, logits_seg) - - probs = torch.softmax(logits_label, dim=1)[0].detach().cpu().numpy() - - label_id = int(np.argmax(probs)) - label_conf = float(probs[label_id]) - label_name = self.label_names.get(label_id, f"label_{label_id}") - - if self.use_compact_aux: - aux_result = { - "type": "label", - "label_id": label_id, - "label_name": label_name, - "label_conf": label_conf, - } + if isinstance(data, dict) and "label_name_by_id" in data: + raw = data["label_name_by_id"] else: - aux_result = { - "type": "label", - "label_id": label_id, - "label_name": label_name, - "label_conf": label_conf, - "label_probs": probs.astype(float).tolist(), - "label_names": self.label_names, - } - if aux_result is not None: - self._last_aux_result = aux_result - self._last_aux_ts = time.time() + raw = data - t_aux1 = time.perf_counter() + if raw is None: + return {} - self.last_infer = time.time() - - t_end = time.perf_counter() - - if self.debug_timing: - print( - f"RUNNER | crop={(t_crop-t0)*1000:.1f}ms | " - f"pre={(t_pre-t_crop)*1000:.1f}ms | " - f"model={(t_model-t_pre)*1000:.1f}ms | " - f"arg={(t_arg1-t_arg0)*1000:.1f}ms | " - f"aux={(t_aux1-t_aux0)*1000:.1f}ms | " - f"total={(t_end-t0)*1000:.1f}ms" - ) - - return pred_ids_np, self.last_infer, roi_resized, (y_fim, y_inicio), aux_result - - @torch.inference_mode() - def infer_ids_seg_only(self, frame_rgb): - """ - Inferência rápida apenas da cabeça de segmentação. - - Diferenças para infer_ids(): - - não pede hidden_states; - - não roda aux_head; - - reutiliza self._last_aux_result, se existir; - - mantém o mesmo formato de retorno: - pred_ids_np, ts, roi_resized, roi_info, aux_result - """ - if frame_rgb is None or not hasattr(frame_rgb, "shape") or frame_rgb.size == 0: - return None, None, None, None, None - - t0 = time.perf_counter() - - H, W = frame_rgb.shape[:2] - y_fim, y_inicio = self.compute_roi_indices( - H, - self.roi_inicio, - self.roi_tamanho - ) - - roi_rgb = frame_rgb[y_fim:y_inicio, 0:W] - t_crop = time.perf_counter() - - if roi_rgb is None or roi_rgb.size == 0: - return None, None, None, (y_fim, y_inicio), None - - img_tensor, roi_resized = self._preprocess_roi(roi_rgb) - t_pre = time.perf_counter() - - use_amp_now = getattr(self, "use_amp", True) and self.device.type == "cuda" - - # Aqui está o ponto principal do teste: - # NÃO pede hidden_states, então o modelo só precisa entregar logits de segmentação. - with torch.autocast( - device_type="cuda", - dtype=torch.float16, - enabled=use_amp_now - ): - out = self.model( - pixel_values=img_tensor, - output_hidden_states=False - ) - - t_model = time.perf_counter() - - logits_seg = out.logits - - t_arg0 = time.perf_counter() - - pred_low = torch.argmax(logits_seg, dim=1)[0] - pred_low_np = pred_low.detach().cpu().numpy().astype(np.uint8) - - pred_ids_np = cv2.resize( - pred_low_np, - (roi_resized.shape[1], roi_resized.shape[0]), - interpolation=cv2.INTER_NEAREST - ) - - t_arg1 = time.perf_counter() - - # Reaproveita o último status conhecido. - # Para o teste inicial, pode ser None mesmo. - aux_result = getattr(self, "_last_aux_result", None) - - if aux_result is not None: - aux_result = dict(aux_result) - aux_result["stale"] = True - aux_result["age_ms"] = ( - time.time() - getattr(self, "_last_aux_ts", time.time()) - ) * 1000.0 - - self.last_infer = time.time() - t_end = time.perf_counter() - - if getattr(self, "debug_timing", False): - print( - f"RUNNER_SEG_ONLY | " - f"crop={(t_crop - t0) * 1000:.1f}ms | " - f"pre={(t_pre - t_crop) * 1000:.1f}ms | " - f"model={(t_model - t_pre) * 1000:.1f}ms | " - f"arg={(t_arg1 - t_arg0) * 1000:.1f}ms | " - f"total={(t_end - t0) * 1000:.1f}ms" - ) - - return pred_ids_np, self.last_infer, roi_resized, (y_fim, y_inicio), aux_result + if isinstance(raw, list): + return {i: str(name) for i, name in enumerate(raw)} + if isinstance(raw, dict): + result = {} + for k, v in raw.items(): + try: + result[int(k)] = str(v) + except Exception: + pass + return result + return {} diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/gpu_priority_controller.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/gpu_priority_controller.py index 154dd1cc1..fde148042 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/gpu_priority_controller.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/gpu_priority_controller.py @@ -37,6 +37,8 @@ class GpuPriorityController: good_cycles_to_recover: int = 5, min_data_age_s: float = 0.0, max_data_age_s: float = 3.0, + ignore_inactive_weed: bool = True, + weed_inactive_age_s: float = 5.0, log_periodic: bool = False, log_warnings: bool = True, ): @@ -118,6 +120,9 @@ class GpuPriorityController: "data_ok": False, } + self.ignore_inactive_weed = bool(ignore_inactive_weed) + self.weed_inactive_age_s = float(weed_inactive_age_s) + self.log_periodic = bool(log_periodic) self.log_warnings = bool(log_warnings) self._last_warning_reason = None @@ -264,6 +269,18 @@ class GpuPriorityController: now = time.time() if not isinstance(ctx, dict) or not ctx: + if self.ignore_inactive_weed: + return { + "health": 1.0, + "reason": "weed_inativo_sem_ctx", + "fps": {}, + "ratios": {}, + "gpu_ms": 0.0, + "data_age_s": None, + "data_ok": False, + "weed_active": False, + } + return { "health": 0.0, "reason": "sem_ctx_weed", @@ -272,6 +289,7 @@ class GpuPriorityController: "gpu_ms": 0.0, "data_age_s": None, "data_ok": False, + "weed_active": False, } perf = self._extract_perf_root(ctx) @@ -282,6 +300,36 @@ class GpuPriorityController: if ts: data_age_s = max(0.0, now - float(ts)) + # Se tem timestamp velho demais, considera weed parado/inativo. + if ( + self.ignore_inactive_weed + and data_age_s is not None + and data_age_s > self.weed_inactive_age_s + ): + return { + "health": 1.0, + "reason": "weed_inativo_timestamp_velho", + "fps": {}, + "ratios": {}, + "gpu_ms": 0.0, + "data_age_s": data_age_s, + "data_ok": False, + "weed_active": False, + } + + # Se não tem timestamp, mas tem contexto velho/estranho, também não deve derrubar visual. + if self.ignore_inactive_weed and ts is None: + return { + "health": 1.0, + "reason": "weed_inativo_sem_timestamp", + "fps": {}, + "ratios": {}, + "gpu_ms": 0.0, + "data_age_s": None, + "data_ok": False, + "weed_active": False, + } + data_ok = True if data_age_s is not None: data_ok = self.min_data_age_s <= data_age_s <= self.max_data_age_s @@ -304,7 +352,6 @@ class GpuPriorityController: target = max(float(target), 0.01) ratios[name] = max(0.0, min(1.5, float(real) / target)) - # O gargalo manda. Se um deles despencou, saúde despenca. valid_ratios = [v for v in ratios.values() if v is not None] health = min(valid_ratios) if valid_ratios else 0.0 @@ -318,8 +365,6 @@ class GpuPriorityController: health = 0.0 reason = "weed_sem_inferencia" - # Latência GPU como alarme extra. - # Não derruba direto para zero, mas limita a saúde. if gpu_ms >= 120: health = min(health, 0.35) reason = "gpu_ms_muito_alto" @@ -338,6 +383,7 @@ class GpuPriorityController: "gpu_ms": float(gpu_ms or 0.0), "data_age_s": data_age_s, "data_ok": bool(data_ok), + "weed_active": True, } def _extract_perf_root(self, ctx: Dict[str, Any]) -> Dict[str, Any]: @@ -507,6 +553,11 @@ class GpuPriorityController: def _log_warning_if_needed(self): h = self._last_health + + if h.get("weed_active") is False: + self._last_warning_reason = None + return + reason = h.get("reason", "ok") health = float(h.get("health", 1.0) or 0.0) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py index 54a26b2d6..483efdbfb 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py @@ -1,78 +1,140 @@ +from __future__ import annotations + import datetime import os import threading import time -from concurrent.futures import ThreadPoolExecutor +from typing import Any, Dict, Optional import cv2 import numpy as np -from visual_worker.utils import converter_valores_numpy, gerar_heatmap -from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao, SegmentacaoManager -from camera_worker.segformer_runner import SegformerNavRunner -from visual_worker.processamento.costmap_fuser import CostmapFuser, unpack_snapshot -from shared.enums import StatusModulo, T_Code, TipoFrameCamera -from shared.utils import converter_mask_ids_para_bgr, get_velocidade_atual_ms + from camera_worker.camera_oak import CameraOak +from camera_worker.segformer_runner import SegformerNavRunner + from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey -from shared.perf_monitor import VisualPerfMonitor +from shared.enums import StatusModulo, T_Code, TipoFrameCamera from shared.gpu_priority_controller import GpuPriorityController +from shared.perf_monitor import VisualPerfMonitor +from shared.utils import get_velocidade_atual_ms + +from visual_worker.utils import converter_valores_numpy +from visual_worker.processamento.segmentacao_semantica import ( + SegmentacaoManager, + SegmentacaoConfig, +) +from visual_worker.processamento.costmap_fuser import CostmapFuser +from visual_worker.processamento.visual_grid_builder import ( + GridGeometryConfig, + GridReferenceBuilder, + GridConfidenceConfig, + VisualGridBuilder, +) +from visual_worker.processamento.visual_debug_renderer import VisualDebugRenderer + class CameraManager: + """ + CameraManager v1 do Visual Worker. + + Responsabilidade: + - Inicializar OAK-D Lite. + - Inicializar SegformerNavRunner ONNX/TensorRT. + - Inicializar SegmentacaoManager v1. + - Inicializar grid builder + CostmapFuser. + - Orquestrar loops: + segmentação -> detecção onboard -> grid/costmap -> publicação -> stream/debug. + - Manter caches leves para Redis/C#/preview. + + Não faz: + - análise pesada de corredor dentro do manager; + - construção manual de overlay dentro do manager; + - construção da grid de confiança dentro do manager; + - render/debug no caminho quente. + """ + + FRAME_TYPES_PREVIEW = { + TipoFrameCamera.Rgb, + TipoFrameCamera.Segmentacao, + TipoFrameCamera.Overlay, + TipoFrameCamera.Debug, + TipoFrameCamera.Heatmap, + TipoFrameCamera.MatrizCusto, + TipoFrameCamera.Deteccoes, + } + def __init__(self, mostrar_log): self.mostrar_log = mostrar_log + self.mx_id = None self.camera = None - self._loop_analise_iniciado = False - self._loop_stream_iniciado = False + + self.seg_config = None + self.det_config = None + + self.seg_runner = None + self.segmentacao_manager = None + + self.grid_ref_builder = None + self.grid_builder = None + self.costmap_fuser = None + self.renderer = None + + self.operante = False + self.iniciando = False + self.debug_visual = False + self.debug_perf = False + + self.largura_robo_m = 0.85 + self._loop_segmentacao_iniciado = False self._loop_deteccao_iniciado = False self._loop_grid_iniciado = False self._loop_publicacao_iniciado = False + self._loop_stream_iniciado = False + self._loop_analise_iniciado = False + self._vida_lock = threading.RLock() + self._cache_lock = threading.RLock() + self._pub_lock = threading.RLock() + self._fechando_camera = False - self.reiniciar_status() - - def reiniciar_status(self): - if self.camera is not None: - try: - self.camera.parar() - except Exception as e: - self.mostrar_log(f"Erro ao parar camera: {e}") - self.camera = None - self.operante = False - self.iniciando = False - self._ultima_analise_anomalias = {} - self._ultima_analise_solo = {} - self._ultima_analise_radar = {} - self._ultima_analise_segmentacao = {} - self._ultima_analise_deteccao = {} - self._ultima_analise_matriz_confianca = {} - self._ultima_analise_matriz_custo = {} - self._ultimo_predictions = None - self._ultimo_detections = None - self._ultimo_snapshot = None - self._ultimo_rgb_frame = None - self._ultimo_depth_frame = None - self._ultimo_heatmap_frame = None - self._ultimo_frame_deteccoes = None - self._ultimo_frame_matriz_custo = None - self._ultimo_frame_corredor = None - self._ts_segmentacao_anterior = 0 - self._ts_deteccao_anterior = 0 - self._pool = ThreadPoolExecutor(max_workers=6) - - self._startup_grace_until = 0.0 - self._em_warmup = False self._falhas_utilizavel = 0 - self._fechando_camera = False - self._cache_lock = threading.RLock() + + self.perf = VisualPerfMonitor(janela=180) + self.gpu_controller = None + + self._reset_runtime_state() + + # ============================================================ + # Estado / inicialização + # ============================================================ + + def _reset_runtime_state(self): + self._ultimo_rgb_frame = None + self._ultimo_depth_frame = None + + self._ultimo_predictions = None + self._ultimo_seg_aux_result = None + self._ultima_analise_segmentacao = None + + self._ultimo_detections = [] + self._ultimo_snapshot = None + self._ultima_grid_conf = None + + self._camera_pronta_em = 0.0 + self._startup_grace_until = 0.0 + self._seg_cache = { "ts": 0.0, "frame_ts": 0.0, "predictions": None, "aux_result": None, + "analise": None, "res": None, + "infer_ms": 0.0, + "post_ms": 0.0, } self._det_cache = { @@ -87,9 +149,9 @@ class CameraManager: "grid_conf": None, } - self._pub_lock = threading.RLock() self._pub_cache = { "ts_analise": 0.0, + "segmentacao": None, "deteccao": None, "matriz_confianca": None, @@ -106,186 +168,241 @@ class CameraManager: "dirty_performance_visual": False, } - self.perf = VisualPerfMonitor(janela=180) - self.gpu_controller = None + self._ultimo_pub_debug = { + "ts": 0.0, + "publicou": 0, + "redis_ms": 0.0, + "campos": 0, + } def inicializar(self, mx_id): if self.iniciando: return - - _camera_conectada = ContextoGlobalRedis.get_cameras().get(mx_id) is not None - if not _camera_conectada: - return - - if mx_id == None: - #self.mostrar_log(f"❌ Camera não definida.") + + if mx_id is None: return - self.iniciando = True - self.mx_id = mx_id - - from visual_worker.config import load_seg_config, load_det_config - seg_config = load_seg_config() - det_config = load_det_config() - - try: - nova = CameraOak(self.mostrar_log, mx_id, modelo_ia_seg=None, modelo_ia_det=det_config, iniciar_imu=True, perf_monitor=self.perf) - if nova.iniciado: - self.camera = nova - except Exception as e: - self.mostrar_log(f"⚠️ Camera com ID {mx_id} não conectada: {e}") - self.iniciando = False + camera_ctx = ContextoGlobalRedis.get_cameras().get(mx_id) + if camera_ctx is None: return - - if self.camera is None: - self.mostrar_log(f"❌ Camera com ID {mx_id} não iniciada.") - else: - self.mostrar_log(f"📷 Camera selecionada: {self.camera.modelo} - {self.camera.mx_id}") - self.largura_robo_m = ContextoGlobalRedis.get(CtxKey.DadosEquipamento, {}).get("largura", 0.85) - self._timestamp_analise = None - - self.grid_ref_shape = (15, 10) - self.grid_ref_base = self.gerar_grid_ref() - self.grid_ref = self.grid_ref_base.copy() - self.data_fuser = CostmapFuser(grid_shape=self.grid_ref_shape, K=3, M=2, central_cols=None, y_range_m=(0.5,5.0), near_is_bottom=True, fov_h_rad=np.radians(self.camera.parametros["fov_h"]), robot_width=self.largura_robo_m) - - if not hasattr(self, "seg_runner") or self.seg_runner is None: - self.seg_runner = SegformerNavRunner(seg_config) - - if not hasattr(self, "segmentacao_manager") or self.segmentacao_manager is None: - self.segmentacao_manager = SegmentacaoManager( - color_map=self.seg_runner.colormap_rgb, - classes=self.seg_runner.classes - ) - - self._ultima_analise_segmentacao = {} - self._ultima_analise_deteccao = {} - self._ultima_analise_matriz_confianca = {} - self._ultima_analise_matriz_custo = {} - self._ts_segmentacao_anterior = 0 - self._ts_deteccao_anterior = 0 - self._ultimo_rgb_frame = None - self._ultimo_depth_frame = None - self._ultimo_heatmap_frame = None - self._ultimo_predictions = None - self._ultimo_detections = None - self._ultimo_snapshot = None - self._ultimo_frame_deteccoes = None - self._ultimo_frame_matriz_custo = None - self._ultimo_frame_corredor = None - self._rgb_frame_necessario = True - self._depth_frame_necessario = True - self._nova_segmentacao_disponivel = False - self._nova_deteccao_disponivel = False - self._nova_grid_conf_disponivel = False - - self._analisando_matriz_custo = False - self._analisando_matriz_confianca = False - self._analisando_segmentacao = False - self._analisando_deteccao = False - - # Mantém os loops bloqueados durante o warmup. - self.operante = False - self._camera_pronta_em = time.time() + 999.0 - - warmup_ok = self._executar_warmup_camera_manager( - n_frames_camera=8, - n_seg=4, - n_det=4, - n_grid=3, - timeout_s=6.0 - ) - - if not warmup_ok: - self.mostrar_log("[WARMUP] finalizou com alerta, liberando operação mesmo assim") - - # Dá uma janela para saúde/cache estabilizarem. - self._startup_grace_until = time.time() + 5.0 - - # Libera operação. - self._camera_pronta_em = time.time() + 0.2 - self.operante = True - - cam_atual = self.camera - if cam_atual is None: - self.mostrar_log("[INIT] camera ficou None antes de iniciar loops; abortando init") - self.iniciando = False - return - - self.debug_perf = bool(seg_config.get("debug_perf", False)) - freq_analise = float(seg_config.get("analise_fps", 20.0)) - freq_grid = float(seg_config.get("grid_fps", 20.0)) - freq_publicacao = float(seg_config.get("publicacao_fps", 15.0)) - freq_inferencia = float(seg_config.get("inferencia_fps", freq_analise)) - freq_deteccao = float(seg_config.get("deteccao_fps", freq_analise)) - - try: - from weed_worker.config import load_seg_config as load_weed_config - weed_config = load_weed_config() or {} - except Exception as e: - self.mostrar_log(f"[GPU_CTRL] erro ao carregar config do weed_worker: {e}") - weed_config = {} - gpu_ctrl_cfg = seg_config.get("gpu_priority", {}) or {} - self.gpu_controller = GpuPriorityController( - mostrar_log=self.mostrar_log, - enabled=bool(gpu_ctrl_cfg.get("enabled", True)), - update_interval_s=float(gpu_ctrl_cfg.get("update_interval_s", 1.0)), - log_interval_s=float(gpu_ctrl_cfg.get("log_interval_s", 3.0)), - log_periodic=bool(gpu_ctrl_cfg.get("log_periodic", False)), - log_warnings=bool(gpu_ctrl_cfg.get("log_warnings", True)), - weed_targets = { - "tensor": float(weed_config.get("tensor_fps", 18.0)), - "inferencia": float(weed_config.get("inferencia_fps", 15.0)), - "deteccao": float(weed_config.get("deteccao_fps", 15.0)), - }, - visual_targets_normal={ - "segmentacao": freq_inferencia, - "grid": freq_grid, - "deteccao": freq_deteccao, - "publicacao": freq_publicacao, - "analise": freq_analise, - }, - mode_configs=gpu_ctrl_cfg.get("mode_configs", None), - bad_cycles_to_degrade=int(gpu_ctrl_cfg.get("bad_cycles_to_degrade", 3)), - good_cycles_to_recover=int(gpu_ctrl_cfg.get("good_cycles_to_recover", 5)), - max_data_age_s=float(gpu_ctrl_cfg.get("max_data_age_s", 3.0)), - ) - - if not self._loop_stream_iniciado: - cam_atual = self.camera - if cam_atual is None: - self.mostrar_log("[INIT] não iniciou stream: camera None") - else: - stream = getattr(cam_atual, "stream", None) - stream_fps = getattr(stream, "_op_fps", 2.0) - self._iniciar_loop_frame_stream(stream_fps) - self._loop_stream_iniciado = True - - if not self._loop_publicacao_iniciado: - self._iniciar_loop_publicacao_visual(freq=freq_publicacao) - self._loop_publicacao_iniciado = True - - if not self._loop_segmentacao_iniciado: - self._iniciar_loop_segmentacao(freq=freq_inferencia) - self._loop_segmentacao_iniciado = True - - if not self._loop_deteccao_iniciado: - self._iniciar_loop_deteccao(freq=freq_deteccao) - self._loop_deteccao_iniciado = True - - if not self._loop_grid_iniciado: - self._iniciar_loop_matriz_confianca(freq=freq_grid) - self._loop_grid_iniciado = True - - if not self._loop_analise_iniciado: - self._iniciar_loop_analise_continua(15.0) - self._loop_analise_iniciado = True - - self.iniciando = False - self.atualizar_saude_camera() with self._vida_lock: - self._fechando_camera = False + self.iniciando = True + + try: + self.mx_id = mx_id + + from visual_worker.config import load_seg_config, load_det_config + + self.seg_config = load_seg_config() + self.det_config = load_det_config() + + self.debug_visual = bool(self.seg_config.get("debug_visual", False)) + self.debug_perf = bool(self.seg_config.get("debug_perf", False)) + + self._inicializar_camera(mx_id) + + if self.camera is None: + self.mostrar_log(f"❌ Camera com ID {mx_id} não iniciada.") + return + + self.mostrar_log( + f"📷 Camera selecionada: {self.camera.modelo} - {self.camera.mx_id}" + ) + + self._reset_runtime_state() + + self.largura_robo_m = float( + ContextoGlobalRedis.get(CtxKey.DadosEquipamento, {}).get("largura", 0.85) + ) + + self._inicializar_segmentador() + self._inicializar_analisador_segmentacao() + self._inicializar_grid() + self._inicializar_renderer() + self._inicializar_gpu_controller() + + self.operante = False + self._em_warmup = True + self._camera_pronta_em = time.time() + 999.0 + + warmup_ok = self._executar_warmup_camera_manager( + n_frames_camera=8, + n_seg=4, + n_det=4, + n_grid=3, + timeout_s=6.0, + ) + + self._em_warmup = False + + if not warmup_ok: + self.mostrar_log("[visual][WARMUP] finalizou com alerta, liberando operação mesmo assim") + + self._camera_pronta_em = time.time() + 0.2 + self._startup_grace_until = time.time() + 5.0 + self.operante = True + + self._iniciar_loops_se_necessario(self.seg_config) + self.atualizar_saude_camera() + + finally: + self.iniciando = False + self._fechando_camera = False + + def _inicializar_camera(self, mx_id): + try: + nova = CameraOak( + self.mostrar_log, + mx_id, + modelo_ia_seg=None, + modelo_ia_det=self.det_config, + iniciar_imu=True, + perf_monitor=self.perf, + ) + + self.camera = nova if nova.iniciado else None + + except Exception as e: + self.camera = None + self.mostrar_log(f"⚠️ Camera com ID {mx_id} não conectada: {e}") + + def _inicializar_segmentador(self): + self.seg_runner = SegformerNavRunner(self.seg_config) + + self.mostrar_log( + "[SEG_ONNX] Runner iniciado | " + f"model={self.seg_config.get('onnx_model_path')} | " + f"provider={self.seg_config.get('onnx_provider', 'tensorrt')} | " + f"resolution={self.seg_config.get('ia_resolution')}" + ) + + def _inicializar_analisador_segmentacao(self): + cfg_dict = self.seg_config.get("segmentacao", {}) or {} + + seg_cfg = SegmentacaoConfig(**{ + k: v + for k, v in cfg_dict.items() + if k in SegmentacaoConfig.__dataclass_fields__ + }) + + self.segmentacao_manager = SegmentacaoManager( + config=seg_cfg, + color_map=getattr(self.seg_runner, "colormap_rgb", None), + classes=getattr(self.seg_runner, "classes", None), + ) + + def _inicializar_grid(self): + grid_cfg = self.seg_config.get("grid", {}) or {} + geom_cfg = grid_cfg.get("geometry", {}) or {} + conf_cfg = grid_cfg.get("confidence", {}) or {} + fuser_cfg = grid_cfg.get("fuser", {}) or {} + + if "grid_shape" not in geom_cfg: + geom_cfg["grid_shape"] = tuple(grid_cfg.get("grid_shape", (15, 10))) + + self.grid_ref_builder = GridReferenceBuilder(GridGeometryConfig(**{ + k: v + for k, v in geom_cfg.items() + if k in GridGeometryConfig.__dataclass_fields__ + })) + + self.grid_builder = VisualGridBuilder(conf_cfg) + + grid_shape = self.grid_ref_builder.grid_shape + + fov_h = float(self.camera.parametros.get("fov_h", 69.0)) + + self.costmap_fuser = CostmapFuser( + config={ + **fuser_cfg, + "grid_shape": grid_shape, + "fov_h_rad": np.radians(fov_h), + "robot_width_m": self.largura_robo_m, + } + ) + + def _inicializar_renderer(self): + self.renderer = VisualDebugRenderer( + color_map=getattr(self.seg_runner, "colormap_rgb", None), + preview_size=tuple(self.seg_config.get("preview_size", (1280, 720))), + ) + + def _inicializar_gpu_controller(self): + freq_analise = float(self.seg_config.get("analise_fps", 8.0)) + freq_grid = float(self.seg_config.get("grid_fps", 5.0)) + freq_publicacao = float(self.seg_config.get("publicacao_fps", 15.0)) + freq_inferencia = float(self.seg_config.get("inferencia_fps", freq_analise)) + freq_deteccao = float(self.seg_config.get("deteccao_fps", freq_analise)) + + try: + from weed_worker.config import load_seg_config as load_weed_config + weed_config = load_weed_config() or {} + except Exception as e: + self.mostrar_log(f"[GPU_CTRL] erro ao carregar config do weed_worker: {e}") + weed_config = {} + + gpu_cfg = self.seg_config.get("gpu_priority", {}) or {} + + self.gpu_controller = GpuPriorityController( + mostrar_log=self.mostrar_log, + enabled=bool(gpu_cfg.get("enabled", True)), + update_interval_s=float(gpu_cfg.get("update_interval_s", 1.0)), + log_interval_s=float(gpu_cfg.get("log_interval_s", 3.0)), + log_periodic=bool(gpu_cfg.get("log_periodic", False)), + log_warnings=bool(gpu_cfg.get("log_warnings", True)), + weed_targets={ + "tensor": float(weed_config.get("tensor_fps", 18.0)), + "inferencia": float(weed_config.get("inferencia_fps", 15.0)), + "deteccao": float(weed_config.get("deteccao_fps", 15.0)), + }, + visual_targets_normal={ + "segmentacao": freq_inferencia, + "grid": freq_grid, + "deteccao": freq_deteccao, + "publicacao": freq_publicacao, + "analise": freq_analise, + }, + mode_configs=gpu_cfg.get("mode_configs", None), + bad_cycles_to_degrade=int(gpu_cfg.get("bad_cycles_to_degrade", 3)), + good_cycles_to_recover=int(gpu_cfg.get("good_cycles_to_recover", 5)), + max_data_age_s=float(gpu_cfg.get("max_data_age_s", 3.0)), + ignore_inactive_weed=bool(gpu_cfg.get("ignore_inactive_weed", True)), + weed_inactive_age_s=float(gpu_cfg.get("weed_inactive_age_s", 5.0)), + ) + + def _iniciar_loops_se_necessario(self, seg_config): + freq_analise = float(seg_config.get("analise_fps", 8.0)) + freq_grid = float(seg_config.get("grid_fps", 5.0)) + freq_publicacao = float(seg_config.get("publicacao_fps", 15.0)) + freq_inferencia = float(seg_config.get("inferencia_fps", freq_analise)) + freq_deteccao = float(seg_config.get("deteccao_fps", freq_analise)) + + if not self._loop_stream_iniciado: + stream = getattr(self.camera, "stream", None) + stream_fps = float(getattr(stream, "_op_fps", 2.0) or 2.0) + self._iniciar_loop_frame_stream(stream_fps) + self._loop_stream_iniciado = True + + if not self._loop_publicacao_iniciado: + self._iniciar_loop_publicacao_visual(freq=freq_publicacao) + self._loop_publicacao_iniciado = True + + if not self._loop_segmentacao_iniciado: + self._iniciar_loop_segmentacao(freq=freq_inferencia) + self._loop_segmentacao_iniciado = True + + if not self._loop_deteccao_iniciado: + self._iniciar_loop_deteccao(freq=freq_deteccao) + self._loop_deteccao_iniciado = True + + if not self._loop_grid_iniciado: + self._iniciar_loop_grid(freq=freq_grid) + self._loop_grid_iniciado = True + + if not self._loop_analise_iniciado: + self._iniciar_loop_analise_continua(freq=freq_analise) + self._loop_analise_iniciado = True def fechar_camera_manager(self, motivo=""): with self._vida_lock: @@ -294,6 +411,7 @@ class CameraManager: self._fechando_camera = True cam = self.camera + self.camera = None self.operante = False self.iniciando = False @@ -302,67 +420,27 @@ class CameraManager: if cam is not None: cam.parar() except Exception as e: - self.mostrar_log(f"Erro ao fechar camera: {e}") + self.mostrar_log(f"[visual] erro ao fechar câmera: {e}") with self._cache_lock: - self._ultimo_rgb_frame = None - self._ultimo_depth_frame = None - self._ultimo_heatmap_frame = None - self._ultimo_predictions = None - self._ultimo_detections = None - self._ultimo_snapshot = None + self._reset_runtime_state() - self._seg_cache = { - "ts": 0.0, - "frame_ts": 0.0, - "predictions": None, - "aux_result": None, - "res": None, - } + self.mostrar_log(f"[visual] Camera Manager fechado: {motivo}") - self._det_cache = { - "ts": 0.0, - "detections": [], - "res": None, - } - - self._grid_cache = { - "ts": 0.0, - "snapshot": None, - "grid_conf": None, - } - - self.mostrar_log(f"Camera Manager fechado: {motivo}") - - def _executar_warmup_camera_manager( - self, - n_frames_camera=8, - n_seg=4, - n_det=4, - n_grid=3, - timeout_s=6.0 - ): - """ - Aquece CameraOak, detector, SegFormer, pós-segmentação, grid e fuser - antes de liberar os loops principais. - - Objetivo: - - evitar primeiro infer_ms gigante; - - garantir cache RGB/depth preenchido; - - garantir detector respondendo; - - preencher cache inicial de segmentação e grid; - - deixar o VisualWorker entrar operante já com dados úteis. - """ - t_all0 = time.perf_counter() + # ============================================================ + # Warmup + # ============================================================ + def _executar_warmup_camera_manager(self, n_frames_camera=8, n_seg=4, n_det=4, n_grid=3, timeout_s=6.0): if self.camera is None: return False - self.mostrar_log("[WARMUP] iniciando warmup do CameraManager...") + self.mostrar_log("[visual][WARMUP] iniciando warmup do CameraManager...") + + t_all0 = time.perf_counter() - # Durante o warmup, não queremos os loops principais rodando. operante_anterior = self.operante - warmup_anterior = getattr(self, "_em_warmup", False) + warmup_anterior = self._em_warmup self.operante = False self._em_warmup = True @@ -371,94 +449,59 @@ class CameraManager: rgb_ts = None depth_frame = None depth_ts = None + predictions = None + aux_result = None + analise_seg = None dets = [] try: - # ===================================================== - # 1) Esperar cache RGB/depth ficar vivo - # ===================================================== t0 = time.time() - rgb_ok = False - depth_ok = False while time.time() - t0 < timeout_s: - try: - rgb_frame, rgb_ts, _ = self.get_rgb_frame() - depth_frame, depth_ts, _ = self.get_depth_frame() + rgb_frame, rgb_ts, _ = self.get_rgb_frame() + depth_frame, depth_ts, _ = self.get_depth_frame() - rgb_ok = rgb_frame is not None and rgb_ts is not None - depth_ok = depth_frame is not None and depth_ts is not None - - if rgb_ok and depth_ok: - break - - except Exception: - pass + if rgb_frame is not None and rgb_ts is not None and depth_frame is not None and depth_ts is not None: + break time.sleep(0.03) + rgb_ok = rgb_frame is not None and rgb_ts is not None + depth_ok = depth_frame is not None and depth_ts is not None + self.mostrar_log( - f"[WARMUP] cache camera rgb_ok={rgb_ok} depth_ok={depth_ok} " + f"[visual][WARMUP] cache camera rgb_ok={rgb_ok} depth_ok={depth_ok} " f"rgb_ts={rgb_ts} depth_ts={depth_ts}" ) if not rgb_ok: - self.mostrar_log("[WARMUP] abortado: sem RGB válido") - self.operante = operante_anterior return False - # Faz algumas leituras extras para estabilizar o cache. for _ in range(max(0, n_frames_camera)): rgb_frame, rgb_ts, _ = self.get_rgb_frame() depth_frame, depth_ts, _ = self.get_depth_frame() time.sleep(0.02) - # ===================================================== - # 2) Warmup detector onboard - # ===================================================== - det_ok_count = 0 - for _ in range(max(0, n_det)): + det_ok = 0 + for i in range(max(0, n_det)): try: dets_tmp, det_ts, det_res = self.get_detections() if dets_tmp is not None and det_ts is not None: dets = dets_tmp - det_ok_count += 1 - - with self._cache_lock: - self._ultimo_detections = dets - self._ultima_analise_deteccao = { - "bboxes": dets, - "ultima_chamada": time.time() - } - self._det_cache = { - "ts": time.time(), - "detections": dets, - "res": det_res, - } - self._nova_deteccao_disponivel = True + det_ok += 1 + self._store_detection(dets, det_ts, det_res) except Exception as e: - self.mostrar_log(f"[WARMUP] detector falhou: {e}") + self.mostrar_log(f"[visual][WARMUP] detector falhou: {e}") time.sleep(0.03) - self.mostrar_log(f"[WARMUP] detector ok_count={det_ok_count}/{n_det}") - - # ===================================================== - # 3) Warmup SegFormer + SegmentacaoManager - # ===================================================== - seg_ok_count = 0 - analise_segmentacao = None - predictions = None - aux_result = None - seg_res = None - frame_ts = rgb_ts - + seg_ok = 0 for i in range(max(0, n_seg)): try: - # Pega frame mais recente antes de cada inferência. - rgb_frame, frame_ts, seg_res = self.get_rgb_frame() + rgb_frame, frame_ts, res = self.get_rgb_frame() + rgb_frame = self._validar_frame_rgb_para_segmentacao(rgb_frame) if rgb_frame is None: continue @@ -471,77 +514,36 @@ class CameraManager: continue t_post0 = time.perf_counter() - analise_segmentacao, log = self.segmentacao_manager.segmentar( - predictions, - aux_result=aux_result - ) + analise_seg = self.segmentacao_manager.analisar(predictions, aux_result=aux_result) t_post1 = time.perf_counter() - if analise_segmentacao is None: - if log: - self.mostrar_log(f"[WARMUP] segmentacao log: {log}") - continue + self._store_segmentacao( + predictions=predictions, + aux_result=aux_result, + analise=analise_seg, + frame_ts=frame_ts, + res=res, + infer_ms=(t_inf1 - t_inf0) * 1000.0, + post_ms=(t_post1 - t_post0) * 1000.0, + ) - if "classes" not in analise_segmentacao: - analise_segmentacao["classes"] = predictions - - seg_ok_count += 1 + seg_ok += 1 self.mostrar_log( - f"[WARMUP] seg {i+1}/{n_seg} " - f"infer={(t_inf1-t_inf0)*1000:.1f}ms " - f"post={(t_post1-t_post0)*1000:.1f}ms" + f"[visual][WARMUP] seg {i + 1}/{n_seg} " + f"infer={(t_inf1 - t_inf0) * 1000.0:.1f}ms " + f"post={(t_post1 - t_post0) * 1000.0:.1f}ms" ) except Exception as e: - self.mostrar_log(f"[WARMUP] segmentacao falhou: {e}") + self.mostrar_log(f"[visual][WARMUP] segmentacao falhou: {e}") time.sleep(0.03) - if analise_segmentacao is not None and predictions is not None: - with self._cache_lock: - self._ultimo_predictions = predictions - self._ultimo_seg_aux_result = aux_result - self._ultima_analise_segmentacao = analise_segmentacao - self._seg_cache = { - "ts": time.time(), - "frame_ts": frame_ts or 0.0, - "predictions": predictions, - "aux_result": aux_result, - "res": seg_res, - } - self._nova_segmentacao_disponivel = True - - self.mostrar_log(f"[WARMUP] segmentacao ok_count={seg_ok_count}/{n_seg}") - - # Sem segmentação, não tem como aquecer grid corretamente. - if analise_segmentacao is None: - self.mostrar_log("[WARMUP] sem segmentação válida para aquecer grid") - self.operante = operante_anterior + if analise_seg is None or predictions is None: return False - # ===================================================== - # 4) Warmup grid + fuser - # ===================================================== - grid_ok_count = 0 - - det_params = { - "w4": 0.18, - "thr_det_soft": 0.45, - "min_cell_coverage": 0.10, - "min_det_conf": 0.45, - "class_weights": { - "person": 1.0, - "dog": 0.7, - "cat": 0.5, - }, - "veto_labels": {"person"}, - "combine": "max", - "conf_drop_alpha": 0.0, - "only_veto_blocks_nav": True, - "non_veto_cost_scale": 0.50, - } - + grid_ok = 0 for i in range(max(0, n_grid)): try: depth_frame, depth_ts, _ = self.get_depth_frame() @@ -549,95 +551,33 @@ class CameraManager: if depth_frame is None: continue - try: - imu_ctx = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} - imu_roll = imu_ctx.get("roll_seg", 0) - except Exception: - imu_roll = 0 - - self.grid_ref = self._ajustar_grid_ref_por_pitch( - pitch_graus=imu_roll - ) - - t_grid0 = time.perf_counter() - grid_conf = self._construir_grid_confianca( - depth_frame, - analise_segmentacao["classes"], - self.grid_ref, - self.grid_ref_shape, + snapshot, grid_conf = self._processar_grid( + depth_frame=depth_frame, + segmentacao=predictions, + dados_visuais_seg=analise_seg.get("dados_visuais", {}), deteccoes=dets, - det_params=det_params + velocidade_ms=0.0, ) - t_grid1 = time.perf_counter() - if grid_conf is None: + if snapshot is None: continue - t_fuser0 = time.perf_counter() - snapshot = self.data_fuser.update( - grid_conf, - velocidade_ms=0.0, - status_seg=analise_segmentacao.get("dados_visuais", {}).get("status_corredor") - ) - t_fuser1 = time.perf_counter() + self._store_grid(snapshot=snapshot, grid_conf=grid_conf) - with self._cache_lock: - self._ultimo_snapshot = snapshot - self._ultima_analise_matriz_confianca = grid_conf - self._grid_cache = { - "ts": time.time(), - "snapshot": snapshot, - "grid_conf": grid_conf, - } - self._nova_grid_conf_disponivel = True - - grid_ok_count += 1 - - self.mostrar_log( - f"[WARMUP] grid {i+1}/{n_grid} " - f"build={(t_grid1-t_grid0)*1000:.1f}ms " - f"fuser={(t_fuser1-t_fuser0)*1000:.1f}ms" - ) + grid_ok += 1 except Exception as e: - self.mostrar_log(f"[WARMUP] grid falhou: {e}") + self.mostrar_log(f"[visual][WARMUP] grid falhou: {e}") time.sleep(0.03) - self.mostrar_log(f"[WARMUP] grid ok_count={grid_ok_count}/{n_grid}") - - # ===================================================== - # 5) Publicação inicial opcional no Redis - # ===================================================== - try: - if analise_segmentacao is not None: - self._set_pub_cache("segmentacao", analise_segmentacao["dados_visuais"]) - - if self._ultimo_snapshot is not None: - self._set_pub_cache("matriz_confianca", self._ultimo_snapshot) - - if dets is not None: - self._set_pub_cache("deteccao", dets) - - except Exception as e: - self.mostrar_log(f"[WARMUP] publicação inicial Redis falhou: {e}") - - dt_all = (time.perf_counter() - t_all0) * 1000.0 self.mostrar_log( - f"[WARMUP] finalizado em {dt_all:.1f}ms | " - f"seg_ok={seg_ok_count}/{n_seg} " - f"grid_ok={grid_ok_count}/{n_grid} " - f"det_ok={det_ok_count}/{n_det}" + f"[visual][WARMUP] finalizado em {(time.perf_counter() - t_all0) * 1000.0:.1f}ms | " + f"seg_ok={seg_ok}/{n_seg} grid_ok={grid_ok}/{n_grid} det_ok={det_ok}/{n_det}" ) - self.operante = operante_anterior - return True + return seg_ok > 0 - except Exception as e: - self.mostrar_log(f"[WARMUP] erro geral: {e}") - self.operante = operante_anterior - return False - finally: self.operante = operante_anterior self._em_warmup = warmup_anterior @@ -646,6 +586,10 @@ class CameraManager: if self.camera is not None: self.camera.perf = self.perf + # ============================================================ + # Câmera + # ============================================================ + def _is_erro_fatal_camera(self, erro): if erro is None: return False @@ -662,140 +606,11 @@ class CameraManager: return any(s in txt for s in sinais) - def gerar_grid_ref(self, n_frames=50): - try: - grid = self._gerar_grid_referencia_geometrico() - - if grid is None or len(grid) != self.grid_ref_shape[1]: - self.mostrar_log("grid_ref inválido ou com tamanho incorreto") - return None - - grid = np.asarray(grid, dtype=np.float32) - - if not np.all(np.isfinite(grid)): - self.mostrar_log("grid_ref contém NaN/Inf") - return None - - if np.any(grid <= 0): - self.mostrar_log("grid_ref contém valores <= 0") - return None - - # monotonicidade esperada - diffs = np.diff(grid) - if not np.all(diffs > 0): - self.mostrar_log(f"[WARN] grid_ref não monotônico: {grid.tolist()}") - - return grid - - except Exception as e: - self.mostrar_log(f"Erro ao gerar grid_ref: {e}") - return None - - def _gerar_grid_referencia_geometrico(self, angulo_inclinacao_graus=28.91, altura_camera_m=0.74): - try: - grid_w, grid_h = self.grid_ref_shape - - def dist_grid_calibrado(grid_h, i, fov_deg, incl_deg, altura_m): - alpha_v = ((i + 0.5) / grid_h - 0.5) * np.radians(abs(fov_deg)) - gamma = np.radians(incl_deg) + alpha_v - - # evita tangente perto de zero - gamma = max(gamma, np.radians(2.0)) - - d = altura_m / np.tan(gamma) - - # saturação plausível - d = float(np.clip(d, 0.2, 20.0)) - return d - - d = np.array( - [dist_grid_calibrado(grid_h, i, -43.28, angulo_inclinacao_graus, altura_camera_m) for i in range(grid_h)], - dtype=np.float32 - ) - - #d = d[::-1] - return d - - except Exception as e: - self.mostrar_log(f"Erro em _gerar_grid_referencia_geometrico: {e}") - return None - - def _ajustar_grid_ref_por_pitch(self, pitch_graus, pitch_gain=1.0): - """ - Ajusta a grid_ref empírica base com base no pitch atual. - Retorna uma nova grid_ref (1D, len = grid_h). - """ - try: - if self.grid_ref_base is None: - return self.grid_ref - - grid_h = self.grid_ref_shape[1] - - # pitch corrigido / limitado - pitch_corr = float(np.clip(pitch_graus * pitch_gain, -10.0, 10.0)) - - # reutiliza a mesma lógica da calibração base - grid_ref_fixed = self._gerar_grid_referencia_geometrico( - angulo_inclinacao_graus=28.91 + pitch_corr, - altura_camera_m=0.74 - ) - - if grid_ref_fixed is None: - return self.grid_ref_base.copy() - - grid_ref_fixed = np.asarray(grid_ref_fixed, dtype=np.float32) - - if len(grid_ref_fixed) != grid_h: - return self.grid_ref_base.copy() - - if not np.all(np.isfinite(grid_ref_fixed)) or np.any(grid_ref_fixed <= 0): - return self.grid_ref_base.copy() - - #print(f"angulo: {pitch_graus:.2f}, {grid_ref_fixed}") - return grid_ref_fixed - - except Exception as e: - self.mostrar_log(f"Erro ao ajustar grid_ref por pitch: {e}") - return self.grid_ref_base.copy() - - def atualizar_saude_camera(self): - #self.mostrar_log("Atualizando saude da camera...") - try: - if self.camera is not None: - self.camera.atualizar_saude() - elif self.mx_id is not None: - from camera_worker.manager import definir_saude_camera - definir_saude_camera(self.mx_id, StatusModulo.DESCONECTADO, 0, ["desconectado"], False, {}, disp=T_Code.Snr, conectado=False) - except Exception as e: - self.mostrar_log(f"[saude] erro: {e}") - - - def _camera_indisponivel_temporaria(self, cam): - if cam is None: - return True - - if not hasattr(cam, "esta_utilizavel"): - return False - - if cam.esta_utilizavel(): - self._falhas_utilizavel = 0 - return False - - if time.time() < getattr(self, "_camera_pronta_em", 0): - return True - - self._falhas_utilizavel = getattr(self, "_falhas_utilizavel", 0) + 1 - - if self._falhas_utilizavel >= 10: - self.fechar_camera_manager("camera não utilizável") - - return True - def _camera_indisponivel_rapida(self, cam): if cam is None: return True - if not self.operante and not getattr(self, "_em_warmup", False): + if not self.operante and not self._em_warmup: return True if getattr(cam, "device", None) is None: @@ -803,6 +618,21 @@ class CameraManager: return False + def _validar_frame_rgb_para_segmentacao(self, rgb_frame): + if rgb_frame is None: + return None + + if not hasattr(rgb_frame, "shape") or rgb_frame.size == 0: + return None + + if rgb_frame.ndim != 3 or rgb_frame.shape[2] != 3: + self.mostrar_log(f"[SEG_ONNX] RGB inválido shape={getattr(rgb_frame, 'shape', None)}") + return None + + if rgb_frame.dtype != np.uint8: + rgb_frame = np.clip(rgb_frame, 0, 255).astype(np.uint8) + + return np.ascontiguousarray(rgb_frame) def get_rgb_frame(self): cam = self.camera @@ -829,7 +659,6 @@ class CameraManager: except Exception as e: self.mostrar_log(f"Erro ao requisitar rgb frame: {e}") - if self._is_erro_fatal_camera(e): self.fechar_camera_manager(f"falha fatal DepthAI: {e}") @@ -863,58 +692,11 @@ class CameraManager: except Exception as e: self.mostrar_log(f"Erro ao requisitar depth frame: {e}") - if self._is_erro_fatal_camera(e): self.fechar_camera_manager(f"falha fatal DepthAI: {e}") return None, None, None - def get_heatmap_frame(self): - cam = self.camera - frame, timestamp, res = self.get_depth_frame() - - if frame is not None and cam is not None: - heatmap = gerar_heatmap(frame, cam.parametros["distancia_maxima"]) - self._ultimo_heatmap_frame = heatmap - return heatmap, timestamp, res - - return None, None, None - - def get_segmentation_predictions(self): - if self.camera is None: - return None, None, None, None - - try: - t_rgb0 = time.perf_counter() - rgb_frame, _ts, res = self.get_rgb_frame() - t_rgb1 = time.perf_counter() - - t_inf0 = time.perf_counter() - predictions, ts, roi_resized, roi_info, aux_result = self.seg_runner.infer_ids(rgb_frame) - t_inf1 = time.perf_counter() - - #print( - # f"GET_SEG | rgb={(t_rgb1-t_rgb0)*1000:.1f}ms | " - # f"infer={(t_inf1-t_inf0)*1000:.1f}ms | " - # f"total={(t_inf1-t_rgb0)*1000:.1f}ms" - #) - - self._ultimo_predictions = predictions - self._ultimo_seg_aux_result = aux_result - - if predictions is not None: - return predictions, ts, res, aux_result - - elif "X_LINK_ERROR" in res.get("erro", ""): - self.reiniciar_status() - - except Exception as e: - self.mostrar_log(f"Erro ao requisitar predictions: {e}") - if "X_LINK_ERROR" in str(e): - self.reiniciar_status() - - return None, None, None, None - def get_detections(self): cam = self.camera @@ -930,305 +712,163 @@ class CameraManager: return None, None, res ts = getattr(cam, "timestamp_ultima_deteccao", None) - - if detections is not None and ts is not None: - with self._cache_lock: - self._ultimo_detections = detections - return detections, ts, res - return detections, ts, res except Exception as e: self.mostrar_log(f"Erro ao requisitar detections: {e}") - if self._is_erro_fatal_camera(e): self.fechar_camera_manager(f"falha fatal DepthAI: {e}") return None, None, None - def get_selected_frame(self, _frame_type: TipoFrameCamera): - _frame = None - if (_frame_type == TipoFrameCamera.Rgb): - _frame = self._ultimo_rgb_frame - elif (_frame_type in [TipoFrameCamera.Segmentacao, TipoFrameCamera.Overlay]): - if _frame_type == TipoFrameCamera.Segmentacao: _alpha = 1.0 - elif _frame_type == TipoFrameCamera.Overlay: _alpha = 0.5 - _frame, _ = self._build_preview(self._ultimo_rgb_frame, self._ultimo_predictions, alpha=_alpha) - elif (_frame_type == TipoFrameCamera.Debug): - _frame = None - elif (_frame_type == TipoFrameCamera.Heatmap): - if (self._ultimo_depth_frame is not None): - _frame = gerar_heatmap(self._ultimo_depth_frame, self.camera.parametros["distancia_maxima"]) - elif (_frame_type == TipoFrameCamera.MatrizCusto): - _frame, _ = self.debug_blockage_imshow(self._ultimo_rgb_frame, self._ultimo_snapshot, show=False) - elif (_frame_type == TipoFrameCamera.Deteccoes): - _frame, _, _ = self._overlay_deteccoes(self._ultimo_rgb_frame, self._ultimo_detections, show=False) - elif (_frame_type == TipoFrameCamera.Corredor): - _frame = self.segmentacao_manager.display_segmentation_debug(self._ultimo_rgb_frame, self.largura_robo_m, self.grid_ref, show=False) - return _frame + # ============================================================ + # Store helpers + # ============================================================ + def _store_segmentacao(self, predictions, aux_result, analise, frame_ts, res, infer_ms=0.0, post_ms=0.0): + agora = time.time() - def _iniciar_loop_analise_continua(self, freq): - def loop(): - while True: - t0 = time.time() + with self._cache_lock: + self._ultimo_predictions = predictions + self._ultimo_seg_aux_result = aux_result + self._ultima_analise_segmentacao = analise - try: - if self.camera is None: - time.sleep(0.5) - continue + self._seg_cache = { + "ts": agora, + "frame_ts": frame_ts or 0.0, + "predictions": predictions, + "aux_result": aux_result, + "analise": analise, + "res": res, + "infer_ms": float(infer_ms or 0.0), + "post_ms": float(post_ms or 0.0), + } - status = StatusModulo( - (self.camera.ultima_saude or {}).get( - "status", - StatusModulo.DESCONECTADO.value - ) - ) + self._set_pub_cache("segmentacao", analise.get("dados_visuais", {})) - ts_status = (self.camera.ultima_saude or {}).get("timestamp", 0) + def _store_detection(self, detections, ts, res): + agora = time.time() - if self.iniciando or t0 < getattr(self, "_startup_grace_until", 0.0): - time.sleep(0.1) - continue + with self._cache_lock: + self._ultimo_detections = detections or [] + self._det_cache = { + "ts": agora, + "frame_ts": ts or 0.0, + "detections": detections or [], + "res": res, + } - if status == StatusModulo.DESCONECTADO and ts_status and (t0 - ts_status) > 10.0: - self.fechar_camera_manager("status desconectado") - continue + self._set_pub_cache("deteccao", detections or []) - agora = time.time() + def _store_grid(self, snapshot, grid_conf): + agora = time.time() - if not hasattr(self, "_ultimo_perf_publish"): - self._ultimo_perf_publish = 0.0 + with self._cache_lock: + self._ultimo_snapshot = snapshot + self._ultima_grid_conf = grid_conf - if agora - self._ultimo_perf_publish >= 1.0: - self._ultimo_perf_publish = agora + self._grid_cache = { + "ts": agora, + "snapshot": snapshot, + "grid_conf": grid_conf, + } - ctrl = getattr(self, "gpu_controller", None) - if ctrl is not None: - ctrl.update_from_redis() + self._set_pub_cache("matriz_confianca", snapshot) - resumo = self.perf.resumo() + # ============================================================ + # Processamento + # ============================================================ - if self.camera is not None and hasattr(self.camera, "get_cache_stats"): - resumo["camera_cache"] = self.camera.get_cache_stats() + def _processar_grid(self, depth_frame, segmentacao, dados_visuais_seg, deteccoes, velocidade_ms): + try: + imu_roll = 0.0 - self._set_pub_cache("performance_visual", resumo) + try: + imu_ctx = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} + imu_roll = float(imu_ctx.get("roll_seg", 0.0) or 0.0) + except Exception: + imu_roll = 0.0 - if not self.debug_perf: - continue + grid_ref = self.grid_ref_builder.atualizar_por_pitch(imu_roll) - loops = resumo.get("loops", {}) - sync = resumo.get("sync", {}) + grid_conf = self.grid_builder.build( + depth_mm=depth_frame, + seg_ids=segmentacao, + grid_ref=grid_ref, + grid_shape=self.grid_ref_builder.grid_shape, + deteccoes=deteccoes, + ) - def _fmt(v, casas=1, default=0.0): - try: - if v is None: - v = default - return f"{float(v):.{casas}f}" - except Exception: - return f"{default:.{casas}f}" + if grid_conf is None: + return None, None - def _m(loop, nome, stat="med", default=0.0): - try: - return ( - loop.get("metrics_ms", {}) - .get(nome, {}) - .get(stat, default) - ) - except Exception: - return default + snapshot = self.costmap_fuser.update( + grid_conf, + velocidade_ms=velocidade_ms, + status_seg=dados_visuais_seg.get("status_corredor"), + ) - def _lat(loop, stat="med", default=0.0): - try: - return loop.get("latencia_ms", {}).get(stat, default) - except Exception: - return default + return snapshot, grid_conf - def _per(loop, stat="med", default=0.0): - try: - return loop.get("periodo_ms", {}).get(stat, default) - except Exception: - return default + except Exception as e: + self.mostrar_log(f"[visual] erro processando grid: {e}") + return None, None - def _fps(loop): - try: - return float(loop.get("fps_real", 0.0) or 0.0) - except Exception: - return 0.0 + # ============================================================ + # Preview / stream + # ============================================================ - cam_rgb = loops.get("camera_rgb", {}) - cam_depth = loops.get("camera_depth", {}) - seg = loops.get("segmentacao", {}) - det = loops.get("deteccao", {}) - grid = loops.get("grid", {}) - pub = loops.get("publicacao", {}) + def get_selected_frame(self, frame_type: TipoFrameCamera): + with self._cache_lock: + rgb = self._ultimo_rgb_frame + depth = self._ultimo_depth_frame + pred = self._ultimo_predictions + dets = list(self._ultimo_detections or []) + snapshot = self._ultimo_snapshot - self.mostrar_log( - "[PERF_VISUAL] " - f"fps rgb={_fps(cam_rgb):.1f} depth={_fps(cam_depth):.1f} " - f"seg={_fps(seg):.1f} det={_fps(det):.1f} grid={_fps(grid):.1f} | " - f"period seg={_fmt(_per(seg))}ms grid={_fmt(_per(grid))}ms | " - f"sync={_fmt(sync.get('rgb_depth_dt_ms'))}ms" - ) + params = getattr(self.camera, "parametros", {}) if self.camera is not None else {} - self.mostrar_log( - "[PERF_DETAIL] " - f"SEG total={_fmt(_lat(seg))} get={_fmt(_m(seg,'get_rgb_ms'))} " - f"infer={_fmt(_m(seg,'infer_ms'))} post={_fmt(_m(seg,'post_ms'))} " - f"redis={_fmt(_m(seg,'redis_ms'))} | " - f"GRID total={_fmt(_lat(grid))} depth={_fmt(_m(grid,'get_depth_ms'))} " - f"build={_fmt(_m(grid,'build_grid_ms'))} fuser={_fmt(_m(grid,'fuser_ms'))} " - f"redis={_fmt(_m(grid,'redis_ms'))} | " - f"DET total={_fmt(_lat(det))} read={_fmt(_m(det,'oak_read_ms'))} " - f"redis={_fmt(_m(det,'redis_ms'))} | " - f"PUB total={_fmt(_lat(pub))} redis={_fmt(_m(pub,'redis_ms'))}" - ) + return self.renderer.get_selected_frame( + frame_type=frame_type, + rgb_frame=rgb, + pred_ids=pred, + depth_frame=depth, + detections=dets, + snapshot=snapshot, + camera_params=params, + alpha=0.50, + ) - except Exception as e: - self.mostrar_log(f"Erro no loop supervisor visual: {e}") + # ============================================================ + # Loops + # ============================================================ - finally: - self._sleep_loop_adaptativo("analise", t0, freq) - - threading.Thread(target=loop, daemon=True).start() - - def _iniciar_loop_frame_stream(self, freq): - def loop(): - while True: - cam = self.camera - if cam is None: - time.sleep(0.5) - continue - - t0 = time.time() - try: - _camera = (ContextoGlobalRedis.get_camera(self.mx_id) or {}) - _stream_on = _camera.get("streaming", False) - if (_stream_on): - _frame_type = TipoFrameCamera(_camera.get("frame_type", TipoFrameCamera.Rgb.value)) - _frame = self.get_selected_frame(_frame_type) - self.camera.enviar_frame_tcp(_frame) - - from visual_worker.config import load_seg_config, load_det_config - seg_config = load_seg_config() - det_config = load_det_config() - if seg_config.get("debug_visual"): - self.segmentacao_manager.display_segmentation_debug(self._ultimo_rgb_frame, self.largura_robo_m, self.grid_ref) - self.debug_blockage_imshow(self._ultimo_rgb_frame, self._ultimo_snapshot) - if det_config.get("debug_visual"): - self._overlay_deteccoes(self._ultimo_rgb_frame, self._ultimo_detections) - - except Exception as e: - self.mostrar_log(f"Erro no loop de stream: {e}") - finally: - latencia = time.time() - t0 - cam = self.camera - stream = getattr(cam, "stream", None) if cam is not None else None - freq_loop = getattr(stream, "_op_fps", freq if freq else 1.0) - time.sleep(max(0, (1.0 / freq_loop) - latencia)) - time.sleep(max(0, (1.0 / freq) - latencia)) - threading.Thread(target=loop, daemon=True).start() - - def _iniciar_loop_publicacao_visual(self, freq=15.0): - def loop(): - ultimo_payload_forcado = 0.0 - - while True: - t0 = time.time() - t_loop0 = time.perf_counter() - - try: - if self.camera is None: - time.sleep(0.5) - continue - - if not self.operante: - time.sleep(0.2) - continue - - # Publica tudo pelo menos 1 vez por segundo, - # mesmo que alguma chave não tenha mudado. - agora = time.time() - publicar_tudo = (agora - ultimo_payload_forcado) >= 1.0 - - payload = self._montar_payload_publicacao_visual( - publicar_tudo=publicar_tudo - ) - - # Se só tem ts e timestamps auxiliares, não precisa mandar sempre. - tem_dado_real = any( - k in payload - for k in [ - "segmentacao", - "deteccao", - "matriz_confianca", - "performance_visual", - ] - ) - - if tem_dado_real: - t_redis0 = time.perf_counter() - - ContextoGlobalRedis.atualizar_ctx_dict( - CtxKey.DadosVisualWorker, - **converter_valores_numpy(payload) - ) - - t_redis1 = time.perf_counter() - - if publicar_tudo: - ultimo_payload_forcado = agora - - self.perf.tick( - "publicacao", - latencia_ms=(time.perf_counter() - t_loop0) * 1000.0, - redis_ms=(t_redis1 - t_redis0) * 1000.0, - campos=len(payload), - publicou=1, - ) - else: - self.perf.tick( - "publicacao", - latencia_ms=(time.perf_counter() - t_loop0) * 1000.0, - redis_ms=0.0, - campos=0, - publicou=0, - ) - - except Exception as e: - self.mostrar_log(f"❌ Erro no loop_publicacao_visual: {e}") - - finally: - self._sleep_loop_adaptativo("publicacao", t0, freq) - - threading.Thread(target=loop, daemon=True).start() - - def _iniciar_loop_segmentacao(self, freq=10.0): + def _iniciar_loop_segmentacao(self, freq=8.0): def loop(): ultimo_frame_ts = 0.0 while True: - t0 = time.time() - if self.iniciando or t0 < getattr(self, "_camera_pronta_em", 0): - time.sleep(0.05) - continue + t_wall0 = time.time() + t_loop0 = time.perf_counter() try: + if self.iniciando or t_wall0 < getattr(self, "_camera_pronta_em", 0): + time.sleep(0.05) + continue + if not self.operante or self.camera is None: time.sleep(0.2) continue - ctrl = getattr(self, "gpu_controller", None) + ctrl = self.gpu_controller if ctrl is not None and not ctrl.allow("segmentacao"): self.perf.inc("segmentacao_bloqueada_gpu_ctrl") time.sleep(0.05) continue - t_loop0 = time.perf_counter() - t0_wall = time.time() - t_get0 = time.perf_counter() rgb_frame, frame_ts, res = self.get_rgb_frame() + rgb_frame = self._validar_frame_rgb_para_segmentacao(rgb_frame) t_get1 = time.perf_counter() if rgb_frame is None or frame_ts is None: @@ -1252,21 +892,18 @@ class CameraManager: continue t_post0 = time.perf_counter() - analise_segmentacao, log = self.segmentacao_manager.segmentar( - predictions, - aux_result=aux_result - ) + analise = self.segmentacao_manager.analisar(predictions, aux_result=aux_result) t_post1 = time.perf_counter() - if analise_segmentacao is None: - self.perf.inc("segmentacao_analise_none") - if log: - self.mostrar_log(log) - continue - - t_pub0 = time.perf_counter() - self._set_pub_cache("segmentacao", analise_segmentacao["dados_visuais"]) - t_pub1 = time.perf_counter() + self._store_segmentacao( + predictions=predictions, + aux_result=aux_result, + analise=analise, + frame_ts=frame_ts, + res=res, + infer_ms=(t_inf1 - t_inf0) * 1000.0, + post_ms=(t_post1 - t_post0) * 1000.0, + ) t_loop1 = time.perf_counter() @@ -1276,53 +913,38 @@ class CameraManager: get_rgb_ms=(t_get1 - t_get0) * 1000.0, infer_ms=(t_inf1 - t_inf0) * 1000.0, post_ms=(t_post1 - t_post0) * 1000.0, - pub_cache_ms=(t_pub1 - t_pub0) * 1000.0, redis_ms=0.0, frame_ts=frame_ts, idade_frame_ms=(time.time() - frame_ts) * 1000.0, ) - with self._cache_lock: - self._ultimo_predictions = predictions - self._ultimo_seg_aux_result = aux_result - self._ultima_analise_segmentacao = analise_segmentacao - self._seg_cache = { - "ts": time.time(), - "frame_ts": frame_ts, - "predictions": predictions, - "aux_result": aux_result, - "res": res, - } - self._nova_segmentacao_disponivel = True - except Exception as e: self.mostrar_log(f"❌ Erro no loop_segmentacao: {e}") finally: - self._sleep_loop_adaptativo("segmentacao", t0, freq) + self._sleep_loop_adaptativo("segmentacao", t_wall0, freq) threading.Thread(target=loop, daemon=True).start() - def _iniciar_loop_deteccao(self, freq=15.0): + def _iniciar_loop_deteccao(self, freq=5.0): def loop(): ultimo_ts = 0.0 while True: - t0 = time.time() - - if self.iniciando or t0 < getattr(self, "_camera_pronta_em", 0): - time.sleep(0.05) - continue + t_wall0 = time.time() + t_loop0 = time.perf_counter() try: + if self.iniciando or t_wall0 < getattr(self, "_camera_pronta_em", 0): + time.sleep(0.05) + continue + if not self.operante or self.camera is None: time.sleep(0.2) continue - t_loop0 = time.perf_counter() - t_det0 = time.perf_counter() - dets, ts, meta = self.get_detections() + dets, ts, res = self.get_detections() t_det1 = time.perf_counter() if ts is None: @@ -1341,46 +963,27 @@ class CameraManager: self.perf.inc("deteccao_none") continue - t_pub0 = time.perf_counter() - - with self._cache_lock: - self._ultimo_detections = dets - self._ultima_analise_deteccao = { - "bboxes": dets, - "ultima_chamada": t0 - } - self._det_cache = { - "ts": time.time(), - "detections": dets, - "res": meta, - } - self._nova_deteccao_disponivel = True - - self._set_pub_cache("deteccao", dets) - - t_pub1 = time.perf_counter() - t_loop1 = time.perf_counter() + self._store_detection(dets, ts, res) self.perf.tick( "deteccao", - latencia_ms=(t_loop1 - t_loop0) * 1000.0, + latencia_ms=(time.perf_counter() - t_loop0) * 1000.0, oak_read_ms=(t_det1 - t_det0) * 1000.0, - pub_cache_ms=(t_pub1 - t_pub0) * 1000.0, redis_ms=0.0, frame_ts=ts, idade_frame_ms=(time.time() - ts) * 1000.0, - n_dets=len(dets), + n_dets=len(dets or []), ) except Exception as e: self.mostrar_log(f"❌ Erro no loop_deteccao: {e}") finally: - self._sleep_loop_adaptativo("deteccao", t0, freq) + self._sleep_loop_adaptativo("deteccao", t_wall0, freq) threading.Thread(target=loop, daemon=True).start() - def _iniciar_loop_matriz_confianca(self, freq=15.0): + def _iniciar_loop_grid(self, freq=5.0): def loop(): ultimo_depth_ts = 0.0 ultimo_seg_ts = 0.0 @@ -1390,49 +993,42 @@ class CameraManager: t_wall0 = time.time() t_loop0 = time.perf_counter() - if self.iniciando or t_wall0 < getattr(self, "_camera_pronta_em", 0): - time.sleep(0.05) - continue - try: + if self.iniciando or t_wall0 < getattr(self, "_camera_pronta_em", 0): + time.sleep(0.05) + continue + if not self.operante or self.camera is None: time.sleep(0.2) continue - ctrl = getattr(self, "gpu_controller", None) + ctrl = self.gpu_controller if ctrl is not None and not ctrl.allow("grid"): self.perf.inc("grid_bloqueada_gpu_ctrl") time.sleep(0.05) continue - # ===================================================== - # 1) Ler depth do cache da CameraOak - # ===================================================== t_depth0 = time.perf_counter() - depth_frame_np, depth_ts, depth_res = self.get_depth_frame() + depth_frame, depth_ts, depth_res = self.get_depth_frame() t_depth1 = time.perf_counter() - if depth_frame_np is None or depth_ts is None: + if depth_frame is None or depth_ts is None: self.perf.inc("grid_sem_depth") time.sleep(0.02) continue - # ===================================================== - # 2) Ler segmentação/detecção do cache do CameraManager - # ===================================================== t_cache0 = time.perf_counter() with self._cache_lock: - analise_seg = self._ultima_analise_segmentacao - segmentacao = analise_seg.get("classes") if analise_seg else None + seg_cache = dict(self._seg_cache) + det_cache = dict(self._det_cache) - dados_visuais_seg = ( - analise_seg.get("dados_visuais", {}) - if analise_seg else {} - ) + segmentacao = seg_cache.get("predictions") + analise_seg = seg_cache.get("analise") or {} + dados_visuais = analise_seg.get("dados_visuais", {}) + deteccoes = list(det_cache.get("detections", []) or []) - deteccoes = list(self._det_cache.get("detections", [])) - seg_ts = float(self._seg_cache.get("ts", 0.0) or 0.0) - det_ts = float(self._det_cache.get("ts", 0.0) or 0.0) + seg_ts = float(seg_cache.get("ts", 0.0) or 0.0) + det_ts = float(det_cache.get("ts", 0.0) or 0.0) t_cache1 = time.perf_counter() if segmentacao is None: @@ -1440,17 +1036,7 @@ class CameraManager: time.sleep(0.02) continue - # ===================================================== - # 3) Evitar recalcular o mesmo pacote - # ===================================================== - mesmo_depth = depth_ts == ultimo_depth_ts - mesma_seg = seg_ts == ultimo_seg_ts - mesma_det = det_ts == ultimo_det_ts - - # A grid depende principalmente de depth + segmentação. - # Detecção sozinha pode mudar custo, então se det mudou, - # vale recalcular também. - if mesmo_depth and mesma_seg and mesma_det: + if depth_ts == ultimo_depth_ts and seg_ts == ultimo_seg_ts and det_ts == ultimo_det_ts: self.perf.inc("grid_pacote_repetido") time.sleep(0.005) continue @@ -1459,953 +1045,251 @@ class CameraManager: ultimo_seg_ts = seg_ts ultimo_det_ts = det_ts - # ===================================================== - # 4) Atualizar referência geométrica pelo IMU - # ===================================================== - t_ref0 = time.perf_counter() - try: - imu_ctx = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {} - imu_roll = imu_ctx.get("roll_seg", 0) - except Exception: - imu_roll = 0 - - self.grid_ref = self._ajustar_grid_ref_por_pitch( - pitch_graus=imu_roll - ) - t_ref1 = time.perf_counter() - - # ===================================================== - # 5) Montar parâmetros - # ===================================================== - det_params = { - "w4": 0.18, - "thr_det_soft": 0.45, - "min_cell_coverage": 0.10, - "min_det_conf": 0.45, - "class_weights": { - "person": 1.0, - "dog": 0.7, - "cat": 0.5, - }, - "veto_labels": {"person"}, - "combine": "max", - "conf_drop_alpha": 0.0, - "only_veto_blocks_nav": True, - "non_veto_cost_scale": 0.50, - } - try: vel = get_velocidade_atual_ms() except Exception: vel = 0.0 - # ===================================================== - # 6) Construir grid de confiança/custo - # ===================================================== t_grid0 = time.perf_counter() - grid_conf = self._construir_grid_confianca( - depth_frame_np, - segmentacao, - self.grid_ref, - self.grid_ref_shape, + snapshot, grid_conf = self._processar_grid( + depth_frame=depth_frame, + segmentacao=segmentacao, + dados_visuais_seg=dados_visuais, deteccoes=deteccoes, - det_params=det_params + velocidade_ms=vel, ) t_grid1 = time.perf_counter() - if grid_conf is None: + if snapshot is None or grid_conf is None: self.perf.inc("grid_conf_none") time.sleep(0.005) continue - # Compatibilidade com performance antiga - t_grid_wall = time.time() - grid_conf["ultima_chamada"] = ( - self._ultima_analise_matriz_confianca - .get("ultima_chamada", t_wall0) - ) - self._calcular_performance(t_wall0, t_grid_wall, grid_conf) - - # ===================================================== - # 7) Fuser - # ===================================================== - t_fuser0 = time.perf_counter() - snapshot = self.data_fuser.update( - grid_conf, - velocidade_ms=vel, - status_seg=dados_visuais_seg.get("status_corredor") - ) - t_fuser1 = time.perf_counter() - - # ===================================================== - # 8) Atualizar caches internos - # ===================================================== - t_store0 = time.perf_counter() - with self._cache_lock: - self._ultimo_snapshot = snapshot - self._ultima_analise_matriz_confianca = grid_conf - self._grid_cache = { - "ts": time.time(), - "snapshot": snapshot, - "grid_conf": grid_conf, - } - self._nova_grid_conf_disponivel = True - t_store1 = time.perf_counter() - - # ===================================================== - # 9) Publicar Redis - # ===================================================== - t_pub0 = time.perf_counter() - self._set_pub_cache("matriz_confianca", snapshot) - t_pub1 = time.perf_counter() - - # ===================================================== - # 10) Registrar performance - # ===================================================== - t_loop1 = time.perf_counter() - agora = time.time() + self._store_grid(snapshot=snapshot, grid_conf=grid_conf) self.perf.tick( "grid", - latencia_ms=(t_loop1 - t_loop0) * 1000.0, + latencia_ms=(time.perf_counter() - t_loop0) * 1000.0, get_depth_ms=(t_depth1 - t_depth0) * 1000.0, cache_read_ms=(t_cache1 - t_cache0) * 1000.0, - grid_ref_ms=(t_ref1 - t_ref0) * 1000.0, build_grid_ms=(t_grid1 - t_grid0) * 1000.0, - fuser_ms=(t_fuser1 - t_fuser0) * 1000.0, - store_ms=(t_store1 - t_store0) * 1000.0, - pub_cache_ms=(t_pub1 - t_pub0) * 1000.0, + fuser_ms=0.0, redis_ms=0.0, - depth_ts=depth_ts, seg_ts=seg_ts, det_ts=det_ts, - - idade_depth_ms=(agora - depth_ts) * 1000.0 if depth_ts else None, - idade_seg_ms=(agora - seg_ts) * 1000.0 if seg_ts else None, - idade_det_ms=(agora - det_ts) * 1000.0 if det_ts else None, - + idade_depth_ms=(time.time() - depth_ts) * 1000.0 if depth_ts else None, + idade_seg_ms=(time.time() - seg_ts) * 1000.0 if seg_ts else None, + idade_det_ms=(time.time() - det_ts) * 1000.0 if det_ts else None, sync_depth_seg_ms=abs(depth_ts - seg_ts) * 1000.0 if depth_ts and seg_ts else None, sync_depth_det_ms=abs(depth_ts - det_ts) * 1000.0 if depth_ts and det_ts else None, - n_dets=len(deteccoes), ) except Exception as e: - self.mostrar_log(f"❌ Erro no loop_matriz_confianca: {e}") + self.mostrar_log(f"❌ Erro no loop_grid: {e}") finally: self._sleep_loop_adaptativo("grid", t_wall0, freq) threading.Thread(target=loop, daemon=True).start() - - def _calcular_performance(self, t0, t1, analise): - latencia = t1 - t0 - freq = 1.0 / max(latencia, 1e-6) - fps = 1.0 / max((t0 - analise.get("ultima_chamada", t0)), 1e-6) - analise["latencia"] = latencia - analise["fps"] = fps - analise["freq"] = freq - analise["ultima_chamada"] = t0 - return latencia, fps, freq + def _iniciar_loop_publicacao_visual(self, freq=15.0): + def loop(): + ultimo_payload_forcado = 0.0 - def _log_performance(self, titulo, analise): - return f"{titulo}: {analise.get('latencia', 0):.3f} s, {analise.get('fps', 0):.2f} FPS, {analise.get('freq', 0):.2f} Hz; " + while True: + t_wall0 = time.time() + t_loop0 = time.perf_counter() - def _overlay_deteccoes( - self, - rgb_frame, - dets, - conf_thr=0.5, # limiar de confiança pra desenhar - roi_frac=None, # (rx1,ry1,rx2,ry2) normalizado da ROI usada no detector (ex.: (0.0,y1,1.0,y2)) - show=True, # se True, faz cv2.imshow - janela="det", # nome da janela - fps_state=None, # dict estado do FPS (persistido fora), ex.: {} - ): - """ - dets: lista de dicts no formato: - { - "label_id": int, - "label": str|None, - "conf": float, - "bbox_norm": [x0,y0,x1,y1] # 0..1 relativo ao input do detector (na ROI) - # opcional: "bbox_full": [x0,y0,x1,y1] em px do frame completo - } - Retorna: (frame_com_overlay, fps_state, keep_loop_bool) - """ + try: + if self.camera is None: + time.sleep(0.5) + continue + + if not self.operante: + time.sleep(0.2) + continue + + agora = time.time() + publicar_tudo = (agora - ultimo_payload_forcado) >= 1.0 + + payload = self._montar_payload_publicacao_visual(publicar_tudo=publicar_tudo) + + tem_dado_real = any( + k in payload + for k in ["segmentacao", "deteccao", "matriz_confianca", "performance_visual"] + ) + + publicou = 0 + campos = 0 + redis_ms = 0.0 + + if tem_dado_real: + t_redis0 = time.perf_counter() + + ContextoGlobalRedis.atualizar_ctx_dict( + CtxKey.DadosVisualWorker, + **converter_valores_numpy(payload), + ) + + redis_ms = (time.perf_counter() - t_redis0) * 1000.0 + publicou = 1 + campos = len(payload) + + if publicar_tudo: + ultimo_payload_forcado = agora + + self._ultimo_pub_debug = { + "ts": time.time(), + "publicou": publicou, + "campos": campos, + "redis_ms": redis_ms, + } + + self.perf.tick( + "publicacao", + latencia_ms=(time.perf_counter() - t_loop0) * 1000.0, + redis_ms=redis_ms, + campos=campos, + publicou=publicou, + ) + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_publicacao_visual: {e}") + + finally: + self._sleep_loop_adaptativo("publicacao", t_wall0, freq) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_frame_stream(self, freq=2.0): + def loop(): + while True: + t_wall0 = time.time() + t_loop0 = time.perf_counter() + + try: + if self.camera is None: + time.sleep(0.5) + continue + + camera_ctx = ContextoGlobalRedis.get_camera(self.mx_id) or {} + stream_on = bool(camera_ctx.get("streaming", False)) + + if stream_on: + frame_type = TipoFrameCamera( + camera_ctx.get("frame_type", TipoFrameCamera.Rgb.value) + ) + + frame = self.get_selected_frame(frame_type) + self.camera.enviar_frame_tcp(frame) + + if self.debug_visual: + dbg = self.get_selected_frame(TipoFrameCamera.Debug) + if dbg is not None: + cv2.imshow("Visual Worker Debug", dbg) + cv2.waitKey(1) + + self.perf.tick( + "stream", + latencia_ms=(time.perf_counter() - t_loop0) * 1000.0, + ) + + except Exception as e: + self.mostrar_log(f"Erro no loop de stream: {e}") + + finally: + lat = time.time() - t_wall0 + + try: + freq_atual = float(getattr(self.camera.stream, "_op_fps", freq) or freq) + except Exception: + freq_atual = freq + + time.sleep(max(0.0, (1.0 / max(freq_atual, 0.1)) - lat)) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_analise_continua(self, freq=8.0): + def loop(): + while True: + t_wall0 = time.time() + + try: + if self.camera is None: + time.sleep(0.5) + continue + + self._verificar_desconexao_camera() + + if self.iniciando or self._em_warmup or time.time() < self._startup_grace_until: + time.sleep(0.1) + continue + + agora = time.time() + if not hasattr(self, "_ultimo_perf_publish"): + self._ultimo_perf_publish = 0.0 + + if agora - self._ultimo_perf_publish >= 1.0: + self._ultimo_perf_publish = agora + + ctrl = self.gpu_controller + if ctrl is not None: + ctrl.update_from_redis() + + self._publicar_e_logar_performance() + + except Exception as e: + self.mostrar_log(f"Erro no loop supervisor visual: {e}") + + finally: + self._sleep_loop_adaptativo("analise", t_wall0, freq) + + threading.Thread(target=loop, daemon=True).start() + + # ============================================================ + # Saúde / publicação / performance + # ============================================================ + + def atualizar_saude_camera(self): try: - if (rgb_frame is None or dets is None): - return None, None, None + if self.camera is not None: + self.camera.atualizar_saude() + elif self.mx_id is not None: + from camera_worker.manager import definir_saude_camera - img = cv2.resize(rgb_frame.copy(), (1280, 720)) - H, W = img.shape[:2] - - # paleta simples por classe - palette = [ - (255, 56, 56), (255, 157, 151), (72, 249, 10), (0, 255, 0), (0, 0, 255), - (255, 0, 255), (0, 255, 255), (255, 191, 0), (52, 148, 230), (147, 112, 219) - ] - - def _map_bbox_norm_to_full(bn): - # bn é [x0n,y0n,x1n,y1n] relativo ao input da ROI (0..1) - x0n, y0n, x1n, y1n = bn - if roi_frac is not None: - rx1, ry1, rx2, ry2 = roi_frac - sx, sy = (rx2 - rx1), (ry2 - ry1) - x0 = int(round((rx1 + x0n * sx) * W)) - y0 = int(round((ry1 + y0n * sy) * H)) - x1 = int(round((rx1 + x1n * sx) * W)) - y1 = int(round((ry1 + y1n * sy) * H)) - else: - x0 = int(round(x0n * W)) - y0 = int(round(y0n * H)) - x1 = int(round(x1n * W)) - y1 = int(round(y1n * H)) - # clamp - x0 = max(0, min(W - 1, x0)); x1 = max(0, min(W - 1, x1)) - y0 = max(0, min(H - 1, y0)); y1 = max(0, min(H - 1, y1)) - return x0, y0, x1, y1 - - def _put_label(img, text, x, y, bg): - (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) - cv2.rectangle(img, (x, max(0, y - th - 6)), (x + tw + 6, y), bg, -1) - cv2.putText(img, text, (x + 3, y - 4), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1, cv2.LINE_AA) - - # desenhar ROI (opcional, ajuda debug) - if roi_frac is not None: - rx1, ry1, rx2, ry2 = roi_frac - x0r, y0r = int(rx1 * W), int(ry1 * H) - x1r, y1r = int(rx2 * W), int(ry2 * H) - cv2.rectangle(img, (x0r, y0r), (x1r, y1r), (60, 60, 60), 1) - - cv2.putText( - img, - f"dets recebidas: {len(dets)}", - (10, 48), - cv2.FONT_HERSHEY_SIMPLEX, - 0.6, - (0, 255, 255), - 2, - cv2.LINE_AA - ) - - # desenhar detecções - for d in dets: - if d.get("conf", 0.0) < conf_thr: - continue - - # bbox em px do frame - if "bbox_full" in d and d["bbox_full"]: - x0, y0, x1, y1 = d["bbox_full"] - - # bbox_full provavelmente está no frame original 1920x1080 - src_w = getattr(self, "frame_size", (1920, 1080))[0] - src_h = getattr(self, "frame_size", (1920, 1080))[1] - - sx = W / float(src_w) - sy = H / float(src_h) - - x0 = int(round(x0 * sx)) - x1 = int(round(x1 * sx)) - y0 = int(round(y0 * sy)) - y1 = int(round(y1 * sy)) - - x0 = max(0, min(W - 1, x0)); x1 = max(0, min(W - 1, x1)) - y0 = max(0, min(H - 1, y0)); y1 = max(0, min(H - 1, y1)) - else: - bn = d.get("bbox_norm", None) - if not bn: - continue - x0, y0, x1, y1 = _map_bbox_norm_to_full(bn) - - if x1 <= x0 or y1 <= y0: - continue - - lid = int(d.get("label_id", -1)) - color = palette[lid % len(palette)] if lid >= 0 else (0, 255, 0) - - cv2.rectangle(img, (x0, y0), (x1, y1), color, 2) - - name = d.get("label", None) - dist = d.get("distancia_m", None) - track_id = d.get("track_id", None) - status = d.get("track_status", None) - - txt = f"{name or f'id:{lid}'} {d.get('conf', 0.0):.2f}" - - if track_id is not None: - txt += f" T:{track_id}" - - if status: - txt += f" {status}" - - if dist is not None: - txt += f" Z:{dist:.2f}m" - _put_label(img, txt, x0, y0, color) - - # FPS (EMA) - now = time.monotonic() - if fps_state is None: - fps_state = {} - t_prev = fps_state.get("t_prev") - fps_ema = fps_state.get("fps_ema") - if t_prev is not None: - dt = now - t_prev - if dt > 0: - fps_inst = 1.0 / dt - alpha = 0.90 - fps_ema = fps_inst if fps_ema is None else (alpha * fps_ema + (1 - alpha) * fps_inst) - fps_state["t_prev"] = now - fps_state["fps_ema"] = fps_ema - - if fps_ema: - cv2.putText(img, f"FPS: {fps_ema:.1f}", (10, 22), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (50, 220, 50), 2, cv2.LINE_AA) - - keep = True - if show: - cv2.imshow(janela, img) - k = cv2.waitKey(1) & 0xFF - keep = (k != 27) # ESC para sair - - self._ultimo_frame_deteccoes = img - - return img, fps_state, keep - except Exception as e: - self.mostrar_log(f"Erro ao gerar overlay de deteccoes") - - def _construir_grid_confianca( - self, - depth_mm, # np.ndarray (H,W) em mm ou None - seg_ids, # np.ndarray (H1,W1) com ids de classe - grid_ref, # np.ndarray (grid_h,) ou (grid_h, grid_w) em metros - grid_shape=(15, 10), # (cols, rows) - valid_mm=(300, 10000), - min_valid_frac=0.30, - conf_params=(0.30, 0.80), # t0,t1 para mapear % depth válido -> conf_depth - w=(0.65, 0.25, 0.10), # pesos do custo: nao_navegavel, anom, (1-conf) - anom_tau_min=0.22, - anom_satur_m=0.50, - deteccoes=None, # lista de dicts - det_params=None, - usar_depth: bool = True, - anom_tau_up=0.15, - anom_satur_up_m=0.35, - anom_tau_down=0.30, - anom_satur_down_m=0.55, - anom_down_weight=0.65, - ): - """ - Constrói uma grid de confiança/risco por célula. - - Filosofia: - - segmentação é a base da navegabilidade - - depth gera anomalia física - - detecção fornece contexto crítico, sem derrubar confiança - - confiança representa qualidade perceptiva, não obstáculo - - custo é um mapa suave de risco, não uma decisão final - - Retorna dict com arrays (grid_h, grid_w): - pct_navegavel, pct_nao_navegavel, - z_med, z_ref, depth_valid_frac, - conf, anom, custo, navegavel, - det_cov_max, det_conf_max, det_score, det_top_label_id, det_top_conf - """ - try: - # -------------------------------------------------- - # 0) Defaults de detecção alinhados com nova lógica - # -------------------------------------------------- - _det = { - "w4": 0.18, # quanto a detecção pesa no custo (suave) - "thr_det_soft": 0.45, # acima disso, reduz navegabilidade se for crítica - "min_cell_coverage": 0.10, - "min_det_conf": 0.45, - "class_weights": {}, # ex: {'person':1.0,'dog':0.7} - "veto_labels": {"person"}, # labels críticas - "combine": "max", # "max" ou "sum_clamped" - "conf_drop_alpha": 0.0, # NOVA FILOSOFIA: detecção não derruba confiança - "only_veto_blocks_nav": True, - "non_veto_cost_scale": 0.50 # labels não críticas pesam menos no custo - } - if det_params: - _det.update(det_params) - - grid_w, grid_h = grid_shape - H1, W1 = seg_ids.shape - - # -------------------------------------------------- - # 1) Pré-processamento do depth - # -------------------------------------------------- - d_small = None - if usar_depth and depth_mm is not None: - d_small = cv2.resize(depth_mm, (W1, H1), interpolation=cv2.INTER_NEAREST).astype(np.float32) - d_small[(d_small < valid_mm[0]) | (d_small > valid_mm[1])] = np.nan - - # -------------------------------------------------- - # 2) Bordas da grid - # -------------------------------------------------- - x_edges = np.linspace(0, W1, grid_w + 1, dtype=int) - y_edges = np.linspace(0, H1, grid_h + 1, dtype=int) - - # -------------------------------------------------- - # 3) Saídas base - # -------------------------------------------------- - pct_navegavel = np.zeros((grid_h, grid_w), np.float32) - pct_nao_navegavel = np.zeros((grid_h, grid_w), np.float32) - z_med = np.full((grid_h, grid_w), np.nan, np.float32) - depth_valid_frac = np.zeros((grid_h, grid_w), np.float32) - - # mapas de detecção - det_cov_max = np.zeros((grid_h, grid_w), np.float32) - det_conf_max = np.zeros((grid_h, grid_w), np.float32) - det_score = np.zeros((grid_h, grid_w), np.float32) - det_top_label_id = -np.ones((grid_h, grid_w), np.int32) - det_top_conf = np.zeros((grid_h, grid_w), np.float32) - det_is_veto = np.zeros((grid_h, grid_w), np.float32) # debug útil - - # -------------------------------------------------- - # 4) grid_ref 2D - # -------------------------------------------------- - grid_ref = np.asarray(grid_ref, dtype=np.float32) - - if grid_ref.ndim == 1: - if grid_ref.shape[0] != grid_h: - raise ValueError(f"grid_ref 1D deve ter len={grid_h}, veio {grid_ref.shape}") - Z_ref = np.repeat(grid_ref[:, None], grid_w, axis=1) - else: - Z_ref = grid_ref - if Z_ref.shape != (grid_h, grid_w): - raise ValueError(f"grid_ref 2D deve ser {(grid_h, grid_w)}, veio {Z_ref.shape}") - - # -------------------------------------------------- - # 5) Loop por célula: segmentação + depth - # -------------------------------------------------- - for j in range(grid_h): - y0, y1 = int(y_edges[j]), int(y_edges[j + 1]) - seg_row = seg_ids[y0:y1, :] - depth_row = d_small[y0:y1, :] if d_small is not None else None - - for i in range(grid_w): - x0, x1 = int(x_edges[i]), int(x_edges[i + 1]) - - seg_block = seg_row[:, x0:x1] - n = seg_block.size - if n == 0: - continue - - n_nav = np.count_nonzero(seg_block == ClassesSegmentacao.NAVEGAVEL.value) - n_naonav = np.count_nonzero(seg_block == ClassesSegmentacao.NAONAVEGAVEL.value) - - pct_navegavel[j, i] = n_nav / n - pct_nao_navegavel[j, i] = n_naonav / n - - if depth_row is not None: - depth_block = depth_row[:, x0:x1] - vals = depth_block[~np.isnan(depth_block)] - valid = vals.size - depth_valid_frac[j, i] = valid / n - if valid >= max(int(min_valid_frac * n), 1): - z_med[j, i] = np.nanmedian(vals) / 1000.0 # mm -> m - - #z_med_linha = np.nanmedian(z_med, axis=1) - #print("grid_ref: ", grid_ref) - #print("z_med_ln: ", z_med_linha) - - # -------------------------------------------------- - # 6) Confiança da célula - # -------------------------------------------------- - t0, t1 = conf_params - conf_seg = np.maximum(pct_navegavel, pct_nao_navegavel) - - if usar_depth and d_small is not None: - conf_dep = np.clip((depth_valid_frac - t0) / max(1e-6, (t1 - t0)), 0.0, 1.0) - conf_cell = 0.65 * conf_seg + 0.35 * conf_dep - else: - conf_dep = np.zeros_like(conf_seg, dtype=np.float32) - conf_cell = conf_seg.copy() - - # -------------------------------------------------- - # 7) Rasterização das detecções - # -------------------------------------------------- - if deteccoes: - for det in deteccoes: - conf = float(det.get("conf", 0.0)) - if conf < _det["min_det_conf"]: - continue - - label = str(det.get("label", "")).strip() - label_id = int(det.get("label_id", -1)) - - is_veto = (label in _det["veto_labels"]) - w_class = float(_det["class_weights"].get(label, 1.0)) - if not is_veto: - w_class *= float(_det["non_veto_cost_scale"]) - - # bbox em px - if "bbox_px" in det and det["bbox_px"]: - x0p, y0p, x1p, y1p = det["bbox_px"] - else: - x0n, y0n, x1n, y1n = det["bbox_norm"] - x0p = int(np.clip(x0n * W1, 0, W1 - 1)) - x1p = int(np.clip(x1n * W1, 0, W1)) - y0p = int(np.clip(y0n * H1, 0, H1 - 1)) - y1p = int(np.clip(y1n * H1, 0, H1)) - - if x1p <= x0p or y1p <= y0p: - continue - - bbox_area = float((x1p - x0p) * (y1p - y0p)) - if bbox_area <= 1.0: - continue - - # células candidatas - i0 = max(0, np.searchsorted(x_edges, x0p, side="right") - 1) - i1 = min(grid_w - 1, np.searchsorted(x_edges, x1p, side="left")) - j0 = max(0, np.searchsorted(y_edges, y0p, side="right") - 1) - j1 = min(grid_h - 1, np.searchsorted(y_edges, y1p, side="left")) - - for j in range(j0, j1 + 1): - y0c, y1c = int(y_edges[j]), int(y_edges[j + 1]) - - for i in range(i0, i1 + 1): - x0c, x1c = int(x_edges[i]), int(x_edges[i + 1]) - - ix0 = max(x0c, x0p) - ix1 = min(x1c, x1p) - iy0 = max(y0c, y0p) - iy1 = min(y1c, y1p) - - if ix1 <= ix0 or iy1 <= iy0: - continue - - inter = float((ix1 - ix0) * (iy1 - iy0)) - cell_area = float((x1c - x0c) * (y1c - y0c)) - if cell_area <= 0: - continue - - cov = inter / cell_area - if cov < _det["min_cell_coverage"]: - continue - - score_local = conf * cov * w_class - - det_cov_max[j, i] = max(det_cov_max[j, i], cov) - det_conf_max[j, i] = max(det_conf_max[j, i], conf) - - if _det["combine"] == "sum_clamped": - det_score[j, i] = np.clip(det_score[j, i] + score_local, 0.0, 1.0) - else: - det_score[j, i] = max(det_score[j, i], score_local) - - # guarda top label por confiança*cobertura, priorizando veto - priority = (2.0 if is_veto else 1.0) * conf * cov - if priority > det_top_conf[j, i]: - det_top_conf[j, i] = priority - det_top_label_id[j, i] = label_id - det_is_veto[j, i] = 1.0 if is_veto else 0.0 - - # -------------------------------------------------- - # 8) Anomalia física (positiva e negativa) - # -------------------------------------------------- - if usar_depth and d_small is not None: - delta_signed = Z_ref - z_med - delta_signed = np.where(np.isnan(z_med), 0.0, delta_signed) - - # Algo mais perto do que deveria: obstáculo / saliência - delta_up = np.maximum(delta_signed, 0.0) - anom_up_raw = np.clip(delta_up / max(1e-6, anom_satur_up_m), 0.0, 1.0) - anom_up = ( - anom_up_raw * - (delta_up > anom_tau_up).astype(np.float32) * - np.maximum(conf_dep, 0.25) + definir_saude_camera( + self.mx_id, + StatusModulo.DESCONECTADO, + 0, + ["desconectado"], + False, + {}, + disp=T_Code.Snr, + conectado=False, ) - # Algo mais longe do que deveria: buraco / vala / queda - delta_down = np.maximum(-delta_signed, 0.0) - anom_down_raw = np.clip(delta_down / max(1e-6, anom_satur_down_m), 0.0, 1.0) - anom_down = ( - anom_down_raw * - (delta_down > anom_tau_down).astype(np.float32) * - np.maximum(conf_dep, 0.25) - ) - - # Anomalia física final combinada - anom = np.clip(np.maximum(anom_up, anom_down_weight * anom_down), 0.0, 1.0) - - else: - anom_up = np.zeros_like(pct_navegavel, dtype=np.float32) - anom_down = np.zeros_like(pct_navegavel, dtype=np.float32) - anom = np.zeros_like(pct_navegavel, dtype=np.float32) - - # -------------------------------------------------- - # 9) Custo suave - # -------------------------------------------------- - nao_navegavel = 1.0 - pct_navegavel - w1, w2, w3 = w - - custo_base = ( - w1 * nao_navegavel + - w2 * anom + - w3 * (1.0 - conf_cell) - ) - - # Detecção pesa pouco no custo geral; veto é deixado para o fuser - custo = np.clip(custo_base + _det["w4"] * det_score, 0.0, 1.0) - - # -------------------------------------------------- - # 10) Navegabilidade por célula - # -------------------------------------------------- - # Regra: - # - segmentação é a base - # - anomalia forte derruba - # - confiança muito baixa derruba somente a navegabilidade local - # - detecção crítica (veto) forte pode derrubar navegabilidade - veto_soft_block = (det_is_veto > 0.5) & (det_score >= _det["thr_det_soft"]) - - navegavel = ( - (pct_navegavel >= 0.55) & - (anom < 0.45) & - (conf_cell >= 0.35) & - (~veto_soft_block if _det["only_veto_blocks_nav"] else (det_score < _det["thr_det_soft"])) - ) - - # -------------------------------------------------- - # 11) Retorno - # -------------------------------------------------- - return { - "pct_navegavel": np.clip(pct_navegavel, 0.0, 1.0), - "pct_nao_navegavel": np.clip(pct_nao_navegavel, 0.0, 1.0), - - "z_med": z_med, - "z_ref": Z_ref, - "depth_valid_frac": np.clip(depth_valid_frac, 0.0, 1.0), - - "conf": np.clip(conf_cell, 0.0, 1.0), - "anom_up": np.clip(anom_up, 0.0, 1.0), - "anom_down": np.clip(anom_down, 0.0, 1.0), - "anom": np.clip(anom, 0.0, 1.0), - "custo": np.clip(custo, 0.0, 1.0), - "navegavel": navegavel.astype(np.uint8), - - "det_cov_max": np.clip(det_cov_max, 0.0, 1.0), - "det_conf_max": np.clip(det_conf_max, 0.0, 1.0), - "det_score": np.clip(det_score, 0.0, 1.0), - "det_top_label_id": det_top_label_id, - "det_top_conf": np.clip(det_top_conf, 0.0, 1.0), - - # debug opcional - "det_is_veto": det_is_veto.astype(np.float32), - } - except Exception as e: - self.mostrar_log(f"Erro ao construir grid de confianca: {e}") - return None - - def debug_blockage_imshow( - self, - rgb_frame, - snapshot, - robot_width_m=0.84, margin_m=0.12, - thr_anom_block=0.50, thr_cost_block=0.65, thr_conf_low=0.35, - rho_block_central=0.70, rho_block_global=0.60, - near_is_bottom=True, - win_name="Block Debug", - alpha=0.35, - velocidade_media=None, # << NOVO (m/s) - a_max_freio=0.8, # << NOVO (m/s²) - margem_parada=0.25, # << NOVO (m) - show: bool = True - ): - try: - if (rgb_frame is None or snapshot is None): - return None, None + self.mostrar_log(f"[saude] erro: {e}") - if (velocidade_media is None): - velocidade_media = get_velocidade_atual_ms() - # --- unpack do snapshot --- - custo_f, conf_f, anom_f, nav_f = unpack_snapshot(snapshot) - row_dist_m = np.asarray(snapshot.get("row_dist_m"), dtype=np.float32) - row_scale_x_m = np.asarray(snapshot.get("row_scale_x_m"), dtype=np.float32) - fuse = snapshot.get("fuse", {}) - central_cols = fuse.get("central_cols", None) - if central_cols is None: - # fallback: terço central - H, W = custo_f.shape - central_cols = (W//3, 2*W//3) - else: - central_cols = (int(central_cols[0]), int(central_cols[1])) - - # prioriza near_is_bottom do snap, se vier - if "near_is_bottom" in fuse: - near_is_bottom = bool(fuse["near_is_bottom"]) - - rgb_frame = cv2.resize(rgb_frame, (1280, 720)) - - Hf, Wf = rgb_frame.shape[:2] - H, W = custo_f.shape - - # --- métricas: usa do snap se existir; senão calcula --- - metrics = snapshot.get("block") - - blocked = bool(metrics.get("blocked", False)) - reason = metrics.get("reason", "none") or "none" - reason_detail = metrics.get("reason_detail", "none") or "none" - d_obs_min = metrics.get("d_obs_true_min_m", None) - j_block = metrics.get("j_block", None) - cov_cent = metrics.get("coverage", {}).get("central_max", 0.0) - cov_glob = metrics.get("coverage", {}).get("global", 0.0) - decision = metrics.get("decision", {}) - sb = metrics.get("side_bias", {}) or {} - side_val = float(sb.get("value", 0.0)) - left_frac = float(sb.get("left_frac", 0.0)) - right_frac= float(sb.get("right_frac", 0.0)) - - # --- overlays --- - mask_rgb, mask_anom, mask_cost, mask_conf = self._colorize_masks( - anom_f, custo_f, conf_f, - thr_anom_block=thr_anom_block, - thr_cost_block=thr_cost_block, - thr_conf_low=thr_conf_low - ) - mask_rgb_resized = cv2.resize(mask_rgb, (Wf, Hf), interpolation=cv2.INTER_NEAREST) - - vis = rgb_frame.copy() - vis = cv2.addWeighted(vis, 1.0, mask_rgb_resized, alpha, 0) - - # --- desenha linha/retângulo na j_block --- - if j_block is not None and 0 <= j_block < H: - y = int((j_block + 0.5) * Hf / H) - color_line = (0, 0, 255) if blocked else (0, 255, 255) - cv2.line(vis, (0, y), (Wf, y), color_line, 2) - - c0, c1 = central_cols - c0 = max(0, min(W-1, int(c0))) - c1 = max(0, min(W, int(c1))) - if c1 <= c0: - c0, c1 = W//3, 2*W//3 - - width_need_m = robot_width_m + margin_m - sx = row_scale_x_m[j_block] if row_scale_x_m.size == H else (width_need_m / max(1, (c1 - c0))) - if not np.isfinite(sx) or sx <= 1e-6: - ncols = (c1 - c0) - else: - ncols = int(np.ceil(width_need_m / sx)) - ncols = max(1, min(W, ncols)) - - mid = (c0 + c1) // 2 - half = ncols // 2 - a = max(0, mid - half) - b = min(W, a + ncols) - a = max(0, b - ncols) - - x1 = int(a * Wf / W) - x2 = int(b * Wf / W) - cv2.rectangle(vis, (x1, max(0, y-12)), (x2, min(Hf-1, y+12)), (0, 255, 0), 2) - - txt = f"{reason.upper()} | d={d_obs_min:.2f} m" if d_obs_min is not None else f"{reason.upper()} | d=-" - cv2.putText(vis, txt, (10, max(20, y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color_line, 2, cv2.LINE_AA) - - # --- HUDs (legíveis) --- - x0, y0 = 10, 30 - bar_w, bar_h = 200, 12 - line_gap = 8 - font = cv2.FONT_HERSHEY_SIMPLEX - font_scale = 0.60 - thick = 2 - - def put_text_outlined(img, text, org, font, font_scale, color_fg, thickness): - # contorno preto - cv2.putText(img, text, org, font, font_scale, (0,0,0), thickness+2, cv2.LINE_AA) - # texto - cv2.putText(img, text, org, font, font_scale, color_fg, thickness, cv2.LINE_AA) - - def draw_bar(img, label, frac, top_left, color_fill=(0,255,255)): - x, y = top_left - # label acima da barra - put_text_outlined(img, label, (x, y-2), font, font_scale, (255,255,255), thick) - # moldura - cv2.rectangle(img, (x, y+4), (x + bar_w, y + 4 + bar_h), (220,220,220), 1) - # preenchimento - fw = int(bar_w * float(np.clip(frac, 0, 1))) - if fw > 0: - cv2.rectangle(img, (x, y+4), (x + fw, y + 4 + bar_h), color_fill, -1) - - # mede a altura ocupada pelo bloco para pintar um painel ao fundo - panel_h = ( - # 2 barras (cada uma tem label+barra) + gaps - (bar_h + 4) * 2 + (line_gap + 14) * 2 - ) - panel_w = max(260, bar_w + 70) - - # painel semi-transparente - overlay = vis.copy() - cv2.rectangle(overlay, (x0-6, y0-6), (x0-6 + panel_w, y0-6 + panel_h), (0,0,0), -1) - cv2.addWeighted(overlay, 0.35, vis, 0.65, 0, vis) - - # barras Global / Central - draw_bar(vis, f"Global {cov_glob:.2f}", cov_glob, (x0, y0)) - y0 += bar_h + line_gap + 14 - draw_bar(vis, f"Central {cov_cent:.2f}", cov_cent, (x0, y0)) - y0 += bar_h + line_gap + 14 - - # SideBias - put_text_outlined(vis, f"SideBias {side_val:+.2f}", (x0, y0), font, font_scale, (255,255,255), thick) - cx = x0 + bar_w//2 - y_bar = y0 + 14 - # linha base -1..+1 - cv2.line(vis, (x0, y_bar), (x0+bar_w, y_bar), (220,220,220), 1) - # marca central - cv2.line(vis, (cx, y_bar-4), (cx, y_bar+4), (255,255,255), 1) - # cursor do bias - bx = int(cx + (bar_w//2) * float(np.clip(side_val, -1, 1))) - cv2.circle(vis, (bx, y_bar), 5, (0,255,0), -1) - - # L / R - put_text_outlined(vis, f"L:{left_frac:.2f} R:{right_frac:.2f}", (x0, y_bar + 20), font, font_scale, (255,255,255), thick) - - # --- DECISION HUD: PARAR / LIVRE --- - # empurra um pouco pra baixo do L/R - y0_dec = y_bar + 50 - - parada_necessaria = decision.get("parar", False) - dec_txt = "PARAR" if parada_necessaria else "LIVRE" - dec_col = (0, 0, 255) if parada_necessaria else (0, 200, 0) - d_necessaria = decision.get('dist_necessaria', 0.0) or 0.0 - dec_info = (f"v={velocidade_media:.3f} m/s d_obs={('-' if d_obs_min is None else f'{d_obs_min:.2f} m')} " - f"d_necess={d_necessaria:.2f} m") - - # painel por trás para legibilidade - panel_w2 = max(280, bar_w + 100) - overlay2 = vis.copy() - cv2.rectangle(overlay2, (x0-6, y0_dec-24), (x0-6 + panel_w2, y0_dec+26), (0,0,0), -1) - cv2.addWeighted(overlay2, 0.35, vis, 0.65, 0, vis) - - # linha 1: PARAR / LIVRE (grande) - put_text_outlined(vis, f"{dec_txt}", (x0, y0_dec-4), font, 0.80, dec_col, thick) - - # linha 2: detalhes (menor) - put_text_outlined(vis, dec_info, (x0, y0_dec+18), font, 0.58, (255,255,255), thick) - - # linha 3: detalhes (menor) - put_text_outlined(vis, reason_detail, (x0, y0_dec+50), font, 0.58, (255,255,255), thick) - - - legend = [ - ("Anom >= thr", (255, 0, 255)), - ("Custo >= thr", (0,165,255)), - ("Conf < thr", (255, 0, 0)), - ] - lx, ly = 10, Hf - 10 - 18*len(legend) - for i, (txt, col) in enumerate(legend): - y = ly + i*18 - cv2.rectangle(vis, (lx, y-12), (lx+18, y+2), col, -1) - cv2.putText(vis, txt, (lx+24, y), cv2.FONT_HERSHEY_SIMPLEX, 0.48, (255,255,255), 1, cv2.LINE_AA) - - status_txt = f"BLOCKED: {blocked} ({reason})" - status_col = (0,0,255) if blocked else (0,255,0) - cv2.putText(vis, status_txt, (Wf - 40 - 8*len(status_txt), 24), cv2.FONT_HERSHEY_SIMPLEX, 0.6, status_col, 2, cv2.LINE_AA) - - if (show): - cv2.imshow(win_name, vis) - cv2.waitKey(1) - - self._ultimo_frame_matriz_custo = vis - - return vis, metrics - except Exception as e: - self.mostrar_log(f"Erro ao criar debug blockage imshow: {e}") - return None, None - - def _colorize_masks(self, anom_f, custo_f, conf_f, thr_anom_block=0.50, thr_cost_block=0.65, thr_conf_low=0.35): - """Overlay BGR com soma segura (clamped) nas regiões de máscara.""" - H, W = anom_f.shape - over = np.zeros((H, W, 3), np.uint8) - - mask_anom = (anom_f >= thr_anom_block) - mask_cost = (custo_f >= thr_cost_block) - mask_conf = (conf_f < thr_conf_low) - - def add_color(mask, bgr): - if not np.any(mask): - return - # soma segura com clip - tmp = over[mask].astype(np.int16) - tmp += np.array(bgr, dtype=np.int16) - np.clip(tmp, 0, 255, out=tmp) - over[mask] = tmp.astype(np.uint8) - - add_color(mask_anom, (255, 0, 255)) # magenta - add_color(mask_cost, ( 0,165,255)) # laranja - add_color(mask_conf, (255, 0, 0)) # azul - - return over, mask_anom, mask_cost, mask_conf - - - def _build_preview(self, bgr, pred_ids, alpha): - t0 = time.time() - - if bgr is None or pred_ids is None: - return None, 0.0 - - # Garante que pred_ids seja 2D - pred_ids = np.array(pred_ids) - if pred_ids.ndim == 3 and pred_ids.shape[-1] == 1: - pred_ids = pred_ids[..., 0] - - pred_bgr = converter_mask_ids_para_bgr(pred_ids, self.seg_runner.colormap_rgb) - - # 🔴 Aqui está a mágica: garantir mesmo tamanho - if pred_bgr.shape[:2] != bgr.shape[:2]: - pred_bgr = cv2.resize( - pred_bgr, - (bgr.shape[1], bgr.shape[0]), # (width, height) - interpolation=cv2.INTER_NEAREST, # mantém os IDs de classe - ) - - a = float(np.clip(alpha, 0.0, 1.0)) - - # Se alpha == 0, devolve só o RGB sem gastar CPU à toa - if a == 0.0: - overlay = bgr.copy() - else: - overlay = (bgr.astype(np.float32) * (1 - a) + pred_bgr.astype(np.float32) * a) - overlay = np.clip(overlay, 0, 255).astype(np.uint8) - - dt = (time.time() - t0) * 1000.0 # ms - return overlay, dt - - - def salvar_frames(self, tipos: list, nome: str, pasta="frames_salvos"): - if not self.operante: - return [] + def _verificar_desconexao_camera(self): + if self.iniciando or self._em_warmup or time.time() < self._startup_grace_until: + return - try: - os.makedirs(pasta, exist_ok=True) - frames_salvos = [] + status = StatusModulo( + (self.camera.ultima_saude or {}).get( + "status", + StatusModulo.DESCONECTADO.value, + ) + ) - frame_name = datetime.datetime.now().strftime('%Y%m%d_%H%M%S') - if nome is not None and nome != "": - frame_name = nome - - for _frame_type in tipos: - _frame = self.get_selected_frame(_frame_type) - if _frame is not None and _frame.size > 0: - nome_frame = f"{frame_name}_{(TipoFrameCamera(_frame_type)).name}.jpeg" - caminho = os.path.join(pasta, nome_frame) - cv2.imwrite(caminho, _frame) - frames_salvos.append(caminho) - - return frames_salvos - except Exception as e: - self.mostrar_log(f"❌ Erro ao salvar frames: {e}") - return [] + ts_status = (self.camera.ultima_saude or {}).get("timestamp", 0) + if status == StatusModulo.DESCONECTADO and ts_status and (time.time() - ts_status) > 10.0: + self.fechar_camera_manager("status desconectado") def _set_pub_cache(self, chave, valor): - """ - Atualiza cache de publicação sem escrever no Redis. - chave: segmentacao, deteccao, matriz_confianca, performance_visual - """ try: agora = time.time() @@ -2416,84 +1300,164 @@ class CameraManager: self._pub_cache["ts_analise"] = agora except Exception as e: - self.mostrar_log(f"Erro ao atualizar pub_cache[{chave}]: {e}") + self.mostrar_log(f"[visual] erro ao atualizar pub_cache[{chave}]: {e}") def _montar_payload_publicacao_visual(self, publicar_tudo=False): - """ - Monta um payload único para o Redis. - Se publicar_tudo=False, manda apenas o que mudou. - """ with self._pub_lock: cache = dict(self._pub_cache) - dirty_segmentacao = cache.get("dirty_segmentacao", False) - dirty_deteccao = cache.get("dirty_deteccao", False) - dirty_matriz = cache.get("dirty_matriz_confianca", False) - dirty_perf = cache.get("dirty_performance_visual", False) - payload = { "ts_analise": time.time(), } - if publicar_tudo or dirty_segmentacao: - if cache.get("segmentacao") is not None: - payload["segmentacao"] = cache["segmentacao"] + for chave in ["segmentacao", "deteccao", "matriz_confianca", "performance_visual"]: + dirty = cache.get(f"dirty_{chave}", False) + valor = cache.get(chave) - if publicar_tudo or dirty_deteccao: - if cache.get("deteccao") is not None: - payload["deteccao"] = cache["deteccao"] + if (publicar_tudo or dirty) and valor is not None: + payload[chave] = valor - if publicar_tudo or dirty_matriz: - if cache.get("matriz_confianca") is not None: - payload["matriz_confianca"] = cache["matriz_confianca"] - - if publicar_tudo or dirty_perf: - if cache.get("performance_visual") is not None: - payload["performance_visual"] = cache["performance_visual"] - - # timestamps auxiliares, leves e úteis payload["ts_segmentacao"] = cache.get("ts_segmentacao", 0.0) payload["ts_deteccao"] = cache.get("ts_deteccao", 0.0) payload["ts_matriz_confianca"] = cache.get("ts_matriz_confianca", 0.0) - payload["ts_performance"] = cache.get("ts_performance_visual", 0.0) + payload["ts_performance_visual"] = cache.get("ts_performance_visual", 0.0) - # limpa dirty somente das chaves que entraram no payload - if "segmentacao" in payload: - self._pub_cache["dirty_segmentacao"] = False - - if "deteccao" in payload: - self._pub_cache["dirty_deteccao"] = False - - if "matriz_confianca" in payload: - self._pub_cache["dirty_matriz_confianca"] = False - - if "performance_visual" in payload: - self._pub_cache["dirty_performance_visual"] = False + for chave in ["segmentacao", "deteccao", "matriz_confianca", "performance_visual"]: + if chave in payload: + self._pub_cache[f"dirty_{chave}"] = False return payload + def _publicar_e_logar_performance(self): + resumo = self.perf.resumo() + + if self.camera is not None and hasattr(self.camera, "get_cache_stats"): + resumo["camera_cache"] = self.camera.get_cache_stats() + + resumo["visual_cache"] = { + "seg_ts": self._seg_cache.get("ts", 0.0), + "det_ts": self._det_cache.get("ts", 0.0), + "grid_ts": self._grid_cache.get("ts", 0.0), + "tem_seg": self._ultimo_predictions is not None, + "tem_grid": self._ultimo_snapshot is not None, + } + + resumo["pub_debug"] = self._ultimo_pub_debug + + self._set_pub_cache("performance_visual", resumo) + + if self.debug_perf: + self._logar_performance(resumo) + + def _logar_performance(self, resumo): + loops = resumo.get("loops", {}) + sync = resumo.get("sync", {}) + + rgb = loops.get("camera_rgb", {}) + depth = loops.get("camera_depth", {}) + seg = loops.get("segmentacao", {}) + det = loops.get("deteccao", {}) + grid = loops.get("grid", {}) + pub = loops.get("publicacao", {}) + + self.mostrar_log( + "[PERF_VISUAL] " + f"fps rgb={self._fps(rgb):.1f} depth={self._fps(depth):.1f} " + f"seg={self._fps(seg):.1f} det={self._fps(det):.1f} grid={self._fps(grid):.1f} " + f"pub={self._fps(pub):.1f} | " + f"period seg={self._fmt(self._per(seg))}ms grid={self._fmt(self._per(grid))}ms | " + f"sync={self._fmt(sync.get('rgb_depth_dt_ms'))}ms" + ) + + self.mostrar_log( + "[PERF_DETAIL] " + f"SEG total={self._fmt(self._lat(seg))} " + f"get={self._fmt(self._m(seg, 'get_rgb_ms'))} " + f"infer={self._fmt(self._m(seg, 'infer_ms'))} " + f"post={self._fmt(self._m(seg, 'post_ms'))} | " + f"GRID total={self._fmt(self._lat(grid))} " + f"depth={self._fmt(self._m(grid, 'get_depth_ms'))} " + f"build={self._fmt(self._m(grid, 'build_grid_ms'))} | " + f"DET total={self._fmt(self._lat(det))} " + f"read={self._fmt(self._m(det, 'oak_read_ms'))} | " + f"PUB total={self._fmt(self._lat(pub))} " + f"redis={self._fmt(self._m(pub, 'redis_ms'))}" + ) def _sleep_loop_adaptativo(self, nome_tarefa, t0_wall, freq_fallback): - """ - Sleep baseado no GpuPriorityController. - - nome_tarefa: - - segmentacao - - grid - - deteccao - - publicacao - - analise - """ try: - ctrl = getattr(self, "gpu_controller", None) - + ctrl = self.gpu_controller if ctrl is not None: ctrl.sleep_for_task(nome_tarefa, t0_wall, freq_fallback) return - except Exception as e: self.mostrar_log(f"[GPU_CTRL] erro no sleep adaptativo {nome_tarefa}: {e}") - latencia = time.time() - t0_wall - time.sleep(max(0.0, (1.0 / max(float(freq_fallback), 0.1)) - latencia)) - + lat = time.time() - t0_wall + time.sleep(max(0.0, (1.0 / max(float(freq_fallback), 0.1)) - lat)) + + # ============================================================ + # Utilidades externas + # ============================================================ + + def salvar_frames(self, tipos: list, nome: str, pasta="frames_salvos"): + if not self.operante: + return [] + + try: + os.makedirs(pasta, exist_ok=True) + frames_salvos = [] + + frame_name = nome or datetime.datetime.now().strftime("%Y%m%d_%H%M%S") + + for frame_type in tipos: + frame = self.get_selected_frame(TipoFrameCamera(frame_type)) + + if frame is not None and hasattr(frame, "size") and frame.size > 0: + nome_frame = f"{frame_name}_{TipoFrameCamera(frame_type).name}.jpeg" + caminho = os.path.join(pasta, nome_frame) + cv2.imwrite(caminho, frame) + frames_salvos.append(caminho) + + return frames_salvos + + except Exception as e: + self.mostrar_log(f"❌ Erro ao salvar frames: {e}") + return [] + + @staticmethod + def _fmt(v, casas=1, default=0.0): + try: + if v is None: + v = default + return f"{float(v):.{casas}f}" + except Exception: + return f"{default:.{casas}f}" + + @staticmethod + def _m(loop, nome, stat="med", default=0.0): + try: + return loop.get("metrics_ms", {}).get(nome, {}).get(stat, default) + except Exception: + return default + + @staticmethod + def _lat(loop, stat="med", default=0.0): + try: + return loop.get("latencia_ms", {}).get(stat, default) + except Exception: + return default + + @staticmethod + def _per(loop, stat="med", default=0.0): + try: + return loop.get("periodo_ms", {}).get(stat, default) + except Exception: + return default + + @staticmethod + def _fps(loop): + try: + return float(loop.get("fps_real", 0.0) or 0.0) + except Exception: + return 0.0 diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py index 312b2c52a..8984b1ca8 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py @@ -1,142 +1,665 @@ -import os import threading import time from visual_worker.camera_manager import CameraManager from shared.contexto_global_redis import ContextoGlobalRedis + module_id = "visual" topico_tx = f"operador/{module_id}/tx" topico_rx = f"operador/{module_id}/rx" debug = True ultima_atualizacao = time.time() + def mostrar_log(mensagem): if debug: print(f"{time.time()} - [{module_id}] {mensagem}") + manager = CameraManager(mostrar_log) + def get_camera_manager(): return manager + def iniciar_camera_manager(mx_id): global manager, ultima_atualizacao + if mx_id is None: return + agora = time.time() - if (not manager.iniciando and (agora - ultima_atualizacao) > 5 and (manager.camera is None or manager.mx_id != mx_id)): + + if ( + not manager.iniciando + and (agora - ultima_atualizacao) > 5 + and (manager.camera is None or manager.mx_id != mx_id) + ): ultima_atualizacao = agora mostrar_log("Iniciando Camera Manager...") manager.inicializar(mx_id=mx_id) + if manager.camera is not None and manager.operante: mostrar_log(f"✅ Camera manager iniciado, com MX_ID: {mx_id}") -_CONFIG_PATH = os.path.join(os.path.dirname(__file__), "config.json") + _CONFIG_CACHE = None -_CONFIG_MTIME = None _CONFIG_LOCK = threading.Lock() -def load_seg_config(force_reload=False): - global _CONFIG_CACHE, _CONFIG_MTIME - with _CONFIG_LOCK: - _CONFIG_CACHE = { - "debug_visual": False, - "debug_perf": False, - "ia_roi_begin": 0.0, - "ia_roi_size": 1.0, - "analise_fps": 8.0, - "grid_fps": 5.0, - "inferencia_fps": 8.0, - "deteccao_fps": 5.0, - "publicacao_fps": 15.0, - "gpu_priority": { - "enabled": True, - "update_interval_s": 1.0, - "log_interval_s": 3.0, - "log_periodic": False, - "log_warnings": True, +# ============================================================ +# CONFIG V1 - VISUAL WORKER +# ============================================================ - "bad_cycles_to_degrade": 3, - "good_cycles_to_recover": 5, - "max_data_age_s": 3.0, - "mode_configs": { - "normal": { - "segmentacao": 8.0, - "grid": 6.0, - "deteccao": 5.0, - "publicacao": 15.0, - "analise": 8.0 - }, - "eco": { - "segmentacao": 4.0, - "grid": 4.0, - "deteccao": 5.0, - "publicacao": 10.0, - "analise": 6.0 - }, - "safe": { - "segmentacao": 2.0, - "grid": 2.0, - "deteccao": 3.0, - "publicacao": 5.0, - "analise": 4.0 - }, - "critical": { - "segmentacao": 0.5, - "grid": 1.0, - "deteccao": 2.0, - "publicacao": 5.0, - "analise": 2.0 - } - } - }, +def construir_mode_configs_adaptativos(cfg: dict) -> dict: + gpu = cfg.get("gpu_priority", {}) or {} - "ia_resolution": [1024,576], - "seg_every_n": 1, - "det_every_n": 1, - "use_amp": True, - "use_channels_last": True, - "use_compact_aux": True, - "debug_timing": False, - "runtime_fast": True, - "gerar_mask_color": False, - "gerar_debug_status": False, - "usar_connected_components": True, - "usar_corridor_grid": True + base = { + "segmentacao": float(cfg.get("inferencia_fps", 8.0)), + "grid": float(cfg.get("grid_fps", 6.0)), + "deteccao": float(cfg.get("deteccao_fps", 5.0)), + "publicacao": float(cfg.get("publicacao_fps", 15.0)), + "analise": float(cfg.get("analise_fps", 8.0)), + } + + if not bool(gpu.get("adaptive_mode_configs", True)): + return gpu.get("mode_configs", { + "normal": base, + "eco": base, + "safe": base, + "critical": gpu.get("critical_mode", base), + }) + + scales = gpu.get("mode_scales", {}) or {} + floors = gpu.get("mode_floors", {}) or {} + + def scaled_mode(mode_name: str, scale: float) -> dict: + floor = floors.get(mode_name, {}) or {} + + return { + nome: max( + float(floor.get(nome, 0.0)), + round(float(valor) * float(scale), 3), + ) + for nome, valor in base.items() } - _equipamento = ContextoGlobalRedis.get_equipamento() - _CONFIG_CACHE["ia_mode"] = _equipamento.get("ia_mode_ruas") - _CONFIG_CACHE["ia_backbone"] = _equipamento.get("ia_backbone_ruas_seg") - _CONFIG_CACHE["ia_model_path"] = _equipamento.get("path_ia_model_ruas_seg") - _CONFIG_CACHE["ia_labelmap_path"] = _equipamento.get("path_ia_labelmap_ruas_seg") - _CONFIG_CACHE["ia_norm_stats_path"] = _equipamento.get("path_ia_norm_stats_ruas_seg") - - return _CONFIG_CACHE -def reload_seg_config(): - return load_seg_config(force_reload=True) + critical = gpu.get("critical_mode", {}) or { + "segmentacao": 0.5, + "grid": 1.0, + "deteccao": 2.0, + "publicacao": 5.0, + "analise": 2.0, + } + + return { + "normal": dict(base), + "eco": scaled_mode("eco", float(scales.get("eco", 0.65))), + "safe": scaled_mode("safe", float(scales.get("safe", 0.35))), + "critical": { + "segmentacao": float(critical.get("segmentacao", 0.5)), + "grid": float(critical.get("grid", 1.0)), + "deteccao": float(critical.get("deteccao", 2.0)), + "publicacao": float(critical.get("publicacao", 5.0)), + "analise": float(critical.get("analise", 2.0)), + }, + } + + +VISUAL_DEFAULT_CONFIG = { + # ============================================================ + # 1) Debug e telemetria + # ============================================================ + # Mostra janelas OpenCV/debug visual. Não usar em runtime de campo. + "debug_visual": False, + + # Publica logs de performance no console. + "debug_perf": True, + + # Tamanho dos frames de preview/debug enviados ao C#. + "preview_size": (1280, 720), + + + # ============================================================ + # 2) Frequências dos loops + # ============================================================ + # Loop supervisor: saúde, performance e GPU controller. + "analise_fps": 15.0, + + # Loop de segmentação ONNX/TensorRT. + "inferencia_fps": 15.0, + + # Loop de detecção onboard MobileNet-SSD na OAK. + "deteccao_fps": 5.0, + + # Loop de construção da matriz de confiança/custo. + "grid_fps": 15.0, + + # Loop de publicação Redis. + "publicacao_fps": 5.0, + + + # ============================================================ + # 3) Modelo ONNX/TensorRT - segmentação de ruas/corredor + # ============================================================ + # Runtime oficial da v1. + "runtime_backend": "onnx", + "onnx_provider": "tensorrt", + + # Resolução esperada pelo ONNX: [W, H]. + "ia_resolution": [1024, 576], + + # ROI vertical da segmentação. + # 0.0 + 1.0 = frame inteiro. + "ia_roi_begin": 0.0, + "ia_roi_size": 1.0, + + # Contrato validado do modelo modelseg-2_0.onnx: + # entrada: float32 [1, 3, 576, 1024], RGB 0..1, NCHW + # saída 1: semantic_logits + # saída 2: label_probs + "onnx_has_preprocess": False, + "onnx_input_layout": "nchw_float", + "onnx_input_scale": 1.0 / 255.0, + "onnx_seg_output_format": "logits", + + # Nomes das entradas/saídas. + # input_name None deixa o runner detectar automaticamente. + "onnx_input_name": None, + "onnx_seg_output_name": "semantic_logits", + "onnx_aux_output_name": "label_probs", + "onnx_aux_output_format": "probs", + + # Auxiliar do status de corredor. + # True = retorna label_id, label_name, label_conf. + # False = inclui também vetor de probabilidades. + "use_compact_aux": True, + + # Logs internos do runner ONNX. + "debug_timing": False, + "debug_session": True, + + + # ============================================================ + # 4) TensorRT + # ============================================================ + "trt_fp16_enable": True, + "trt_engine_cache_enable": True, + "trt_engine_cache_path": "./trt_cache_visual_worker", + + # Deixe None salvo se não quiser fixar workspace. + "trt_max_workspace_size": None, + + # Threads do ONNX Runtime. + # Para TensorRT/CUDA, 1 costuma ser suficiente e evita ruído. + "onnx_intra_op_num_threads": 1, + "onnx_inter_op_num_threads": 1, + + + # ============================================================ + # 5) SegmentacaoManager v1 + # ============================================================ + # Este bloco controla apenas a análise da máscara: + # pred_ids + label_probs -> dados_visuais. + "segmentacao": { + # Inclui debug textual no payload de segmentação. + # Não gera imagem. + "include_debug": False, + + # Inclui timing interno do SegmentacaoManager. + "debug_timing": False, + + # ID das classes no modelo de segmentação. + "id_nao_navegavel": 0, + "id_navegavel": 1, + + # Frações verticais usadas para estimar centro/ângulo do corredor. + # 0.0 = topo, 1.0 = base. + "scanline_fracs": (0.96, 0.86, 0.74, 0.62, 0.50, 0.38, 0.26), + + # A região próxima ao robô está na base da imagem. + "near_is_bottom": True, + + # Suavização das saídas usadas pelo controle. + "ema_alpha_ang": 0.25, + "ema_alpha_lat": 0.25, + "ema_alpha_conf": 0.20, + + # Histerese temporal do status do corredor. + "status_window_s": 1.5, + "status_expected_fps": 10.0, + + # Confiança da cabeça auxiliar ONNX. + # >= accept: modelo manda. + # >= soft: modelo ajuda quando heurística está indefinida. + "model_conf_accept": 0.70, + "model_conf_soft": 0.45, + + # Grid leve interna para centro/fallback do corredor. + "corridor_grid_rows": 6, + "corridor_grid_cols": 21, + "corridor_jump_penalty": 0.60, + + # Runs navegáveis nas scanlines. + "min_run_width_frac": 0.035, + "prefer_center_weight": 1.10, + "prefer_prev_weight": 0.65, + "prefer_width_weight": 1.00, + + # Heurística de status quando o modelo auxiliar não está confiante. + "thr_parado_global": 0.30, + "thr_direcionando_global": 0.82, + "thr_caminhando_score": 0.48, + "thr_entrando_near": 0.78, + "thr_saindo_far": 0.70, + }, + + + # ============================================================ + # 6) Grid de confiança/custo + # ============================================================ + "grid": { + # Formato global da grid: (cols, rows). + "grid_shape": (15, 10), + + # Geometria da câmera para gerar grid_ref por linha. + "geometry": { + "grid_shape": (15, 10), + + # Inclinação fixa da câmera em relação ao solo. + "camera_pitch_deg": 28.91, + + # Altura física da câmera. + "camera_height_m": 0.74, + + # FOV vertical usado na aproximação geométrica. + "fov_v_deg": 43.28, + + # Correção dinâmica usando IMU. + "pitch_gain": 1.0, + "pitch_limit_deg": 10.0, + + # Saturação de distâncias plausíveis. + "min_dist_m": 0.2, + "max_dist_m": 20.0, + }, + + # Construção da matriz de confiança/custo por célula. + "confidence": { + # Faixa válida do depth em mm. + "valid_mm": (300, 10000), + + # Fração mínima de pixels depth válidos na célula. + "min_valid_frac": 0.30, + + # Mapeamento de fração válida de depth para confiança. + "conf_params": (0.30, 0.80), + + # Pesos do custo: + # nao_navegavel, anomalia, incerteza. + "weights": (0.65, 0.25, 0.10), + + # Usa depth para anomalia física. + "usar_depth": True, + + # Obstáculo/saliência: algo mais perto que o esperado. + "anom_tau_up": 0.15, + "anom_satur_up_m": 0.35, + + # Buraco/queda: algo mais longe que o esperado. + "anom_tau_down": 0.30, + "anom_satur_down_m": 0.55, + "anom_down_weight": 0.65, + + # Regras para célula navegável. + "min_pct_navegavel": 0.55, + "max_anom_navegavel": 0.45, + "min_conf_navegavel": 0.35, + + # Integração de detecções onboard na grid. + "det": { + # Peso da detecção no custo suave. + "w4": 0.18, + + # Acima disso, detecção crítica pode bloquear navegabilidade. + "thr_det_soft": 0.45, + + # Cobertura mínima da bbox sobre uma célula. + "min_cell_coverage": 0.10, + + # Confiança mínima da detecção. + "min_det_conf": 0.45, + + # Pesos por classe. + "class_weights": { + "person": 1.0, + "dog": 0.7, + "cat": 0.5, + }, + + # Classes críticas para segurança. + "veto_labels": {"person"}, + + # Como combinar múltiplas detecções na mesma célula. + # "max" ou "sum_clamped". + "combine": "max", + + # v1: detecção não derruba confiança, só custo/navegabilidade. + "conf_drop_alpha": 0.0, + + # Se True, só classes veto bloqueiam navegabilidade. + "only_veto_blocks_nav": True, + + # Classes não críticas pesam menos no custo. + "non_veto_cost_scale": 0.50, + }, + }, + + # Fusão temporal e decisão de bloqueio. + "fuser": { + "K": 3, + "M": 2, + + "central_cols": None, + "y_range_m": (0.5, 5.0), + "near_is_bottom": True, + + "margin_m": 0.12, + + "fuse_cost": "ema", + "fuse_anom": "ema", + "fuse_conf": "mean", + "fuse_det": "ema", + "ema_alpha": 0.60, + + "thr_anom_block": 0.50, + "thr_cost_block": 0.70, + "thr_conf_low": 0.35, + "thr_nav_low": 0.45, + + "rho_block_central": 0.70, + "rho_conf_blackout_global": 0.40, + + "thr_det_consider": 0.25, + "thr_det_block": 0.45, + + # MobileNet-SSD: person normalmente é ID 15 no labelmap usado. + "det_veto_label_ids": {15}, + + "use_persistence": True, + "a_max_freio": 0.20, + "margem_parada_m": 0.60, + "stop_on_frames": 2, + "stop_off_frames": 3, + + "use_trend": True, + "trend_alpha": 0.35, + "trend_watch": 0.38, + "trend_slowdown": 0.58, + "trend_prepare_stop": 0.78, + + "include_debug": True, + }, + }, + + + # ============================================================ + # 7) GPU Priority Controller + # ============================================================ + # Controla FPS do Visual Worker conforme saúde do Weed Worker. + "gpu_priority": { + "enabled": True, + + "update_interval_s": 1.0, + "log_interval_s": 3.0, + "log_periodic": False, + "log_warnings": True, + + # Se o weed estiver parado/inativo, não rebaixa o visual. + "ignore_inactive_weed": True, + "weed_inactive_age_s": 5.0, + + # Histerese dos modos. + "bad_cycles_to_degrade": 3, + "good_cycles_to_recover": 5, + "max_data_age_s": 3.0, + + # FPS por modo do Visual Worker. + # Se adaptive_mode_configs=True, os modos normal/eco/safe + # são calculados a partir dos FPS principais definidos no bloco 2. + "adaptive_mode_configs": True, + + # Multiplicadores dos modos intermediários. + # normal sempre usa os FPS principais. + "mode_scales": { + "eco": 0.65, + "safe": 0.35, + }, + + # Pisos mínimos dos modos intermediários. + "mode_floors": { + "eco": { + "segmentacao": 4.0, + "grid": 4.0, + "deteccao": 3.0, + "publicacao": 5.0, + "analise": 6.0, + }, + "safe": { + "segmentacao": 2.0, + "grid": 2.0, + "deteccao": 2.0, + "publicacao": 5.0, + "analise": 4.0, + }, + }, + + # Modo crítico fica fixo, como piso de sobrevivência. + "critical_mode": { + "segmentacao": 0.5, + "grid": 1.0, + "deteccao": 2.0, + "publicacao": 5.0, + "analise": 2.0, + }, + }, +} + + +DET_DEFAULT_CONFIG = { + # ============================================================ + # 1) Debug + # ============================================================ + "debug_visual": False, + + # Tracker onboard/offboard. Desligado no baseline. + "com_track": False, + + + # ============================================================ + # 2) Detector OAK MobileNet-SSD + # ============================================================ + "ia_roi_begin": 0.0, + "ia_roi_size": 1.0, + + # Resolução do modelo detector. + "ia_resolution": [300, 300], + + # Confiança mínima do detector. + "ia_conf": 0.5, + + # Classes do MobileNet-SSD. + "classes": [ + "background", + "aeroplane", "bicycle", "bird", "boat", "bottle", + "bus", "car", "cat", "chair", "cow", + "diningtable", "dog", "horse", "motorbike", "person", + "pottedplant", "sheep", "sofa", "train", "tvmonitor", + ], +} + + +# ============================================================ +# Overrides Redis / runtime +# ============================================================ + +def aplicar_overrides_redis_seg(cfg: dict) -> dict: + equipamento = ContextoGlobalRedis.get_equipamento() + + # Modelo ONNX oficial de ruas/corredor. + cfg["ia_onnx_path"] = equipamento.get("path_ia_model_ruas_seg") + + # Labelmap da segmentação. + cfg["ia_labelmap_path"] = equipamento.get("path_ia_labelmap_ruas_seg") + + return cfg + + +def normalizar_config_runtime_seg(cfg: dict) -> dict: + # Alias único interno, para logs e validações. + cfg["onnx_model_path"] = cfg.get("ia_onnx_path") + + if not cfg.get("ia_onnx_path"): + mostrar_log("[WARN] path do modelo ONNX de ruas não definido no Redis/equipamento.") + + if not cfg.get("ia_labelmap_path"): + mostrar_log("[WARN] path do labelmap de ruas não definido no Redis/equipamento.") + + # Garante tuplas onde as dataclasses esperam tuplas. + cfg["preview_size"] = tuple(cfg.get("preview_size", (1280, 720))) + + grid = cfg.get("grid", {}) or {} + + if "grid_shape" in grid: + grid["grid_shape"] = tuple(grid["grid_shape"]) + + geometry = grid.get("geometry", {}) or {} + if "grid_shape" in geometry: + geometry["grid_shape"] = tuple(geometry["grid_shape"]) + + confidence = grid.get("confidence", {}) or {} + + if "valid_mm" in confidence: + confidence["valid_mm"] = tuple(confidence["valid_mm"]) + + if "conf_params" in confidence: + confidence["conf_params"] = tuple(confidence["conf_params"]) + + if "weights" in confidence: + confidence["weights"] = tuple(confidence["weights"]) + + det = confidence.get("det", {}) or {} + if "veto_labels" in det and not isinstance(det["veto_labels"], set): + det["veto_labels"] = set(det["veto_labels"]) + + fuser = grid.get("fuser", {}) or {} + if "y_range_m" in fuser: + fuser["y_range_m"] = tuple(fuser["y_range_m"]) + + cfg["grid"] = grid + + segmentacao = cfg.get("segmentacao", {}) or {} + if "scanline_fracs" in segmentacao: + segmentacao["scanline_fracs"] = tuple(segmentacao["scanline_fracs"]) + + cfg["segmentacao"] = segmentacao + + gpu = cfg.get("gpu_priority", {}) or {} + gpu["mode_configs"] = construir_mode_configs_adaptativos(cfg) + cfg["gpu_priority"] = gpu + + fuser = grid.get("fuser", {}) or {} + if "y_range_m" in fuser: + fuser["y_range_m"] = tuple(fuser["y_range_m"]) + + if "central_cols" in fuser and fuser["central_cols"] is not None: + fuser["central_cols"] = tuple(fuser["central_cols"]) + + if "det_veto_label_ids" in fuser and not isinstance(fuser["det_veto_label_ids"], set): + fuser["det_veto_label_ids"] = set(fuser["det_veto_label_ids"]) + + return cfg + + +def aplicar_overrides_redis_det(cfg: dict) -> dict: + equipamento = ContextoGlobalRedis.get_equipamento() + + cfg["ia_model_path"] = equipamento.get( + "path_ia_model_ruas_det", + "C:/AgroBaseModels/Ruas/modeldet-1_0.blob", + ) + + return cfg + + +def normalizar_config_runtime_det(cfg: dict) -> dict: + if not cfg.get("ia_model_path"): + mostrar_log("[WARN] path do modelo detector de ruas não definido.") + + return cfg + + +# ============================================================ +# Loaders públicos +# ============================================================ + +def load_seg_config(): + global _CONFIG_CACHE + + with _CONFIG_LOCK: + cfg = dict(VISUAL_DEFAULT_CONFIG) + + # Cópia profunda simples dos blocos aninhados. + # Evita compartilhar dict interno entre chamadas. + cfg["segmentacao"] = dict(VISUAL_DEFAULT_CONFIG["segmentacao"]) + cfg["grid"] = { + "grid_shape": VISUAL_DEFAULT_CONFIG["grid"]["grid_shape"], + "geometry": dict(VISUAL_DEFAULT_CONFIG["grid"]["geometry"]), + "confidence": dict(VISUAL_DEFAULT_CONFIG["grid"]["confidence"]), + "fuser": dict(VISUAL_DEFAULT_CONFIG["grid"]["fuser"]), + } + cfg["grid"]["confidence"]["det"] = dict( + VISUAL_DEFAULT_CONFIG["grid"]["confidence"]["det"] + ) + cfg["grid"]["confidence"]["det"]["class_weights"] = dict( + VISUAL_DEFAULT_CONFIG["grid"]["confidence"]["det"]["class_weights"] + ) + cfg["grid"]["confidence"]["det"]["veto_labels"] = set( + VISUAL_DEFAULT_CONFIG["grid"]["confidence"]["det"]["veto_labels"] + ) + cfg["gpu_priority"] = dict(VISUAL_DEFAULT_CONFIG["gpu_priority"]) + + cfg["gpu_priority"]["mode_scales"] = dict( + VISUAL_DEFAULT_CONFIG["gpu_priority"].get("mode_scales", {}) + ) + + cfg["gpu_priority"]["mode_floors"] = { + k: dict(v) + for k, v in VISUAL_DEFAULT_CONFIG["gpu_priority"].get("mode_floors", {}).items() + } + + cfg["gpu_priority"]["critical_mode"] = dict( + VISUAL_DEFAULT_CONFIG["gpu_priority"].get("critical_mode", {}) + ) + + if "mode_configs" in VISUAL_DEFAULT_CONFIG["gpu_priority"]: + cfg["gpu_priority"]["mode_configs"] = { + k: dict(v) + for k, v in VISUAL_DEFAULT_CONFIG["gpu_priority"]["mode_configs"].items() + } + + cfg = aplicar_overrides_redis_seg(cfg) + cfg = normalizar_config_runtime_seg(cfg) + + _CONFIG_CACHE = cfg + return _CONFIG_CACHE + def load_det_config(): - _CONFIG_DET = { - "debug_visual": False, - "com_track": False, - "ia_roi_begin": 0.0, - "ia_roi_size": 1.0, - "ia_resolution": [300,300], - "seg_every_n": 1, - "det_every_n": 1, - "ia_conf": 0.5, - "classes": [ - "background", - "aeroplane","bicycle","bird","boat","bottle", - "bus","car","cat","chair","cow", - "diningtable","dog","horse","motorbike","person", - "pottedplant","sheep","sofa","train","tvmonitor", - ] - } - _CONFIG_DET["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ruas_det", "C:/AgroBaseModels/Ruas/modeldet-1_0.blob") - return _CONFIG_DET - + cfg = dict(DET_DEFAULT_CONFIG) + cfg = aplicar_overrides_redis_det(cfg) + cfg = normalizar_config_runtime_det(cfg) + return cfg diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py index d9baff6c4..dce8e830a 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py @@ -1,845 +1,772 @@ +from __future__ import annotations + import math import time +from collections import deque +from dataclasses import dataclass, field +from typing import Any, Dict, Optional, Sequence, Tuple + import numpy as np + from shared.enums import StatusCarroMapa +@dataclass +class CostmapFuserConfig: + """ + Configuração v1 do CostmapFuser. + + O fuser não sabe de câmera, ONNX, Redis, config global ou detector. + Ele só recebe a grid já pronta e devolve um snapshot fundido. + """ + + # Buffer temporal. + K: int = 3 + M: int = 2 + + # Formato da grid: (cols, rows). + grid_shape: Tuple[int, int] = (15, 10) + + # Colunas centrais usadas para decisão de passagem. + # None = calcula automaticamente em torno do meio. + central_cols: Optional[Tuple[int, int]] = None + + # Linha inferior é a mais próxima do robô. + near_is_bottom: bool = True + + # Fallback de distância por linha quando z_ref não existe. + y_range_m: Tuple[float, float] = (0.5, 5.0) + + # Geometria lateral. + fov_h_rad: Optional[float] = None + robot_width_m: float = 0.84 + margin_m: float = 0.12 + + # Fusão temporal por canal. + fuse_cost: str = "ema" + fuse_anom: str = "ema" + fuse_conf: str = "mean" + fuse_det: str = "ema" + ema_alpha: float = 0.60 + + # Thresholds principais. + thr_anom_block: float = 0.50 + thr_cost_block: float = 0.70 + thr_conf_low: float = 0.35 + thr_nav_low: float = 0.45 + + # Cobertura para bloqueio. + rho_block_central: float = 0.70 + rho_conf_blackout_global: float = 0.40 + + # Detecção crítica. + thr_det_consider: float = 0.25 + thr_det_block: float = 0.45 + det_veto_label_ids: set[int] = field(default_factory=set) + + # Dinâmica de parada. + use_persistence: bool = True + a_max_freio: float = 0.20 + margem_parada_m: float = 0.60 + stop_on_frames: int = 2 + stop_off_frames: int = 3 + + # Tendência. + use_trend: bool = True + trend_alpha: float = 0.35 + trend_watch: float = 0.38 + trend_slowdown: float = 0.58 + trend_prepare_stop: float = 0.78 + + # Empacotamento. + include_debug: bool = True + + class CostmapFuser: + """ + Fuser v1 da matriz de custo/confiança. + + Entrada: + grid_dict com arrays (rows, cols): + - custo + - conf + - anom + - navegavel + opcionais: + - z_ref + - z_med + - det_score + - det_top_label_id + - det_top_conf + + Saída: + snapshot compacto serializável para Redis/C#/debug. + """ + def __init__( self, - grid_shape=(15, 10), # (cols, rows) - K=3, # tamanho do buffer temporal - M=2, # persistência M-de-N p/ navegável - fuse_method="q0.7", # legado / fallback - central_cols=None, # (i0, i1) inclusivo; None = 3 colunas centrais - y_range_m=(0.5, 5.0), # m: perto..longe (se não houver z_ref) - near_is_bottom=True, # linha de baixo é mais perto? + grid_shape=(15, 10), + K=3, + M=2, + central_cols=None, + y_range_m=(0.5, 5.0), + near_is_bottom=True, fov_h_rad=None, - robot_width=0.84 + robot_width=0.84, + config: Optional[CostmapFuserConfig | Dict[str, Any]] = None, + **legacy_kwargs, ): - self.grid_w, self.grid_h = grid_shape + self.cfg = self._normalizar_config( + config=config, + grid_shape=grid_shape, + K=K, + M=M, + central_cols=central_cols, + y_range_m=y_range_m, + near_is_bottom=near_is_bottom, + fov_h_rad=fov_h_rad, + robot_width=robot_width, + legacy_kwargs=legacy_kwargs, + ) - self.K = max(1, int(K)) - self.M = max(1, min(int(M), self.K)) + self.grid_w, self.grid_h = self.cfg.grid_shape + self.K = max(1, int(self.cfg.K)) + self.M = max(1, min(int(self.cfg.M), self.K)) - # legado / fallback geral - self.fuse_method = fuse_method + self.central_cols = self._resolver_central_cols(self.cfg.central_cols) - # fusão por canal - self.fuse_method_cost = "ema" - self.fuse_method_anom = "ema" - self.fuse_method_conf = "mean" - self.fuse_method_det = "ema" + self._frames = deque(maxlen=self.K) - self.y_range_m = y_range_m - self.near_is_bottom = near_is_bottom - self.fov_h_rad = fov_h_rad - self.robot_width_m = float(robot_width) - - if central_cols is None: - mid = self.grid_w // 2 - self.central_cols = (max(0, mid - 1), min(self.grid_w - 1, mid + 1)) - else: - self.central_cols = (int(central_cols[0]), int(central_cols[1])) - - # ring-buffers principais - self.buf_custo = [] - self.buf_conf = [] - self.buf_anom = [] - self.buf_nav = [] - self.buf_zref = [] - self.buf_zmed = [] - self.buf_ts = [] - - # ring-buffers de detecção - self.buf_det_score = [] - self.buf_det_lbl = [] - self.buf_det_strength = [] - - self._blk_state = { + self._stop_state = { "on": 0, "off": 0, "latched": False, - "reason": "none", - "dmin": None, - "last_decision": "LIVRE" } self._trend_state = { "risk_ema": 0.0, "d_obs_prev": None, + "d_det_prev": None, "central_prev": None, "side_bias_prev": None, "conf_prev": None, "frames_risk_on": 0, "frames_risk_off": 0, - "risk_band": "free" + "band": "free", } self.seq = 0 - def _stack(self, lst, fallback_val=0.0): - """Empilha listas de arrays (grid_h,grid_w). Se vazio, devolve um array fill.""" - try: - if len(lst) == 0: - return np.full((0, self.grid_h, self.grid_w), fallback_val, np.float32) - return np.stack(lst, axis=0) - except Exception as e: - mostrar_log(f"Erro no _stack: {e}") - return np.full((0, self.grid_h, self.grid_w), fallback_val, np.float32) + # ------------------------------------------------------------------ + # API principal + # ------------------------------------------------------------------ - def _fuse_array(self, stack, method="mean", q=0.7, alpha=0.60): - """ - Fusão temporal de um stack (T, H, W). - - Parâmetros: - stack : np.ndarray shape (T, H, W) - method : "max", "mean", "median", "q", "ema" - q : quantil quando method == "q" - alpha : fator da EMA (0..1), maior = mais peso ao frame recente - - Retorna: - np.ndarray shape (H, W), float32 - """ - try: - if stack is None or stack.shape[0] == 0: - return np.zeros((self.grid_h, self.grid_w), dtype=np.float32) - - method = str(method).lower().strip() - - # segurança: garante float32 para operações - stack = stack.astype(np.float32, copy=False) - - if method == "max": - return np.max(stack, axis=0).astype(np.float32) - - elif method in ("mean", "avg", "media"): - return np.mean(stack, axis=0).astype(np.float32) - - elif method in ("median", "mediana"): - return np.median(stack, axis=0).astype(np.float32) - - elif method == "q": - q = float(np.clip(q, 0.0, 1.0)) - return np.quantile(stack, q, axis=0).astype(np.float32) - - elif method in ("ema", "ewma"): - # EMA temporal: frames mais recentes têm maior peso - # stack[0] = mais antigo, stack[-1] = mais recente - T = stack.shape[0] - - # pesos exponenciais crescentes para os frames mais recentes - weights = np.array([(1.0 - alpha) ** (T - 1 - i) for i in range(T)], dtype=np.float32) - weights *= alpha - - # normaliza para soma 1 - wsum = np.sum(weights) - if wsum <= 1e-8: - weights = np.ones((T,), dtype=np.float32) / float(T) - else: - weights /= wsum - - # aplica pesos: (T,1,1) * (T,H,W) -> soma em T - return np.sum(stack * weights[:, None, None], axis=0).astype(np.float32) - - else: - # fallback seguro - mostrar_log(f"_fuse_array: método desconhecido '{method}', usando mean") - return np.mean(stack, axis=0).astype(np.float32) - - except Exception as e: - mostrar_log(f"Erro no _fuse_array: {e}") - return np.zeros((self.grid_h, self.grid_w), dtype=np.float32) - - def _row_distances(self, z_ref_2d=None): - """Distância (m) por linha (H,), usando z_ref se disponível.""" - try: - if z_ref_2d is not None and np.isfinite(z_ref_2d).any(): - # mediana por linha na janela central - i0, i1 = self.central_cols - z_line = np.nanmedian(z_ref_2d[:, i0:i1 + 1], axis=1) # (H,) - if np.isfinite(z_line).any(): - return z_line - # fallback linear: mapeia linhas para [y_min..y_max] - y_min, y_max = self.y_range_m - lin = np.linspace(y_min, y_max, self.grid_h).astype(np.float32) - if self.near_is_bottom: - # bottom (j=H-1) = y_min; top (j=0) = y_max - return lin[::-1] - return lin - except Exception as e: - mostrar_log(f"Erro no _row_distances: {e}") - y_min, y_max = self.y_range_m - lin = np.linspace(y_min, y_max, self.grid_h).astype(np.float32) - return lin[::-1] if self.near_is_bottom else lin - - def _compute_blockage_metrics( + def update( self, - custo_f, anom_f, conf_f, nav_f, zmed_f, - row_dist_m, row_scale_x_m, central_cols, - robot_width_m=0.84, margin_m=0.12, - - thr_anom_block=0.50, - thr_cost_block=0.70, - thr_conf_low=0.35, - thr_nav_low=0.45, - - rho_block_central=0.70, - rho_block_global=0.40, - - near_is_bottom=True, - - use_persistence=False, - velocidade_mps=None, - a_max_freio=0.8, - margem_parada=0.5, - N_on=2, - N_off=3, - blackout_imediato=False, - - det_score_f=None, - det_label_id=None, - det_strength=None, - thr_det_consider=0.15, - thr_det_block=0.45, - det_veto_labels=None, - - status_seg=StatusCarroMapa.Direcionando - ): + grid_dict: Dict[str, Any], + ts: Optional[float] = None, + velocidade_ms: float = 0.0, + status_seg=StatusCarroMapa.Direcionando, + ) -> Optional[Dict[str, Any]]: try: - H, W = custo_f.shape + ts = float(ts if ts is not None else time.time()) - # ----------------------------- - # 0) Preparação / defaults - # ----------------------------- - c0, c1 = central_cols - c0 = max(0, min(W - 1, int(c0))) - c1 = max(0, min(W, int(c1))) - if c1 <= c0: - c0, c1 = W // 3, 2 * W // 3 + frame = self._parse_grid(grid_dict, ts=ts) + self._frames.append(frame) - if det_veto_labels is None: - det_veto_labels = {"person"} + fused = self._fuse_frames() + geometry = self._compute_geometry(fused.get("z_ref")) - labelmap_det = None - if det_score_f is not None: - try: - from visual_worker.config import load_det_config - classes = load_det_config().get("classes") - if classes is not None: - labelmap_det = {i: name for i, name in enumerate(classes)} - except Exception: - labelmap_det = None - - # sanitiza zmed - zmed = None - if zmed_f is not None: - zmed = np.array(zmed_f, dtype=np.float32, copy=True) - invalid = (~np.isfinite(zmed)) | (zmed <= 0.0) - zmed[invalid] = np.nan - - # ----------------------------- - # 1) Máscaras base - # ----------------------------- - # Obstáculo físico forte - mask_anom = (anom_f >= thr_anom_block) - - # Custo só deve pesar quando a navegabilidade não confirma caminho livre - # custo alto sozinho não manda parar - nav_ratio = nav_f.astype(np.float32) - mask_nav_bad = (nav_ratio < thr_nav_low) - mask_cost_support = (custo_f >= thr_cost_block) & mask_nav_bad - - # percepção degradada = confiança ruim - mask_conf_low = (conf_f < thr_conf_low) - - # insegurança "física/fundida" - unsafe = mask_anom | mask_cost_support - - # ----------------------------- - # 2) Máscara de detecção crítica - # ----------------------------- - det_present = None - det_critical = None - - if det_score_f is not None: - det_present = (det_score_f >= float(thr_det_consider)) - det_critical = (det_score_f >= float(thr_det_block)).astype(np.bool_) - - if det_label_id is not None and labelmap_det is not None: - veto_mask = np.zeros_like(det_critical, dtype=np.bool_) - for lid, name in labelmap_det.items(): - if name in det_veto_labels: - veto_mask |= (det_label_id == lid) - det_critical = det_critical & veto_mask - else: - det_present = np.zeros((H, W), dtype=np.bool_) - det_critical = np.zeros((H, W), dtype=np.bool_) - - # ----------------------------- - # 3) Largura necessária por linha - # ----------------------------- - width_need_m = robot_width_m + margin_m - cols_need = np.empty(H, dtype=int) - - for j in range(H): - sx = row_scale_x_m[j] if row_scale_x_m is not None else None - if (sx is None) or (sx <= 1e-6): - cols_need[j] = max(1, (c1 - c0)) - else: - ncols = int(np.ceil(width_need_m / sx)) - cols_need[j] = max(1, min(W, ncols)) - - def central_window(j, ncols): - mid = (c0 + c1) // 2 - half = ncols // 2 - a = max(0, mid - half) - b = min(W, a + ncols) - a = max(0, b - ncols) - return a, b - - # ----------------------------- - # 4) Varredura da faixa central - # ----------------------------- - coverage_central_unsafe = np.zeros(H, np.float32) - coverage_central_conf = np.zeros(H, np.float32) - coverage_central_det = np.zeros(H, np.float32) - coverage_central_detcrit = np.zeros(H, np.float32) - - det_best_label = -np.ones(H, np.int32) - det_best_strength = np.zeros(H, np.float32) - - j_block = None - j_det_block = None - - it = range(H - 1, -1, -1) if near_is_bottom else range(H) - - for j in it: - a, b = central_window(j, cols_need[j]) - if b <= a: - coverage_central_unsafe[j] = 1.0 - coverage_central_conf[j] = 1.0 - coverage_central_det[j] = 0.0 - coverage_central_detcrit[j] = 0.0 - continue - - unsafe_row = unsafe[j, a:b] - conf_row = mask_conf_low[j, a:b] - det_row = det_present[j, a:b] - detcrit_row = det_critical[j, a:b] - - cov_unsafe = float(unsafe_row.mean()) - cov_conf = float(conf_row.mean()) - cov_det = float(det_row.mean()) - cov_detcrit = float(detcrit_row.mean()) - - coverage_central_unsafe[j] = cov_unsafe - coverage_central_conf[j] = cov_conf - coverage_central_det[j] = cov_det - coverage_central_detcrit[j] = cov_detcrit - - if (j_block is None) and (cov_unsafe >= rho_block_central): - j_block = j - - if (j_det_block is None) and (cov_detcrit > 0.0): - j_det_block = j - - # detalhes da melhor detecção por linha - if np.any(det_row): - if (det_strength is not None) and (det_label_id is not None): - str_win = det_strength[j, a:b] - lbl_win = det_label_id[j, a:b] - - # pega o pixel com maior força dentre os que têm detecção - masked_strength = np.where(det_row, str_win, -1.0) - k = int(np.argmax(masked_strength)) - best_s = float(masked_strength.ravel()[k]) - if best_s > 0: - det_best_strength[j] = best_s - det_best_label[j] = int(lbl_win.ravel()[k]) - - if zmed is not None: - z_win = zmed[j, a:b] - z_sel = z_win[det_row] - z_sel = z_sel[np.isfinite(z_sel) & (z_sel > 0)] - - # ----------------------------- - # 5) Cobertura global - # ----------------------------- - global_cov_unsafe = float(unsafe.mean()) - global_cov_conf = float(mask_conf_low.mean()) - global_cov_detcrit = float(det_critical.mean()) - conf_mean = float(np.mean(conf_f)) - - # ----------------------------- - # 6) Distâncias reais na faixa central - # ----------------------------- - d_block_line_m_z = None - d_obs_true_min_m_z = None - d_det_true_min_m_z = None - - j_obs_true_min = None - j_det_true_min = None - - it2 = range(H - 1, -1, -1) if near_is_bottom else range(H) - - for j in it2: - a, b = central_window(j, cols_need[j]) - if b <= a: - continue - - if zmed is not None: - # obstáculo fundido/físico - bad = unsafe[j, a:b] - if np.any(bad): - z_bad = zmed[j, a:b][bad] - z_bad = z_bad[np.isfinite(z_bad)] - if z_bad.size: - z_min = float(np.min(z_bad)) - if j_obs_true_min is None: - j_obs_true_min = j - d_obs_true_min_m_z = z_min - - # detecção crítica - detb = det_critical[j, a:b] - if np.any(detb): - z_det = zmed[j, a:b][detb] - z_det = z_det[np.isfinite(z_det)] - if z_det.size: - z_min_det = float(np.min(z_det)) - if j_det_true_min is None: - j_det_true_min = j - d_det_true_min_m_z = z_min_det - - if (j_block is not None) and (j == j_block) and (zmed is not None): - bad_block = unsafe[j, a:b] - if np.any(bad_block): - z_block = zmed[j, a:b][bad_block] - z_block = z_block[np.isfinite(z_block)] - if z_block.size: - d_block_line_m_z = float(np.min(z_block)) - - # ----------------------------- - # 7) Viés lateral - # ----------------------------- - midc = (c0 + c1) // 2 - left = float(unsafe[:, c0:midc].mean()) if (midc > c0) else 0.0 - right = float(unsafe[:, midc:c1].mean()) if (c1 > midc) else 0.0 - side_bias_val = float(np.clip((right - left) / max(1e-6, (right + left + 1e-6)), -1.0, 1.0)) - - # ----------------------------- - # 7.5) Tendências temporais - # ----------------------------- - central_unsafe_max = float(np.max(coverage_central_unsafe)) - trend = self._compute_trend_metrics( - central_unsafe_max=central_unsafe_max, - global_cov_unsafe=global_cov_unsafe, - conf_mean=conf_mean, - d_obs_true_min_m=d_obs_true_min_m_z, - d_det_true_min_m=d_det_true_min_m_z, - side_bias=side_bias_val + block = self._compute_blockage( + fused=fused, + geometry=geometry, + velocidade_ms=float(velocidade_ms or 0.0), + status_seg=status_seg, ) - # ----------------------------- - # 8) Classificação raw - # ----------------------------- - def _fmt_m(x): - return "-" if (x is None or not np.isfinite(x)) else f"{float(x):.2f} m" + self.seq += 1 - def _min_non_none(a, b): - if a is None and b is None: - return None - if a is None: - return b - if b is None: - return a - return a if a <= b else b - - blocked_raw = False - reason = "free" - reason_detail = "Caminho livre." - - central_conf_max = float(np.max(coverage_central_conf)) - central_detcrit_max = float(np.max(coverage_central_detcrit)) - - # 8.1 detecção crítica primeiro: pessoa no centro deve mandar muito - if j_det_block is not None: - blocked_raw = True - reason = "detected_critical" - - d_used = d_det_true_min_m_z - - lbl_txt = "objeto crítico" - if labelmap_det is not None and j_det_block is not None: - lid = int(det_best_label[j_det_block]) - if lid >= 0: - lbl_txt = labelmap_det.get(lid, f"label#{lid}") - - strength_txt = "" - s_best = float(det_best_strength[j_det_block]) if j_det_block is not None else 0.0 - if s_best > 0: - strength_txt = f", score={s_best:.2f}" - - reason_detail = ( - f"Deteccao critica no centro: {lbl_txt}. " - f"Distancia estimada: {_fmt_m(d_used)}. " - f"Ocupacao critica da faixa central: {central_detcrit_max:.0%}." - + (f" Confiança da deteccao: {s_best:.2f}." if s_best > 0 else "") - ) - - # 8.2 obstáculo físico/fundido - elif j_block is not None: - blocked_raw = True - reason = "obstacle" - - d_used = _min_non_none(d_obs_true_min_m_z, d_block_line_m_z) - - reason_detail = ( - f"Faixa central bloqueada a frente. " - f"Cobertura bloqueada: {central_unsafe_max:.0%} " - f"(limite: {rho_block_central:.0%}). " - f"Distancia estimada: {_fmt_m(d_used)} " - f"[linha: {j_block} | pixel: {_fmt_m(d_obs_true_min_m_z)}]." - ) - - # 8.3 percepção degradada / blackout - elif (global_cov_conf >= rho_block_global) and (conf_mean < thr_conf_low): - blocked_raw = False - reason = "blackout" - reason_detail = ( - f"Percepcao degradada. " - f"Grande parte da area esta com baixa confianca ({global_cov_conf:.0%}) " - f"e a confianca media esta em {conf_mean:.0%}. " - f"Ha incerteza na leitura do cenario, sem obstaculo confirmado." - ) - - # 8.4 corredor estreito / cautela - elif (central_unsafe_max > 0.45) and (left > 0.7 or right > 0.7): - blocked_raw = False - reason = "narrow" - lado = "direita" if right > left else "esquerda" - lado_frac = max(left, right) - reason_detail = ( - f"Corredor estreito a frente. " - f"A lateral {lado} esta mais fechada ({lado_frac:.0%}) " - f"e a regiao central ja mostra restricao de {central_unsafe_max:.0%}." - ) - - # ----------------------------- - # 9) Decisão dinâmica com persistência - # ----------------------------- - decision = { - "parar": False, - "dist_necessaria": None, - "v_max_sugerida_mps": None, - "frames_on": 0, - "frames_off": 0, - "N_on": N_on, - "N_off": N_off, - "mode": "free" - } - - blocked_out = bool(blocked_raw) - - if use_persistence: - v = float(max(0.0, velocidade_mps or 0.0)) - dist_freio = (v * v) / max(1e-9, 2.0 * a_max_freio) - dist_necessaria = dist_freio + margem_parada - decision["dist_necessaria"] = float(dist_necessaria) - - d_stop_ref = None - if reason == "detected_critical": - d_stop_ref = d_det_true_min_m_z - elif reason == "obstacle": - d_stop_ref = _min_non_none(d_block_line_m_z, d_obs_true_min_m_z) - - trend_band = "free" - risk_ema = 0.0 - frames_risk_on = 0 - - if trend is not None: - trend_band = str(trend.get("band", "free")) - risk_ema = float(trend.get("risk_ema", 0.0)) - frames_risk_on = int(trend.get("frames_risk_on", 0)) - - trend_watch = trend_band in ("watch", "slowdown", "prepare_stop") - trend_slow = trend_band in ("slowdown", "prepare_stop") - trend_stop = (trend_band == "prepare_stop" and frames_risk_on >= 2) - - stop_now = False - v_max_sug = None - mode = "free" - - # ------------------------- - # 1) Casos duros - # ------------------------- - if reason == "detected_critical": - mode = "stop" - stop_now = True - - elif reason == "obstacle": - if (d_stop_ref is not None) and np.isfinite(d_stop_ref) and (d_stop_ref <= dist_necessaria): - mode = "stop" - stop_now = True - else: - mode = "slowdown" - stop_now = False - if (d_stop_ref is not None) and np.isfinite(d_stop_ref) and (d_stop_ref > margem_parada): - v_max_sug = float(np.sqrt(max(0.0, 2.0 * a_max_freio * (d_stop_ref - margem_parada)))) - - # ------------------------- - # 2) Casos moles modulados por trend - # ------------------------- - elif reason == "blackout": - if trend_stop: - mode = "stop" - stop_now = True - elif trend_slow: - mode = "slowdown" - stop_now = False - elif trend_watch: - mode = "watch" - stop_now = False - else: - mode = "caution" - stop_now = False - - elif reason == "narrow": - if trend_band == "prepare_stop" and (d_obs_true_min_m_z is not None) and (d_obs_true_min_m_z <= dist_necessaria * 1.20): - mode = "stop" - stop_now = True - elif trend_band in ("slowdown", "prepare_stop"): - mode = "slowdown" - stop_now = False - if (d_obs_true_min_m_z is not None) and np.isfinite(d_obs_true_min_m_z) and (d_obs_true_min_m_z > margem_parada): - v_max_sug = float(np.sqrt(max(0.0, 2.0 * a_max_freio * (d_obs_true_min_m_z - margem_parada)))) - else: - mode = "watch" - stop_now = False - - else: # reason == "free" - if trend_band == "prepare_stop" and frames_risk_on >= 2: - mode = "slowdown" - stop_now = False - elif trend_band == "slowdown": - mode = "slowdown" - stop_now = False - elif trend_band == "watch": - mode = "watch" - stop_now = False - else: - mode = "free" - stop_now = False - - st = self._blk_state - - if stop_now: - st["on"] = min(N_on, st["on"] + 1) - st["off"] = 0 - else: - st["off"] = min(N_off, st["off"] + 1) - st["on"] = 0 - - if (not st["latched"]) and (st["on"] >= N_on): - st["latched"] = True - elif st["latched"] and (st["off"] >= N_off): - st["latched"] = False - - blocked_out = bool(st["latched"]) - - decision.update({ - "parar": blocked_out, - "v_max_sugerida_mps": v_max_sug, - "frames_on": int(st["on"]), - "frames_off": int(st["off"]), - "mode": mode, - "trend_band": trend_band, - "risk_ema": risk_ema, - "frames_risk_on": frames_risk_on - }) - - st["reason"] = reason - st["d_block_line_m"] = None if d_block_line_m_z is None else float(d_block_line_m_z) - st["d_obs_true_min_m"] = None if d_obs_true_min_m_z is None else float(d_obs_true_min_m_z) - st["d_det_true_min_m"] = None if d_det_true_min_m_z is None else float(d_det_true_min_m_z) - st["last_decision"] = "PARAR" if st["latched"] else "LIVRE" - - # ----------------------------- - # 10) retorno - # ----------------------------- - return { - "d_block_line_m": d_block_line_m_z, - "d_obs_true_min_m": d_obs_true_min_m_z, - "d_det_true_min_m": d_det_true_min_m_z, - - "blocked": bool(blocked_out), - "blocked_raw": bool(blocked_raw), - - "reason": reason, - "reason_detail": reason_detail, - - "coverage": { - "central_max": float(central_unsafe_max), - "global": float(global_cov_unsafe), - "conf_low_global": float(global_cov_conf), - "det_critical_global": float(global_cov_detcrit), - "central_conf_max": float(central_conf_max), - "central_detcrit_max": float(central_detcrit_max), - }, - - "side_bias": { - "value": side_bias_val, - "left_frac": float(left), - "right_frac": float(right), - }, - - "j_block": None if j_block is None else int(j_block), - "j_obs_true_min": None if j_obs_true_min is None else int(j_obs_true_min), - "j_det_true_min": None if j_det_true_min is None else int(j_det_true_min), - - "trend": trend, - - "decision": decision - } + return self._pack_snapshot( + ts=ts, + fused=fused, + geometry=geometry, + block=block, + ) except Exception as e: - mostrar_log(f"Erro no _compute_blockage_metrics: {e}") + return self._error_snapshot(str(e)) + + # ------------------------------------------------------------------ + # Config + # ------------------------------------------------------------------ + + @staticmethod + def _normalizar_config( + config, + grid_shape, + K, + M, + central_cols, + y_range_m, + near_is_bottom, + fov_h_rad, + robot_width, + legacy_kwargs, + ) -> CostmapFuserConfig: + if isinstance(config, CostmapFuserConfig): + return config + + base = { + "grid_shape": tuple(grid_shape), + "K": int(K), + "M": int(M), + "central_cols": central_cols, + "y_range_m": tuple(y_range_m), + "near_is_bottom": bool(near_is_bottom), + "fov_h_rad": fov_h_rad, + "robot_width_m": float(robot_width), + } + + if isinstance(config, dict): + base.update(config) + + # Compatibilidade mínima com chamadas atuais. + if "robot_width" in legacy_kwargs: + base["robot_width_m"] = float(legacy_kwargs["robot_width"]) + + allowed = set(CostmapFuserConfig.__dataclass_fields__.keys()) + clean = {k: v for k, v in base.items() if k in allowed} + + if clean.get("central_cols") is not None: + clean["central_cols"] = tuple(clean["central_cols"]) + + if "grid_shape" in clean: + clean["grid_shape"] = tuple(clean["grid_shape"]) + + if "y_range_m" in clean: + clean["y_range_m"] = tuple(clean["y_range_m"]) + + if "det_veto_label_ids" in clean and not isinstance(clean["det_veto_label_ids"], set): + clean["det_veto_label_ids"] = set(clean["det_veto_label_ids"]) + + return CostmapFuserConfig(**clean) + + def _resolver_central_cols(self, central_cols): + if central_cols is None: + mid = self.grid_w // 2 + return max(0, mid - 1), min(self.grid_w, mid + 2) + + c0, c1 = int(central_cols[0]), int(central_cols[1]) + + # Convenção v1: c0 inclusivo, c1 exclusivo. + c0 = max(0, min(self.grid_w - 1, c0)) + c1 = max(c0 + 1, min(self.grid_w, c1)) + + return c0, c1 + + # ------------------------------------------------------------------ + # Entrada + # ------------------------------------------------------------------ + + def _parse_grid(self, grid: Dict[str, Any], ts: float) -> Dict[str, Any]: + required = ["custo", "conf", "anom", "navegavel"] + + for k in required: + if k not in grid: + raise ValueError(f"grid_dict sem campo obrigatório: {k}") + + def arr(name, dtype=np.float32, optional=False): + value = grid.get(name) + if value is None: + if optional: + return None + raise ValueError(f"grid_dict[{name}]=None") + + a = np.asarray(value, dtype=dtype) + + if a.shape != (self.grid_h, self.grid_w): + raise ValueError( + f"{name}.shape={a.shape}, esperado={(self.grid_h, self.grid_w)}" + ) + + return np.ascontiguousarray(a) + + return { + "ts": ts, + "custo": np.clip(arr("custo"), 0.0, 1.0), + "conf": np.clip(arr("conf"), 0.0, 1.0), + "anom": np.clip(arr("anom"), 0.0, 1.0), + "nav": arr("navegavel").astype(np.float32), + "z_ref": arr("z_ref", optional=True), + "z_med": arr("z_med", optional=True), + "det_score": arr("det_score", optional=True), + "det_label": arr("det_top_label_id", dtype=np.int32, optional=True), + "det_strength": arr("det_top_conf", optional=True), + } + + # ------------------------------------------------------------------ + # Fusão temporal + # ------------------------------------------------------------------ + + def _fuse_frames(self) -> Dict[str, Any]: + frames = list(self._frames) + + def stack(name, fill=0.0): + vals = [f[name] for f in frames if f.get(name) is not None] + + if not vals: + return None + + return np.stack(vals, axis=0).astype(np.float32, copy=False) + + custo = self._fuse_stack(stack("custo"), self.cfg.fuse_cost) + conf = self._fuse_stack(stack("conf"), self.cfg.fuse_conf) + anom = self._fuse_stack(stack("anom"), self.cfg.fuse_anom) + + nav_stack = stack("nav") + if nav_stack is None: + nav = np.zeros((self.grid_h, self.grid_w), dtype=np.uint8) + else: + nav = (np.sum(nav_stack > 0.5, axis=0) >= self.M).astype(np.uint8) + + det_score = self._fuse_stack(stack("det_score"), self.cfg.fuse_det, allow_none=True) + det_strength = self._fuse_stack(stack("det_strength"), self.cfg.fuse_det, allow_none=True) + + # Labels: usa o mais recente válido. + det_label = None + for f in reversed(frames): + if f.get("det_label") is not None: + det_label = f["det_label"].astype(np.int32, copy=False) + break + + # z_ref e z_med: usa o mais recente válido. É simples, previsível e leve. + z_ref = None + z_med = None + + for f in reversed(frames): + if z_ref is None and f.get("z_ref") is not None: + z_ref = f["z_ref"].astype(np.float32, copy=False) + + if z_med is None and f.get("z_med") is not None: + z_med = f["z_med"].astype(np.float32, copy=False) + + if z_ref is not None and z_med is not None: + break + + return { + "custo": custo, + "conf": conf, + "anom": anom, + "nav": nav, + "z_ref": z_ref, + "z_med": z_med, + "det_score": det_score, + "det_strength": det_strength, + "det_label": det_label, + } + + def _fuse_stack(self, stack, method: str, allow_none=False): + if stack is None or stack.shape[0] == 0: + if allow_none: + return None + return np.zeros((self.grid_h, self.grid_w), dtype=np.float32) + + method = str(method or "mean").lower().strip() + stack = stack.astype(np.float32, copy=False) + + if method in ("mean", "avg", "media"): + return np.mean(stack, axis=0).astype(np.float32) + + if method in ("median", "mediana"): + return np.median(stack, axis=0).astype(np.float32) + + if method == "max": + return np.max(stack, axis=0).astype(np.float32) + + if method.startswith("q"): + try: + q = float(method[1:]) + except Exception: + q = 0.60 + q = float(np.clip(q, 0.0, 1.0)) + return np.quantile(stack, q, axis=0).astype(np.float32) + + if method in ("ema", "ewma"): + return self._ema_stack(stack, alpha=self.cfg.ema_alpha) + + return np.mean(stack, axis=0).astype(np.float32) + + @staticmethod + def _ema_stack(stack, alpha=0.60): + T = stack.shape[0] + + if T <= 1: + return stack[-1].astype(np.float32) + + alpha = float(np.clip(alpha, 0.01, 1.0)) + out = stack[0].astype(np.float32) + + for i in range(1, T): + out = (1.0 - alpha) * out + alpha * stack[i] + + return out.astype(np.float32) + + # ------------------------------------------------------------------ + # Geometria + # ------------------------------------------------------------------ + + def _compute_geometry(self, z_ref_2d=None) -> Dict[str, Any]: + row_dist_m = self._row_distances(z_ref_2d) + row_scale_x_m = self._row_scale_x(row_dist_m) + + return { + "row_dist_m": row_dist_m, + "row_scale_x_m": row_scale_x_m, + "central_cols": self.central_cols, + } + + def _row_distances(self, z_ref_2d=None): + if z_ref_2d is not None and np.isfinite(z_ref_2d).any(): + c0, c1 = self.central_cols + z_line = np.nanmedian(z_ref_2d[:, c0:c1], axis=1) + + if np.isfinite(z_line).any(): + z_line = np.where(np.isfinite(z_line), z_line, np.nanmedian(z_line)) + return z_line.astype(np.float32) + + y_min, y_max = self.cfg.y_range_m + lin = np.linspace(y_min, y_max, self.grid_h, dtype=np.float32) + + return lin[::-1] if self.cfg.near_is_bottom else lin + + def _row_scale_x(self, row_dist_m): + if self.cfg.fov_h_rad is None: + return None + + width_m = 2.0 * row_dist_m * math.tan(0.5 * float(self.cfg.fov_h_rad)) + scale = width_m / float(self.grid_w) + + return scale.astype(np.float32) + + def _central_window_for_row(self, row_idx, row_scale_x_m): + c0, c1 = self.central_cols + + if row_scale_x_m is None: + return c0, c1 + + sx = float(row_scale_x_m[row_idx]) + + if not np.isfinite(sx) or sx <= 1e-6: + return c0, c1 + + width_need = self.cfg.robot_width_m + self.cfg.margin_m + cols_need = int(np.ceil(width_need / sx)) + cols_need = max(1, min(self.grid_w, cols_need)) + + mid = (c0 + c1) // 2 + half = cols_need // 2 + + a = max(0, mid - half) + b = min(self.grid_w, a + cols_need) + a = max(0, b - cols_need) + + return a, b + + # ------------------------------------------------------------------ + # Decisão + # ------------------------------------------------------------------ + + def _compute_blockage(self, fused, geometry, velocidade_ms, status_seg): + custo = fused["custo"] + conf = fused["conf"] + anom = fused["anom"] + nav = fused["nav"].astype(np.float32) + z_med = fused.get("z_med") + det_score = fused.get("det_score") + det_label = fused.get("det_label") + det_strength = fused.get("det_strength") + + row_scale_x = geometry["row_scale_x_m"] + + unsafe = self._compute_unsafe_mask(custo, anom, nav) + conf_low = conf < self.cfg.thr_conf_low + det_critical = self._compute_det_critical(det_score, det_label) + + coverage = self._scan_central_coverage( + unsafe=unsafe, + conf_low=conf_low, + det_critical=det_critical, + row_scale_x_m=row_scale_x, + ) + + distances = self._compute_distances( + unsafe=unsafe, + det_critical=det_critical, + z_med=z_med, + row_scale_x_m=row_scale_x, + ) + + side_bias = self._compute_side_bias(unsafe) + + trend = self._compute_trend( + central_unsafe_max=coverage["central_unsafe_max"], + global_unsafe=float(np.mean(unsafe)), + conf_mean=float(np.mean(conf)), + d_obs=distances["d_obs_true_min_m"], + d_det=distances["d_det_true_min_m"], + side_bias=side_bias["value"], + ) + + raw = self._classify_raw( + coverage=coverage, + distances=distances, + side_bias=side_bias, + trend=trend, + ) + + decision = self._decide_with_persistence( + raw=raw, + distances=distances, + trend=trend, + velocidade_ms=velocidade_ms, + ) + + result = { + "blocked": bool(decision["parar"]), + "blocked_raw": bool(raw["blocked_raw"]), + "reason": raw["reason"], + "reason_detail": raw["reason_detail"], + + "decision": decision, + "coverage": coverage, + "side_bias": side_bias, + "trend": trend, + + "d_obs_true_min_m": distances["d_obs_true_min_m"], + "d_det_true_min_m": distances["d_det_true_min_m"], + "d_block_line_m": distances["d_block_line_m"], + + "j_block": distances["j_block"], + "j_obs_true_min": distances["j_obs_true_min"], + "j_det_true_min": distances["j_det_true_min"], + } + + if not self.cfg.include_debug: + result.pop("reason_detail", None) + + return result + + def _compute_unsafe_mask(self, custo, anom, nav): + mask_anom = anom >= self.cfg.thr_anom_block + mask_nav_bad = nav < self.cfg.thr_nav_low + mask_cost = (custo >= self.cfg.thr_cost_block) & mask_nav_bad + + return mask_anom | mask_cost + + def _compute_det_critical(self, det_score, det_label): + if det_score is None: + return np.zeros((self.grid_h, self.grid_w), dtype=bool) + + det = det_score >= self.cfg.thr_det_block + + if det_label is not None and self.cfg.det_veto_label_ids: + veto = np.zeros_like(det, dtype=bool) + + for lid in self.cfg.det_veto_label_ids: + veto |= det_label == int(lid) + + det = det & veto + + return det + + def _scan_central_coverage(self, unsafe, conf_low, det_critical, row_scale_x_m): + central_unsafe = np.zeros(self.grid_h, dtype=np.float32) + central_conf = np.zeros(self.grid_h, dtype=np.float32) + central_detcrit = np.zeros(self.grid_h, dtype=np.float32) + + order = range(self.grid_h - 1, -1, -1) if self.cfg.near_is_bottom else range(self.grid_h) + + j_block = None + j_det = None + + for j in order: + a, b = self._central_window_for_row(j, row_scale_x_m) + + if b <= a: + central_unsafe[j] = 1.0 + central_conf[j] = 1.0 + central_detcrit[j] = 0.0 + continue + + central_unsafe[j] = float(np.mean(unsafe[j, a:b])) + central_conf[j] = float(np.mean(conf_low[j, a:b])) + central_detcrit[j] = float(np.mean(det_critical[j, a:b])) + + if j_block is None and central_unsafe[j] >= self.cfg.rho_block_central: + j_block = int(j) + + if j_det is None and central_detcrit[j] > 0.0: + j_det = int(j) + + return { + "central_unsafe_by_row": central_unsafe.tolist() if self.cfg.include_debug else None, + "central_conf_by_row": central_conf.tolist() if self.cfg.include_debug else None, + "central_detcrit_by_row": central_detcrit.tolist() if self.cfg.include_debug else None, + + "central_max": float(np.max(central_unsafe)), + "central_unsafe_max": float(np.max(central_unsafe)), + "central_conf_max": float(np.max(central_conf)), + "central_detcrit_max": float(np.max(central_detcrit)), + + "global": float(np.mean(unsafe)), + "conf_low_global": float(np.mean(conf_low)), + "det_critical_global": float(np.mean(det_critical)), + + "j_block": j_block, + "j_det": j_det, + } + + def _compute_distances(self, unsafe, det_critical, z_med, row_scale_x_m): + d_obs = None + d_det = None + d_block = None + + j_obs = None + j_det = None + j_block = None + + if z_med is None: return { - "d_block_line_m": None, "d_obs_true_min_m": None, "d_det_true_min_m": None, - "blocked": False, - "blocked_raw": False, - "reason": "error", - "reason_detail": str(e), - "coverage": { - "central_max": 0.0, - "global": 0.0, - "conf_low_global": 0.0, - "det_critical_global": 0.0, - "central_conf_max": 0.0, - "central_detcrit_max": 0.0, - }, - "side_bias": { - "value": 0.0, - "left_frac": 0.0, - "right_frac": 0.0, - }, - "j_block": None, + "d_block_line_m": None, "j_obs_true_min": None, "j_det_true_min": None, - "trend": {}, - "decision": { - "parar": False, - "dist_necessaria": None, - "v_max_sugerida_mps": None, - "frames_on": 0, - "frames_off": 0, - "N_on": N_on, - "N_off": N_off, - "mode": "error" - } + "j_block": None, + } + + z = np.asarray(z_med, dtype=np.float32) + z = np.where(np.isfinite(z) & (z > 0.0), z, np.nan) + + order = range(self.grid_h - 1, -1, -1) if self.cfg.near_is_bottom else range(self.grid_h) + + for j in order: + a, b = self._central_window_for_row(j, row_scale_x_m) + + if b <= a: + continue + + bad = unsafe[j, a:b] + if j_obs is None and np.any(bad): + vals = z[j, a:b][bad] + vals = vals[np.isfinite(vals)] + if vals.size: + j_obs = int(j) + d_obs = float(np.min(vals)) + + det = det_critical[j, a:b] + if j_det is None and np.any(det): + vals = z[j, a:b][det] + vals = vals[np.isfinite(vals)] + if vals.size: + j_det = int(j) + d_det = float(np.min(vals)) + + if j_block is None and np.mean(bad) >= self.cfg.rho_block_central: + vals = z[j, a:b][bad] + vals = vals[np.isfinite(vals)] + if vals.size: + j_block = int(j) + d_block = float(np.min(vals)) + + return { + "d_obs_true_min_m": d_obs, + "d_det_true_min_m": d_det, + "d_block_line_m": d_block, + "j_obs_true_min": j_obs, + "j_det_true_min": j_det, + "j_block": j_block, + } + + def _compute_side_bias(self, unsafe): + c0, c1 = self.central_cols + mid = (c0 + c1) // 2 + + left = float(np.mean(unsafe[:, c0:mid])) if mid > c0 else 0.0 + right = float(np.mean(unsafe[:, mid:c1])) if c1 > mid else 0.0 + + value = (right - left) / max(1e-6, right + left + 1e-6) + value = float(np.clip(value, -1.0, 1.0)) + + return { + "value": value, + "left_frac": left, + "right_frac": right, + } + + def _compute_trend(self, central_unsafe_max, global_unsafe, conf_mean, d_obs, d_det, side_bias): + if not self.cfg.use_trend: + return { + "risk_raw": 0.0, + "risk_ema": 0.0, + "band": "free", + "frames_risk_on": 0, + "frames_risk_off": 0, } - def _compute_trend_metrics( - self, - central_unsafe_max, - global_cov_unsafe, - conf_mean, - d_obs_true_min_m, - d_det_true_min_m, - side_bias, - alpha_risk=0.35 - ): st = self._trend_state - # valores anteriores - d_obs_prev = st["d_obs_prev"] - central_prev = st["central_prev"] - side_prev = st["side_bias_prev"] - conf_prev = st["conf_prev"] - - # ------------------------- - # 1) deltas / tendência - # ------------------------- - delta_central = 0.0 if central_prev is None else (central_unsafe_max - central_prev) - delta_side = 0.0 if side_prev is None else abs(side_bias - side_prev) - delta_conf = 0.0 if conf_prev is None else (conf_prev - conf_mean) # queda de confiança => risco + delta_central = 0.0 if st["central_prev"] is None else central_unsafe_max - st["central_prev"] + delta_side = 0.0 if st["side_bias_prev"] is None else abs(side_bias - st["side_bias_prev"]) + delta_conf = 0.0 if st["conf_prev"] is None else st["conf_prev"] - conf_mean approaching_obs = 0.0 - if d_obs_prev is not None and d_obs_true_min_m is not None: - dd = d_obs_prev - d_obs_true_min_m # positivo => obstáculo aproximando - approaching_obs = max(0.0, dd) + if st["d_obs_prev"] is not None and d_obs is not None: + approaching_obs = max(0.0, st["d_obs_prev"] - d_obs) approaching_det = 0.0 - if st.get("d_det_prev") is not None and d_det_true_min_m is not None: - dd = st["d_det_prev"] - d_det_true_min_m - approaching_det = max(0.0, dd) + if st["d_det_prev"] is not None and d_det is not None: + approaching_det = max(0.0, st["d_det_prev"] - d_det) - # ------------------------- - # 2) normalizações - # ------------------------- - risco_central = float(np.clip(central_unsafe_max, 0.0, 1.0)) - risco_global = float(np.clip(global_cov_unsafe, 0.0, 1.0)) - risco_conf = float(np.clip(1.0 - conf_mean, 0.0, 1.0)) - - risco_crescendo = float(np.clip(delta_central / 0.20, 0.0, 1.0)) - risco_lateral = float(np.clip(delta_side / 0.25, 0.0, 1.0)) - risco_aprox_obs = float(np.clip(approaching_obs / 0.40, 0.0, 1.0)) - risco_aprox_det = float(np.clip(approaching_det / 0.40, 0.0, 1.0)) - - # ------------------------- - # 3) score bruto - # ------------------------- risk_raw = ( - 0.30 * risco_central + - 0.15 * risco_global + - 0.15 * risco_conf + - 0.15 * risco_crescendo + - 0.10 * risco_lateral + - 0.10 * risco_aprox_obs + - 0.05 * risco_aprox_det + 0.30 * np.clip(central_unsafe_max, 0.0, 1.0) + + 0.15 * np.clip(global_unsafe, 0.0, 1.0) + + 0.15 * np.clip(1.0 - conf_mean, 0.0, 1.0) + + 0.15 * np.clip(delta_central / 0.20, 0.0, 1.0) + + 0.10 * np.clip(delta_side / 0.25, 0.0, 1.0) + + 0.10 * np.clip(approaching_obs / 0.40, 0.0, 1.0) + + 0.05 * np.clip(approaching_det / 0.40, 0.0, 1.0) ) risk_raw = float(np.clip(risk_raw, 0.0, 1.0)) + risk_ema = (1.0 - self.cfg.trend_alpha) * st["risk_ema"] + self.cfg.trend_alpha * risk_raw - # ------------------------- - # 4) EMA temporal - # ------------------------- - risk_ema_prev = float(st["risk_ema"]) - risk_ema = (1.0 - alpha_risk) * risk_ema_prev + alpha_risk * risk_raw - - # ------------------------- - # 5) faixa de risco - # ------------------------- - if risk_ema >= 0.78: + if risk_ema >= self.cfg.trend_prepare_stop: band = "prepare_stop" - elif risk_ema >= 0.58: + elif risk_ema >= self.cfg.trend_slowdown: band = "slowdown" - elif risk_ema >= 0.38: + elif risk_ema >= self.cfg.trend_watch: band = "watch" else: band = "free" - # persistência leve do risco - if band in ("watch", "slowdown", "prepare_stop"): - st["frames_risk_on"] += 1 - st["frames_risk_off"] = 0 - else: + if band == "free": st["frames_risk_off"] += 1 st["frames_risk_on"] = 0 + else: + st["frames_risk_on"] += 1 + st["frames_risk_off"] = 0 - # salva histórico st["risk_ema"] = float(risk_ema) - st["d_obs_prev"] = None if d_obs_true_min_m is None else float(d_obs_true_min_m) - st["d_det_prev"] = None if d_det_true_min_m is None else float(d_det_true_min_m) + st["d_obs_prev"] = None if d_obs is None else float(d_obs) + st["d_det_prev"] = None if d_det is None else float(d_det) st["central_prev"] = float(central_unsafe_max) st["side_bias_prev"] = float(side_bias) st["conf_prev"] = float(conf_mean) - st["risk_band"] = band + st["band"] = band return { - "risk_raw": float(risk_raw), + "risk_raw": risk_raw, "risk_ema": float(risk_ema), "band": band, "delta_central": float(delta_central), @@ -847,331 +774,254 @@ class CostmapFuser: "approaching_obs_m": float(approaching_obs), "approaching_det_m": float(approaching_det), "frames_risk_on": int(st["frames_risk_on"]), - "frames_risk_off": int(st["frames_risk_off"]) + "frames_risk_off": int(st["frames_risk_off"]), } - def _row_scale_x(self, row_dist_m): - """metros por célula em X para cada linha, dado FOV_H.""" - try: - if self.fov_h_rad is None: - return None - # largura coberta naquela linha - row_width = 2.0 * row_dist_m * math.tan(0.5 * float(self.fov_h_rad)) # (H,) - # metros por coluna - return (row_width / float(self.grid_w)).astype(np.float32) # (H,) - except Exception as e: - mostrar_log(f"Erro no _row_scale_x: {e}") - return None + def _classify_raw(self, coverage, distances, side_bias, trend): + j_det = coverage["j_det"] + j_block = coverage["j_block"] - def update(self, grid_dict, ts=None, velocidade_ms=0.0, status_seg=StatusCarroMapa.Direcionando): - """ - Atualiza o estado temporal da costmap fundida e retorna um snapshot serializável. + central = float(coverage["central_unsafe_max"]) + conf_global = float(coverage["conf_low_global"]) + conf_central = float(coverage["central_conf_max"]) - Espera em grid_dict os arrays (grid_h, grid_w): - - custo - - conf - - anom - - navegavel - - Opcionais: - - z_ref - - z_med - - det_score - - det_top_label_id - - det_top_conf - - Filosofia: - - segmentação/navegabilidade = base - - anomalia/depth = evidência física - - detecção = evidência contextual / override crítico - - confiança = qualidade perceptiva, não obstáculo - """ - try: - if ts is None: - ts = time.time() - - # ----------------------------- - # 1) Leitura e validação básica - # ----------------------------- - custo = grid_dict["custo"].astype(np.float32) - conf = grid_dict["conf"].astype(np.float32) - anom = grid_dict["anom"].astype(np.float32) - nav = grid_dict["navegavel"].astype(np.float32) # esperado 0/1 - - zref = grid_dict.get("z_ref", None) - if zref is not None: - zref = zref.astype(np.float32) - - zmed = grid_dict.get("z_med", None) - if zmed is not None: - zmed = zmed.astype(np.float32) - - det_score = grid_dict.get("det_score", None) - if det_score is not None: - det_score = det_score.astype(np.float32) - - det_lbl = grid_dict.get("det_top_label_id", None) - if det_lbl is not None: - det_lbl = det_lbl.astype(np.int32) - - det_strength = grid_dict.get("det_top_conf", None) - if det_strength is not None: - det_strength = det_strength.astype(np.float32) - - H, W = custo.shape - if (H, W) != (self.grid_h, self.grid_w): - raise ValueError(f"grid {H, W} != {(self.grid_h, self.grid_w)}") - - # ----------------------------- - # 2) Buffers temporais - # ----------------------------- - self.buf_custo.append(custo) - self.buf_conf.append(conf) - self.buf_anom.append(anom) - self.buf_nav.append(nav) - self.buf_ts.append(ts) - self.buf_zref.append(zref) - self.buf_zmed.append(zmed) - - # buffers novos para detecção - if not hasattr(self, "buf_det_score"): - self.buf_det_score = [] - if not hasattr(self, "buf_det_lbl"): - self.buf_det_lbl = [] - if not hasattr(self, "buf_det_strength"): - self.buf_det_strength = [] - - self.buf_det_score.append(det_score) - self.buf_det_lbl.append(det_lbl) - self.buf_det_strength.append(det_strength) - - # mantém no máximo K - while len(self.buf_custo) > self.K: - self.buf_custo.pop(0) - self.buf_conf.pop(0) - self.buf_anom.pop(0) - self.buf_nav.pop(0) - self.buf_ts.pop(0) - self.buf_zref.pop(0) - self.buf_zmed.pop(0) - - self.buf_det_score.pop(0) - self.buf_det_lbl.pop(0) - self.buf_det_strength.pop(0) - - # ----------------------------- - # 3) Empilhamento temporal - # ----------------------------- - S_custo = self._stack(self.buf_custo, 0.0) - S_conf = self._stack(self.buf_conf, 0.0) - S_anom = self._stack(self.buf_anom, 0.0) - S_nav = self._stack(self.buf_nav, 0.0) - - S_det_score = self._stack([x for x in self.buf_det_score if x is not None], 0.0) \ - if any(x is not None for x in self.buf_det_score) else None - - S_det_strength = self._stack([x for x in self.buf_det_strength if x is not None], 0.0) \ - if any(x is not None for x in self.buf_det_strength) else None - - # ----------------------------- - # 4) Métodos de fusão por canal - # ----------------------------- - fuse_cost = getattr(self, "fuse_method_cost", getattr(self, "fuse_method", "ema")) - fuse_anom = getattr(self, "fuse_method_anom", getattr(self, "fuse_method", "ema")) - fuse_conf = getattr(self, "fuse_method_conf", "mean") - fuse_det = getattr(self, "fuse_method_det", "ema") - - # custo - custo_f = self._fuse_by_method(S_custo, fuse_cost, default_q=0.60) - - # anomalia - anom_f = self._fuse_by_method(S_anom, fuse_anom, default_q=0.60) - - # confiança: por padrão média/EMA, nunca max - conf_f = self._fuse_by_method(S_conf, fuse_conf, default_q=0.50) - - # navegável: manter M-de-N como base estável - if S_nav.shape[0] > 0: - nav_f = (S_nav.sum(axis=0) >= self.M).astype(np.uint8) - else: - nav_f = np.zeros((self.grid_h, self.grid_w), dtype=np.uint8) - - # detecção - det_score_f = None - if S_det_score is not None and S_det_score.shape[0] > 0: - det_score_f = self._fuse_by_method(S_det_score, fuse_det, default_q=0.60) - - det_strength_f = None - if S_det_strength is not None and S_det_strength.shape[0] > 0: - det_strength_f = self._fuse_by_method(S_det_strength, fuse_det, default_q=0.60) - - # label dominante de detecção: - # por enquanto pega o mais recente não-nulo. - # depois, se quisermos, fazemos voto temporal ponderado. - det_label_f = None - for lbl in reversed(self.buf_det_lbl): - if lbl is not None: - det_label_f = lbl - break - - # ----------------------------- - # 5) z_ref e z_med fundidos - # ----------------------------- - zref_f = None - if any(z is not None for z in self.buf_zref): - for z in reversed(self.buf_zref): - if z is not None: - zref_f = z - break - - zmed_f = None - if any(z is not None for z in self.buf_zmed): - # aqui também podemos futuramente usar mean/median por célula - for z in reversed(self.buf_zmed): - if z is not None: - zmed_f = z - break - - # ----------------------------- - # 6) Geometria por linha - # ----------------------------- - row_dist_m = self._row_distances(zref_f).astype(np.float32) - row_scale_x = self._row_scale_x(row_dist_m) - - # ----------------------------- - # 7) Métricas de bloqueio - # ----------------------------- - block = self._compute_blockage_metrics( - custo_f=custo_f, - anom_f=anom_f, - conf_f=conf_f, - nav_f=nav_f, - zmed_f=zmed_f, - row_dist_m=row_dist_m, - row_scale_x_m=row_scale_x, - central_cols=self.central_cols, - use_persistence=True, - velocidade_mps=velocidade_ms, - a_max_freio=0.2, - margem_parada=0.60, - N_on=2, - N_off=3, - blackout_imediato=False, - - det_score_f=det_score_f, - det_label_id=det_label_f, - det_strength=det_strength_f, - thr_det_consider=0.25, - status_seg=status_seg - ) - - # ----------------------------- - # 8) Seq - # ----------------------------- - self.seq += 1 - - # ----------------------------- - # 9) Empacotamento leve - # ----------------------------- - def to_u8_list(a): - if a is None: - return None - return np.clip((a * 255.0), 0, 255).astype(np.uint8).ravel().tolist() - - snap = { - "ts": float(ts), - "seq": int(self.seq), - "grid_w": int(self.grid_w), - "grid_h": int(self.grid_h), - "fuse": { - "K": int(self.K), - "M": int(self.M), - "cost_method": str(fuse_cost), - "anom_method": str(fuse_anom), - "conf_method": str(fuse_conf), - "det_method": str(fuse_det), - "central_cols": [int(self.central_cols[0]), int(self.central_cols[1])], - "near_is_bottom": bool(self.near_is_bottom), - }, - "y_range_m": [float(self.y_range_m[0]), float(self.y_range_m[1])], - "row_dist_m": row_dist_m.tolist(), - "row_scale_x_m": row_scale_x.tolist() if row_scale_x is not None else None, - - "custo_u8": to_u8_list(custo_f), - "conf_u8": to_u8_list(conf_f), - "anom_u8": to_u8_list(anom_f), - "nav_mask": nav_f.astype(np.uint8).ravel().tolist(), - - # debug útil - "det_score_u8": to_u8_list(det_score_f) if det_score_f is not None else None, - "det_strength_u8": to_u8_list(det_strength_f) if det_strength_f is not None else None, - - "block": block + if j_det is not None: + return { + "blocked_raw": True, + "reason": "detected_critical", + "reason_detail": "Detecção crítica na faixa central.", + "mode_hint": "stop", } - return snap + if j_block is not None: + return { + "blocked_raw": True, + "reason": "obstacle", + "reason_detail": "Faixa central bloqueada por custo/anomalia.", + "mode_hint": "slowdown_or_stop", + } - except Exception as e: - mostrar_log(f"Erro ao atualizar dados do costmap: {e}") - return None - - def _fuse_by_method(self, stack, method, default_q=0.60): - """ - Aplica fusão temporal ao stack (T,H,W) conforme o método configurado. - Requer que _fuse_array suporte pelo menos: - - max - - mean - - median - - q - - ema - """ - if stack is None or stack.shape[0] == 0: - return np.zeros((self.grid_h, self.grid_w), dtype=np.float32) + if conf_global >= self.cfg.rho_conf_blackout_global and conf_central >= self.cfg.thr_conf_low: + return { + "blocked_raw": False, + "reason": "blackout", + "reason_detail": "Percepção degradada sem obstáculo confirmado.", + "mode_hint": "caution", + } - if method is None: - method = "mean" + if central > 0.45 and max(side_bias["left_frac"], side_bias["right_frac"]) > 0.70: + return { + "blocked_raw": False, + "reason": "narrow", + "reason_detail": "Corredor estreito ou lateral muito fechada.", + "mode_hint": "watch", + } - method = str(method).lower().strip() + return { + "blocked_raw": False, + "reason": "free", + "reason_detail": "Caminho livre.", + "mode_hint": "free", + } - if method.startswith("q"): - try: - q = float(method[1:]) - except Exception: - q = default_q - return self._fuse_array(stack, method="q", q=q) + def _decide_with_persistence(self, raw, distances, trend, velocidade_ms): + v = float(max(0.0, velocidade_ms or 0.0)) + dist_freio = (v * v) / max(1e-9, 2.0 * self.cfg.a_max_freio) + dist_necessaria = dist_freio + self.cfg.margem_parada_m - if method in ("mean", "avg", "media"): - return self._fuse_array(stack, method="mean") + reason = raw["reason"] + mode = "free" + stop_now = False + v_max = None - if method in ("median", "mediana"): - return self._fuse_array(stack, method="median") + d_stop_ref = None - if method in ("ema", "ewma"): - # alpha maior = responde mais rápido ao presente - return self._fuse_array(stack, method="ema", alpha=0.60) + if reason == "detected_critical": + d_stop_ref = distances["d_det_true_min_m"] + mode = "stop" + stop_now = True - if method == "max": - return self._fuse_array(stack, method="max") + elif reason == "obstacle": + d_stop_ref = distances["d_block_line_m"] or distances["d_obs_true_min_m"] - # fallback seguro - return self._fuse_array(stack, method="mean") - + if d_stop_ref is not None and np.isfinite(d_stop_ref) and d_stop_ref <= dist_necessaria: + mode = "stop" + stop_now = True + else: + mode = "slowdown" + stop_now = False - + if d_stop_ref is not None and np.isfinite(d_stop_ref) and d_stop_ref > self.cfg.margem_parada_m: + v_max = float( + np.sqrt( + max( + 0.0, + 2.0 * self.cfg.a_max_freio * (d_stop_ref - self.cfg.margem_parada_m), + ) + ) + ) + else: + band = str(trend.get("band", "free")) + + if band == "prepare_stop" and int(trend.get("frames_risk_on", 0)) >= 2: + mode = "slowdown" + elif band == "slowdown": + mode = "slowdown" + elif band == "watch": + mode = "watch" + elif reason == "blackout": + mode = "caution" + elif reason == "narrow": + mode = "watch" + else: + mode = "free" + + if not self.cfg.use_persistence: + return { + "parar": bool(stop_now), + "mode": mode, + "dist_necessaria": float(dist_necessaria), + "v_max_sugerida_mps": v_max, + "frames_on": 1 if stop_now else 0, + "frames_off": 0 if stop_now else 1, + "N_on": self.cfg.stop_on_frames, + "N_off": self.cfg.stop_off_frames, + } + + st = self._stop_state + + if stop_now: + st["on"] = min(self.cfg.stop_on_frames, st["on"] + 1) + st["off"] = 0 + else: + st["off"] = min(self.cfg.stop_off_frames, st["off"] + 1) + st["on"] = 0 + + if not st["latched"] and st["on"] >= self.cfg.stop_on_frames: + st["latched"] = True + elif st["latched"] and st["off"] >= self.cfg.stop_off_frames: + st["latched"] = False + + return { + "parar": bool(st["latched"]), + "mode": mode, + "dist_necessaria": float(dist_necessaria), + "v_max_sugerida_mps": v_max, + "frames_on": int(st["on"]), + "frames_off": int(st["off"]), + "N_on": int(self.cfg.stop_on_frames), + "N_off": int(self.cfg.stop_off_frames), + } + + # ------------------------------------------------------------------ + # Snapshot + # ------------------------------------------------------------------ + + def _pack_snapshot(self, ts, fused, geometry, block): + def to_u8_list(a): + if a is None: + return None + return np.clip(a * 255.0, 0, 255).astype(np.uint8).ravel().tolist() + + row_dist = geometry["row_dist_m"] + row_scale = geometry["row_scale_x_m"] + + snap = { + "ts": float(ts), + "seq": int(self.seq), + + "grid_w": int(self.grid_w), + "grid_h": int(self.grid_h), + + "fuse": { + "K": int(self.K), + "M": int(self.M), + "cost_method": self.cfg.fuse_cost, + "anom_method": self.cfg.fuse_anom, + "conf_method": self.cfg.fuse_conf, + "det_method": self.cfg.fuse_det, + "central_cols": [int(self.central_cols[0]), int(self.central_cols[1])], + "near_is_bottom": bool(self.cfg.near_is_bottom), + }, + + "y_range_m": [float(self.cfg.y_range_m[0]), float(self.cfg.y_range_m[1])], + "row_dist_m": row_dist.astype(float).tolist(), + "row_scale_x_m": row_scale.astype(float).tolist() if row_scale is not None else None, + + "custo_u8": to_u8_list(fused["custo"]), + "conf_u8": to_u8_list(fused["conf"]), + "anom_u8": to_u8_list(fused["anom"]), + "nav_mask": fused["nav"].astype(np.uint8).ravel().tolist(), + + "det_score_u8": to_u8_list(fused.get("det_score")), + "det_strength_u8": to_u8_list(fused.get("det_strength")), + + "block": block, + } + + return snap + + def _error_snapshot(self, erro: str): + self.seq += 1 + + return { + "ts": time.time(), + "seq": int(self.seq), + "grid_w": int(self.grid_w), + "grid_h": int(self.grid_h), + "fuse": { + "K": int(self.K), + "M": int(self.M), + "central_cols": [int(self.central_cols[0]), int(self.central_cols[1])], + "near_is_bottom": bool(self.cfg.near_is_bottom), + }, + "custo_u8": [0] * (self.grid_w * self.grid_h), + "conf_u8": [0] * (self.grid_w * self.grid_h), + "anom_u8": [0] * (self.grid_w * self.grid_h), + "nav_mask": [0] * (self.grid_w * self.grid_h), + "det_score_u8": None, + "det_strength_u8": None, + "row_dist_m": [], + "row_scale_x_m": None, + "block": { + "blocked": False, + "blocked_raw": False, + "reason": "error", + "reason_detail": erro, + "decision": { + "parar": False, + "mode": "error", + "dist_necessaria": None, + "v_max_sugerida_mps": None, + "frames_on": 0, + "frames_off": 0, + "N_on": self.cfg.stop_on_frames, + "N_off": self.cfg.stop_off_frames, + }, + }, + } def unpack_snapshot(snap): - H, W = snap["grid_h"], snap["grid_w"] - custo_u8 = np.array(snap["custo_u8"], dtype=np.uint8).reshape(H, W) - custo = custo_u8.astype(np.float32) / 255.0 - conf_u8 = np.array(snap["conf_u8"], dtype=np.uint8).reshape(H, W) - conf = conf_u8.astype(np.float32) / 255.0 - anom_u8 = np.array(snap["anom_u8"], dtype=np.uint8).reshape(H, W) - anom = anom_u8.astype(np.float32) / 255.0 - nav_u8 = np.array(snap["nav_mask"], dtype=np.uint8).reshape(H, W) - nav = nav_u8.astype(bool) - return custo, conf, anom, nav + H, W = int(snap["grid_h"]), int(snap["grid_w"]) -def mostrar_log(mensagem): - print(f"[COSTMAP_FUSER] {mensagem}") + def from_u8(name): + data = snap.get(name) + if data is None: + return None + + arr = np.array(data, dtype=np.uint8).reshape(H, W) + return arr.astype(np.float32) / 255.0 + + custo = from_u8("custo_u8") + conf = from_u8("conf_u8") + anom = from_u8("anom_u8") + + nav_data = snap.get("nav_mask") + if nav_data is None: + nav = np.zeros((H, W), dtype=bool) + else: + nav = np.array(nav_data, dtype=np.uint8).reshape(H, W).astype(bool) + + return custo, conf, anom, nav diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py index 39d90c376..a3067e2b4 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py @@ -1,709 +1,796 @@ -from enum import IntEnum +from __future__ import annotations + import time +from dataclasses import dataclass, field +from enum import IntEnum +from collections import deque +from typing import Any, Dict, List, Optional, Sequence, Tuple + import cv2 import numpy as np -from collections import deque -from shared.utils import encode_image_base64 from shared.enums import StatusCarroMapa + class ClassesSegmentacao(IntEnum): NAONAVEGAVEL = 0 NAVEGAVEL = 1 -class SegmentacaoManager: - def __init__(self, color_map, classes): - from visual_worker.config import load_seg_config - config = load_seg_config() - self.runtime_fast = bool(config.get("runtime_fast", True)) - self.gerar_mask_color = bool(config.get("gerar_mask_color", False)) - self.gerar_debug_status = bool(config.get("gerar_debug_status", False)) - self.usar_connected_components = bool(config.get("usar_connected_components", True)) - self.usar_corridor_grid = bool(config.get("usar_corridor_grid", True)) +@dataclass +class SegmentacaoConfig: + """ + Configuração v1 do analisador semântico do Visual Worker. - resolucao = config.get("ia_resolution") + Esta classe controla apenas análise de máscara. + Não controla ONNX, câmera, render, preview, stream nem Redis. + """ + + # IDs esperados na máscara de segmentação. + id_nao_navegavel: int = int(ClassesSegmentacao.NAONAVEGAVEL) + id_navegavel: int = int(ClassesSegmentacao.NAVEGAVEL) + + # Scanlines usadas para estimar centro, largura e ângulo do corredor. + # Frações verticais do frame: 0.0 = topo, 1.0 = base. + scanline_fracs: Tuple[float, ...] = (0.96, 0.86, 0.74, 0.62, 0.50, 0.38, 0.26) + + # O frame da OAK-D está com o chão próximo na parte inferior. + near_is_bottom: bool = True + + # Suavização temporal das saídas usadas pelo controle. + ema_alpha_ang: float = 0.25 + ema_alpha_lat: float = 0.25 + ema_alpha_conf: float = 0.20 + + # Histórico para estabilizar status do corredor. + status_window_s: float = 1.5 + status_expected_fps: float = 10.0 + + # Prioridade do status vindo da cabeça auxiliar ONNX. + model_conf_accept: float = 0.70 + model_conf_soft: float = 0.45 + + # Grid leve usada para pontuar corredor e fallback de centro. + corridor_grid_rows: int = 6 + corridor_grid_cols: int = 21 + corridor_jump_penalty: float = 0.60 + + # Validação de runs navegáveis nas scanlines. + min_run_width_frac: float = 0.035 + prefer_center_weight: float = 1.10 + prefer_prev_weight: float = 0.65 + prefer_width_weight: float = 1.00 + + # Limiares da heurística de status. + thr_parado_global: float = 0.30 + thr_direcionando_global: float = 0.82 + thr_caminhando_score: float = 0.48 + thr_entrando_near: float = 0.78 + thr_saindo_far: float = 0.70 + + # Debug leve: inclui métricas extras no retorno, sem criar imagens. + include_debug: bool = True + debug_timing: bool = False + + +class SegmentacaoManager: + """ + Analisador de segmentação semântica v1. + + Responsabilidade única: + pred_ids + aux_result -> dados_visuais + + Não renderiza. + Não colore máscara. + Não lê config global sozinho. + Não depende de PyTorch, ONNX ou câmera. + """ + + def __init__( + self, + config: Optional[SegmentacaoConfig | Dict[str, Any]] = None, + color_map: Optional[Sequence[Sequence[int]]] = None, + classes: Optional[Sequence[str]] = None, + ): + self.config = self._normalizar_config(config) + + # Mantidos como metadados úteis, mas não usados no caminho quente. self.color_map = color_map self.classes = classes - self.resolucao = (resolucao[0], resolucao[1]) - self.pred_rgb = np.empty((self.resolucao[1], self.resolucao[0], 3), dtype=np.uint8) - self.color_lut = np.array(self.color_map, np.uint8) - self.lut = np.zeros((256, 3), dtype=np.uint8) - for i, color in enumerate(color_map): - #self.lut[i] = color - self.lut[i] = (color[2], color[1], color[0]) # converte pra (B, G, R) - IGNORE_ID = 255 - self.lut[IGNORE_ID] = (255, 255, 255) - self.predictions = None - self.log = None - self.dados_visuais = {} + max_len = max( + 3, + int(self.config.status_window_s * self.config.status_expected_fps * 1.5), + ) - self._ema_ang = None - self._ema_lat = None - self._ema_alpha_ang = 0.25 # suavização do ângulo (0..1) - self._ema_alpha_lat = 0.25 # suavização do lateral (0..1) - self._prev_cx = None - - max_len = None - janela_s = 1.5 - fps_esperado = 10.0 - # janela temporal para a MAIORIA - self.janela_s = float(janela_s) - # tamanho máximo da deque (fallback se o FPS variar) - self.max_len = max_len or int(self.janela_s * fps_esperado * 1.5) - self._status_hist = deque(maxlen=self.max_len) + self._status_hist = deque(maxlen=max_len) self._status_final_hist = deque(maxlen=2) - self.debug_timing = bool(config.get("debug_timing_segmentacao", False)) + self._ema_ang: Optional[float] = None + self._ema_lat: Optional[float] = None + self._ema_conf: Optional[float] = None + self._prev_cx: Optional[float] = None + self._ultimo_resultado: Optional[Dict[str, Any]] = None + self._ultimo_erro: Optional[str] = None - def segmentar(self, predictions, aux_result=None): + # --------------------------------------------------------------------- + # API principal + # --------------------------------------------------------------------- + + def segmentar(self, predictions: np.ndarray, aux_result: Optional[Dict[str, Any]] = None): + """ + Compatibilidade nominal para o CameraManager atual. + + Para a v1, o método preferido é analisar(). + Retorna: + resultado, erro + """ try: - t0 = time.perf_counter() - - resultado = self._segmentar_predictions( - predictions, - gerar_mask_color=self.gerar_mask_color - ) - t_mask = time.perf_counter() - - if resultado is None: - print("[Erro] Segmentação vazia ou falhou") - return None, self.log - - if predictions is None: - print("[Erro] Máscara de classes não encontrada no resultado") - return None, self.log - - self.dados_visuais = self._analisar_corredor_visual( - predictions, - aux_result=aux_result - ) - t_ana = time.perf_counter() - - resultado["dados_visuais"] = self.dados_visuais - - if getattr(self, "debug_timing", False): - print( - f"SEGMENTACAO_MANAGER | " - f"mask={(t_mask-t0)*1000:.1f}ms | " - f"analise={(t_ana-t_mask)*1000:.1f}ms | " - f"total={(t_ana-t0)*1000:.1f}ms" - ) - - return resultado, self.log - + resultado = self.analisar(predictions, aux_result=aux_result) + return resultado, None except Exception as e: - self.log = f"❌ Erro na segmentação: {e}" - return None, self.log + self._ultimo_erro = f"Erro na análise de segmentação: {e}" + return None, self._ultimo_erro - def _segmentar_predictions(self, predictions, gerar_mask_color=None): - try: - if gerar_mask_color is None: - gerar_mask_color = self.gerar_mask_color + def analisar(self, predictions: np.ndarray, aux_result: Optional[Dict[str, Any]] = None) -> Dict[str, Any]: + t0 = time.perf_counter() - mask_color = None + pred = self._validar_predictions(predictions) + H, W = pred.shape - if gerar_mask_color: - self.pred_rgb[:] = self.lut[predictions] - mask_color = self.pred_rgb + mask_nav = pred == self.config.id_navegavel - return { - "timestamp": time.time(), - "frame": { - "timestamp": time.time(), - "frame": None - }, - "mask_color": mask_color, - "classes": predictions + t_base = time.perf_counter() + + corridor_score, centers_col, left_col, right_col, grid_debug = self._corridor_score_grid(pred) + + centros, larguras_px, scan_debug = self._scan_corredor( + mask_nav=mask_nav, + centers_col=centers_col, + left_col=left_col, + right_col=right_col, + ) + + erro_angular = self._calcular_erro_angular(centros, H) + erro_lateral = self._calcular_erro_lateral(centros, W) + + t_geo = time.perf_counter() + + status_now, status_final, status_before, status_debug = self._resolver_status( + pred=pred, + mask_nav=mask_nav, + corridor_score=corridor_score, + aux_result=aux_result, + ) + + t_status = time.perf_counter() + + confianca = self._suavizar_confianca(corridor_score) + + dados_visuais = { + "timestamp": time.time(), + "height": int(H), + "width": int(W), + + "erro_angular": erro_angular, + "erro_lateral_pct": erro_lateral, + "confianca": round(float(confianca), 4), + + "status_corredor": int(status_final.value), + "status_corredor_nome": str(status_final.name), + "status_corredor_anterior": int(status_before.value), + "status_corredor_anterior_nome": str(status_before.name), + + "centros_corredor": centros, + "larguras_px": larguras_px, + } + + if self.config.include_debug: + dados_visuais["status_corredor_debug"] = status_debug + dados_visuais["debug_corredor"] = { + "status_now": int(status_now.value), + "status_now_nome": str(status_now.name), + "corridor_score": round(float(corridor_score), 4), + "scan": scan_debug, + "grid": grid_debug, } - except Exception as e: - print(f"Erro ao processar predictions: {e}") - return None + resultado = { + "timestamp": time.time(), + "classes": pred, + "dados_visuais": dados_visuais, + } - def _analisar_corredor_visual(self, predictions, aux_result=None, grid_rows_y_px=None, near_is_bottom=True): - # --- inputs/base --- - self.predictions = predictions - H, W = self.predictions.shape - cx_img = W // 2 + if self.config.debug_timing: + resultado["timing_ms"] = { + "base": round((t_base - t0) * 1000.0, 3), + "geometria": round((t_geo - t_base) * 1000.0, 3), + "status": round((t_status - t_geo) * 1000.0, 3), + "total": round((time.perf_counter() - t0) * 1000.0, 3), + } - status_modelo_now, status_final_fast, status_before_fast, debug_fast = self._resolver_status_modelo_rapido(aux_result) - status_fast_ok = status_final_fast is not None + self._ultimo_resultado = resultado + self._ultimo_erro = None - mask_corredor = self._extrair_corredor_principal(self.predictions).astype(bool) + return resultado - # --- score de corredor por grid (robusto) --- - score, centers_col, left_col, right_col = self._corridor_score_grid(self.predictions) + # --------------------------------------------------------------------- + # Validação + # --------------------------------------------------------------------- - # --- scanlines para centro/ângulo (telemetria) --- - ys = self._get_scanlines_y(H, grid_rows_y_px, near_is_bottom) - centros, larguras = [], [] + @staticmethod + def _normalizar_config(config: Optional[SegmentacaoConfig | Dict[str, Any]]) -> SegmentacaoConfig: + if config is None: + return SegmentacaoConfig() - if mask_corredor.any(): - for y in ys: - row = mask_corredor[y] - idx = np.flatnonzero(row) - if idx.size == 0: - centros.append((None, y)) - larguras.append(0) - else: - x0, x1 = int(idx[0]), int(idx[-1]) - cx = (x0 + x1) // 2 - w = (x1 - x0 + 1) - centros.append((cx, y)) - larguras.append(int(w)) - else: - # Fallback: usa o centro estimado do GRID (coluna centers_col) - # mapeia coluna -> pixel no centro da célula - cols = max(1, np.max(centers_col)+1) - col_w = max(1, W // max(cols, 1)) - for i, y in enumerate(ys): - j = centers_col[min(i, len(centers_col)-1)] - cx = int((j + 0.5) * col_w) - centros.append((cx, y)) - # tenta derivar largura a partir de left/right_col - lj = left_col[min(i, len(left_col)-1)] - rj = right_col[min(i, len(right_col)-1)] - if lj >= 0 and rj >= 0 and rj > lj: - larguras.append(int((rj - lj + 1) * col_w)) - else: - larguras.append(int(0.4 * W)) # fallback razoável + if isinstance(config, SegmentacaoConfig): + return config - # --- Ângulo do corredor (x = a*y + b) ponderado (perto pesa mais) --- - pts = [(x, y) for (x, y) in centros if x is not None] - ang_out = None - if len(pts) >= 2: - ys_fit = np.array([p[1] for p in pts], dtype=np.float32) - xs_fit = np.array([p[0] for p in pts], dtype=np.float32) - a = self._fit_linha_ponderada(ys_fit, xs_fit, near_is_bottom=near_is_bottom) - if a is not None: - ang_rad = np.arctan(a) - # EMA em graus - deg = float(np.degrees(ang_rad)) - if getattr(self, "_ema_ang", None) is None: - self._ema_ang = deg + if not isinstance(config, dict): + raise TypeError("config deve ser SegmentacaoConfig, dict ou None") + + # Aceita dict direto do config novo. + allowed = set(SegmentacaoConfig.__dataclass_fields__.keys()) + kwargs = {k: v for k, v in config.items() if k in allowed} + return SegmentacaoConfig(**kwargs) + + @staticmethod + def _validar_predictions(predictions: np.ndarray) -> np.ndarray: + if predictions is None: + raise ValueError("predictions=None") + + pred = np.asarray(predictions) + + if pred.ndim == 3: + if pred.shape[0] == 1: + pred = pred[0] + elif pred.shape[-1] == 1: + pred = pred[:, :, 0] else: - alpha = self._ema_alpha_ang - delta = ((deg - self._ema_ang + 180.0) % 360.0) - 180.0 - self._ema_ang = self._ema_ang + alpha * delta - ang_out = round(self._ema_ang, 3) + raise ValueError(f"predictions 3D inválido: shape={pred.shape}") - # --- Erro lateral (% da largura na base) + EMA --- - # pega a primeira scanline (mais perto, conforme near_is_bottom) - base_ix = 0 - if not near_is_bottom: - base_ix = len(centros) - 1 if len(centros) else 0 + if pred.ndim != 2: + raise ValueError(f"predictions precisa ser 2D, veio shape={pred.shape}") - erro_lateral_pct = 0.0 - if centros and centros[base_ix][0] is not None: - x_base, _ = centros[base_ix] - err_px = x_base - cx_img - erro_lateral_pct = (err_px / cx_img) * 100.0 + if pred.size == 0: + raise ValueError("predictions vazio") - if getattr(self, "_ema_lat", None) is None: - self._ema_lat = float(erro_lateral_pct) - else: - self._ema_lat = (1 - self._ema_alpha_lat) * self._ema_lat + self._ema_alpha_lat * float(erro_lateral_pct) - lat_out = round(float(self._ema_lat), 3) + if not np.issubdtype(pred.dtype, np.integer): + pred = pred.astype(np.uint8, copy=False) - if status_fast_ok: - status_final = status_final_fast - status_before = status_before_fast - debug = debug_fast - else: - mask_nav = (self.predictions == ClassesSegmentacao.NAVEGAVEL.value) - status_now, status_final, status_before, debug = self.resolver_status_corredor( - mask_nav=mask_nav, - aux_result=aux_result + return np.ascontiguousarray(pred) + + # --------------------------------------------------------------------- + # Geometria do corredor + # --------------------------------------------------------------------- + + def _scan_corredor( + self, + mask_nav: np.ndarray, + centers_col: np.ndarray, + left_col: np.ndarray, + right_col: np.ndarray, + ) -> Tuple[List[Tuple[Optional[int], int]], List[int], Dict[str, Any]]: + H, W = mask_nav.shape + ys = self._get_scanlines_y(H) + + centros: List[Tuple[Optional[int], int]] = [] + larguras: List[int] = [] + + missing = 0 + prev_cx = self._prev_cx if self._prev_cx is not None else W * 0.5 + + for idx, y in enumerate(ys): + y = int(np.clip(y, 0, H - 1)) + + hint_cx = self._hint_cx_from_grid( + idx=idx, + W=W, + centers_col=centers_col, ) - return { - "timestamp": time.time(), - "height": H, - "width": W, - "erro_angular": ang_out, - "erro_lateral_pct": lat_out, - "status_corredor": status_final.value, - "status_corredor_nome": status_final.name, - "status_corredor_anterior": status_before.value, - "status_corredor_anterior_nome": status_before.name, - "status_corredor_debug": debug, - "centros_corredor": centros, - "larguras_px": larguras, - "confianca": round(float(score), 3), + if hint_cx is None: + hint_cx = prev_cx + + cx, width = self._best_run_in_row( + row=mask_nav[y], + hint_cx=float(hint_cx), + prev_cx=prev_cx, + ) + + if cx is None: + missing += 1 + cx_fallback, width_fallback = self._fallback_run_from_grid( + idx=idx, + W=W, + centers_col=centers_col, + left_col=left_col, + right_col=right_col, + ) + centros.append((cx_fallback, y)) + larguras.append(width_fallback) + + if cx_fallback is not None: + prev_cx = float(cx_fallback) + else: + centros.append((int(cx), y)) + larguras.append(int(width)) + prev_cx = float(cx) + + valid = [c for c, _ in centros if c is not None] + if valid: + self._prev_cx = float(valid[0]) + + debug = { + "n_scanlines": len(ys), + "missing_scanlines": int(missing), + "valid_scanlines": int(len(ys) - missing), } - - def resolver_status_corredor(self, mask_nav: np.ndarray, aux_result=None): - if aux_result and aux_result.get("type") == "label": - conf = float(aux_result.get("label_conf", 0.0)) - label_id = int(aux_result.get("label_id", StatusCarroMapa.Indefinido.value)) - if conf >= 0.70: - status_now = StatusCarroMapa(label_id) - debug = { - "origem_status": "modelo", - "status_modelo": label_id, - "status_modelo_conf": conf, - "heuristica_executada": False, - } + return centros, larguras, debug - self._status_hist.append((status_now, self._now())) - status_final = self._maioria_ultimos() - self._status_final_hist.append(status_final) - status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final + def _get_scanlines_y(self, H: int) -> List[int]: + fracs = self.config.scanline_fracs + ys = [int(np.clip(round(float(f) * (H - 1)), 0, H - 1)) for f in fracs] - return status_now, status_final, status_before, debug - - status_heur, _, _, debug_heur = self.classificar_status_corredor(mask_nav) + # Mantém ordem perto -> longe. + ys = sorted(set(ys), reverse=self.config.near_is_bottom) - status_modelo = None - label_conf = 0.0 - label_probs = None - label_name = None + return ys - if aux_result and aux_result.get("type") == "label": - try: - label_id = int(aux_result.get("label_id")) - label_conf = float(aux_result.get("label_conf", 0.0)) - label_probs = aux_result.get("label_probs") - label_name = aux_result.get("label_name") - status_modelo = StatusCarroMapa(label_id) - except Exception: - status_modelo = None + def _best_run_in_row( + self, + row: np.ndarray, + hint_cx: float, + prev_cx: Optional[float], + ) -> Tuple[Optional[int], int]: + runs = self._true_runs(row) - if status_modelo is not None and label_conf >= 0.70: - status_now = status_modelo - origem = "modelo" - elif status_modelo is not None and label_conf >= 0.45: - status_now = status_modelo - origem = "modelo_baixa_conf" + if not runs: + return None, 0 + + W = row.shape[0] + min_width = max(2, int(W * self.config.min_run_width_frac)) + + best_score = -1e18 + best_cx = None + best_width = 0 + + for x0, x1 in runs: + width = x1 - x0 + 1 + + if width < min_width: + continue + + cx = 0.5 * (x0 + x1) + + width_score = width / max(1.0, W) + center_score = 1.0 - min(1.0, abs(cx - hint_cx) / max(1.0, W * 0.5)) + + prev_score = 0.0 + if prev_cx is not None: + prev_score = 1.0 - min(1.0, abs(cx - prev_cx) / max(1.0, W * 0.5)) + + score = ( + self.config.prefer_width_weight * width_score + + self.config.prefer_center_weight * center_score + + self.config.prefer_prev_weight * prev_score + ) + + if score > best_score: + best_score = score + best_cx = int(round(cx)) + best_width = int(width) + + return best_cx, best_width + + @staticmethod + def _true_runs(row: np.ndarray) -> List[Tuple[int, int]]: + r = np.asarray(row, dtype=np.uint8) + + if r.size == 0: + return [] + + padded = np.pad(r, (1, 1), mode="constant", constant_values=0) + diff = np.diff(padded.astype(np.int16)) + + starts = np.flatnonzero(diff == 1) + ends = np.flatnonzero(diff == -1) - 1 + + return [(int(s), int(e)) for s, e in zip(starts, ends) if e >= s] + + @staticmethod + def _hint_cx_from_grid(idx: int, W: int, centers_col: np.ndarray) -> Optional[int]: + if centers_col is None or len(centers_col) == 0: + return None + + grid_cols = max(1, int(np.max(centers_col)) + 1) + j = int(centers_col[min(idx, len(centers_col) - 1)]) + + if j < 0: + return None + + col_w = W / float(grid_cols) + return int(round((j + 0.5) * col_w)) + + @staticmethod + def _fallback_run_from_grid( + idx: int, + W: int, + centers_col: np.ndarray, + left_col: np.ndarray, + right_col: np.ndarray, + ) -> Tuple[Optional[int], int]: + if centers_col is None or len(centers_col) == 0: + return None, 0 + + grid_cols = max(1, int(np.max(centers_col)) + 1) + col_w = W / float(grid_cols) + + k = min(idx, len(centers_col) - 1) + c = int(centers_col[k]) + + if c < 0: + return None, 0 + + cx = int(round((c + 0.5) * col_w)) + + width = int(round(0.35 * W)) + if left_col is not None and right_col is not None and len(left_col) > k and len(right_col) > k: + l = int(left_col[k]) + r = int(right_col[k]) + if l >= 0 and r > l: + width = int(round((r - l + 1) * col_w)) + + return cx, max(0, width) + + def _calcular_erro_angular(self, centros: List[Tuple[Optional[int], int]], H: int) -> Optional[float]: + pts = [(float(x), float(y)) for x, y in centros if x is not None] + + if len(pts) < 2: + return None + + xs = np.array([p[0] for p in pts], dtype=np.float32) + ys = np.array([p[1] for p in pts], dtype=np.float32) + + # Peso maior nas linhas próximas. + if self.config.near_is_bottom: + weights = np.clip(ys / max(1.0, H - 1), 0.15, 1.0) else: - status_now = status_heur - origem = "heuristica" + weights = np.clip(1.0 - ys / max(1.0, H - 1), 0.15, 1.0) + + a = self._fit_x_por_y(ys, xs, weights) + + if a is None: + return None + + deg = float(np.degrees(np.arctan(a))) + + if self._ema_ang is None: + self._ema_ang = deg + else: + delta = ((deg - self._ema_ang + 180.0) % 360.0) - 180.0 + self._ema_ang += self.config.ema_alpha_ang * delta + + return round(float(self._ema_ang), 3) + + @staticmethod + def _fit_x_por_y(ys: np.ndarray, xs: np.ndarray, weights: np.ndarray) -> Optional[float]: + try: + if len(xs) < 2: + return None + + w = weights.astype(np.float32) + y = ys.astype(np.float32) + x = xs.astype(np.float32) + + sw = float(np.sum(w)) + if sw <= 1e-9: + return None + + y_mean = float(np.sum(w * y) / sw) + x_mean = float(np.sum(w * x) / sw) + + dy = y - y_mean + dx = x - x_mean + + den = float(np.sum(w * dy * dy)) + if den <= 1e-9: + return None + + a = float(np.sum(w * dy * dx) / den) + return a + except Exception: + return None + + def _calcular_erro_lateral(self, centros: List[Tuple[Optional[int], int]], W: int) -> float: + cx_img = W * 0.5 + + base_cx = None + for cx, _y in centros: + if cx is not None: + base_cx = float(cx) + break + + if base_cx is None: + erro = 0.0 + else: + erro = ((base_cx - cx_img) / max(1.0, cx_img)) * 100.0 + + if self._ema_lat is None: + self._ema_lat = float(erro) + else: + self._ema_lat = ( + (1.0 - self.config.ema_alpha_lat) * self._ema_lat + + self.config.ema_alpha_lat * float(erro) + ) + + return round(float(self._ema_lat), 3) + + # --------------------------------------------------------------------- + # Score de corredor por grid + # --------------------------------------------------------------------- + + def _corridor_score_grid(self, pred: np.ndarray): + H, W = pred.shape + rows = max(2, int(self.config.corridor_grid_rows)) + cols = max(5, int(self.config.corridor_grid_cols)) + + row_h = max(1, H // rows) + col_w = max(1, W // cols) + + mask_nao = (pred == self.config.id_nao_navegavel).astype(np.uint8) + integral = cv2.integral(mask_nao) + + frac_nao = np.zeros((rows, cols), dtype=np.float32) + + for i in range(rows): + y0 = i * row_h + y1 = H if i == rows - 1 else min(H, (i + 1) * row_h) + + for j in range(cols): + x0 = j * col_w + x1 = W if j == cols - 1 else min(W, (j + 1) * col_w) + + area = max(1, (y1 - y0) * (x1 - x0)) + s = self._sum_integral(integral, y0, y1, x0, x1) + frac_nao[i, j] = s / float(area) + + centers_col = self._dp_centerline(frac_nao) + left_col, right_col, valid_row = self._walls_from_grid(frac_nao, centers_col) + + score = float(np.mean(valid_row)) if valid_row.size else 0.0 + + debug = { + "rows": int(rows), + "cols": int(cols), + "valid_rows": int(np.count_nonzero(valid_row)), + "score": round(score, 4), + "centers_col": centers_col.astype(int).tolist(), + "left_col": left_col.astype(int).tolist(), + "right_col": right_col.astype(int).tolist(), + } + + return score, centers_col, left_col, right_col, debug + + @staticmethod + def _sum_integral(ii: np.ndarray, y0: int, y1: int, x0: int, x1: int) -> float: + return float(ii[y1, x1] - ii[y0, x1] - ii[y1, x0] + ii[y0, x0]) + + def _dp_centerline(self, frac_nao: np.ndarray) -> np.ndarray: + rows, cols = frac_nao.shape + + costs = np.full((rows, cols), np.inf, dtype=np.float32) + back = np.full((rows, cols), -1, dtype=np.int32) + + cols_idx = np.arange(cols, dtype=np.float32) + + costs[0] = self._smooth1d(frac_nao[0]) + + for i in range(1, rows): + p = self._smooth1d(frac_nao[i]) + prev = costs[i - 1] + + for j in range(cols): + cand = prev + self.config.corridor_jump_penalty * np.abs(cols_idx - float(j)) + kprev = int(np.argmin(cand)) + costs[i, j] = p[j] + cand[kprev] + back[i, j] = kprev + + centers = np.zeros(rows, dtype=np.int32) + centers[-1] = int(np.argmin(costs[-1])) + + for i in range(rows - 2, -1, -1): + centers[i] = int(back[i + 1, centers[i + 1]]) + + return centers + + @staticmethod + def _smooth1d(x: np.ndarray) -> np.ndarray: + if len(x) < 3: + return x.copy() + + y = x.astype(np.float32, copy=True) + y[1:-1] = (x[:-2] + x[1:-1] + x[2:]) / 3.0 + return y + + @staticmethod + def _walls_from_grid(frac_nao: np.ndarray, centers: np.ndarray): + rows, cols = frac_nao.shape + + left_col = np.full(rows, -1, dtype=np.int32) + right_col = np.full(rows, -1, dtype=np.int32) + valid_row = np.zeros(rows, dtype=bool) + + # Perto: laterais podem ser pouco ocupadas. + # Longe: esperamos mais parede/cana lateral. + idx = np.linspace(0.0, 1.0, rows, dtype=np.float32) + lado_min = 0.05 + (0.25 - 0.05) * idx + canal_max = 0.70 + (0.15 - 0.70) * idx + + wall_win = 2 + + for i in range(rows): + c = int(centers[i]) + p = frac_nao[i] + + lf = None + for j in range(c - 1, -1, -1): + l0 = max(0, j - wall_win) + l1 = j + 1 + if float(np.mean(p[l0:l1])) >= float(lado_min[i]): + lf = j + break + + rf = None + for j in range(c + 1, cols): + r0 = j + r1 = min(cols, j + wall_win + 1) + if float(np.mean(p[r0:r1])) >= float(lado_min[i]): + rf = j + break + + canal_ok = p[c] <= canal_max[i] + ok = lf is not None and rf is not None and rf > lf + 2 and canal_ok + + valid_row[i] = bool(ok) + if ok: + left_col[i] = int(lf) + right_col[i] = int(rf) + + return left_col, right_col, valid_row + + # --------------------------------------------------------------------- + # Status do corredor + # --------------------------------------------------------------------- + + def _resolver_status( + self, + pred: np.ndarray, + mask_nav: np.ndarray, + corridor_score: float, + aux_result: Optional[Dict[str, Any]], + ): + model_status, model_conf, model_debug = self._status_from_aux(aux_result) + + heur_status, heur_debug = self._status_heuristico(pred, mask_nav, corridor_score) + + origem = "heuristica" + status_now = heur_status + + if model_status is not None and model_conf >= self.config.model_conf_accept: + status_now = model_status + origem = "modelo" + elif model_status is not None and model_conf >= self.config.model_conf_soft: + # Modelo com média confiança só vence quando a heurística está indefinida. + if heur_status == StatusCarroMapa.Indefinido: + status_now = model_status + origem = "modelo_soft" + else: + status_now = heur_status + origem = "heuristica_com_modelo_soft" self._status_hist.append((status_now, self._now())) - status_final = self._maioria_ultimos() - + status_final = self._majority_status() self._status_final_hist.append(status_final) - status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final + + status_before = ( + self._status_final_hist[0] + if len(self._status_final_hist) > 1 + else status_final + ) debug = { "origem_status": origem, - "status_modelo": status_modelo.value if status_modelo is not None else None, - "status_modelo_nome": status_modelo.name if status_modelo is not None else None, - "status_modelo_label_name": label_name, - "status_modelo_conf": round(label_conf, 4), - "status_modelo_probs": label_probs, - "status_heuristico": status_heur.value, - "status_heuristico_nome": status_heur.name, - "heuristica_debug": debug_heur, + "modelo": model_debug, + "heuristica": heur_debug, } return status_now, status_final, status_before, debug - def _extrair_corredor_principal(self, mask_nav): - H, W = mask_nav.shape - cx_img = W // 2 - - num_labels, labels, stats, _ = cv2.connectedComponentsWithStats(mask_nav, connectivity=8) - if num_labels <= 1: - return np.zeros_like(mask_nav, dtype=np.uint8) - - best_i, best_score = -1, -1e9 - - # pesos do score - w_area = 1.0 - w_center = 1.0 - w_bottom = 1.0 - w_vertical = 0.5 - w_prev = 0.75 - - prev_cx = self._prev_cx - - for i in range(1, num_labels): # 0 = fundo - x, y, w, h, area = stats[i] - if area < 50: # lixo - continue - - cx = x + w // 2 - # normalizações 0..1 - area_n = area / float(H * W) - center_n = 1.0 - min(1.0, abs(cx - cx_img) / (W * 0.5)) - vertical = h / max(1.0, w) # alongamento vertical - bottom_touch = 1.0 if (y + h >= H - 2) else 0.0 - - prev_bias = 0.0 - if prev_cx is not None: - prev_bias = 1.0 - min(1.0, abs(cx - prev_cx) / (W * 0.5)) - - score = (w_area*area_n + - w_center*center_n + - w_bottom*bottom_touch + - w_vertical*vertical + - w_prev*prev_bias) - - if score > best_score: - best_score = score - best_i = i - - self._prev_cx = None - if best_i == -1: - return np.zeros_like(mask_nav, dtype=np.uint8) - - # guarda cx do blob vencedor pro próximo frame - bx, by, bw, bh, _ = stats[best_i] - self._prev_cx = bx + bw // 2 - self._prev_y0 = by - - return (labels == best_i).astype(np.uint8) - - def _corridor_score_grid(self, mask_classes, rows=6, cols=21): - H, W = mask_classes.shape - row_h = H // rows - col_w = W // cols - - mask_nao = (mask_classes == ClassesSegmentacao.NAONAVEGAVEL.value).astype(np.uint8) - ii = cv2.integral(mask_nao) # shape (H+1, W+1) - - frac = np.zeros((rows, cols), np.float32) - - for i in range(rows): - y0 = i * row_h - y1 = H if i == rows - 1 else (i + 1) * row_h - - for j in range(cols): - x0 = j * col_w - x1 = W if j == cols - 1 else (j + 1) * col_w - - area = max(1, (y1 - y0) * (x1 - x0)) - s = self._sum_integral(ii, y0, y1, x0, x1) - frac[i, j] = s / float(area) - - # limiares por faixa (modelo em “A”: mais chão embaixo, mais cana no topo) - near_lado_min, near_canal_max = 0.05, 0.70 - far_lado_min, far_canal_max = 0.25, 0.15 - idx = np.linspace(0, 1, rows) # 0=embaixo (perto), 1=topo (longe) - lado_min = near_lado_min + (far_lado_min - near_lado_min) * idx - canal_max = near_canal_max + (far_canal_max - near_canal_max) * idx - - # centro do canal por DP (argmin com suavidade vertical) - def smooth1d(x): - if len(x) < 3: return x.copy() - y = x.copy() - y[1:-1] = (x[:-2] + x[1:-1] + x[2:]) / 3.0 - return y - - costs = np.full((rows, cols), np.inf, np.float32) - back = np.full((rows, cols), -1, np.int32) - cols_idx = np.arange(cols) - jump_penalty = 0.6 - - p = smooth1d(frac[0]); costs[0] = p - for i in range(1, rows): - p = smooth1d(frac[i]) - for j in range(cols): - cand = costs[i-1] + jump_penalty * np.abs(cols_idx - j) - kprev = np.argmin(cand) - costs[i, j] = p[j] + cand[kprev] - back[i, j] = kprev - - centers = np.zeros(rows, np.int32) - centers[-1] = int(np.argmin(costs[-1])) - for i in range(rows-2, -1, -1): - centers[i] = int(back[i+1, centers[i+1]]) - - # encontra primeiras paredes ao redor do canal - wall_win = 2 - valid_row = np.zeros(rows, bool) - left_col = np.full(rows, -1, np.int32) - right_col = np.full(rows, -1, np.int32) - - for i in range(rows): - c = centers[i]; p = frac[i] - - # esquerda: primeira janela com cana suficiente - lf = None - for j in range(c-1, -1, -1): - l0 = max(0, j-wall_win); l1 = j - if p[l0:l1+1].mean() >= lado_min[i]: - lf = j; break - - # direita: idem - rf = None - for j in range(c+1, cols): - r0 = j; r1 = min(cols-1, j+wall_win) - if p[r0:r1+1].mean() >= lado_min[i]: - rf = j; break - - canal_ok = (p[c] <= canal_max[i]) - ok = (lf is not None and rf is not None and canal_ok and rf > lf+2) - valid_row[i] = ok - if ok: - left_col[i], right_col[i] = lf, rf - - score = valid_row.mean() # 0..1 - return float(score), centers, left_col, right_col - - def _get_scanlines_y(self, H, grid_rows_y_px=None, near_is_bottom=True): - if grid_rows_y_px and len(grid_rows_y_px) >= 5: - ys = [int(np.clip(y, 0, H-1)) for y in grid_rows_y_px] - # reordena do "perto" para o "longe" - ys = sorted(ys, reverse=near_is_bottom) - return ys - # fallback por frações - fracs = [0.98, 0.85, 0.70, 0.55, 0.40, 0.25, 0.10] - return [min(H-1, max(0, int(H*f))) for f in fracs] - - def classificar_status_corredor(self, mask_nav: np.ndarray): - """ - mask_nav: (H,W) com 1 = navegável, 0 = não-navegável - - Retorna: - status_now : StatusCarroMapa (instantâneo deste frame) - status_final : StatusCarroMapa (com histerese) - probs : dict[StatusCarroMapa, float] (one-hot) - debug : métricas pra log/diagnóstico - """ - if mask_nav is None or mask_nav.size == 0: - status_now = StatusCarroMapa.Indefinido - self._status_hist.append((status_now, self._now())) - status_final = self._maioria_ultimos() - self._status_final_hist.append(status_final) - status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final - return status_now, status_final, status_before, { - "nav_global": 0.0, - "nav_near": 0.0, - "nav_mid": 0.0, - "nav_far": 0.0, + def _status_from_aux(self, aux_result: Optional[Dict[str, Any]]): + if not aux_result or aux_result.get("type") != "label": + return None, 0.0, { + "disponivel": False, + "motivo": "sem_aux_result", } - H, W = mask_nav.shape - nav = (mask_nav > 0).astype(np.float32) + try: + label_id = int(aux_result.get("label_id", StatusCarroMapa.Indefinido.value)) + conf = float(aux_result.get("label_conf", 0.0)) + status = StatusCarroMapa(label_id) - def faixa_mean(y0: int, y1: int) -> float: - fatia = nav[y0:y1, :] - if fatia.size == 0: + return status, conf, { + "disponivel": True, + "status": int(status.value), + "status_nome": str(status.name), + "label_name": aux_result.get("label_name"), + "conf": round(conf, 4), + } + + except Exception as e: + return None, 0.0, { + "disponivel": False, + "motivo": f"aux_invalido: {e}", + } + + def _status_heuristico( + self, + pred: np.ndarray, + mask_nav: np.ndarray, + corridor_score: float, + ): + H, W = pred.shape + nav = mask_nav.astype(np.float32) + + def band_mean(y0_frac: float, y1_frac: float) -> float: + y0 = int(np.clip(round(y0_frac * H), 0, H)) + y1 = int(np.clip(round(y1_frac * H), 0, H)) + if y1 <= y0: return 0.0 - return float(fatia.mean()) + return float(np.mean(nav[y0:y1])) - # 3 faixas verticais: far (topo), mid (meio), near (embaixo) - y_far_top = 0 - y_far_bot = int(0.25 * H) - y_mid_top = y_far_bot - y_mid_bot = int(0.5 * H) - y_near_top = y_mid_bot - y_near_bot = H + nav_far = band_mean(0.00, 0.28) + nav_mid = band_mean(0.28, 0.58) + nav_near = band_mean(0.58, 1.00) + nav_global = float(np.mean(nav)) - nav_far = faixa_mean(y_far_top, y_far_bot) - nav_mid = faixa_mean(y_mid_top, y_mid_bot) - nav_near = faixa_mean(y_near_top, y_near_bot) - nav_global = float(nav.mean()) + d_near_far = nav_near - nav_far + d_far_mid = nav_far - nav_mid - # diferenças entre faixas (pra medir quão "desbalanceado" está) - d_nm = abs(nav_near - nav_mid) - d_mf = abs(nav_mid - nav_far) - d_nf = abs(nav_near - nav_far) - max_delta = max(d_nm, d_mf, d_nf) - - # ---- Blobs 2D na região distante (parede esquerda x direita) ---- - y_blob_top = 0 - y_blob_bot = int(0.35 * H) # um pouco mais profundo que o nav_far - - faixa_obs = (mask_nav[y_blob_top:y_blob_bot, :] == 0).astype(np.uint8) # 1 = obstáculo - - H_blob, W_blob = faixa_obs.shape - num_blobs_far = 0 - corridor_nav_far = 0.0 - corridor_width_frac = 0.0 - - if H_blob > 0 and W_blob > 0 and faixa_obs.max() > 0: - # connectedComponents espera 0/255 - faixa_obs_bin = (faixa_obs * 255).astype(np.uint8) - - num_labels, labels = cv2.connectedComponents(faixa_obs_bin) - - # ignora blobs muito pequenos (ruído) - MIN_AREA = 0.005 * H_blob * W_blob # 0.5% da área da faixa - blobs = [] - - for label in range(1, num_labels): # 0 é o fundo - ys, xs = np.where(labels == label) - area = len(xs) - if area < MIN_AREA: - continue - - x_min, x_max = xs.min(), xs.max() - y_min, y_max = ys.min(), ys.max() - blobs.append({ - "area": area, - "bbox": (x_min, y_min, x_max, y_max), - "x_center": float(xs.mean()), - }) - - num_blobs_far = len(blobs) - - if num_blobs_far >= 2: - # ordena da parede mais à esquerda pra mais à direita - blobs_sorted = sorted(blobs, key=lambda b: b["x_center"]) - left_blob = blobs_sorted[0] - right_blob = blobs_sorted[-1] - - # corredor é a região entre x_max da esquerda e x_min da direita - x_left = left_blob["bbox"][2] + 1 # x_max (inclusivo) -> +1 pra slice - x_right = right_blob["bbox"][0] # x_min - - if x_right > x_left: - corridor_width = x_right - x_left - corridor_width_frac = corridor_width / float(W) - - faixa_nav_corridor = (mask_nav[y_blob_top:y_blob_bot, x_left:x_right] > 0).astype(np.float32) - if faixa_nav_corridor.size > 0: - corridor_nav_far = float(faixa_nav_corridor.mean()) - - # ===== Limiares (podemos tunar depois) ===== - THR_BLOCKED = 0.30 # global bem baixo -> quase sem caminho - THR_OPEN = 0.90 # global bem alto -> mundo aberto - THR_DELTA_COR = 0.15 # diferença "significativa" entre faixas - THR_COR_FAR_LARGO = 0.65 # far ainda relativamente alto em corredor largo - THR_OPEN_MUITO_LIMPO = 0.90 # quase tudo navegável - THR_DELTA_OBST_PEQ = 0.20 # desbalance vertical máximo para considerar "campo aberto com obstáculo pequeno" - MIN_CORRIDOR_WIDTH_FRAC = 0.10 # corredor tem que ter pelo menos ~10% da largura - MAX_CORRIDOR_WIDTH_FRAC = 0.70 # evita chamar de corredor quando é um "campo" gigante - - # 1) Parado: quase não há caminho à frente (meio+frente mortos) cond_parado = ( - nav_global <= THR_BLOCKED - and nav_mid < 0.20 - and nav_far < 0.10 - ) - - # 2) Direcionando: mundo aberto, sem corredor marcado - # 2.1 base: tudo alto e muito homogêneo - cond_direcionando_base = ( - nav_global >= THR_OPEN - and max_delta < 0.10 - ) - - # 2.2 modo "campo aberto com obstáculo pequeno": - # quase tudo navegável, e até o far é bem alto - cond_direcionando_obst_peq = ( - nav_global >= THR_OPEN_MUITO_LIMPO and # >= 0.90 - nav_near >= 0.95 and - nav_mid >= 0.80 and # um pouco mais permissivo - nav_far >= 0.50 and # aceita far um pouco mais fechado - num_blobs_far <= 1 # no máximo UMA parede grande - ) - - # 2.3 modo "campo aberto com parede na frente": - # chão bem navegável perto, sem corredor definido, e FAR quase todo bloqueado - cond_direcionando_frente_fe_chada = ( - nav_near >= 0.80 and # perto bem aberto - nav_mid >= 0.30 and # meio ainda razoável - nav_far <= 0.10 and # topo praticamente bloqueado (parede) - nav_global >= 0.50 and # ainda tem bastante área navegável no frame - num_blobs_far <= 1 # no máximo uma "parede", nada de corredor - ) - - # 2.3 modo "campo aberto com borda lateral": - # cena razoavelmente aberta, uma parede forte de um lado, mas sem corredor fechado - cond_direcionando_borda_lateral = ( - nav_global >= 0.60 and # já tem boa área navegável - nav_near >= 0.70 and - nav_mid >= 0.50 and - nav_far >= 0.40 and # far não está "morrendo", só mais sujo - nav_far <= 0.80 and # não é mundão 100% limpo - num_blobs_far == 1 # exatamente UMA parede grande - ) - - cond_direcionando_aberto = ( - nav_global >= 0.75 and - nav_near >= 0.70 and - nav_mid >= 0.70 and - nav_far >= 0.70 and - max_delta <= 0.12 and - num_blobs_far <= 1 + nav_global <= self.config.thr_parado_global + and nav_near < 0.35 + and nav_mid < 0.30 ) cond_direcionando = ( - cond_direcionando_base - or cond_direcionando_obst_peq - or cond_direcionando_frente_fe_chada - or cond_direcionando_borda_lateral - or cond_direcionando_aberto - ) - - # 3) CaminhandoRua: dentro do corredor "clássico" - cond_caminhando_base = ( - nav_near >= 0.55 and - nav_mid >= 0.25 and - nav_far <= 0.50 and - (nav_near - nav_far) >= 0.20 and - num_blobs_far >= 2 # precisa de DUAS paredes - ) - - # 3.1 CaminhandoRua em corredor mais largo, com parede só de um lado - # enquadra bem os casos: - # nav_global ~0.66–0.77, near ~0.75–0.88, mid ~0.57–0.70, far ~0.56–0.63 - cond_caminhando_largo = ( - nav_global >= 0.60 and - nav_near >= 0.75 and - nav_mid >= 0.50 and - nav_far >= 0.50 and - nav_far <= THR_COR_FAR_LARGO and - (nav_near - nav_far) >= THR_DELTA_COR and - num_blobs_far >= 2 # corredor largo, mas ainda corredor - ) - - cond_caminhando_multi_corredores = ( - nav_global >= 0.50 and # tem chão suficiente - nav_mid >= 0.55 and # meio bem limpo - nav_near >= 0.50 and # perto também ok - num_blobs_far >= 2 and # pelo menos duas "paredes" - corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and - corridor_width_frac <= MAX_CORRIDOR_WIDTH_FRAC and - corridor_nav_far >= 0.55 # corredor entre paredes bem navegável + nav_global >= self.config.thr_direcionando_global + and nav_near >= 0.75 + and nav_mid >= 0.65 + and corridor_score < 0.65 ) cond_caminhando = ( - cond_caminhando_base - or cond_caminhando_largo - or cond_caminhando_multi_corredores + corridor_score >= self.config.thr_caminhando_score + and nav_near >= 0.45 + and nav_mid >= 0.25 ) cond_entrando = ( - nav_global >= 0.75 and - nav_near >= 0.90 and - nav_mid >= 0.60 and - nav_far >= 0.40 and - nav_far <= 0.85 and - (nav_near - nav_far) >= 0.10 and # far mais fechado que near - num_blobs_far >= 2 and # duas paredes detectadas - corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and - corridor_nav_far >= 0.60 # corredor entre as paredes bem navegável + nav_near >= self.config.thr_entrando_near + and nav_mid >= 0.50 + and d_near_far >= 0.15 + and corridor_score >= 0.35 ) - # 5) SaindoRua - cond_saindo_1 = ( - nav_near >= 0.50 and - nav_mid >= 0.25 and - nav_far >= 0.55 and - (nav_far - nav_mid) >= 0.10 and - nav_far >= nav_near - 0.15 + cond_saindo = ( + nav_far >= self.config.thr_saindo_far + and nav_mid >= 0.45 + and d_far_mid >= 0.08 ) - cond_saindo_2 = ( - nav_global >= 0.75 and - nav_near >= 0.70 and - nav_mid >= 0.60 and - nav_far >= 0.75 and - nav_far >= nav_mid - ) - - cond_saindo = cond_saindo_1 or cond_saindo_2 - - # ===== Decisão (ordem importa!) ===== if cond_parado: status = StatusCarroMapa.Parado elif cond_direcionando: @@ -718,216 +805,93 @@ class SegmentacaoManager: status = StatusCarroMapa.Indefinido debug = { - "nav_global": nav_global, - "nav_near": nav_near, - "nav_mid": nav_mid, - "nav_far": nav_far, - "d_nm": d_nm, - "d_mf": d_mf, - "d_nf": d_nf, - "max_delta": max_delta, - "num_blobs_far": num_blobs_far, - "corridor_nav_far": corridor_nav_far, - "corridor_width_frac": corridor_width_frac, - "cond_parado": cond_parado, - "cond_direcionando_base": cond_direcionando_base, - "cond_direcionando_obst_peq": cond_direcionando_obst_peq, - "cond_caminhando_base": cond_caminhando_base, - "cond_caminhando_largo": cond_caminhando_largo, - "cond_entrando": cond_entrando, - "cond_saindo": cond_saindo, - "THR_BLOCKED": THR_BLOCKED, - "THR_OPEN": THR_OPEN, - "THR_DELTA_COR": THR_DELTA_COR, - "THR_COR_FAR_LARGO": THR_COR_FAR_LARGO, - "THR_OPEN_MUITO_LIMPO": THR_OPEN_MUITO_LIMPO, + "status": int(status.value), + "status_nome": str(status.name), + "nav_global": round(nav_global, 4), + "nav_near": round(nav_near, 4), + "nav_mid": round(nav_mid, 4), + "nav_far": round(nav_far, 4), + "corridor_score": round(float(corridor_score), 4), + "d_near_far": round(float(d_near_far), 4), + "d_far_mid": round(float(d_far_mid), 4), + "cond_parado": bool(cond_parado), + "cond_direcionando": bool(cond_direcionando), + "cond_saindo": bool(cond_saindo), + "cond_entrando": bool(cond_entrando), + "cond_caminhando": bool(cond_caminhando), } - status_now = status - #print(status_now.name, debug) + return status, debug - # Histerese temporal: mantém teu esquema de histórico - self._status_hist.append((status_now, self._now())) - status_final = self._maioria_ultimos() - self._status_final_hist.append(status_final) - status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final - - return status_now, status_final, status_before, debug - @staticmethod - def _now(): - # monotonic evita saltos de relógio + def _now() -> float: return time.monotonic() - def _bool_hysteresis(self, prev: bool, x: float, thr_on: float, thr_off: float) -> bool: - return (x >= (thr_off if prev else thr_on)) + def _majority_status(self, janela_s: Optional[float] = None) -> StatusCarroMapa: + J = self.config.status_window_s if janela_s is None else float(janela_s) + now = self._now() - def _maioria_ultimos(self, janela_s: float | None = None) -> StatusCarroMapa: - """Maioria ponderada pelos últimos 'janela_s' segundos. - Se 'janela_s' None → usa self.janela_s.""" - J = self.janela_s if janela_s is None else float(janela_s) - t_now = self._now() - - # 1) limpa itens FORA da janela - while self._status_hist and (t_now - self._status_hist[0][1] > J): + while self._status_hist and (now - self._status_hist[0][1] > J): self._status_hist.popleft() if not self._status_hist: - # fallback razoável return StatusCarroMapa.Direcionando - # 2) maioria simples (pode trocar por peso exponencial se quiser) - cont = {} - for st, _t in self._status_hist: - cont[st] = cont.get(st, 0) + 1 + counts: Dict[StatusCarroMapa, int] = {} - # regra de desempate: prioriza estado mais recente em caso de empate - top_freq = max(cont.values()) - empatados = [st for st, c in cont.items() if c == top_freq] - if len(empatados) == 1: - return empatados[0] + for status, _t in self._status_hist: + counts[status] = counts.get(status, 0) + 1 + + top = max(counts.values()) + tied = [status for status, count in counts.items() if count == top] + + if len(tied) == 1: + return tied[0] + + # Desempate: status mais recente. + for status, _t in reversed(self._status_hist): + if status in tied: + return status + + return StatusCarroMapa.Direcionando + + # --------------------------------------------------------------------- + # Suavização + # --------------------------------------------------------------------- + + def _suavizar_confianca(self, score: float) -> float: + score = float(np.clip(score, 0.0, 1.0)) + + if self._ema_conf is None: + self._ema_conf = score else: - # desempata olhando do fim pro início (mais recente primeiro) - for st, _t in reversed(self._status_hist): - if st in empatados: - return st + self._ema_conf = ( + (1.0 - self.config.ema_alpha_conf) * self._ema_conf + + self.config.ema_alpha_conf * score + ) + return float(np.clip(self._ema_conf, 0.0, 1.0)) - def largura_robo_px_por_distancia(self, d_m, largura_robo_m, largura_frame_px, fov_h_graus): - fov_h_rad = np.radians(fov_h_graus) - largura_real_visivel_m = 2.0 * d_m * np.tan(fov_h_rad / 2.0) + # --------------------------------------------------------------------- + # Utilitários externos + # --------------------------------------------------------------------- - if largura_real_visivel_m <= 1e-6: - return None + def reset_estado_temporal(self): + self._status_hist.clear() + self._status_final_hist.clear() - frac = largura_robo_m / largura_real_visivel_m - frac = np.clip(frac, 0.0, 1.2) # evita exagero visual + self._ema_ang = None + self._ema_lat = None + self._ema_conf = None + self._prev_cx = None - return int(frac * largura_frame_px) + self._ultimo_resultado = None + self._ultimo_erro = None - def display_segmentation_debug(self, frame, largura_robo_m, grid_ref, show: bool = True): - try: - if (frame is None): - return None - - original = cv2.resize(frame, self.resolucao) - - seg_color = np.zeros_like(original) - for class_id in range(len(self.color_map)): - seg_color[self.predictions == class_id] = self.lut[class_id] - - overlay = cv2.addWeighted(original, 0.5, seg_color, 0.5, 0) - - centros_corredor = self.dados_visuais.get("centros_corredor") - - # 1. Linha do centro do corredor + largura do robô - if centros_corredor and len(centros_corredor) >= 2: - for ponto in centros_corredor: - x, y = ponto - if x is None or y is None: continue - cv2.circle(overlay, ponto, 4, (0, 255, 255), -1) - for i in range(len(centros_corredor) - 1): - cv2.line(overlay, centros_corredor[i], centros_corredor[i + 1], (0, 255, 255), 2) - H, W = overlay.shape[:2] - for idx, (x, y) in enumerate(centros_corredor): - if x is None or y is None: - continue - # mapear idx -> distância real - if idx >= len(grid_ref): - continue - d_m = grid_ref[len(grid_ref) - 1 - idx] - largura_px = self.largura_robo_px_por_distancia(d_m, largura_robo_m=largura_robo_m, largura_frame_px=W, fov_h_graus=69.0) - if largura_px is None: - continue - cv2.line(overlay, (x - largura_px // 2, y), (x + largura_px // 2, y), (255, 0, 255), 1) - - # 4. Texto de métricas - if self.dados_visuais: - erro_lateral_pct = self.dados_visuais["erro_lateral_pct"] - erro_angular = self.dados_visuais["erro_angular"] if self.dados_visuais["erro_angular"] else 0 - texto = [ - f"Erro angular: {erro_angular:.2f} graus", - f"Erro lateral: {erro_lateral_pct:.2f} %", - f"Status: {StatusCarroMapa(self.dados_visuais['status_corredor']).name}" - ] - for i, t in enumerate(texto): - cv2.putText(overlay, t, (10, 25 + i * 25), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) - - # 5. Legenda das classes - legenda_inicio_y = 140 - for i, _ in enumerate(self.color_map): - nome = self.classes[i] - pos_y = legenda_inicio_y + i * 30 - cor_bgr = tuple(int(c) for c in self.lut[i]) - cv2.rectangle(overlay, (10, pos_y - 15), (30, pos_y + 5), cor_bgr, -1) - cv2.putText(overlay, nome, (40, pos_y + 2), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,255,255), 1, cv2.LINE_AA) - - # Mostrar - if (show): - cv2.imshow("Segmentacao - Overlay", overlay) - cv2.waitKey(1) - #cv2.destroyAllWindows() - - return overlay - except Exception as e: - print(f"Erro ao gerar display_segmentation_debug: {e}") - - - def _resolver_status_modelo_rapido(self, aux_result): - if not aux_result or aux_result.get("type") != "label": - return None, None, None, None - - conf = float(aux_result.get("label_conf", 0.0)) - label_id = int(aux_result.get("label_id", StatusCarroMapa.Indefinido.value)) - - if conf < 0.70: - return None, None, None, None - - status_now = StatusCarroMapa(label_id) - - self._status_hist.append((status_now, self._now())) - status_final = self._maioria_ultimos() - self._status_final_hist.append(status_final) - status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final - - debug = None - if self.gerar_debug_status: - debug = { - "origem_status": "modelo_fast", - "status_modelo": label_id, - "status_modelo_conf": conf, - "heuristica_executada": False, - } - - return status_now, status_final, status_before, debug - - def _fit_linha_ponderada(self, ys_fit, xs_fit, near_is_bottom=True): - n = len(ys_fit) - if n < 2: - return None - - if near_is_bottom: - w = np.linspace(2.0, 1.0, n, dtype=np.float32) - else: - w = np.linspace(1.0, 2.0, n, dtype=np.float32) - - y = ys_fit.astype(np.float32) - x = xs_fit.astype(np.float32) - - sw = np.sum(w) - y_mean = np.sum(w * y) / max(sw, 1e-6) - x_mean = np.sum(w * x) / max(sw, 1e-6) - - dy = y - y_mean - dx = x - x_mean - - denom = np.sum(w * dy * dy) - if abs(denom) < 1e-6: - return 0.0 - - a = np.sum(w * dy * dx) / denom - return float(a) - - def _sum_integral(self, ii, y0, y1, x0, x1): - return ii[y1, x1] - ii[y0, x1] - ii[y1, x0] + ii[y0, x0] + @property + def ultimo_resultado(self) -> Optional[Dict[str, Any]]: + return self._ultimo_resultado + @property + def ultimo_erro(self) -> Optional[str]: + return self._ultimo_erro \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_debug_renderer.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_debug_renderer.py new file mode 100644 index 000000000..66b0d0e56 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_debug_renderer.py @@ -0,0 +1,285 @@ +from __future__ import annotations + +from typing import Any, Dict, Optional, Sequence, Tuple + +import cv2 +import numpy as np + +from shared.enums import TipoFrameCamera +from shared.utils import converter_mask_ids_para_bgr +from visual_worker.processamento.costmap_fuser import unpack_snapshot +from visual_worker.utils import gerar_heatmap + + +class VisualDebugRenderer: + def __init__(self, color_map=None, preview_size=(1280, 720)): + self.color_map = color_map + self.preview_size = tuple(preview_size) + + self._ultimo_preview_ts = 0.0 + self._ultimo_rgb = None + self._ultimo_seg = None + self._ultimo_overlay = None + self._ultimo_heatmap = None + self._ultimo_grid = None + self._ultimo_dets = None + + def get_selected_frame( + self, + frame_type: TipoFrameCamera, + rgb_frame=None, + pred_ids=None, + depth_frame=None, + detections=None, + snapshot=None, + camera_params=None, + alpha=0.50, + ): + if frame_type == TipoFrameCamera.Rgb: + return self._resize_rgb(rgb_frame) + + if frame_type == TipoFrameCamera.Segmentacao: + return self.build_segmentation_preview(pred_ids) + + if frame_type == TipoFrameCamera.Overlay: + return self.build_overlay(rgb_frame, pred_ids, alpha=alpha) + + if frame_type == TipoFrameCamera.Heatmap: + distancia_maxima = 10000 + if camera_params: + distancia_maxima = camera_params.get("distancia_maxima", distancia_maxima) + return self.build_heatmap(depth_frame, distancia_maxima=distancia_maxima) + + if frame_type == TipoFrameCamera.MatrizCusto: + return self.build_costmap_debug(rgb_frame, snapshot) + + if frame_type == TipoFrameCamera.Deteccoes: + return self.build_detection_overlay(rgb_frame, detections) + + if frame_type == TipoFrameCamera.Debug: + overlay = self.build_overlay(rgb_frame, pred_ids, alpha=alpha) + grid = self.build_costmap_debug(rgb_frame, snapshot) + return grid if grid is not None else overlay + + return None + + def _resize_rgb(self, rgb_frame): + if rgb_frame is None: + return None + + img = rgb_frame.copy() + + if img.shape[1::-1] != self.preview_size: + img = cv2.resize(img, self.preview_size, interpolation=cv2.INTER_AREA) + + return img + + def build_segmentation_preview(self, pred_ids): + if pred_ids is None or self.color_map is None: + return None + + pred_ids = np.asarray(pred_ids) + + if pred_ids.ndim == 3 and pred_ids.shape[-1] == 1: + pred_ids = pred_ids[..., 0] + + seg_bgr = converter_mask_ids_para_bgr(pred_ids, self.color_map) + + if seg_bgr.shape[1::-1] != self.preview_size: + seg_bgr = cv2.resize(seg_bgr, self.preview_size, interpolation=cv2.INTER_NEAREST) + + self._ultimo_seg = seg_bgr + return seg_bgr + + def build_overlay(self, rgb_frame, pred_ids, alpha=0.50): + if rgb_frame is None or pred_ids is None or self.color_map is None: + return None + + rgb = self._resize_rgb(rgb_frame) + seg = self.build_segmentation_preview(pred_ids) + + if rgb is None or seg is None: + return None + + a = float(np.clip(alpha, 0.0, 1.0)) + overlay = cv2.addWeighted(rgb, 1.0 - a, seg, a, 0) + + self._ultimo_overlay = overlay + return overlay + + def build_heatmap(self, depth_frame, distancia_maxima=10000): + if depth_frame is None: + return None + + heatmap = gerar_heatmap(depth_frame, distancia_maxima) + + if heatmap.shape[1::-1] != self.preview_size: + heatmap = cv2.resize(heatmap, self.preview_size, interpolation=cv2.INTER_AREA) + + self._ultimo_heatmap = heatmap + return heatmap + + def build_detection_overlay(self, rgb_frame, dets, conf_thr=0.5): + if rgb_frame is None: + return None + + img = self._resize_rgb(rgb_frame) + + if img is None: + return None + + if not dets: + self._ultimo_dets = img + return img + + H, W = img.shape[:2] + + palette = [ + (255, 56, 56), (255, 157, 151), (72, 249, 10), + (0, 255, 0), (0, 0, 255), (255, 0, 255), + (0, 255, 255), (255, 191, 0), (52, 148, 230), + (147, 112, 219), + ] + + for d in dets: + if float(d.get("conf", 0.0)) < conf_thr: + continue + + bbox = d.get("bbox_norm") + bbox_full = d.get("bbox_full") or d.get("bbox_px") + + if bbox_full: + x0, y0, x1, y1 = bbox_full + + # Assume origem 1920x1080 quando bbox vem do frame cheio. + sx = W / 1920.0 + sy = H / 1080.0 + + x0 = int(round(x0 * sx)) + x1 = int(round(x1 * sx)) + y0 = int(round(y0 * sy)) + y1 = int(round(y1 * sy)) + + elif bbox: + x0n, y0n, x1n, y1n = bbox + x0 = int(round(x0n * W)) + x1 = int(round(x1n * W)) + y0 = int(round(y0n * H)) + y1 = int(round(y1n * H)) + else: + continue + + x0 = max(0, min(W - 1, x0)) + x1 = max(0, min(W - 1, x1)) + y0 = max(0, min(H - 1, y0)) + y1 = max(0, min(H - 1, y1)) + + if x1 <= x0 or y1 <= y0: + continue + + lid = int(d.get("label_id", -1)) + color = palette[lid % len(palette)] if lid >= 0 else (0, 255, 0) + + cv2.rectangle(img, (x0, y0), (x1, y1), color, 2) + + label = d.get("label") or f"id:{lid}" + conf = float(d.get("conf", 0.0)) + txt = f"{label} {conf:.2f}" + + dist = d.get("distancia_m") + if dist is not None: + txt += f" {float(dist):.2f}m" + + self._put_label(img, txt, x0, y0, color) + + self._ultimo_dets = img + return img + + def build_costmap_debug(self, rgb_frame, snapshot, alpha=0.35): + if rgb_frame is None or snapshot is None: + return None + + try: + img = self._resize_rgb(rgb_frame) + + custo, conf, anom, nav = unpack_snapshot(snapshot) + + mask = self._colorize_costmap(anom, custo, conf) + mask = cv2.resize(mask, self.preview_size, interpolation=cv2.INTER_NEAREST) + + vis = cv2.addWeighted(img, 1.0, mask, alpha, 0) + + metrics = snapshot.get("block", {}) or {} + blocked = bool(metrics.get("blocked", False)) + reason = str(metrics.get("reason", "free")) + d_obs = metrics.get("d_obs_true_min_m") + + status_txt = f"{'PARAR' if blocked else 'LIVRE'} | {reason}" + if d_obs is not None: + status_txt += f" | d={float(d_obs):.2f}m" + + color = (0, 0, 255) if blocked else (0, 220, 0) + + cv2.putText( + vis, + status_txt, + (20, 35), + cv2.FONT_HERSHEY_SIMPLEX, + 0.8, + (0, 0, 0), + 4, + cv2.LINE_AA, + ) + cv2.putText( + vis, + status_txt, + (20, 35), + cv2.FONT_HERSHEY_SIMPLEX, + 0.8, + color, + 2, + cv2.LINE_AA, + ) + + self._ultimo_grid = vis + return vis + + except Exception: + return None + + @staticmethod + def _colorize_costmap(anom_f, custo_f, conf_f, thr_anom=0.50, thr_cost=0.65, thr_conf=0.35): + H, W = anom_f.shape + over = np.zeros((H, W, 3), np.uint8) + + masks = [ + (anom_f >= thr_anom, (255, 0, 255)), + (custo_f >= thr_cost, (0, 165, 255)), + (conf_f < thr_conf, (255, 0, 0)), + ] + + for mask, bgr in masks: + if np.any(mask): + tmp = over[mask].astype(np.int16) + tmp += np.array(bgr, dtype=np.int16) + np.clip(tmp, 0, 255, out=tmp) + over[mask] = tmp.astype(np.uint8) + + return over + + @staticmethod + def _put_label(img, text, x, y, bg): + (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) + + y0 = max(0, y - th - 6) + cv2.rectangle(img, (x, y0), (x + tw + 6, y), bg, -1) + cv2.putText( + img, + text, + (x + 3, y - 4), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (0, 0, 0), + 1, + cv2.LINE_AA, + ) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_grid_builder.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_grid_builder.py new file mode 100644 index 000000000..04db412a4 --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_grid_builder.py @@ -0,0 +1,456 @@ +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Dict, Optional, Sequence, Tuple + +import cv2 +import numpy as np + +from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao + + +@dataclass +class GridGeometryConfig: + grid_shape: Tuple[int, int] = (15, 10) # (cols, rows) + camera_pitch_deg: float = 28.91 + camera_height_m: float = 0.74 + fov_v_deg: float = 43.28 + pitch_gain: float = 1.0 + pitch_limit_deg: float = 10.0 + min_dist_m: float = 0.2 + max_dist_m: float = 20.0 + + +@dataclass +class DetectionGridConfig: + w4: float = 0.18 + thr_det_soft: float = 0.45 + min_cell_coverage: float = 0.10 + min_det_conf: float = 0.45 + class_weights: Dict[str, float] = field(default_factory=lambda: { + "person": 1.0, + "dog": 0.7, + "cat": 0.5, + }) + veto_labels: set[str] = field(default_factory=lambda: {"person"}) + combine: str = "max" + conf_drop_alpha: float = 0.0 + only_veto_blocks_nav: bool = True + non_veto_cost_scale: float = 0.50 + + +@dataclass +class GridConfidenceConfig: + valid_mm: Tuple[int, int] = (300, 10000) + min_valid_frac: float = 0.30 + conf_params: Tuple[float, float] = (0.30, 0.80) + + weights: Tuple[float, float, float] = (0.65, 0.25, 0.10) + + usar_depth: bool = True + + anom_tau_up: float = 0.15 + anom_satur_up_m: float = 0.35 + + anom_tau_down: float = 0.30 + anom_satur_down_m: float = 0.55 + anom_down_weight: float = 0.65 + + min_pct_navegavel: float = 0.55 + max_anom_navegavel: float = 0.45 + min_conf_navegavel: float = 0.35 + + det: DetectionGridConfig = field(default_factory=DetectionGridConfig) + + +class GridReferenceBuilder: + def __init__(self, config: Optional[GridGeometryConfig | Dict[str, Any]] = None): + self.config = self._normalizar_config(config) + self.grid_ref_base = self.gerar_grid_ref(pitch_graus=0.0) + self.grid_ref_atual = self.grid_ref_base.copy() + + @staticmethod + def _normalizar_config(config): + if config is None: + return GridGeometryConfig() + if isinstance(config, GridGeometryConfig): + return config + if isinstance(config, dict): + allowed = set(GridGeometryConfig.__dataclass_fields__.keys()) + return GridGeometryConfig(**{k: v for k, v in config.items() if k in allowed}) + raise TypeError("config inválido para GridReferenceBuilder") + + @property + def grid_shape(self) -> Tuple[int, int]: + return self.config.grid_shape + + def gerar_grid_ref(self, pitch_graus: float = 0.0) -> np.ndarray: + cfg = self.config + grid_w, grid_h = cfg.grid_shape + + pitch_corr = float(np.clip( + pitch_graus * cfg.pitch_gain, + -cfg.pitch_limit_deg, + cfg.pitch_limit_deg, + )) + + incl_deg = cfg.camera_pitch_deg + pitch_corr + + dist = [] + for i in range(grid_h): + alpha_v = ((i + 0.5) / grid_h - 0.5) * np.radians(abs(cfg.fov_v_deg)) + gamma = np.radians(incl_deg) + alpha_v + gamma = max(gamma, np.radians(2.0)) + + d = cfg.camera_height_m / np.tan(gamma) + d = float(np.clip(d, cfg.min_dist_m, cfg.max_dist_m)) + dist.append(d) + + return np.asarray(dist, dtype=np.float32) + + def atualizar_por_pitch(self, pitch_graus: float) -> np.ndarray: + try: + ref = self.gerar_grid_ref(pitch_graus=pitch_graus) + + grid_h = self.config.grid_shape[1] + + if ref is None or len(ref) != grid_h: + return self.grid_ref_atual + + if not np.all(np.isfinite(ref)) or np.any(ref <= 0): + return self.grid_ref_atual + + self.grid_ref_atual = ref + return self.grid_ref_atual + + except Exception: + return self.grid_ref_atual + + +class VisualGridBuilder: + """ + Constrói a grid de confiança/custo usada pelo CostmapFuser. + + Entrada: + - depth_mm + - seg_ids + - grid_ref + - detecções + + Saída: + dict com pct_navegavel, conf, anom, custo, navegavel e canais de detecção. + """ + + def __init__(self, config: Optional[GridConfidenceConfig | Dict[str, Any]] = None): + self.config = self._normalizar_config(config) + + @staticmethod + def _normalizar_config(config): + if config is None: + return GridConfidenceConfig() + if isinstance(config, GridConfidenceConfig): + return config + if isinstance(config, dict): + allowed = set(GridConfidenceConfig.__dataclass_fields__.keys()) + data = {k: v for k, v in config.items() if k in allowed} + + if isinstance(data.get("det"), dict): + det_allowed = set(DetectionGridConfig.__dataclass_fields__.keys()) + data["det"] = DetectionGridConfig(**{ + k: v for k, v in data["det"].items() if k in det_allowed + }) + + return GridConfidenceConfig(**data) + + raise TypeError("config inválido para VisualGridBuilder") + + def build( + self, + depth_mm: Optional[np.ndarray], + seg_ids: np.ndarray, + grid_ref: np.ndarray, + grid_shape: Tuple[int, int], + deteccoes: Optional[Sequence[Dict[str, Any]]] = None, + ) -> Optional[Dict[str, Any]]: + try: + cfg = self.config + det_cfg = cfg.det + + seg_ids = np.asarray(seg_ids) + + if seg_ids.ndim != 2: + raise ValueError(f"seg_ids precisa ser 2D, veio {seg_ids.shape}") + + grid_w, grid_h = grid_shape + H1, W1 = seg_ids.shape + + d_small = None + if cfg.usar_depth and depth_mm is not None: + d_small = cv2.resize( + depth_mm, + (W1, H1), + interpolation=cv2.INTER_NEAREST, + ).astype(np.float32) + + d_small[ + (d_small < cfg.valid_mm[0]) | + (d_small > cfg.valid_mm[1]) + ] = np.nan + + x_edges = np.linspace(0, W1, grid_w + 1, dtype=int) + y_edges = np.linspace(0, H1, grid_h + 1, dtype=int) + + pct_navegavel = np.zeros((grid_h, grid_w), np.float32) + pct_nao_navegavel = np.zeros((grid_h, grid_w), np.float32) + z_med = np.full((grid_h, grid_w), np.nan, np.float32) + depth_valid_frac = np.zeros((grid_h, grid_w), np.float32) + + det_cov_max = np.zeros((grid_h, grid_w), np.float32) + det_conf_max = np.zeros((grid_h, grid_w), np.float32) + det_score = np.zeros((grid_h, grid_w), np.float32) + det_top_label_id = -np.ones((grid_h, grid_w), np.int32) + det_top_conf = np.zeros((grid_h, grid_w), np.float32) + det_is_veto = np.zeros((grid_h, grid_w), np.float32) + + grid_ref = np.asarray(grid_ref, dtype=np.float32) + + if grid_ref.ndim == 1: + if grid_ref.shape[0] != grid_h: + raise ValueError(f"grid_ref 1D deve ter len={grid_h}, veio {grid_ref.shape}") + z_ref = np.repeat(grid_ref[:, None], grid_w, axis=1) + else: + z_ref = grid_ref + if z_ref.shape != (grid_h, grid_w): + raise ValueError(f"grid_ref 2D deve ser {(grid_h, grid_w)}, veio {z_ref.shape}") + + for j in range(grid_h): + y0, y1 = int(y_edges[j]), int(y_edges[j + 1]) + seg_row = seg_ids[y0:y1, :] + depth_row = d_small[y0:y1, :] if d_small is not None else None + + for i in range(grid_w): + x0, x1 = int(x_edges[i]), int(x_edges[i + 1]) + + seg_block = seg_row[:, x0:x1] + n = seg_block.size + if n == 0: + continue + + n_nav = np.count_nonzero(seg_block == ClassesSegmentacao.NAVEGAVEL.value) + n_naonav = np.count_nonzero(seg_block == ClassesSegmentacao.NAONAVEGAVEL.value) + + pct_navegavel[j, i] = n_nav / n + pct_nao_navegavel[j, i] = n_naonav / n + + if depth_row is not None: + depth_block = depth_row[:, x0:x1] + vals = depth_block[~np.isnan(depth_block)] + valid = vals.size + depth_valid_frac[j, i] = valid / n + + if valid >= max(int(cfg.min_valid_frac * n), 1): + z_med[j, i] = np.nanmedian(vals) / 1000.0 + + t0, t1 = cfg.conf_params + conf_seg = np.maximum(pct_navegavel, pct_nao_navegavel) + + if cfg.usar_depth and d_small is not None: + conf_dep = np.clip( + (depth_valid_frac - t0) / max(1e-6, (t1 - t0)), + 0.0, + 1.0, + ) + conf_cell = 0.65 * conf_seg + 0.35 * conf_dep + else: + conf_dep = np.zeros_like(conf_seg, dtype=np.float32) + conf_cell = conf_seg.copy() + + if deteccoes: + self._rasterizar_deteccoes( + deteccoes=deteccoes, + x_edges=x_edges, + y_edges=y_edges, + W1=W1, + H1=H1, + det_cfg=det_cfg, + det_cov_max=det_cov_max, + det_conf_max=det_conf_max, + det_score=det_score, + det_top_label_id=det_top_label_id, + det_top_conf=det_top_conf, + det_is_veto=det_is_veto, + ) + + if cfg.usar_depth and d_small is not None: + delta_signed = z_ref - z_med + delta_signed = np.where(np.isnan(z_med), 0.0, delta_signed) + + delta_up = np.maximum(delta_signed, 0.0) + anom_up_raw = np.clip(delta_up / max(1e-6, cfg.anom_satur_up_m), 0.0, 1.0) + anom_up = ( + anom_up_raw * + (delta_up > cfg.anom_tau_up).astype(np.float32) * + np.maximum(conf_dep, 0.25) + ) + + delta_down = np.maximum(-delta_signed, 0.0) + anom_down_raw = np.clip(delta_down / max(1e-6, cfg.anom_satur_down_m), 0.0, 1.0) + anom_down = ( + anom_down_raw * + (delta_down > cfg.anom_tau_down).astype(np.float32) * + np.maximum(conf_dep, 0.25) + ) + + anom = np.clip( + np.maximum(anom_up, cfg.anom_down_weight * anom_down), + 0.0, + 1.0, + ) + else: + anom_up = np.zeros_like(pct_navegavel, dtype=np.float32) + anom_down = np.zeros_like(pct_navegavel, dtype=np.float32) + anom = np.zeros_like(pct_navegavel, dtype=np.float32) + + nao_navegavel = 1.0 - pct_navegavel + w1, w2, w3 = cfg.weights + + custo_base = ( + w1 * nao_navegavel + + w2 * anom + + w3 * (1.0 - conf_cell) + ) + + custo = np.clip(custo_base + det_cfg.w4 * det_score, 0.0, 1.0) + + veto_soft_block = (det_is_veto > 0.5) & (det_score >= det_cfg.thr_det_soft) + + navegavel = ( + (pct_navegavel >= cfg.min_pct_navegavel) & + (anom < cfg.max_anom_navegavel) & + (conf_cell >= cfg.min_conf_navegavel) & + ( + ~veto_soft_block + if det_cfg.only_veto_blocks_nav + else (det_score < det_cfg.thr_det_soft) + ) + ) + + return { + "pct_navegavel": np.clip(pct_navegavel, 0.0, 1.0), + "pct_nao_navegavel": np.clip(pct_nao_navegavel, 0.0, 1.0), + + "z_med": z_med, + "z_ref": z_ref, + "depth_valid_frac": np.clip(depth_valid_frac, 0.0, 1.0), + + "conf": np.clip(conf_cell, 0.0, 1.0), + "anom_up": np.clip(anom_up, 0.0, 1.0), + "anom_down": np.clip(anom_down, 0.0, 1.0), + "anom": np.clip(anom, 0.0, 1.0), + "custo": np.clip(custo, 0.0, 1.0), + "navegavel": navegavel.astype(np.uint8), + + "det_cov_max": np.clip(det_cov_max, 0.0, 1.0), + "det_conf_max": np.clip(det_conf_max, 0.0, 1.0), + "det_score": np.clip(det_score, 0.0, 1.0), + "det_top_label_id": det_top_label_id, + "det_top_conf": np.clip(det_top_conf, 0.0, 1.0), + "det_is_veto": det_is_veto.astype(np.float32), + } + + except Exception: + return None + + @staticmethod + def _rasterizar_deteccoes( + deteccoes, + x_edges, + y_edges, + W1, + H1, + det_cfg, + det_cov_max, + det_conf_max, + det_score, + det_top_label_id, + det_top_conf, + det_is_veto, + ): + grid_h, grid_w = det_score.shape + + for det in deteccoes: + conf = float(det.get("conf", 0.0)) + if conf < det_cfg.min_det_conf: + continue + + label = str(det.get("label", "")).strip() + label_id = int(det.get("label_id", -1)) + + is_veto = label in det_cfg.veto_labels + w_class = float(det_cfg.class_weights.get(label, 1.0)) + + if not is_veto: + w_class *= float(det_cfg.non_veto_cost_scale) + + if "bbox_px" in det and det["bbox_px"]: + x0p, y0p, x1p, y1p = det["bbox_px"] + else: + bbox = det.get("bbox_norm") + if not bbox: + continue + + x0n, y0n, x1n, y1n = bbox + x0p = int(np.clip(x0n * W1, 0, W1 - 1)) + x1p = int(np.clip(x1n * W1, 0, W1)) + y0p = int(np.clip(y0n * H1, 0, H1 - 1)) + y1p = int(np.clip(y1n * H1, 0, H1)) + + if x1p <= x0p or y1p <= y0p: + continue + + i0 = max(0, np.searchsorted(x_edges, x0p, side="right") - 1) + i1 = min(grid_w - 1, np.searchsorted(x_edges, x1p, side="left")) + j0 = max(0, np.searchsorted(y_edges, y0p, side="right") - 1) + j1 = min(grid_h - 1, np.searchsorted(y_edges, y1p, side="left")) + + for j in range(j0, j1 + 1): + y0c, y1c = int(y_edges[j]), int(y_edges[j + 1]) + + for i in range(i0, i1 + 1): + x0c, x1c = int(x_edges[i]), int(x_edges[i + 1]) + + ix0 = max(x0c, x0p) + ix1 = min(x1c, x1p) + iy0 = max(y0c, y0p) + iy1 = min(y1c, y1p) + + if ix1 <= ix0 or iy1 <= iy0: + continue + + inter = float((ix1 - ix0) * (iy1 - iy0)) + cell_area = float((x1c - x0c) * (y1c - y0c)) + + if cell_area <= 0: + continue + + cov = inter / cell_area + + if cov < det_cfg.min_cell_coverage: + continue + + score_local = conf * cov * w_class + + det_cov_max[j, i] = max(det_cov_max[j, i], cov) + det_conf_max[j, i] = max(det_conf_max[j, i], conf) + + if det_cfg.combine == "sum_clamped": + det_score[j, i] = np.clip(det_score[j, i] + score_local, 0.0, 1.0) + else: + det_score[j, i] = max(det_score[j, i], score_local) + + priority = (2.0 if is_veto else 1.0) * conf * cov + + if priority > det_top_conf[j, i]: + det_top_conf[j, i] = priority + det_top_label_id[j, i] = label_id + det_is_veto[j, i] = 1.0 if is_veto else 0.0 diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py index c84708f4a..d7dae6731 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py @@ -4,22 +4,65 @@ import threading import time import cv2 -import numpy as np from camera_worker.camera_multispectral import CameraMultispectral from camera_worker.oak_fcc3_core.segformer_service import MultiSpecSegformerService -from shared.enums import StatusModulo, StatusOperacao, T_Code, TipoFrameCamera, WeedWorkerCommandType +from shared.enums import ( + StatusModulo, + StatusOperacao, + T_Code, + TipoFrameCamera, + WeedWorkerCommandType, +) from shared.contexto_global_redis import CmdKey, ContextoGlobalRedis, CtxKey from weed_worker.weed_detector import WeedDetector from visual_worker.utils import converter_valores_numpy from shared.perf_monitor import VisualPerfMonitor + class CameraManager: + """ + CameraManager v1 do Weed Worker. + + Responsabilidade: + - Inicializar câmera multiespectral. + - Inicializar runtime ONNX/TensorRT. + - Inicializar WeedDetector target_binary. + - Orquestrar loops: + tensor -> inferência -> detecção -> publicação -> stream/debug. + - Manter caches leves para Redis/C#/preview. + + Contrato visual: + - preview/overlay/debug ficam aqui. + - WeedDetector não gera imagem, base64, colormap nem overlay. + + Contrato IA: + - MultiSpecSegformerService retorna target_mask uint8 HxW. + - WeedDetector recebe target_mask e retorna controle dos bicos. + """ + + FRAME_TYPES_PREVIEW = { + TipoFrameCamera.Rgb, + TipoFrameCamera.Segmentacao, + TipoFrameCamera.Overlay, + TipoFrameCamera.Debug, + } + def __init__(self, mostrar_log): self.mostrar_log = mostrar_log + self.mx_id = None + self.camera = None self.model_svc = None + self.weed_detector = None + self.seg_config = None + + self.operante = False + self.iniciando = False + self.debug_visual = False + self.debug_perf = False + self.qtd_bicos = 7 self._loop_tensor_iniciado = False self._loop_inferencia_iniciado = False @@ -30,49 +73,34 @@ class CameraManager: self._vida_lock = threading.RLock() self._fechando_camera = False + self._em_warmup = False - self.reiniciar_status() + self._lock_tensor = threading.Lock() + self._pred_lock = threading.RLock() + self._pub_lock = threading.RLock() - def reiniciar_status(self): - self.camera = None - self.operante = False - self.iniciando = False - self.weed_detector = None - self.seg_config = None - self.qtd_bicos = 0 - self.debug_visual = False - self._ultimo_rgb_frame = None + self.perf = VisualPerfMonitor(janela=180) + + self._reset_runtime_state() + + # ============================================================ + # Estado / inicialização + # ============================================================ + + def _reset_runtime_state(self): self._ultima_analise = {} - self._ts_segmentacao_anterior = 0 - self._ts_ultima_analise = 0 + self._ultimo_predictions = None self._ultimo_predictions_full = None self._ultimo_raw_base = None self._ultimo_raw_input = None self._ultimo_controle = None - self.tipo_camera_atual = None - self._analisando_segmentacao = False - self._ultimo_preview_ts = 0 - self._ultimo_preview_rgb = None - self._ultimo_preview_seg = None - self._ultimo_preview_overlay = None - self._ultimo_preview_debug = None - self._lock_tensor = threading.Lock() + self._tensor_pronto = None self._tensor_res = None self._tensor_ts = 0.0 self._tensor_consumido_ts = 0.0 - self._fps_infer_last_ts = None - self._fps_infer_ema = 0.0 - self._ultimo_infer_ms = 0.0 - self._ultimo_infer_gpu_ms = 0.0 - self._ultimo_loop_analise_fps = 0.0 - self._em_warmup = False - self._falhas_utilizavel = 0 - self._fechando_camera = False - - self._pred_lock = threading.RLock() self._pred_cache = { "ts": 0.0, "tensor_ts": 0.0, @@ -80,12 +108,29 @@ class CameraManager: "res": None, "infer_ms": 0.0, "infer_gpu_ms": 0.0, + "infer_prepare_ms": 0.0, + "infer_post_ms": 0.0, "fps_model": 0.0, } self._pred_consumido_ts = 0.0 - self._cache_lock = threading.RLock() - self._pub_lock = threading.RLock() + self._ultimo_preview_ts = 0.0 + self._ultimo_preview_rgb = None + self._ultimo_preview_seg = None + self._ultimo_preview_overlay = None + self._ultimo_preview_debug = None + + self._fps_infer_last_ts = None + self._fps_infer_ema = 0.0 + self._ultimo_infer_ms = 0.0 + self._ultimo_infer_gpu_ms = 0.0 + self._ultimo_loop_analise_fps = 0.0 + + self._ultimo_config_update_ts = 0.0 + + self._dbg_img = None + self._dbg_layer = None + self._dbg_shape = (706, 560) self._pub_cache = { "ts_analise": 0.0, @@ -118,36 +163,76 @@ class CameraManager: "fps_inferencia": 0.0, "infer_ms": 0.0, "infer_gpu_ms": 0.0, - "loop_ms": 0.0, - "tensor_ms": 0.0, "detector_ms": 0.0, + "convert_ms": 0.0, "ctx_read_ms": 0.0, - "controle_ms": 0.0, + "control_ms": 0.0, "pub_cache_ms": 0.0, + "total_ms": 0.0, } - self.perf = VisualPerfMonitor(janela=180) - def inicializar(self, mx_id): if self.iniciando: return - - #_camera_conectada = ContextoGlobalRedis.get_cameras().get(mx_id) is not None - #if not _camera_conectada: - # return - - self.iniciando = True - if mx_id == None: - self.iniciando = False - #self.mostrar_log(f"❌ Camera não definida.") - return - - self.mx_id = mx_id - - from weed_worker.config import load_seg_config - seg_config = load_seg_config() - self.seg_config = seg_config + if mx_id is None: + return + + with self._vida_lock: + self.iniciando = True + + try: + self.mx_id = mx_id + + from weed_worker.config import load_seg_config + self.seg_config = load_seg_config() + + self._inicializar_camera(mx_id, self.seg_config) + + if self.camera is None: + self.mostrar_log(f"❌ Camera com ID {mx_id} não iniciada.") + return + + self.mostrar_log( + f"📷 Camera selecionada: {self.camera.modelo} - {self.camera.mx_id}" + ) + + self._reset_runtime_state() + + self.qtd_bicos = int(self.seg_config.get("qtd_bicos", 7) or 7) + self.debug_visual = bool(self.seg_config.get("debug_visual", False)) + self.debug_perf = bool(self.seg_config.get("debug_perf", False)) + + self._inicializar_modelo() + self._inicializar_detector() + + self.operante = False + self._em_warmup = True + + warmup_ok = self._executar_warmup_camera_manager( + n_tensors=5, + n_infer=4, + n_detector=2, + timeout_s=8.0, + ) + + self._em_warmup = False + + if not warmup_ok: + self.mostrar_log( + "[weed][WARMUP] finalizou com alerta, liberando operação mesmo assim" + ) + + self.operante = True + + self._iniciar_loops_se_necessario(self.seg_config) + + self.atualizar_saude_camera() + + finally: + self.iniciando = False + + def _inicializar_camera(self, mx_id, seg_config): try: nova = CameraMultispectral( self.mostrar_log, @@ -155,125 +240,98 @@ class CameraManager: module_calibration_json=seg_config.get("module_calibration_json"), width=int(seg_config.get("camera_width", 1280)), height=int(seg_config.get("camera_height", 800)), - fps=int(seg_config.get("camera_fps", 20)), + fps=int(seg_config.get("camera_fps", 40)), target_size=seg_config.get("ia_resolution", [1024, 640]), ) - self.tipo_camera_atual = "multispectral" if nova.iniciado: self.camera = nova + else: + self.camera = None except Exception as e: + self.camera = None self.mostrar_log(f"⚠️ Camera com ID {mx_id} não conectada: {e}") - self.iniciando = False + + def _inicializar_modelo(self): + if self.model_svc is not None: return - if self.camera is None: - self.mostrar_log(f"❌ Camera com ID {mx_id} não iniciada.") - else: - self.mostrar_log(f"📷 Camera selecionada: {self.camera.modelo} - {self.camera.mx_id}") - self._ultima_analise = {} - self._ultimo_rgb_frame = None - self._ultimo_predictions = None - self._ultimo_raw_base = None - self._ultimo_raw_input = None - self._ultimo_controle = None - self._analisando_segmentacao = False - self.qtd_bicos = seg_config.get("qtd_bicos", 7) - self.debug_visual = seg_config.get("debug_visual", False) + self.model_svc = MultiSpecSegformerService( + model_config=self.seg_config, + mostrar_log=self.mostrar_log, + ) - if not hasattr(self, "model_svc") or self.model_svc is None: - self.model_svc = MultiSpecSegformerService( - model_config=seg_config, - mostrar_log=self.mostrar_log - ) + self.mostrar_log( + f"[weed][MODEL] backend={getattr(self.model_svc, 'runtime_backend', 'onnx')} " + f"runtime_mode={getattr(self.model_svc, 'runtime_mode', None)}" + ) - classes = self.model_svc.get_classes() - colormap_rgb = self.model_svc.get_colormap() + def _inicializar_detector(self): + if self.weed_detector is not None: + return - if not hasattr(self, "weed_detector") or self.weed_detector is None: - self.weed_detector = WeedDetector( - color_map=colormap_rgb, - classes=classes - ) + self.weed_detector = WeedDetector() - # Bloqueia runtime durante warmup. - self.operante = False - self._em_warmup = True + def _iniciar_loops_se_necessario(self, seg_config): + freq_analise = float(seg_config.get("analise_fps", 15.0)) + freq_tensor = float(seg_config.get("tensor_fps", 25.0)) + freq_publicacao = float(seg_config.get("publicacao_fps", 5.0)) + freq_inferencia = float(seg_config.get("inferencia_fps", freq_tensor)) + freq_deteccao = float(seg_config.get("deteccao_fps", freq_inferencia)) - warmup_ok = self._executar_warmup_camera_manager( - n_tensors=5, - n_infer=4, - n_detector=2, - timeout_s=8.0 - ) + if not self._loop_stream_iniciado: + stream = getattr(self.camera, "stream", None) + stream_fps = float(getattr(stream, "_op_fps", 2.0) or 2.0) + self._iniciar_loop_frame_stream(stream_fps) + self._loop_stream_iniciado = True - self._em_warmup = False + if not self._loop_publicacao_iniciado: + self._iniciar_loop_publicacao_weed(freq=freq_publicacao) + self._loop_publicacao_iniciado = True - if not warmup_ok: - self.mostrar_log("[weed][WARMUP] finalizou com alerta, liberando operação mesmo assim") + if not self._loop_tensor_iniciado: + self._iniciar_loop_captura_tensor(freq=freq_tensor) + self._loop_tensor_iniciado = True - self.operante = True + if not self._loop_inferencia_iniciado: + self._iniciar_loop_inferencia(freq=freq_inferencia) + self._loop_inferencia_iniciado = True - self.debug_perf = bool(seg_config.get("debug_perf", False)) - freq_analise = float(seg_config.get("analise_fps", 20.0)) - freq_tensor = float(seg_config.get("tensor_fps", 20.0)) - freq_publicacao = float(seg_config.get("publicacao_fps", 15.0)) - freq_inferencia = float(seg_config.get("inferencia_fps", freq_analise)) - freq_deteccao = float(seg_config.get("deteccao_fps", freq_analise)) + if not self._loop_deteccao_iniciado: + self._iniciar_loop_deteccao_weed(freq=freq_deteccao) + self._loop_deteccao_iniciado = True - if not self._loop_stream_iniciado: - cam_atual = self.camera - if cam_atual is None: - self.mostrar_log("[INIT] não iniciou stream: camera None") - else: - stream = getattr(cam_atual, "stream", None) - stream_fps = getattr(stream, "_op_fps", 2.0) - self._iniciar_loop_frame_stream(stream_fps) - self._loop_stream_iniciado = True - - if not self._loop_publicacao_iniciado: - self._iniciar_loop_publicacao_weed(freq=freq_publicacao) - self._loop_publicacao_iniciado = True - - if not self._loop_tensor_iniciado: - self._iniciar_loop_captura_tensor(freq_tensor) - self._loop_tensor_iniciado = True - - if not self._loop_inferencia_iniciado: - self._iniciar_loop_inferencia(freq=freq_inferencia) - self._loop_inferencia_iniciado = True - - if not self._loop_deteccao_iniciado: - self._iniciar_loop_deteccao_weed(freq=freq_deteccao) - self._loop_deteccao_iniciado = True - - if not self._loop_analise_iniciado: - self._iniciar_loop_analise_continua(freq=15.0) - self._loop_analise_iniciado = True - - self.iniciando = False - self.atualizar_saude_camera() + if not self._loop_analise_iniciado: + self._iniciar_loop_analise_continua(freq=freq_analise) + self._loop_analise_iniciado = True def fechar_camera_manager(self, motivo=""): - try: - if self.camera is not None: - self.camera.parar() - except Exception as e: - self.mostrar_log(f"[weed] Erro ao fechar câmera: {e}") + with self._vida_lock: + self._fechando_camera = True - self.camera = None - self.camera_id = None - self.camera_manager_iniciado = False + try: + if self.camera is not None: + self.camera.parar() + except Exception as e: + self.mostrar_log(f"[weed] Erro ao fechar câmera: {e}") - self.mostrar_log(f"[weed] Camera Manager fechado: {motivo}") + self.camera = None + self.operante = False + self._fechando_camera = False + + self.mostrar_log(f"[weed] Camera Manager fechado: {motivo}") + + # ============================================================ + # Warmup + # ============================================================ def _executar_warmup_camera_manager( self, n_tensors=5, n_infer=4, n_detector=2, - timeout_s=8.0 + timeout_s=8.0, ): if self.camera is None: return False @@ -281,7 +339,7 @@ class CameraManager: self.mostrar_log("[weed][WARMUP] iniciando warmup do CameraManager...") operante_anterior = self.operante - warmup_anterior = getattr(self, "_em_warmup", False) + warmup_anterior = self._em_warmup self.operante = False self._em_warmup = True @@ -289,15 +347,11 @@ class CameraManager: tensor5 = None res = None predictions = None - analise_completa = None try: - # ===================================================== - # 1) Aquecer captura/fusão multispectral - # ===================================================== tensor_ok = 0 - t0 = time.time() + while time.time() - t0 < timeout_s and tensor_ok < n_tensors: try: t_cap0 = time.perf_counter() @@ -327,9 +381,6 @@ class CameraManager: self.mostrar_log("[weed][WARMUP] abortado: sem tensor válido") return False - # ===================================================== - # 2) Aquecer modelo - # ===================================================== infer_ok = 0 for i in range(max(0, n_infer)): @@ -337,16 +388,22 @@ class CameraManager: t_inf0 = time.perf_counter() predictions = self.model_svc.infer_tensor_fast( tensor5, - keep_probs=False + keep_probs=False, ) t_inf1 = time.perf_counter() infer_full = getattr(self.model_svc, "_ultimo_predictions_full", {}) or {} - infer_gpu_ms = infer_full.get("infer_ms", None) + + infer_forward_ms = infer_full.get( + "forward_ms", + infer_full.get("infer_ms", None), + ) + infer_prepare_ms = infer_full.get("prepare_ms", 0.0) + infer_post_ms = infer_full.get("post_ms", 0.0) self._atualizar_fps_inferencia( infer_ms=(t_inf1 - t_inf0) * 1000.0, - infer_gpu_ms=infer_gpu_ms + infer_gpu_ms=infer_forward_ms, ) if predictions is not None: @@ -357,9 +414,11 @@ class CameraManager: self._ultimo_predictions_full = infer_full self.mostrar_log( - f"[weed][WARMUP] infer {i+1}/{n_infer} " + f"[weed][WARMUP] infer {i + 1}/{n_infer} " f"total={(t_inf1 - t_inf0) * 1000.0:.1f}ms " - f"gpu={infer_gpu_ms if infer_gpu_ms is not None else -1}" + f"prep={infer_prepare_ms:.1f}ms " + f"fwd={infer_forward_ms if infer_forward_ms is not None else -1:.1f}ms " + f"post={infer_post_ms:.1f}ms" ) except Exception as e: @@ -371,15 +430,12 @@ class CameraManager: self.mostrar_log("[weed][WARMUP] sem predictions válidas") return False - # ===================================================== - # 3) Aquecer WeedDetector - # ===================================================== det_ok = 0 for i in range(max(0, n_detector)): try: t_det0 = time.perf_counter() - analise_completa = self.detectar_ervas(predictions, None) + analise_completa = self.detectar_ervas(predictions) t_det1 = time.perf_counter() if isinstance(analise_completa, dict): @@ -389,7 +445,7 @@ class CameraManager: self._set_pub_cache("analise", analise) self.mostrar_log( - f"[weed][WARMUP] detector {i+1}/{n_detector} " + f"[weed][WARMUP] detector {i + 1}/{n_detector} " f"total={(t_det1 - t_det0) * 1000.0:.1f}ms" ) @@ -419,18 +475,69 @@ class CameraManager: if self.camera is not None: self.camera.perf = self.perf + # ============================================================ + # Saúde / config dinâmica + # ============================================================ + def atualizar_saude_camera(self): self._ultima_saude_ts = time.time() - #self.mostrar_log("Atualizando saude da camera...") + try: if self.camera is not None: self.camera.atualizar_saude() elif self.mx_id is not None: from camera_worker.manager import definir_saude_camera - definir_saude_camera(self.mx_id, StatusModulo.DESCONECTADO, 0, ["desconectado"], False, {}, disp=T_Code.Cam, conectado=False) + + definir_saude_camera( + self.mx_id, + StatusModulo.DESCONECTADO, + 0, + ["desconectado"], + False, + {}, + disp=T_Code.Cam, + conectado=False, + ) + except Exception as e: self.mostrar_log(f"[saude] erro: {e}") + def _atualizar_config_dinamica_detector(self, intervalo_s=0.20): + """ + Atualiza config do detector em baixa frequência. + + Isso permite refletir: + - velocidade atual; + - área/faixa de atuação; + - limiares do C#; + - qtd_bicos. + + Sem obrigar o detector a ler Redis sozinho em todo frame. + """ + agora = time.time() + + if (agora - self._ultimo_config_update_ts) < intervalo_s: + return + + try: + from weed_worker.config import load_seg_config + + cfg = load_seg_config() + self.seg_config = cfg + self.qtd_bicos = int(cfg.get("qtd_bicos", self.qtd_bicos or 7) or 7) + + if self.weed_detector is not None: + self.weed_detector.atualizar_config(cfg) + + self._ultimo_config_update_ts = agora + + except Exception as e: + self.mostrar_log(f"[weed] erro ao atualizar config dinâmica: {e}") + + # ============================================================ + # Caches + # ============================================================ + def _get_tensor_novo_para_inferencia(self): with self._lock_tensor: tensor5 = self._tensor_pronto @@ -461,6 +568,10 @@ class CameraManager: self._pred_consumido_ts = pred_ts return cache + # ============================================================ + # Preview / stream / debug + # ============================================================ + def get_rgb_frame(self): if self.camera is None: return None, None, None @@ -468,353 +579,536 @@ class CameraManager: try: frame, res = self.camera.requisitar_frame_rgb() if frame is not None: - self._ultimo_rgb_frame = frame return frame, self.camera.timestamp_ultimo_frame_rgb, res - elif "X_LINK_ERROR" in res["erro"]: - self.reiniciar_status() + + if isinstance(res, dict) and "X_LINK_ERROR" in str(res.get("erro", "")): + self._reset_runtime_state() + except Exception as e: self.mostrar_log(f"Erro ao requisitar rgb frame: {e}") if "X_LINK_ERROR" in str(e): - self.reiniciar_status() + self._reset_runtime_state() return None, None, None - def get_segmentation_predictions(self): - if self.camera is None: - return None, None, None + def get_selected_frame(self, _frame_type: TipoFrameCamera): + agora = time.time() try: - t_total0 = time.perf_counter() + if _frame_type not in self.FRAME_TYPES_PREVIEW: + return None - if hasattr(self.camera, "requisitar_tensor_multispec"): - # ========================== - # 1) Captura + decode + fusão - # ========================== - t0 = time.perf_counter() - with self._lock_tensor: - tensor5 = self._tensor_pronto - res = self._tensor_res - ts_tensor = self._tensor_ts + if self.model_svc is None or self._ultimo_raw_input is None: + return None - if tensor5 is None or ts_tensor == self._tensor_consumido_ts: - return None, None, res + if (agora - self._ultimo_preview_ts) < 0.20: + cached = self._get_cached_preview_frame(_frame_type) + if cached is not None: + return cached - self._tensor_consumido_ts = ts_tensor - t_capture_ms = (time.perf_counter() - t0) * 1000.0 + rgb_frame, seg_frame, overlay_frame, _, _ = self.model_svc.preview_infer_cached( + self._ultimo_raw_input, + self._ultimo_predictions, + alpha=0.5, + ) - #perf = res.get("perf", {}) if isinstance(res, dict) else {} - #self.mostrar_log( - # "[PERF][TENSOR] " - # f"get_decoded={perf.get('get_decoded_ms', -1):.1f}ms " - # f"build={perf.get('build_ms', -1):.1f}ms " - # f"validate={perf.get('validate_ms', -1):.1f}ms " - # f"post={perf.get('post_ms', -1):.1f}ms " - # f"preview={perf.get('preview_ms', -1):.1f}ms " - # f"total={perf.get('total_ms', -1):.1f}ms " - # f"origem={perf.get('origem_tensor')}" - #) - - if tensor5 is None: - return None, None, res - - # ========================== - # 2) Estatística simples - # ========================== - t0 = time.perf_counter() - sig = float(np.mean(tensor5[0])) if tensor5 is not None else -1 - t_stats_ms = (time.perf_counter() - t0) * 1000.0 - - # ========================== - # 3) Inferência IA - # ========================== - t0 = time.perf_counter() - predictions = self.model_svc.infer_tensor_fast(tensor5, keep_probs=False) - t_infer_ms = (time.perf_counter() - t0) * 1000.0 - - infer_full = getattr(self.model_svc, "_ultimo_predictions_full", {}) or {} - infer_gpu_ms = infer_full.get("infer_ms", None) - - self._atualizar_fps_inferencia( - infer_ms=t_infer_ms, - infer_gpu_ms=infer_gpu_ms + debug_frame = None + if _frame_type == TipoFrameCamera.Debug: + debug_frame = self.get_debug_frame( + mostrar=False, + overlay_bgr=overlay_frame, ) - # ========================== - # 4) Cache local - # ========================== - t0 = time.perf_counter() - ts = time.time() + self._ultimo_preview_ts = agora + self._ultimo_preview_rgb = rgb_frame + self._ultimo_preview_seg = seg_frame + self._ultimo_preview_overlay = overlay_frame + self._ultimo_preview_debug = debug_frame - self._ultimo_raw_base = tensor5 - self._ultimo_raw_input = tensor5 - self._ultimo_predictions = predictions - self._ultimo_predictions_full = getattr(self.model_svc, "_ultimo_predictions_full", None) - - # ========================== - # Log limitado - # ========================== - #t_cache_ms = (time.perf_counter() - t0) * 1000.0 - #t_total_ms = (time.perf_counter() - t_total0) * 1000.0 - #agora_log = time.time() - #if not hasattr(self, "_ultimo_log_multispec_ts"): - # self._ultimo_log_multispec_ts = 0 - #if (agora_log - self._ultimo_log_multispec_ts) >= 1.0: - # self._ultimo_log_multispec_ts = agora_log - # self.mostrar_log( - # "[PERF][SEG] " - # f"buffer_read_ms={t_capture_ms:.1f} " - # f"tensor_core_ms={res.get('perf', {}).get('total_ms', -1):.1f} " - # f"infer_call_ms={t_infer_ms:.1f} " - # f"infer_gpu_ms={infer_gpu_ms if infer_gpu_ms is not None else -1:.1f} " - # f"stats_ms={t_stats_ms:.1f} " - # f"cache_ms={t_cache_ms:.1f} " - # f"total_ms={t_total_ms:.1f} " - # f"fps_teorico={1000.0 / max(t_total_ms, 1e-6):.2f} " - # f"tensor_shape={tensor5.shape} " - # f"mean_R={sig:.6f} " - # f"core_dur={res.get('duracao') if isinstance(res, dict) else None}" - # ) - - if predictions is not None: - return predictions, ts, res - - return None, ts, res + return self._get_cached_preview_frame(_frame_type) except Exception as e: - self.mostrar_log(f"Erro ao requisitar predictions: {e}") - if "X_LINK_ERROR" in str(e): - self.reiniciar_status() + self.mostrar_log(f"[weed] erro em get_selected_frame: {e}") + return None - return None, None, None + def _get_cached_preview_frame(self, frame_type): + if frame_type == TipoFrameCamera.Rgb: + return self._ultimo_preview_rgb + if frame_type == TipoFrameCamera.Segmentacao: + return self._ultimo_preview_seg + if frame_type == TipoFrameCamera.Overlay: + return self._ultimo_preview_overlay + if frame_type == TipoFrameCamera.Debug: + return self._ultimo_preview_debug + return None + + def get_debug_frame(self, mostrar=False, overlay_bgr=None): + overlay = overlay_bgr if overlay_bgr is not None else self._ultimo_preview_overlay + + if overlay is None: + return None - def get_debug_frame(self, mostrar = False): metricas_perf = { - "fps_dbg": None, "fps_infer": self._fps_infer_ema, "infer_ms": self._ultimo_infer_ms, "infer_gpu_ms": self._ultimo_infer_gpu_ms, "fps_loop": self._ultimo_loop_analise_fps, } - self._ultimo_preview_debug = self.weed_detector._mostrar_debug_bicos_overlay( - self._ultimo_preview_overlay, - [], - self._ultimo_controle or {}, - self.seg_config, - show=mostrar, - metricas_perf=metricas_perf + + self._ultimo_preview_debug = self._montar_debug_overlay( + overlay_bgr=overlay, + atuacao_bicos=self._ultimo_controle or {}, + config=self.seg_config or {}, + metricas_perf=metricas_perf, + mostrar=mostrar, ) return self._ultimo_preview_debug - def get_selected_frame(self, _frame_type: TipoFrameCamera): - agora = time.time() + def _montar_debug_overlay( + self, + overlay_bgr, + atuacao_bicos, + config, + metricas_perf=None, + mostrar=False, + ): + try: + if overlay_bgr is None: + return None - if _frame_type in [TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao, TipoFrameCamera.Overlay, TipoFrameCamera.Debug]: - if self.model_svc is not None and self._ultimo_raw_input is not None: - # Limita preview/overlay a 5 FPS - if (agora - self._ultimo_preview_ts) < 0.20: - if _frame_type == TipoFrameCamera.Rgb and self._ultimo_preview_rgb is not None: - return self._ultimo_preview_rgb - if _frame_type == TipoFrameCamera.Segmentacao and self._ultimo_preview_seg is not None: - return self._ultimo_preview_seg - if _frame_type == TipoFrameCamera.Overlay and self._ultimo_preview_overlay is not None: - return self._ultimo_preview_overlay - if _frame_type == TipoFrameCamera.Debug and self._ultimo_preview_debug is not None: - return self._ultimo_preview_debug + metricas_perf = metricas_perf or {} - rgb_frame, seg_frame, overlay_frame, _, _ = self.model_svc.preview_infer_cached( - self._ultimo_raw_input, - self._ultimo_predictions, - alpha=0.5 + W, H = self._dbg_shape + + if self._dbg_img is None or self._dbg_img.shape[:2] != (H, W): + self._dbg_img = cv2.resize( + overlay_bgr, + (W, H), + interpolation=cv2.INTER_AREA, + ) + self._dbg_layer = self._dbg_img.copy() + self._dbg_layer[:] = 0 + else: + h_src, w_src = overlay_bgr.shape[:2] + if (w_src, h_src) != (W, H): + cv2.resize( + overlay_bgr, + (W, H), + dst=self._dbg_img, + interpolation=cv2.INTER_AREA, + ) + else: + self._dbg_img[...] = overlay_bgr + + qtd_bicos = int(config.get("qtd_bicos", self.qtd_bicos or 1) or 1) + zona_inicio = float(config.get("faixa_atuacao_bicos", 0.7)) + faixa_atuacao = float(config.get("area_atuacao_bicos", 0.1)) + + y_inicio = int((1.0 - zona_inicio) * H) + y_fim = int((1.0 - (zona_inicio + faixa_atuacao)) * H) + y0, y1 = min(y_inicio, y_fim), max(y_inicio, y_fim) + + self._dbg_layer.fill(0) + cv2.rectangle( + self._dbg_layer, + (0, y0), + (W, y1), + (220, 220, 100), + thickness=-1, + ) + cv2.addWeighted(self._dbg_layer, 0.18, self._dbg_img, 0.82, 0, dst=self._dbg_img) + cv2.rectangle(self._dbg_img, (0, y0), (W, y1), (180, 180, 80), 2) + + largura_bico = W / float(max(qtd_bicos, 1)) + cores = [ + (0, 255, 0), + (255, 0, 0), + (0, 255, 255), + (255, 128, 0), + (255, 0, 255), + (0, 128, 255), + (128, 255, 0), + (0, 0, 255), + ] + + self._dbg_layer.fill(0) + + for i in range(qtd_bicos): + x0 = int(i * largura_bico) + x1 = int((i + 1) * largura_bico) + cor = cores[i % len(cores)] + + if atuacao_bicos.get(i, False): + cv2.rectangle(self._dbg_layer, (x0, y0), (x1, y1), cor, thickness=-1) + + cv2.addWeighted(self._dbg_layer, 0.15, self._dbg_img, 0.85, 0, dst=self._dbg_img) + + for i in range(qtd_bicos): + x0 = int(i * largura_bico) + x1 = int((i + 1) * largura_bico) + cor = cores[i % len(cores)] + status = "ON" if atuacao_bicos.get(i, False) else "OFF" + + cv2.rectangle(self._dbg_img, (x0, y0), (x1, y1), cor, 1) + cv2.putText( + self._dbg_img, + f"Bico {i} {status}", + (x0 + 5, min(H - 5, y1 + 20)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.55, + cor, + 2, ) - debug_frame = None - if _frame_type == TipoFrameCamera.Debug: - debug_frame = self.get_debug_frame() + fps_infer = float(metricas_perf.get("fps_infer") or 0.0) + infer_ms = float(metricas_perf.get("infer_ms") or 0.0) + infer_gpu_ms = float(metricas_perf.get("infer_gpu_ms") or 0.0) + fps_loop = float(metricas_perf.get("fps_loop") or 0.0) - self._ultimo_preview_ts = agora - self._ultimo_preview_rgb = rgb_frame - self._ultimo_preview_seg = seg_frame - self._ultimo_preview_overlay = overlay_frame - self._ultimo_preview_debug = debug_frame + cv2.putText( + self._dbg_img, + f"Infer FPS: {fps_infer:.1f} | infer: {infer_ms:.1f}ms", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 255, 255), + 2, + ) - frame = rgb_frame - if _frame_type == TipoFrameCamera.Rgb: - frame = rgb_frame - elif _frame_type == TipoFrameCamera.Segmentacao: - frame = seg_frame - elif _frame_type == TipoFrameCamera.Overlay: - frame = overlay_frame - elif _frame_type == TipoFrameCamera.Debug: - frame = debug_frame - return frame + cv2.putText( + self._dbg_img, + f"Loop FPS: {fps_loop:.1f} | GPU: {infer_gpu_ms:.1f}ms", + (10, 60), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 255, 255), + 2, + ) - def _iniciar_loop_analise_continua(self, freq=15.0): + if mostrar: + cv2.imshow("Debug Weed Worker", self._dbg_img) + cv2.waitKey(1) + + return self._dbg_img + + except Exception as e: + self.mostrar_log(f"Erro ao montar debug overlay weed: {e}") + return None + + # ============================================================ + # Loops + # ============================================================ + + def _iniciar_loop_captura_tensor(self, freq=25.0): def loop(): while True: - t0 = time.time() + t0_wall = time.time() + t0 = time.perf_counter() try: if self.camera is None: - time.sleep(0.5) + time.sleep(1.0) continue - status = StatusModulo( - (self.camera.ultima_saude or {}).get( - "status", - StatusModulo.DESCONECTADO.value - ) - ) + tensor5, res = self.camera.requisitar_tensor_multispec(force=True) - ts_status = (self.camera.ultima_saude or {}).get("timestamp", 0) + if isinstance(res, dict) and res.get("erro"): + erro = res.get("erro") + if self.camera._is_erro_fatal_depthai(erro): + self.fechar_camera_manager(f"falha fatal DepthAI: {erro}") + return - if status == StatusModulo.DESCONECTADO and (t0 - ts_status) > 10.0: - try: - if getattr(self.camera, "imu", None): - self.camera.imu.parar() - except Exception: - pass + if tensor5 is not None: + agora = time.time() + perf = res.get("perf", {}) if isinstance(res, dict) else {} - self.reiniciar_status() - continue + with self._lock_tensor: + self._tensor_pronto = tensor5 + self._tensor_res = res + self._tensor_ts = agora - agora = time.time() + t1 = time.perf_counter() - if not hasattr(self, "_ultimo_perf_publish"): - self._ultimo_perf_publish = 0.0 - - if agora - self._ultimo_perf_publish >= 1.0: - self._ultimo_perf_publish = agora - - resumo = self.perf.resumo() - resumo["weed_cache"] = { - "tensor_ts": self._tensor_ts, - "tensor_age_ms": (time.time() - self._tensor_ts) * 1000.0 if self._tensor_ts else None, - "tensor_consumido_ts": self._tensor_consumido_ts, - "tem_tensor": self._tensor_pronto is not None, - } - resumo["pub_debug"] = getattr(self, "_ultimo_pub_debug", {}) - - self._set_pub_cache("performance_weed", resumo) - - if not self.debug_perf: - continue - - loops = resumo.get("loops", {}) - - def _fmt(v, casas=1, default=0.0): - try: - if v is None: - v = default - return f"{float(v):.{casas}f}" - except Exception: - return f"{default:.{casas}f}" - - def _m(loop, nome, stat="med", default=0.0): - try: - return loop.get("metrics_ms", {}).get(nome, {}).get(stat, default) - except Exception: - return default - - def _lat(loop, stat="med", default=0.0): - try: - return loop.get("latencia_ms", {}).get(stat, default) - except Exception: - return default - - def _per(loop, stat="med", default=0.0): - try: - return loop.get("periodo_ms", {}).get(stat, default) - except Exception: - return default - - def _fps(loop): - try: - return float(loop.get("fps_real", 0.0) or 0.0) - except Exception: - return 0.0 - - tensor = loops.get("tensor", {}) - inf = loops.get("inferencia", {}) - det = loops.get("deteccao", {}) - pub = loops.get("publicacao", {}) - stream = loops.get("stream", {}) - - pub_dbg = getattr(self, "_ultimo_pub_debug", {}) - - self.mostrar_log( - "[WEED_PERF] " - f"fps tensor={_fps(tensor):.1f} " - f"inf={_fps(inf):.1f} " - f"det={_fps(det):.1f} " - f"pub={_fps(pub):.1f} " - f"stream={_fps(stream):.1f} | " - f"period inf={_fmt(_per(inf))}ms " - f"det={_fmt(_per(det))}ms " - f"tensor={_fmt(_per(tensor))}ms" - ) - - self.mostrar_log( - "[WEED_DETAIL] " - f"INF total={_fmt(_lat(inf))} " - f"get_tensor={_fmt(_m(inf, 'get_tensor_ms'))} " - f"infer={_fmt(_m(inf, 'infer_ms'))} " - f"gpu={_fmt(_m(inf, 'infer_gpu_ms'))} | " - f"DET total={_fmt(_lat(det))} " - f"get_pred={_fmt(_m(det, 'get_pred_ms'))} " - f"detector={_fmt(_m(det, 'detector_ms'))} " - f"ctx={_fmt(_m(det, 'ctx_read_ms'))} " - f"ctrl={_fmt(_m(det, 'control_ms'))} " - f"cache={_fmt(_m(det, 'pub_cache_ms'))} | " - f"TENSOR total={_fmt(_lat(tensor))} " - f"core={_fmt(_m(tensor, 'tensor_core_ms'))} | " - f"PUB total={_fmt(_lat(pub))} redis={_fmt(_m(pub, 'redis_ms'))} " - f"debug_pub={pub_dbg.get('publicou', 0)} " - f"campos={pub_dbg.get('campos', 0)} " - f"redis_dbg={pub_dbg.get('redis_ms', 0):.1f}ms" + self.perf.tick( + "tensor", + latencia_ms=(t1 - t0) * 1000.0, + tensor_core_ms=float(perf.get("total_ms", 0.0) or 0.0), + get_decoded_ms=float(perf.get("get_decoded_ms", 0.0) or 0.0), + build_ms=float(perf.get("build_ms", 0.0) or 0.0), + validate_ms=float(perf.get("validate_ms", 0.0) or 0.0), + post_ms=float(perf.get("post_ms", 0.0) or 0.0), + preview_ms=float(perf.get("preview_ms", 0.0) or 0.0), + frame_ts=agora, + idade_frame_ms=0.0, ) except Exception as e: - self.mostrar_log(f"Erro no loop supervisor weed: {e}") + self.mostrar_log(f"[CAPTURE] erro: {e}") finally: - latencia = time.time() - t0 - time.sleep(max(0.0, (1.0 / freq) - latencia)) + lat = time.time() - t0_wall + time.sleep(max(0.0, (1.0 / freq) - lat)) threading.Thread(target=loop, daemon=True).start() - def _iniciar_loop_frame_stream(self, freq): + def _iniciar_loop_inferencia(self, freq=25.0): def loop(): while True: - if self.camera is None: - time.sleep(5) - continue - t0 = time.time() + t0_wall = time.time() t_loop0 = time.perf_counter() - try: - _camera = (ContextoGlobalRedis.get_camera(self.mx_id) or {}) - _stream_on = _camera.get("streaming", False) - if (_stream_on): - _frame_type = TipoFrameCamera(_camera.get("frame_type", TipoFrameCamera.Rgb.value)) - _frame = self.get_selected_frame(_frame_type) - self.camera.enviar_frame_tcp(_frame) - if (self.debug_visual): - self.get_debug_frame(True) - except Exception as e: - self.mostrar_log(f"Erro no loop de stream: {e}") - finally: - t_loop1 = time.perf_counter() - self.perf.tick( - "stream", - latencia_ms=(t_loop1 - t_loop0) * 1000.0, + try: + if self.iniciando: + time.sleep(0.05) + continue + + if not self.operante or self.camera is None: + time.sleep(0.2) + continue + + t_get0 = time.perf_counter() + tensor5, tensor_ts, res = self._get_tensor_novo_para_inferencia() + t_get1 = time.perf_counter() + + if tensor5 is None or tensor_ts is None: + self.perf.inc("infer_sem_tensor_novo") + time.sleep(0.005) + continue + + t_inf0 = time.perf_counter() + predictions = self.model_svc.infer_tensor_fast(tensor5, keep_probs=False) + t_inf1 = time.perf_counter() + + infer_ms = (t_inf1 - t_inf0) * 1000.0 + infer_full = getattr(self.model_svc, "_ultimo_predictions_full", {}) or {} + + infer_forward_ms = infer_full.get( + "forward_ms", + infer_full.get("infer_ms", 0.0), ) - latencia = time.time() - t0 - freq = self.camera.stream._op_fps - time.sleep(max(0, (1.0 / freq) - latencia)) + infer_prepare_ms = infer_full.get("prepare_ms", 0.0) + infer_post_ms = infer_full.get("post_ms", 0.0) + + self._atualizar_fps_inferencia( + infer_ms=infer_ms, + infer_gpu_ms=infer_forward_ms, + ) + + if predictions is None: + self.perf.inc("infer_predictions_none") + continue + + pred_ts = time.time() + + with self._pred_lock: + self._pred_cache = { + "ts": pred_ts, + "tensor_ts": tensor_ts, + "predictions": predictions, + "res": res, + "infer_ms": float(infer_ms), + "infer_gpu_ms": float(infer_forward_ms or 0.0), + "infer_prepare_ms": float(infer_prepare_ms or 0.0), + "infer_post_ms": float(infer_post_ms or 0.0), + "fps_model": float(self._fps_infer_ema or 0.0), + } + + self._ultimo_raw_base = tensor5 + self._ultimo_raw_input = tensor5 + self._ultimo_predictions = predictions + self._ultimo_predictions_full = infer_full + + t_loop1 = time.perf_counter() + + self.perf.tick( + "inferencia", + latencia_ms=(t_loop1 - t_loop0) * 1000.0, + get_tensor_ms=(t_get1 - t_get0) * 1000.0, + infer_ms=infer_ms, + infer_forward_ms=float(infer_forward_ms or 0.0), + infer_prepare_ms=float(infer_prepare_ms or 0.0), + infer_post_ms=float(infer_post_ms or 0.0), + infer_gpu_ms=float(infer_forward_ms or 0.0), + tensor_ts=tensor_ts, + frame_ts=pred_ts, + idade_tensor_ms=(time.time() - tensor_ts) * 1000.0 if tensor_ts else None, + ) + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_inferencia weed: {e}") + + finally: + lat = time.time() - t0_wall + time.sleep(max(0.0, (1.0 / freq) - lat)) + threading.Thread(target=loop, daemon=True).start() - def _iniciar_loop_publicacao_weed(self, freq=15.0): + def _iniciar_loop_deteccao_weed(self, freq=25.0): + def loop(): + while True: + t0_wall = time.time() + t_loop0 = time.perf_counter() + + try: + if self.iniciando: + time.sleep(0.05) + continue + + if not self.operante or self.camera is None: + time.sleep(0.2) + continue + + self._atualizar_config_dinamica_detector(intervalo_s=0.20) + + t_pred0 = time.perf_counter() + pred_cache = self._get_prediction_nova_para_deteccao() + t_pred1 = time.perf_counter() + + if pred_cache is None: + self.perf.inc("det_sem_prediction_nova") + time.sleep(0.005) + continue + + predictions = pred_cache.get("predictions") + pred_ts = float(pred_cache.get("ts", 0.0) or 0.0) + tensor_ts = float(pred_cache.get("tensor_ts", 0.0) or 0.0) + + if predictions is None: + self.perf.inc("det_predictions_none") + continue + + t_det0 = time.perf_counter() + analise_completa = self.detectar_ervas(predictions) + t_det1 = time.perf_counter() + + if not isinstance(analise_completa, dict): + self.mostrar_log("[WEED] análise inválida retornada pelo WeedDetector") + continue + + analise = analise_completa.get("dados_visuais", {}) + detector_ms = (t_det1 - t_det0) * 1000.0 + + t_conv0 = time.perf_counter() + analise_convertida = converter_valores_numpy(analise) + t_conv1 = time.perf_counter() + + t_pub_analise0 = time.perf_counter() + self._set_pub_cache("analise", { + "ts_analise": pred_ts, + "fps_model": pred_cache.get("fps_model", 0.0), + "fps_inferencia": self._fps_infer_ema, + "infer_ms": pred_cache.get("infer_ms", 0.0), + "infer_gpu_ms": pred_cache.get("infer_gpu_ms", 0.0), + "analise": analise_convertida, + }) + t_pub_analise1 = time.perf_counter() + + t_ctx0 = time.perf_counter() + operacao = ContextoGlobalRedis.get_operacao() + controle = ContextoGlobalRedis.get_controle() + + status_operacao = StatusOperacao( + operacao.get("status", StatusOperacao.NaoIniciado.value) + ) + finalizando = bool(operacao.get("finalizando", False)) + pulverizador_automatico = bool( + controle.get("pulverizador_automatico", False) + ) + t_ctx1 = time.perf_counter() + + t_ctrl0 = time.perf_counter() + controle_detectado = analise.get("controle") or {} + + if status_operacao == StatusOperacao.EmAndamento and not finalizando: + atuacao_bicos = controle_detectado + else: + atuacao_bicos = {i: False for i in controle_detectado.keys()} + + if not atuacao_bicos: + atuacao_bicos = {i: False for i in range(self.qtd_bicos)} + + analise["controle"] = atuacao_bicos + self._ultimo_controle = atuacao_bicos + t_ctrl1 = time.perf_counter() + + t_pub_ctrl0 = time.perf_counter() + self._set_pub_cache("controle_bicos", atuacao_bicos) + t_pub_ctrl1 = time.perf_counter() + + t_pub_cmd0 = time.perf_counter() + if pulverizador_automatico: + self._set_pub_cache("cmd_controle", { + "cmd": WeedWorkerCommandType.EnviarDadosControle.value, + "params": atuacao_bicos, + }) + t_pub_cmd1 = time.perf_counter() + + self._ultima_analise = analise_completa.copy() + + t_loop1 = time.perf_counter() + total_ms = (t_loop1 - t_loop0) * 1000.0 + self._ultimo_loop_analise_fps = 1000.0 / max(total_ms, 1e-6) + + self._perf_weed = { + "fps_model": pred_cache.get("fps_model", 0.0), + "fps_inferencia": self._fps_infer_ema, + "infer_ms": pred_cache.get("infer_ms", 0.0), + "infer_gpu_ms": pred_cache.get("infer_gpu_ms", 0.0), + + "detector_ms": detector_ms, + "convert_ms": (t_conv1 - t_conv0) * 1000.0, + "ctx_read_ms": (t_ctx1 - t_ctx0) * 1000.0, + "control_ms": (t_ctrl1 - t_ctrl0) * 1000.0, + + "pub_analise_ms": (t_pub_analise1 - t_pub_analise0) * 1000.0, + "pub_controle_ms": (t_pub_ctrl1 - t_pub_ctrl0) * 1000.0, + "pub_cmd_ms": (t_pub_cmd1 - t_pub_cmd0) * 1000.0, + + "total_ms": total_ms, + "fps_loop": self._ultimo_loop_analise_fps, + "pub_debug": getattr(self, "_ultimo_pub_debug", {}), + "pred_age_ms": (time.time() - pred_ts) * 1000.0 if pred_ts else None, + "tensor_age_ms": (time.time() - tensor_ts) * 1000.0 if tensor_ts else None, + } + + pub_cache_ms = ( + (t_pub_analise1 - t_pub_analise0) + + (t_pub_ctrl1 - t_pub_ctrl0) + + (t_pub_cmd1 - t_pub_cmd0) + ) * 1000.0 + + self.perf.tick( + "deteccao", + latencia_ms=total_ms, + get_pred_ms=(t_pred1 - t_pred0) * 1000.0, + detector_ms=detector_ms, + convert_ms=(t_conv1 - t_conv0) * 1000.0, + ctx_read_ms=(t_ctx1 - t_ctx0) * 1000.0, + control_ms=(t_ctrl1 - t_ctrl0) * 1000.0, + pub_analise_ms=(t_pub_analise1 - t_pub_analise0) * 1000.0, + pub_controle_ms=(t_pub_ctrl1 - t_pub_ctrl0) * 1000.0, + pub_cmd_ms=(t_pub_cmd1 - t_pub_cmd0) * 1000.0, + pub_cache_ms=pub_cache_ms, + infer_ms=pred_cache.get("infer_ms", 0.0), + infer_gpu_ms=pred_cache.get("infer_gpu_ms", 0.0), + pred_ts=pred_ts, + tensor_ts=tensor_ts, + frame_ts=pred_ts, + idade_frame_ms=(time.time() - pred_ts) * 1000.0 if pred_ts else None, + idade_tensor_ms=(time.time() - tensor_ts) * 1000.0 if tensor_ts else None, + ) + + except Exception as e: + self.mostrar_log(f"❌ Erro no loop_deteccao_weed: {e}") + + finally: + lat = time.time() - t0_wall + time.sleep(max(0.0, (1.0 / freq) - lat)) + + threading.Thread(target=loop, daemon=True).start() + + def _iniciar_loop_publicacao_weed(self, freq=5.0): def loop(): ultimo_payload_forcado = 0.0 @@ -835,36 +1129,33 @@ class CameraManager: publicar_tudo = (agora - ultimo_payload_forcado) >= 1.0 payload_weed, payload_controle, cmd_controle = self._montar_payload_publicacao_weed( - publicar_tudo=publicar_tudo + publicar_tudo=publicar_tudo, ) publicou = 0 campos = 0 t_redis0 = time.perf_counter() - # Dados do WeedWorker if len(payload_weed.keys()) > 1: ContextoGlobalRedis.atualizar_ctx_dict( CtxKey.DadosWeedWorker, - **converter_valores_numpy(payload_weed) + **converter_valores_numpy(payload_weed), ) publicou = 1 campos += len(payload_weed) - # DadosControle if payload_controle is not None: ContextoGlobalRedis.atualizar_ctx_dict( CtxKey.DadosControle, - **converter_valores_numpy(payload_controle) + **converter_valores_numpy(payload_controle), ) publicou = 1 campos += len(payload_controle) - # Comando para TX if cmd_controle is not None: ContextoGlobalRedis.publicar_comando( CmdKey.WeedWorkerTx, - cmd_controle + cmd_controle, ) publicou = 1 campos += 1 @@ -882,6 +1173,7 @@ class CameraManager: } t_loop1 = time.perf_counter() + self.perf.tick( "publicacao", latencia_ms=(t_loop1 - t_loop0) * 1000.0, @@ -899,412 +1191,119 @@ class CameraManager: threading.Thread(target=loop, daemon=True).start() - def _iniciar_loop_captura_tensor(self, freq=20.0): + def _iniciar_loop_frame_stream(self, freq=2.0): def loop(): while True: - if self.camera is None: - time.sleep(1) - continue - - t0 = time.perf_counter() - - try: - tensor5, res = self.camera.requisitar_tensor_multispec(force=True) - - if res.get("erro"): - erro = res.get("erro") - - if self.camera._is_erro_fatal_depthai(erro): - self.fechar_camera_manager(f"falha fatal DepthAI: {erro}") - return - - agora_log = time.time() - if not hasattr(self, "_ultimo_log_capture_perf_ts"): - self._ultimo_log_capture_perf_ts = 0.0 - - if agora_log - self._ultimo_log_capture_perf_ts >= 1.0: - self._ultimo_log_capture_perf_ts = agora_log - - #perf = res.get("perf", {}) if isinstance(res, dict) else {} - #self.mostrar_log( - # "[PERF][CAPTURE] " - # f"total={perf.get('total_ms', -1):.1f}ms " - # f"get_decoded={perf.get('get_decoded_ms', -1):.1f}ms " - # f"build={perf.get('build_ms', -1):.1f}ms " - # f"fps_capture={1000.0 / max(perf.get('total_ms', 1), 1e-6):.2f} " - # f"shape={None if tensor5 is None else tensor5.shape}" - #) - - if tensor5 is not None: - with self._lock_tensor: - self._tensor_pronto = tensor5 - self._tensor_res = res - self._tensor_ts = time.time() - - t_loop1 = time.perf_counter() - perf = res.get("perf", {}) if isinstance(res, dict) else {} - self.perf.tick( - "tensor", - latencia_ms=(t_loop1 - t0) * 1000.0, - tensor_core_ms=float(perf.get("total_ms", 0.0) or 0.0), - get_decoded_ms=float(perf.get("get_decoded_ms", 0.0) or 0.0), - build_ms=float(perf.get("build_ms", 0.0) or 0.0), - validate_ms=float(perf.get("validate_ms", 0.0) or 0.0), - post_ms=float(perf.get("post_ms", 0.0) or 0.0), - preview_ms=float(perf.get("preview_ms", 0.0) or 0.0), - frame_ts=self._tensor_ts, - idade_frame_ms=(time.time() - self._tensor_ts) * 1000.0 if self._tensor_ts else None, - ) - - except Exception as e: - self.mostrar_log(f"[CAPTURE] erro: {e}") - - lat = time.perf_counter() - t0 - time.sleep(max(0.0, (1.0 / freq) - lat)) - - threading.Thread(target=loop, daemon=True).start() - - def _iniciar_loop_inferencia(self, freq=20.0): - def loop(): - while True: - t0_wall = time.time() + t0 = time.time() t_loop0 = time.perf_counter() - if self.iniciando: - time.sleep(0.05) - continue - try: - if not self.operante or self.camera is None: - time.sleep(0.2) + if self.camera is None: + time.sleep(5.0) continue - # ===================================================== - # 1) Pegar tensor novo - # ===================================================== - t_get0 = time.perf_counter() - tensor5, tensor_ts, res = self._get_tensor_novo_para_inferencia() - t_get1 = time.perf_counter() + camera_ctx = ContextoGlobalRedis.get_camera(self.mx_id) or {} + stream_on = bool(camera_ctx.get("streaming", False)) - if tensor5 is None or tensor_ts is None: - self.perf.inc("infer_sem_tensor_novo") - time.sleep(0.005) - continue + if stream_on: + frame_type = TipoFrameCamera( + camera_ctx.get("frame_type", TipoFrameCamera.Rgb.value) + ) + frame = self.get_selected_frame(frame_type) + self.camera.enviar_frame_tcp(frame) - # ===================================================== - # 2) Inferência IA - # ===================================================== - t_inf0 = time.perf_counter() - predictions = self.model_svc.infer_tensor_fast( - tensor5, - keep_probs=False - ) - t_inf1 = time.perf_counter() + if self.debug_visual: + self.get_debug_frame(mostrar=True) - infer_ms = (t_inf1 - t_inf0) * 1000.0 - - infer_full = getattr(self.model_svc, "_ultimo_predictions_full", {}) or {} - infer_gpu_ms = infer_full.get("infer_ms", None) - - self._atualizar_fps_inferencia( - infer_ms=infer_ms, - infer_gpu_ms=infer_gpu_ms - ) - - if predictions is None: - self.perf.inc("infer_predictions_none") - continue - - # ===================================================== - # 3) Atualizar caches locais - # ===================================================== - pred_ts = time.time() - - with self._pred_lock: - self._pred_cache = { - "ts": pred_ts, - "tensor_ts": tensor_ts, - "predictions": predictions, - "res": res, - "infer_ms": float(infer_ms), - "infer_gpu_ms": float(infer_gpu_ms or 0.0), - "fps_model": float(self._fps_infer_ema or 0.0), - } - - self._ultimo_raw_base = tensor5 - self._ultimo_raw_input = tensor5 - self._ultimo_predictions = predictions - self._ultimo_predictions_full = infer_full + except Exception as e: + self.mostrar_log(f"Erro no loop de stream: {e}") + finally: t_loop1 = time.perf_counter() self.perf.tick( - "inferencia", + "stream", latencia_ms=(t_loop1 - t_loop0) * 1000.0, - get_tensor_ms=(t_get1 - t_get0) * 1000.0, - infer_ms=infer_ms, - infer_gpu_ms=float(infer_gpu_ms or 0.0), - tensor_ts=tensor_ts, - frame_ts=pred_ts, - idade_tensor_ms=(time.time() - tensor_ts) * 1000.0 if tensor_ts else None, ) - except Exception as e: - self.mostrar_log(f"❌ Erro no loop_inferencia weed: {e}") + lat = time.time() - t0 - finally: - latencia = time.time() - t0_wall - time.sleep(max(0.0, (1.0 / freq) - latencia)) + try: + freq_atual = float(getattr(self.camera.stream, "_op_fps", freq) or freq) + except Exception: + freq_atual = freq + + time.sleep(max(0.0, (1.0 / freq_atual) - lat)) threading.Thread(target=loop, daemon=True).start() - def _iniciar_loop_deteccao_weed(self, freq=20.0): + def _iniciar_loop_analise_continua(self, freq=15.0): def loop(): while True: - t0_wall = time.time() - t_loop0 = time.perf_counter() - - if self.iniciando: - time.sleep(0.05) - continue + t0 = time.time() try: - if not self.operante or self.camera is None: - time.sleep(0.2) + if self.camera is None: + time.sleep(0.5) continue - # ===================================================== - # 1) Pegar prediction nova - # ===================================================== - t_pred0 = time.perf_counter() - pred_cache = self._get_prediction_nova_para_deteccao() - t_pred1 = time.perf_counter() + self._verificar_desconexao_camera() - if pred_cache is None: - self.perf.inc("det_sem_prediction_nova") - time.sleep(0.005) - continue + agora = time.time() + if not hasattr(self, "_ultimo_perf_publish"): + self._ultimo_perf_publish = 0.0 - predictions = pred_cache.get("predictions") - pred_ts = float(pred_cache.get("ts", 0.0) or 0.0) - tensor_ts = float(pred_cache.get("tensor_ts", 0.0) or 0.0) - - if predictions is None: - self.perf.inc("det_predictions_none") - continue - - # ===================================================== - # 2) WeedDetector - # ===================================================== - t_det0 = time.perf_counter() - analise_completa = self.detectar_ervas(predictions, None) - t_det1 = time.perf_counter() - - if not isinstance(analise_completa, dict): - self.mostrar_log("[WEED] análise inválida retornada pelo WeedDetector") - continue - - analise = analise_completa.get("dados_visuais", {}) - t_detector_ms = (t_det1 - t_det0) * 1000.0 - - # ===================================================== - # 3) Conversão numpy - # ===================================================== - t_conv0 = time.perf_counter() - analise_convertida = converter_valores_numpy(analise) - t_conv1 = time.perf_counter() - - # ===================================================== - # 4) Cache publicação da análise - # ===================================================== - t_pub_analise0 = time.perf_counter() - self._set_pub_cache("analise", { - "ts_analise": pred_ts, - "fps_model": pred_cache.get("fps_model", 0.0), - "fps_inferencia": self._fps_infer_ema, - "infer_ms": pred_cache.get("infer_ms", 0.0), - "infer_gpu_ms": pred_cache.get("infer_gpu_ms", 0.0), - "analise": analise_convertida, - }) - t_pub_analise1 = time.perf_counter() - - # ===================================================== - # 5) Estado operação/controle - # ===================================================== - t_ctx0 = time.perf_counter() - _operacao = ContextoGlobalRedis.get_operacao() - status_operacao = StatusOperacao( - _operacao.get("status", StatusOperacao.NaoIniciado.value) - ) - finalizando = _operacao.get("finalizando", False) - pulverizador_automatico = ContextoGlobalRedis.get_controle().get( - "pulverizador_automatico", - False - ) - t_ctx1 = time.perf_counter() - - # ===================================================== - # 6) Controle dos bicos - # ===================================================== - t_ctrl0 = time.perf_counter() - controle_detectado = analise.get("controle") or {} - - if status_operacao == StatusOperacao.EmAndamento and not finalizando: - atuacao_bicos = controle_detectado - else: - atuacao_bicos = {i: False for i in controle_detectado.keys()} - - if not atuacao_bicos: - atuacao_bicos = {i: False for i in range(self.qtd_bicos)} - - analise["controle"] = atuacao_bicos - self._ultimo_controle = atuacao_bicos - t_ctrl1 = time.perf_counter() - - # ===================================================== - # 7) Cache publicação controle - # ===================================================== - t_pub_ctrl0 = time.perf_counter() - self._set_pub_cache("controle_bicos", atuacao_bicos) - t_pub_ctrl1 = time.perf_counter() - - # ===================================================== - # 8) Cache comando TX - # ===================================================== - t_pub_cmd0 = time.perf_counter() - if pulverizador_automatico: - self._set_pub_cache("cmd_controle", { - "cmd": WeedWorkerCommandType.EnviarDadosControle.value, - "params": atuacao_bicos - }) - t_pub_cmd1 = time.perf_counter() - - self._ultima_analise = analise_completa.copy() - - t_loop1 = time.perf_counter() - total_ms = (t_loop1 - t_loop0) * 1000.0 - - self._ultimo_loop_analise_fps = 1000.0 / max(total_ms, 1e-6) - - self._perf_weed = { - "fps_model": pred_cache.get("fps_model", 0.0), - "fps_inferencia": self._fps_infer_ema, - "infer_ms": pred_cache.get("infer_ms", 0.0), - "infer_gpu_ms": pred_cache.get("infer_gpu_ms", 0.0), - - "detector_ms": t_detector_ms, - "convert_ms": (t_conv1 - t_conv0) * 1000.0, - "ctx_read_ms": (t_ctx1 - t_ctx0) * 1000.0, - "control_ms": (t_ctrl1 - t_ctrl0) * 1000.0, - - "pub_analise_ms": (t_pub_analise1 - t_pub_analise0) * 1000.0, - "pub_controle_ms": (t_pub_ctrl1 - t_pub_ctrl0) * 1000.0, - "pub_cmd_ms": (t_pub_cmd1 - t_pub_cmd0) * 1000.0, - - "total_ms": total_ms, - "fps_loop": self._ultimo_loop_analise_fps, - "pub_debug": getattr(self, "_ultimo_pub_debug", {}), - "pred_age_ms": (time.time() - pred_ts) * 1000.0 if pred_ts else None, - "tensor_age_ms": (time.time() - tensor_ts) * 1000.0 if tensor_ts else None, - } - - self.perf.tick( - "deteccao", - latencia_ms=total_ms, - get_pred_ms=(t_pred1 - t_pred0) * 1000.0, - detector_ms=t_detector_ms, - convert_ms=(t_conv1 - t_conv0) * 1000.0, - ctx_read_ms=(t_ctx1 - t_ctx0) * 1000.0, - control_ms=(t_ctrl1 - t_ctrl0) * 1000.0, - pub_analise_ms=(t_pub_analise1 - t_pub_analise0) * 1000.0, - pub_controle_ms=(t_pub_ctrl1 - t_pub_ctrl0) * 1000.0, - pub_cmd_ms=(t_pub_cmd1 - t_pub_cmd0) * 1000.0, - pub_cache_ms=( - (t_pub_analise1 - t_pub_analise0) - + (t_pub_ctrl1 - t_pub_ctrl0) - + (t_pub_cmd1 - t_pub_cmd0) - ) * 1000.0, - infer_ms=pred_cache.get("infer_ms", 0.0), - infer_gpu_ms=pred_cache.get("infer_gpu_ms", 0.0), - pred_ts=pred_ts, - tensor_ts=tensor_ts, - frame_ts=pred_ts, - idade_frame_ms=(time.time() - pred_ts) * 1000.0 if pred_ts else None, - idade_tensor_ms=(time.time() - tensor_ts) * 1000.0 if tensor_ts else None, - ) + if agora - self._ultimo_perf_publish >= 1.0: + self._ultimo_perf_publish = agora + self._publicar_e_logar_performance() except Exception as e: - self.mostrar_log(f"❌ Erro no loop_deteccao_weed: {e}") + self.mostrar_log(f"Erro no loop supervisor weed: {e}") finally: - latencia = time.time() - t0_wall - time.sleep(max(0.0, (1.0 / freq) - latencia)) + lat = time.time() - t0 + time.sleep(max(0.0, (1.0 / freq) - lat)) threading.Thread(target=loop, daemon=True).start() - def detectar_ervas(self, predictions, rgb_frame): + def _verificar_desconexao_camera(self): + status = StatusModulo( + (self.camera.ultima_saude or {}).get( + "status", + StatusModulo.DESCONECTADO.value, + ) + ) + + ts_status = (self.camera.ultima_saude or {}).get("timestamp", 0) + + if status == StatusModulo.DESCONECTADO and (time.time() - ts_status) > 10.0: + try: + if getattr(self.camera, "imu", None): + self.camera.imu.parar() + except Exception: + pass + + self.camera = None + self.operante = False + + # ============================================================ + # Detecção / atuação + # ============================================================ + + def detectar_ervas(self, predictions): if self.weed_detector is None: self.mostrar_log("WeedDetector não inicializado!") return None + try: - return self.weed_detector.detectar(predictions, rgb_frame) + return self.weed_detector.detectar(predictions, None) except Exception as e: self.mostrar_log(f"Erro na detecção de ervas: {e}") return None - def salvar_frames(self, tipos: list, nome: str, pasta="frames_salvos"): - if not self.operante: - return [] - - try: - os.makedirs(pasta, exist_ok=True) - frames_salvos = [] - - frame_name = datetime.datetime.now().strftime('%Y%m%d_%H%M%S') - if nome is not None and nome != "": - frame_name = nome - - for _frame_type in tipos: - _frame = self.get_selected_frame(_frame_type) - if _frame is not None and _frame.size > 0: - nome_frame = f"{frame_name}_{(TipoFrameCamera(_frame_type)).name}" - if _frame_type == TipoFrameCamera.Raw4: - caminho = os.path.join(pasta, f"{nome_frame}.raw") - _frame.astype(np.float32).tofile(caminho) - else: - caminho = os.path.join(pasta, f"{nome_frame}.jpeg") - cv2.imwrite(caminho, _frame) - frames_salvos.append(caminho) - - return frames_salvos - except Exception as e: - self.mostrar_log(f"❌ Erro ao salvar frames: {e}") - return [] - - - def _atualizar_fps_inferencia(self, infer_ms=None, infer_gpu_ms=None, alpha=0.2): - agora = time.time() - - if self._fps_infer_last_ts is not None: - dt = agora - self._fps_infer_last_ts - if dt > 1e-6: - fps_inst = 1.0 / dt - if self._fps_infer_ema <= 0: - self._fps_infer_ema = fps_inst - else: - self._fps_infer_ema = (1.0 - alpha) * self._fps_infer_ema + alpha * fps_inst - - self._fps_infer_last_ts = agora - - if infer_ms is not None: - self._ultimo_infer_ms = float(infer_ms) - - if infer_gpu_ms is not None: - self._ultimo_infer_gpu_ms = float(infer_gpu_ms) - - return self._fps_infer_ema - - + # ============================================================ + # Publicação / cache Redis + # ============================================================ def _set_pub_cache(self, chave, valor): try: @@ -1343,7 +1342,7 @@ class CameraManager: if publicar_tudo or cache.get("dirty_controle_bicos", False): if cache.get("controle_bicos") is not None: payload_controle = { - "controle_bicos": cache["controle_bicos"] + "controle_bicos": cache["controle_bicos"], } if cache.get("dirty_cmd_controle", False): @@ -1362,3 +1361,172 @@ class CameraManager: self._pub_cache["dirty_cmd_controle"] = False return payload_weed, payload_controle, cmd_controle + + # ============================================================ + # Performance + # ============================================================ + + def _publicar_e_logar_performance(self): + resumo = self.perf.resumo() + + resumo["weed_cache"] = { + "tensor_ts": self._tensor_ts, + "tensor_age_ms": (time.time() - self._tensor_ts) * 1000.0 if self._tensor_ts else None, + "tensor_consumido_ts": self._tensor_consumido_ts, + "tem_tensor": self._tensor_pronto is not None, + } + + resumo["pub_debug"] = getattr(self, "_ultimo_pub_debug", {}) + + self._set_pub_cache("performance_weed", resumo) + + if not self.debug_perf: + return + + self._logar_performance(resumo) + + def _logar_performance(self, resumo): + loops = resumo.get("loops", {}) + + tensor = loops.get("tensor", {}) + inf = loops.get("inferencia", {}) + det = loops.get("deteccao", {}) + pub = loops.get("publicacao", {}) + stream = loops.get("stream", {}) + + pub_dbg = getattr(self, "_ultimo_pub_debug", {}) + + self.mostrar_log( + "[WEED_PERF] " + f"fps tensor={self._fps(tensor):.1f} " + f"inf={self._fps(inf):.1f} " + f"det={self._fps(det):.1f} " + f"pub={self._fps(pub):.1f} " + f"stream={self._fps(stream):.1f} | " + f"period inf={self._fmt(self._per(inf))}ms " + f"det={self._fmt(self._per(det))}ms " + f"tensor={self._fmt(self._per(tensor))}ms" + ) + + self.mostrar_log( + "[WEED_DETAIL] " + f"INF total={self._fmt(self._lat(inf))} " + f"get_tensor={self._fmt(self._m(inf, 'get_tensor_ms'))} " + f"infer={self._fmt(self._m(inf, 'infer_ms'))} " + f"prep={self._fmt(self._m(inf, 'infer_prepare_ms'))} " + f"fwd={self._fmt(self._m(inf, 'infer_forward_ms'))} " + f"post={self._fmt(self._m(inf, 'infer_post_ms'))} " + f"gpu={self._fmt(self._m(inf, 'infer_gpu_ms'))} | " + f"DET total={self._fmt(self._lat(det))} " + f"get_pred={self._fmt(self._m(det, 'get_pred_ms'))} " + f"detector={self._fmt(self._m(det, 'detector_ms'))} " + f"ctx={self._fmt(self._m(det, 'ctx_read_ms'))} " + f"ctrl={self._fmt(self._m(det, 'control_ms'))} " + f"cache={self._fmt(self._m(det, 'pub_cache_ms'))} | " + f"TENSOR total={self._fmt(self._lat(tensor))} " + f"core={self._fmt(self._m(tensor, 'tensor_core_ms'))} | " + f"PUB total={self._fmt(self._lat(pub))} " + f"redis={self._fmt(self._m(pub, 'redis_ms'))} " + f"debug_pub={pub_dbg.get('publicou', 0)} " + f"campos={pub_dbg.get('campos', 0)} " + f"redis_dbg={pub_dbg.get('redis_ms', 0):.1f}ms" + ) + + @staticmethod + def _fmt(v, casas=1, default=0.0): + try: + if v is None: + v = default + return f"{float(v):.{casas}f}" + except Exception: + return f"{default:.{casas}f}" + + @staticmethod + def _m(loop, nome, stat="med", default=0.0): + try: + return loop.get("metrics_ms", {}).get(nome, {}).get(stat, default) + except Exception: + return default + + @staticmethod + def _lat(loop, stat="med", default=0.0): + try: + return loop.get("latencia_ms", {}).get(stat, default) + except Exception: + return default + + @staticmethod + def _per(loop, stat="med", default=0.0): + try: + return loop.get("periodo_ms", {}).get(stat, default) + except Exception: + return default + + @staticmethod + def _fps(loop): + try: + return float(loop.get("fps_real", 0.0) or 0.0) + except Exception: + return 0.0 + + def _atualizar_fps_inferencia(self, infer_ms=None, infer_gpu_ms=None, alpha=0.2): + agora = time.time() + + if self._fps_infer_last_ts is not None: + dt = agora - self._fps_infer_last_ts + if dt > 1e-6: + fps_inst = 1.0 / dt + if self._fps_infer_ema <= 0: + self._fps_infer_ema = fps_inst + else: + self._fps_infer_ema = ( + (1.0 - alpha) * self._fps_infer_ema + + alpha * fps_inst + ) + + self._fps_infer_last_ts = agora + + if infer_ms is not None: + self._ultimo_infer_ms = float(infer_ms) + + if infer_gpu_ms is not None: + self._ultimo_infer_gpu_ms = float(infer_gpu_ms) + + return self._fps_infer_ema + + # ============================================================ + # Utilidades externas + # ============================================================ + + def salvar_frames(self, tipos: list, nome: str, pasta="frames_salvos"): + if not self.operante: + return [] + + try: + os.makedirs(pasta, exist_ok=True) + frames_salvos = [] + + frame_name = datetime.datetime.now().strftime("%Y%m%d_%H%M%S") + if nome: + frame_name = nome + + for frame_type in tipos: + frame = self.get_selected_frame(frame_type) + + if frame is not None and frame.size > 0: + nome_frame = f"{frame_name}_{TipoFrameCamera(frame_type).name}" + + if frame_type == TipoFrameCamera.Raw4: + caminho = os.path.join(pasta, f"{nome_frame}.raw") + frame.astype("float32").tofile(caminho) + else: + caminho = os.path.join(pasta, f"{nome_frame}.jpeg") + cv2.imwrite(caminho, frame) + + frames_salvos.append(caminho) + + return frames_salvos + + except Exception as e: + self.mostrar_log(f"❌ Erro ao salvar frames: {e}") + return [] diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py index affc7f890..6d8f6c45b 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py @@ -1,7 +1,5 @@ import time -import json import threading -import os from weed_worker.camera_manager import CameraManager from shared.contexto_global_redis import ContextoGlobalRedis @@ -34,147 +32,209 @@ def iniciar_camera_manager(mx_id): mostrar_log(f"✅ Camera manager iniciado, com MX_ID: {mx_id}") -_CONFIG_PATH = os.path.join(os.path.dirname(__file__), "config.json") _CONFIG_CACHE = None -_CONFIG_MTIME = None _CONFIG_LOCK = threading.Lock() -def load_seg_config(force_reload=False): - global _CONFIG_CACHE, _CONFIG_MTIME +WEED_DEFAULT_CONFIG = { + # ============================================================ + # 1) Debug e telemetria + # ============================================================ + # Mostra janela OpenCV/debug visual. Não usar em runtime de campo. + "debug_visual": False, + + # Publica logs de performance no console. + "debug_perf": True, + + # Inclui tempos internos do WeedDetector no payload de análise. + # Barato e útil nesta fase; pode desligar na versão final final. + "detector_debug_perf": True, + + + # ============================================================ + # 2) Frequências dos loops + # ============================================================ + # Pipeline oficial validado em ~25 FPS. + "tensor_fps": 25.0, + "inferencia_fps": 25.0, + "deteccao_fps": 25.0, + + # Supervisor/performance. Não precisa ser igual ao pipeline. + "analise_fps": 15.0, + + # Publicação Redis. Mantém baixo para não virar ruído. + "publicacao_fps": 5.0, + + + # ============================================================ + # 3) Câmera multiespectral + # ============================================================ + "camera_width": 1280, + "camera_height": 800, + + # FPS solicitado na câmera/OAK. Pode ser maior que o pipeline. + "camera_fps": 40, + + # Tamanho final do tensor entregue ao modelo: [W, H]. + "ia_resolution": [1024, 640], + + # Ordem oficial do tensor multiespectral. + # Deve bater com o modelo ONNX exportado. + "input_channels": ["R", "G", "B", "RE", "NIR"], + + + # ============================================================ + # 4) Modelo ONNX/TensorRT + # ============================================================ + # Runtime oficial da primeira versão. + "runtime_backend": "onnx", + "onnx_provider": "tensorrt", + + # Modo operacional: usar diretamente a cabeça target. + "runtime_mode": "target_direct", + "onnx_output_mode": "target_direct", + + # Contrato do ONNX novo: + # entrada 0..1 crua -> normalização interna -> resize -> argmax -> target_mask. + "onnx_output_kind": "mask", + "onnx_preprocess_norm": False, + + # TensorRT FP16 validado. + "trt_fp16": True, + + # Assume que o tensor já vem float32 CHW 0..1, contíguo e limpo. + "trust_input": True, + + # Não retornar dict completo no caminho quente. + "return_full_fast": False, + + # Sincronização CUDA só para debug fino. Deixar False no runtime. + "sync_for_timing": False, + + + # ============================================================ + # 5) Contrato da máscara para o WeedDetector + # ============================================================ + # target_binary: + # 0 = fundo / não pulverizar + # 1 = alvo pulverizável + "prediction_contract": "target_binary", + + + # ============================================================ + # 6) Radar global de alvo + # ============================================================ + # Gate global: só libera bicos quando existe alvo suficiente no frame. + "usar_radar_global_gate": True, + + # Histerese global da fração de alvo no frame completo. + "min_frac_erva_global_on": 0.0020, + "min_frac_erva_global_off": 0.0015, + + # Suavização temporal da fração global. + "erva_frac_ema": 0.30, + + # Ajuste do limiar global pela velocidade. + # 0.0 desativa; valores maiores deixam o gate mais sensível com velocidade. + "erva_thresh_vel_gain": 0.40, + + + # ============================================================ + # 7) Controle por bico + # ============================================================ + "qtd_bicos": 7, + + # Suavização temporal por bico. + "ema_frac_bico": 0.35, + + # Debounce temporal por bico. + "on_frames_required": 3, + "off_frames_required": 2, + + # Ajuste local do limiar por velocidade. + "erva_thresh_vel_gain_local": 0.60, + + # Desloca a ROI vertical para compensar latência em movimento. + "k_roi_shift_px_per_vnorm": 24.0, + + + # ============================================================ + # 8) Filtros opcionais da máscara + # ============================================================ + # No contrato target_binary, a própria IA já entrega o alvo final. + # Deixar desligado no baseline oficial. + "usar_morfologia": False, + "kernel_morf": 3, + + # Remove componentes pequenos. 0 desativa. + "min_area_erva_px": 0, +} + +def aplicar_overrides_redis(cfg: dict) -> dict: + dados_atu = ContextoGlobalRedis.get_operacao().get("Atu", {}) + contexto = ContextoGlobalRedis.get_contexto() + equipamento = ContextoGlobalRedis.get_equipamento() + + cfg["qtd_bicos"] = int(equipamento.get("qtd_bicos") or cfg.get("qtd_bicos", 7) or 7) + + # Modelo ONNX full-runtime validado para o Weed Worker. + cfg["ia_model_path"] = equipamento.get("path_ia_model_ervas") + cfg["ia_module_params_path"] = equipamento.get("path_ia_module_params_ervas") + + cfg["velocidade_robo"] = float( + contexto.get("Gerais", {}).get("velocidade_ms", 0.0) or 0.0 + ) + + cfg["faixa_atuacao_bicos"] = float( + dados_atu.get("percent_vertical_deteccao", 0.7) + ) + + cfg["area_atuacao_bicos"] = float( + dados_atu.get("height_area_deteccao", 0.1) + ) + + cfg["min_frac_erva_por_bico_on"] = float( + dados_atu.get("pct_erva_bico_on", 0.02) + ) + + cfg["min_frac_erva_por_bico_off"] = float( + dados_atu.get("pct_erva_bico_off", 0.01) + ) + + return cfg + +def normalizar_config_runtime(cfg: dict) -> dict: + cfg["onnx_model_path"] = cfg.get("ia_model_path") + cfg["module_calibration_json"] = cfg.get("ia_module_params_path") + + cfg["camera_fps"] = int(cfg.get("camera_fps", 40)) + + input_channels = cfg.get("input_channels", ["R", "G", "B", "RE", "NIR"]) + if isinstance(input_channels, str): + input_channels = [c.strip().upper() for c in input_channels.split(",") if c.strip()] + else: + input_channels = [str(c).upper() for c in input_channels] + + cfg["input_channels"] = input_channels + + if not cfg.get("onnx_model_path"): + mostrar_log("[WARN] path do modelo ONNX de ervas não definido no Redis/equipamento.") + + if not cfg.get("module_calibration_json"): + mostrar_log("[WARN] path de calibração/module_params do WeedWorker não definido.") + + if cfg["runtime_backend"] == "onnx" and cfg["onnx_preprocess_norm"]: + mostrar_log("[WARN] onnx_preprocess_norm=True não é permitido no modelo full-runtime.") + + return cfg + +def load_seg_config(): + global _CONFIG_CACHE + with _CONFIG_LOCK: - _CONFIG_CACHE = { - "debug_visual": False, - "debug_perf": False, - "frames_consecutivos": 3, - "frames_histerese": 2, - "min_area_px": 400, - "max_area_frac": 0.2, - "ia_roi_begin": 0.0, - "ia_roi_size": 1.0, + cfg = dict(WEED_DEFAULT_CONFIG) - "analise_fps": 15.0, - "inferencia_fps": 15.0, - "deteccao_fps": 15.0, - "tensor_fps": 18.0, - "publicacao_fps": 15.0, + cfg = aplicar_overrides_redis(cfg) + cfg = normalizar_config_runtime(cfg) - "tipo_camera_solo": "multispectral", - "camera_width": 1280, - "camera_height": 800, - "fps": 40, - "ia_resolution": [1024,640], - "ia_channels": 5, - "ia_input_channels": ["R", "G", "B", "RE", "NIR"], - "ia_use_ndvi": False, - "amp": True, - "fold_input_norm": True, - "runtime_mode": "target_direct", - "prediction_contract": "target_binary", - "output_mask_fullres": False, - "lowres_argmax": True, - "trust_input": True, - "channels_last": False, - "model_half": True, - "sync_for_timing": False, - "torch_compile": False, - "torch_compile_mode": "reduce-overhead", - "return_full_fast": False, - - "erva_top_band_frac": 0.30, - "erva_frac_ema": 0.3, - "erva_thresh_vel_gain": 0.4, - "min_frac_erva_global_on": 0.0020, - "min_frac_erva_global_off": 0.0015, - "min_frac_erva_top_on": 0.0015, - "min_frac_erva_top_off": 0.0010, - "min_frac_erva_por_bico": 0.02, - "usar_morfologia": True, - "kernel_morf": 3, - - "usar_radar_global_gate": True, - "max_frac_cana_por_bico": 0.009, - "ema_frac_bico": 0.35, - "on_frames_required": 3, - "off_frames_required": 2, - "cana_halo_px": 5, - "min_area_erva_px": 80, - "erva_thresh_vel_gain_local": 0.6, - "k_roi_shift_px_per_vnorm": 24.0, - - "heads": { - "semantic": { - "enabled": True, - "type": "multiclass", - "num_classes": 3, - "classes": {"chao": 0, "cana": 1, "erva": 2}, - "ignore_index": 255 - }, - "vegetation": { - "enabled": True, - "type": "binary", - "num_classes": 2, - "classes": {"background": 0, "vegetation": 1}, - "ignore_index": 255 - }, - "cana": { - "enabled": True, - "type": "binary", - "num_classes": 2, - "classes": {"not_cana": 0, "cana": 1}, - "ignore_index": 255 - }, - "target": { - "enabled": True, - "type": "binary", - "num_classes": 2, - "classes": {"background": 0, "target": 1}, - "ignore_index": 255 - }, - } - } - dadosAtu = ContextoGlobalRedis.get_operacao().get("Atu", {}) - contexto = ContextoGlobalRedis.get_contexto() - equipamento = ContextoGlobalRedis.get_equipamento() - _CONFIG_CACHE["qtd_bicos"] = equipamento.get("qtd_bicos") - _CONFIG_CACHE["velocidade_robo"] = contexto.get("Gerais", {}).get("velocidade_ms", 0.0) - - _CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ervas") - _CONFIG_CACHE["ia_labelmap_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_labelmap_ervas") - _CONFIG_CACHE["ia_norm_stats_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_norm_stats_ervas") - _CONFIG_CACHE["ia_module_params_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_module_params_ervas") - _CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ervas") - - _CONFIG_CACHE["faixa_atuacao_bicos"] = dadosAtu.get("percent_vertical_deteccao", 0.7) - _CONFIG_CACHE["area_atuacao_bicos"] = dadosAtu.get("height_area_deteccao", 0.1) - _CONFIG_CACHE["min_frac_erva_por_bico_on"] = dadosAtu.get("pct_erva_bico_on", 0.02) - _CONFIG_CACHE["min_frac_erva_por_bico_off"] = dadosAtu.get("pct_erva_bico_off", 0.01) - - # ============================================================ - # Compatibilidade MultiSpecSegformerService - # ============================================================ - input_channels = _CONFIG_CACHE.get("ia_input_channels", ["R", "G", "B", "RE", "NIR"]) - if isinstance(input_channels, str): - input_channels = [c.strip().upper() for c in input_channels.split(",") if c.strip()] - else: - input_channels = [str(c).upper() for c in input_channels] - - _CONFIG_CACHE["input_channels"] = input_channels - _CONFIG_CACHE["channels"] = int(_CONFIG_CACHE.get("ia_channels") or len(input_channels)) - - if _CONFIG_CACHE["channels"] != len(input_channels): - mostrar_log( - f"[WARN] ia_channels={_CONFIG_CACHE['channels']} diferente de " - f"len(input_channels)={len(input_channels)}. Usando len(input_channels)." - ) - _CONFIG_CACHE["channels"] = len(input_channels) - - _CONFIG_CACHE["backbone"] = _CONFIG_CACHE.get("ia_backbone") or "nvidia/mit-b1" - _CONFIG_CACHE["ckpt"] = _CONFIG_CACHE.get("ia_model_path") - _CONFIG_CACHE["norm_stats_path"] = _CONFIG_CACHE.get("ia_norm_stats_path") - _CONFIG_CACHE["module_calibration_json"] = (_CONFIG_CACHE.get("ia_module_params_path")) - _CONFIG_CACHE["camera_fps"] = int(_CONFIG_CACHE.get("fps")) - - return _CONFIG_CACHE - -def reload_seg_config(): - return load_seg_config(force_reload=True) + _CONFIG_CACHE = cfg + return _CONFIG_CACHE diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py index eae408a0b..f9caaaad1 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py @@ -1,657 +1,530 @@ -from enum import IntEnum +from __future__ import annotations + import time +from typing import Optional, Tuple + import cv2 import numpy as np -from shared.utils import encode_image_base64 -class ClassesSegmentacao(IntEnum): - CHAO = 0 - CANA = 1 - ERVA = 2 class WeedDetector: - def __init__(self, color_map, classes): + """ + WeedDetector v1. + + Contrato oficial: + predictions: np.ndarray uint8 HxW + 0 = fundo / não pulverizar + 1 = alvo pulverizável + + Responsabilidade: + - Receber target_mask pronta do ONNX/TensorRT. + - Calcular presença global de alvo no frame. + - Calcular atuação dos bicos por faixa vertical. + - Aplicar EMA, histerese e debounce. + + Este arquivo NÃO: + - gera imagem; + - colore máscara; + - gera base64; + - cria overlay; + - interpreta classes semânticas antigas. + """ + + CONTRATO_OFICIAL = "target_binary" + TARGET_ID = 1 + + def __init__(self): from weed_worker.config import load_seg_config - config = load_seg_config() - resolucao = config.get("ia_resolution") - self.color_map = color_map - self.classes = classes - self.runtime_mode = str(config.get("runtime_mode", "semantic")).lower() - self.prediction_contract = str(config.get("prediction_contract", "") or "").lower() - if not self.prediction_contract: - if self.runtime_mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): - self.prediction_contract = "target_binary" - else: - self.prediction_contract = "semantic" - if self.prediction_contract in ("target_binary", "binary_target", "target"): - self.classes = {"background": 0, "target": 1} - self.color_map = [ - (30, 30, 30), # background - (255, 70, 30), # target - ] - else: - self.classes = classes - self.color_map = color_map - self.resolucao = (resolucao[0], resolucao[1]) - # LUT completa 0..255 em BGR, segura para OpenCV e ignore_id. - self.lut = np.zeros((256, 3), dtype=np.uint8) - for i, color in enumerate(self.color_map): - if i >= 256: - break + self.config = load_seg_config() - # color_map vem em RGB. OpenCV/debug/base64 atual usa BGR. - r, g, b = int(color[0]), int(color[1]), int(color[2]) - self.lut[i] = (b, g, r) + self.prediction_contract = str( + self.config.get("prediction_contract", self.CONTRATO_OFICIAL) + ).lower() - IGNORE_ID = 255 - self.lut[IGNORE_ID] = (255, 255, 255) + if self.prediction_contract != self.CONTRATO_OFICIAL: + raise RuntimeError( + f"WeedDetector v1 aceita apenas prediction_contract='{self.CONTRATO_OFICIAL}'. " + f"Recebido: {self.prediction_contract}" + ) - # Mantém compatibilidade com métodos antigos de debug. - # Antes era array curto; agora fica LUT completa também. - self.color_lut = self.lut + resolucao = self.config.get("ia_resolution", [1024, 640]) + self.resolucao: Tuple[int, int] = ( + int(resolucao[0]), + int(resolucao[1]), + ) - self._reiniciar_deteccoes() - self.use_mock = False - self.img_mock = "C:\\ZendionInc\\agrobot_base\\AgroBase\\AgroBase\\bin\\x64\\Debug\\Operacoes\\25_07_2025_14_39_14\\Cam0\\85_rgb.jpeg" - self.predictions = None - self.dados_visuais = {} - self._dbg_img = None - self._dbg_img_shape = (706, 560) - self._mostrar_debug = True + self.qtd_bicos = int(self.config.get("qtd_bicos", 7) or 7) + if self.qtd_bicos <= 0: + self.qtd_bicos = 7 - self._dbg_last_ts = None - self._dbg_fps_ema = None # fps do "ciclo de debug" (pós-segmentação) - self._seg_last_ts = None - self._seg_fps_ema = None # opcional: chame quando terminar a segmentação + self.detector_debug_perf = bool( + self.config.get("detector_debug_perf", False) + ) - def _reiniciar_deteccoes(self): - from weed_worker.config import load_seg_config - config = load_seg_config() - qtd_bicos = config.get("qtd_bicos") + self._inicializar_estado() - self.ervas_ativas_radar = [] - self.ervas_ativas_filtradas = [] - self.next_erva_id = 1 - self.frame_idx = 0 - self.ultimo_status_bicos = {i: False for i in range(qtd_bicos)} - self.ervas_registradas_bico = [set() for _ in range(qtd_bicos)] - self.pred_rgb = np.empty((self.resolucao[1], self.resolucao[0], 3), dtype=np.uint8) + # ============================================================ + # Estado + # ============================================================ - # LUTs de interpretação da máscara de entrada. - # semantic: - # 0=chao, 1=cana, 2=erva - # target_binary: - # 0=background, 1=target/alvo pulverizável - self._is_weed = np.zeros(256, dtype=bool) - self._is_cane = np.zeros(256, dtype=bool) + def _inicializar_estado(self): + self.ultimo_status_bicos = { + i: False for i in range(self.qtd_bicos) + } - runtime_mode = str(config.get("runtime_mode", getattr(self, "runtime_mode", "semantic"))).lower() - contract = str(config.get("prediction_contract", getattr(self, "prediction_contract", "")) or "").lower() + # LUT booleana para interpretar target_mask. + # Isso evita comparação direta espalhada no código e protege + # caso venha algum valor extra inesperado. + self._is_target = np.zeros(256, dtype=bool) + self._is_target[self.TARGET_ID] = True - if not contract: - if runtime_mode in ("target_direct", "direct_target", "target_head", "target", "spray", "operational"): - contract = "target_binary" - else: - contract = "semantic" + w, h = self.resolucao + self._inv_total = 1.0 / max(1, w * h) - self.prediction_contract = contract + # Radar global. + self._target_frac_global_ema = 0.0 + self._target_no_radar = False + self._target_no_radar_percent = 0.0 - if contract in ("target_binary", "binary_target", "target"): - # A máscara já é o alvo final: 1 = pulverizável. - self._is_weed[1] = True + # Estado temporal por bico. + self._ema_frac_bico = np.zeros(self.qtd_bicos, dtype=np.float32) + self._on_cnt = np.zeros(self.qtd_bicos, dtype=np.int32) + self._off_cnt = np.zeros(self.qtd_bicos, dtype=np.int32) + self._estado_bico = np.zeros(self.qtd_bicos, dtype=bool) - # Não há classe cana nessa saída. - # O veto por cana fica naturalmente inativo porque mask_cana será tudo False. - self._is_cane[:] = False - - elif contract in ("semantic", "semantic_3class"): - self._is_weed[int(ClassesSegmentacao.ERVA.value)] = True - self._is_cane[int(ClassesSegmentacao.CANA.value)] = True - - else: - raise RuntimeError(f"prediction_contract inválido: {contract}") - - self._inv_total = 1.0 / (self.resolucao[1] * self.resolucao[0]) - - # EMA global - self._erva_frac_global_ema = 0.0 - self._ervas_no_radar = False - self._ervas_no_radar_percent = 0.0 - self.debug_estat = True - - # ---- NOVO: estado temporal por bico ---- - self._ema_frac_bico = np.zeros(qtd_bicos, dtype=np.float32) - self._on_cnt = np.zeros(qtd_bicos, dtype=np.int32) - self._off_cnt = np.zeros(qtd_bicos, dtype=np.int32) - self._estado_bico = np.zeros(qtd_bicos, dtype=bool) # estado pós-debounce - - # caches morfológicos - self._morf_cache_k = None - self._halo_cache_k = None + # Cache morfológico opcional. + self._morf_cache_k: Optional[int] = None self._morf_kernel = None - self._halo_kernel = None - def _segmentar_predictions(self, predictions): - try: - self.pred_rgb[:] = self.lut[predictions] + def atualizar_config(self, config: Optional[dict] = None): + """ + Atualiza config do detector sem recriar o objeto. - mask_color = self.pred_rgb - frame_color = encode_image_base64(mask_color) - - return { - "timestamp": time.time(), - "frame": { - "timestamp": time.time(), - "frame": frame_color - }, - "mask_color": mask_color, - "classes": predictions - } - - except Exception as e: - print(f"Erro ao processar predictions: {e}") - return None - - def detectar(self, predictions, rgb_frame=None): - try: - # 🔸 Constrói a máscara colorida e outras saídas com base na predictions já pronta - resultado = self._segmentar_predictions(predictions) - if resultado is None: - print("[Erro] Segmentação vazia ou falhou") - return None - - if predictions is None: - print("[Erro] Máscara de classes não encontrada no resultado") - return None - + Útil para refletir mudanças vindas do Redis/C#. + Se qtd_bicos mudar, reinicializa o estado temporal. + """ + if config is None: from weed_worker.config import load_seg_config config = load_seg_config() - # vel_norm pode vir do contexto (0..1 da sua Vmax). Se não tiver, manda 0.0 - vel_norm = float(config.get("velocidade_robo", 0.0)) - ervas_no_radar, estat_erva = self._decidir_ervas_no_radar(predictions, config, vel_norm=vel_norm) - controle_bicos, estat_bicos = self._atuacao_por_mascara(predictions, config) # sua lógica de CANA por setor + self.config = config + contrato = str( + self.config.get("prediction_contract", self.CONTRATO_OFICIAL) + ).lower() - resultado["dados_visuais"] = { - "timestamp": time.time(), - "height": predictions.shape[0], - "width": predictions.shape[1], + if contrato != self.CONTRATO_OFICIAL: + raise RuntimeError( + f"WeedDetector v1 aceita apenas prediction_contract='{self.CONTRATO_OFICIAL}'. " + f"Recebido: {contrato}" + ) + + self.prediction_contract = contrato + + novo_qtd_bicos = int( + self.config.get("qtd_bicos", self.qtd_bicos) or self.qtd_bicos + ) + + if novo_qtd_bicos <= 0: + novo_qtd_bicos = 7 + + if novo_qtd_bicos != self.qtd_bicos: + self.qtd_bicos = novo_qtd_bicos + self._inicializar_estado() + + self.detector_debug_perf = bool( + self.config.get("detector_debug_perf", self.detector_debug_perf) + ) + + # ============================================================ + # API principal + # ============================================================ + + def detectar(self, predictions: np.ndarray, rgb_frame=None): + """ + Executa o ciclo leve de detecção/controle. + + rgb_frame é aceito apenas por compatibilidade com a chamada atual + do CameraManager, mas não é usado. + """ + try: + if predictions is None: + print("[WeedDetector] predictions=None") + return None + + if not isinstance(predictions, np.ndarray): + predictions = np.asarray(predictions) + + if predictions.ndim != 2: + raise RuntimeError( + f"predictions inválido: esperado HxW, veio shape={predictions.shape}" + ) + + if predictions.dtype != np.uint8: + predictions = predictions.astype(np.uint8, copy=False) + + t0 = time.perf_counter() + + cfg = self.config + vel_norm = float(cfg.get("velocidade_robo", 0.0) or 0.0) + + t_rad0 = time.perf_counter() + target_no_radar, estat_target = self._decidir_target_no_radar( + predictions, + cfg, + vel_norm=vel_norm, + ) + t_rad1 = time.perf_counter() + + t_bic0 = time.perf_counter() + controle_bicos, estat_bicos = self._atuacao_por_mascara( + predictions, + cfg, + vel_norm=vel_norm, + ) + t_bic1 = time.perf_counter() + + agora = time.time() + + dados_visuais = { + "timestamp": agora, + "height": int(predictions.shape[0]), + "width": int(predictions.shape[1]), "controle": controle_bicos, - "ervas_no_radar": ervas_no_radar, + + # Mantém o nome antigo para não quebrar payload/C# agora. + # Semântica atual: True se há alvo pulverizável no radar. + "ervas_no_radar": bool(target_no_radar), + "estatisticas": { - "erva": estat_erva, - "bicos": estat_bicos - } + # Mantém a chave "erva" para compatibilidade do payload. + # Semântica atual: estatística de target/alvo pulverizável. + "erva": estat_target, + "bicos": estat_bicos, + }, } - #frame = rgb_frame if rgb_frame is not None else resultado["mask_color"] - #self._mostrar_debug_bicos(frame, resultado["classes"], [], controle_bicos, config) + if self.detector_debug_perf: + dados_visuais["perf_detector"] = { + "radar_ms": (t_rad1 - t_rad0) * 1000.0, + "bicos_ms": (t_bic1 - t_bic0) * 1000.0, + "total_ms": (time.perf_counter() - t0) * 1000.0, + } - return resultado + return { + "timestamp": agora, + "dados_visuais": dados_visuais, + } except Exception as e: - print(f"Erro ao detectar ervas com predictions: {e}") + print(f"Erro ao detectar alvos com predictions: {e}") return None - def _atuacao_por_mascara(self, predictions, config): - """ - Versão robusta: filtro de ruído, halo anti-cana, veto por cana, - histerese+debounce por bico, threshold adaptativo à velocidade e ROI shift. - """ - qtd_bicos = int(config.get("qtd_bicos")) - zona_inicio = float(config.get("faixa_atuacao_bicos")) - zona_altura = float(config.get("area_atuacao_bicos")) - usar_morf = bool(config.get("usar_morfologia", False)) - kernel_morf = int(config.get("kernel_morf", 3)) + # ============================================================ + # Radar global + # ============================================================ - # ---------- NOVOS PARÂMETROS (com defaults seguros) ---------- - thr_on = float(config.get("min_frac_erva_por_bico_on", 0.030)) # 3% - thr_off = float(config.get("min_frac_erva_por_bico_off", 0.015)) # 1.5% (histerese) - thr_cana = float(config.get("max_frac_cana_por_bico", 0.005)) # 0.5%: veto de cana - - ema_alpha = float(config.get("ema_frac_bico", 0.35)) - on_frames = int(config.get("on_frames_required", 2)) - off_frames = int(config.get("off_frames_required", 2)) - - cane_halo_px = int(config.get("cana_halo_px", 8)) # halo anti-cana (px) - area_min_px = int(config.get("min_area_erva_px", 120)) # remove grãos minúsculos - - # thresholds sobem com a velocidade (0..1 normalizada que você já passa) - vel_norm = float(config.get("velocidade_robo", 0.0)) - vel_gain_local = float(config.get("erva_thresh_vel_gain_local", 0.6)) # 60% a + no thr_on/off em vel=1 - scale = 1.0 + vel_gain_local * max(0.0, min(1.0, vel_norm)) - thr_on_eff = thr_on * scale - thr_off_eff = thr_off * scale - - # gate global (se quiser só atuar quando há erva "de verdade" no quadro) - usar_gate_global = bool(config.get("usar_radar_global_gate", True)) - if usar_gate_global and not self._ervas_no_radar: - zeros = np.zeros(qtd_bicos, dtype=bool) - estat = { - "frac_erva_por_bico": zeros.astype(float), - "frac_cana_por_bico": zeros.astype(float), - "ema_frac_por_bico": self._ema_frac_bico.copy(), - "faixa": {"y_top": 0, "y_bot": 0}, - "larguras": np.zeros(qtd_bicos, int), - "motivo": "gate_global_off" - } - self.ultimo_status_bicos = {i: False for i in range(qtd_bicos)} - self._estado_bico[:] = False - return {i: False for i in range(qtd_bicos)}, estat - - H, W = predictions.shape[:2] - - # -------- ROI vertical (com "adiantamento" por velocidade) -------- - y_inicio = int((1.0 - zona_inicio) * H) - y_fim = int((1.0 - (zona_inicio + zona_altura)) * H) - y_top = min(y_inicio, y_fim) - y_bot = max(y_inicio, y_fim) - - # desloca ROI para cima em função da velocidade (compensa latência spray/atuador) - roi_shift_per_v = float(config.get("k_roi_shift_px_per_vnorm", 24.0)) # px quando vel_norm=1 - shift = int(roi_shift_per_v * max(0.0, min(1.0, vel_norm))) - y_top = max(0, y_top - shift) - y_bot = max(y_top, min(H, y_bot - shift)) - - if y_bot <= y_top: - zeros = np.zeros(qtd_bicos, dtype=bool) - estat = {"contagem_cana_px_por_bico": np.zeros(qtd_bicos, int), - "contagem_cana_frac_por_bico": np.zeros(qtd_bicos, float), - "frac_erva_por_bico": np.zeros(qtd_bicos, float), - "frac_cana_por_bico": np.zeros(qtd_bicos, float), - "faixa": {"y_top": y_top, "y_bot": y_bot}, - "larguras": np.zeros(qtd_bicos, int)} - self.ultimo_status_bicos = {i: False for i in range(qtd_bicos)} - self._estado_bico[:] = False - return {i: False for i in range(qtd_bicos)}, estat - - band = predictions[y_top:y_bot, :] - - # ---- máscaras básicas ---- - mask_erva = self._is_weed[band] # bool - mask_cana = self._is_cane[band] # bool - - # ---- morfologia leve para tirar pontinhos (opcional) ---- - if usar_morf and kernel_morf >= 3 and (kernel_morf & 1): - if self._morf_cache_k != kernel_morf: - self._morf_cache_k = kernel_morf - self._morf_kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (kernel_morf, kernel_morf)) - mask_erva = cv2.morphologyEx(mask_erva.astype(np.uint8), cv2.MORPH_OPEN, self._morf_kernel).astype(bool) - - # ---- HALO anti-cana: não pulveriza erva colada na cana ---- - if cane_halo_px > 0: - if self._halo_cache_k != cane_halo_px: - self._halo_cache_k = cane_halo_px - self._halo_kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (cane_halo_px, cane_halo_px)) - halo = cv2.dilate(mask_cana.astype(np.uint8), self._halo_kernel).astype(bool) - mask_erva &= ~halo - - # ---- Remove componentes muito pequenos (área mínima) ---- - if area_min_px > 0: - num, labels, stats, _ = cv2.connectedComponentsWithStats(mask_erva.astype(np.uint8), connectivity=8) - if num > 1: - areas = stats[:, cv2.CC_STAT_AREA] - valid_ids = np.where(areas >= area_min_px)[0] - # remove background (id 0) - valid_ids = valid_ids[valid_ids != 0] - if len(valid_ids) > 0: - mask_erva = np.isin(labels, valid_ids) - else: - mask_erva = np.zeros_like(mask_erva, dtype=bool) - - # ---- soma por colunas e binning por bicos (sem loop) ---- - col_sums_erva = mask_erva.sum(axis=0).astype(np.int32) # (W,) - col_sums_cana = mask_cana.sum(axis=0).astype(np.int32) - - edges = np.linspace(0, W, qtd_bicos + 1, dtype=np.int32) - px_erva_por_bico = np.add.reduceat(col_sums_erva, edges[:-1]) - px_cana_por_bico = np.add.reduceat(col_sums_cana, edges[:-1]) - - alturas = (y_bot - y_top) - larguras = np.diff(edges) - area_por_bico = alturas * larguras - - with np.errstate(divide="ignore", invalid="ignore"): - frac_erva_por_bico = np.where(area_por_bico > 0, px_erva_por_bico / area_por_bico, 0.0) - frac_cana_por_bico = np.where(area_por_bico > 0, px_cana_por_bico / area_por_bico, 0.0) - - # ---- EMA por bico ---- - self._ema_frac_bico = (1.0 - ema_alpha) * self._ema_frac_bico + ema_alpha * frac_erva_por_bico - - # ---- VETO por CANA (hard) ---- - cane_veto = (frac_cana_por_bico >= thr_cana) - - # ---- Debounce + histerese ---- - cond_on = (self._ema_frac_bico >= thr_on_eff) & (~cane_veto) - cond_off = (self._ema_frac_bico < thr_off_eff) | (cane_veto) - - self._on_cnt = np.where(cond_on, self._on_cnt + 1, 0) - self._off_cnt = np.where(cond_off, self._off_cnt + 1, 0) - - estado = self._estado_bico.copy() - estado = np.where(self._on_cnt >= on_frames, True, estado) - estado = np.where(self._off_cnt >= off_frames, False, estado) - estado = np.where(cane_veto, False, estado) # veto de cana sempre derruba pra OFF - self._estado_bico = estado - - atuacao_bicos = {i: bool(estado[i]) for i in range(qtd_bicos)} - self.ultimo_status_bicos = atuacao_bicos.copy() - - estatisticas = { - "frac_erva_por_bico": frac_erva_por_bico, - "frac_cana_por_bico": frac_cana_por_bico, - "ema_frac_por_bico": self._ema_frac_bico.copy(), - "thr_on_eff": thr_on_eff, - "thr_off_eff": thr_off_eff, - "thr_cana": thr_cana, - "cane_veto": cane_veto, - "faixa": {"y_top": y_top, "y_bot": y_bot}, - "larguras": larguras, - "shift_px": shift - } - return atuacao_bicos, estatisticas - - def _decidir_ervas_no_radar(self, predictions, cfg, vel_norm=0.0): - # thresholds base - on_global = float(cfg.get("min_frac_erva_global_on", 0.0020)) + def _decidir_target_no_radar( + self, + predictions: np.ndarray, + cfg: dict, + vel_norm: float = 0.0, + ): + on_global = float(cfg.get("min_frac_erva_global_on", 0.0020)) off_global = float(cfg.get("min_frac_erva_global_off", 0.0015)) - # ajuste por velocidade - k = float(cfg.get("erva_thresh_vel_gain", 0.0)) + # Ajuste por velocidade: + # valores maiores deixam o gate global mais sensível com velocidade. + k = float(cfg.get("erva_thresh_vel_gain", 0.0) or 0.0) if k: - adj = 1.0 - k * float(vel_norm) - if adj < 0.5: adj = 0.5 - elif adj > 1.0: adj = 1.0 + adj = 1.0 - k * self._clamp01(vel_norm) + adj = min(1.0, max(0.5, adj)) on_global *= adj else: adj = 1.0 - # fração global (sem ==) - weed_sum = int(self._is_weed[predictions].sum()) - frac_global = weed_sum * self._inv_total + target_sum = int(self._is_target[predictions].sum()) + frac_global = target_sum * self._inv_total - # EMA - alpha = float(cfg.get("erva_frac_ema", 0.3)) - ema_g = self._erva_frac_global_ema = (1 - alpha) * self._erva_frac_global_ema + alpha * frac_global + alpha = float(cfg.get("erva_frac_ema", 0.30)) + alpha = min(1.0, max(0.0, alpha)) - # histerese - prev = self._ervas_no_radar - thr = off_global if prev else on_global - ervas_no_radar = ema_g >= thr - self._ervas_no_radar = ervas_no_radar - self._ervas_no_radar_percent = frac_global + ema_g = self._target_frac_global_ema = ( + (1.0 - alpha) * self._target_frac_global_ema + + alpha * frac_global + ) - if self.debug_estat: - return ervas_no_radar, { - "frac_global": frac_global, - "ema_global": ema_g, - "thr_on_global": on_global, - "thr_off_global": off_global, - "vel_adj": adj + prev = bool(self._target_no_radar) + thr = off_global if prev else on_global + + target_no_radar = bool(ema_g >= thr) + + self._target_no_radar = target_no_radar + self._target_no_radar_percent = float(frac_global) + + return target_no_radar, { + "frac_global": float(frac_global), + "ema_global": float(ema_g), + "thr_on_global": float(on_global), + "thr_off_global": float(off_global), + "vel_adj": float(adj), + } + + # ============================================================ + # Controle por bico + # ============================================================ + + def _atuacao_por_mascara( + self, + predictions: np.ndarray, + cfg: dict, + vel_norm: float = 0.0, + ): + qtd_bicos = self.qtd_bicos + + if qtd_bicos <= 0: + return {}, { + "motivo": "qtd_bicos_invalido", + "frac_erva_por_bico": np.zeros(0, dtype=np.float32), + "ema_frac_por_bico": np.zeros(0, dtype=np.float32), } - return ervas_no_radar, None - def _mostrar_debug_bicos(self, rgb_frame, classes_mask, detections, atuacao_bicos, config=None): - try: - dbg_fps = self._fps_update('_dbg_last_ts', '_dbg_fps_ema') + zona_inicio = float(cfg.get("faixa_atuacao_bicos", 0.7)) + zona_altura = float(cfg.get("area_atuacao_bicos", 0.1)) - if not self._mostrar_debug: - #print(f"FPS {dbg_fps:.2f}") - return + usar_gate_global = bool(cfg.get("usar_radar_global_gate", True)) + if usar_gate_global and not self._target_no_radar: + return self._retornar_bicos_off( + motivo="gate_global_off", + y_top=0, + y_bot=0, + shift=0, + ) - # --- cache/config --- - if config is None: - config = self._cached_cfg # já carregado fora do loop, atualize quando mudar - qtd_bicos = int(config.get("qtd_bicos")) - zona_inicio = float(config.get("faixa_atuacao_bicos")) - faixa_atuacao = float(config.get("area_atuacao_bicos")) + h, w = predictions.shape[:2] - # --- resize sem alocar --- - if not hasattr(self, "_dbg_img") or self._dbg_img.shape[:2] != self._dbg_img_shape: - self._dbg_img = np.empty((self._dbg_img_shape[1], self._dbg_img_shape[0], 3), dtype=np.uint8) - self._seg_color = np.empty((self._dbg_img_shape[1], self._dbg_img_shape[0], 3), dtype=np.uint8) - self._layer = np.zeros_like(self._dbg_img) + y_top, y_bot, shift = self._calcular_roi_vertical( + h=h, + zona_inicio=zona_inicio, + zona_altura=zona_altura, + vel_norm=vel_norm, + cfg=cfg, + ) - cv2.resize(rgb_frame, self._dbg_img_shape, dst=self._dbg_img, interpolation=cv2.INTER_AREA) - cm_resized = cv2.resize(classes_mask, self._dbg_img_shape, interpolation=cv2.INTER_NEAREST) + if y_bot <= y_top: + return self._retornar_bicos_off( + motivo="roi_invalida", + y_top=y_top, + y_bot=y_bot, + shift=shift, + ) - # --- colore segmentação por LUT (BGR) --- - # self.color_lut: (256,3) uint8 BGR (prepare uma vez no __init__) - self._seg_color[:] = self.color_lut[cm_resized] + band = predictions[y_top:y_bot, :] + mask_target = self._is_target[band] - # --- overlay da segmentação (um addWeighted) --- - cv2.addWeighted(self._seg_color, 0.35, self._dbg_img, 0.65, 0, dst=self._dbg_img) + mask_target = self._aplicar_filtros_opcionais( + mask_target=mask_target, + cfg=cfg, + ) - W, H = self._dbg_img_shape - largura_bico = W / float(qtd_bicos) + col_sums_target = mask_target.sum(axis=0).astype(np.int32) - # --- faixa de atuação: desenha em layer e blend uma vez --- - y_inicio = int((1.0 - zona_inicio) * H) - y_fim = int((1.0 - (zona_inicio + faixa_atuacao)) * H) - y0, y1 = min(y_inicio, y_fim), max(y_inicio, y_fim) + edges = np.linspace(0, w, qtd_bicos + 1, dtype=np.int32) - self._layer.fill(0) # zera layer (sem realocar) - cv2.rectangle(self._layer, (0, y0), (W, y1), (220, 220, 100), thickness=-1) - cv2.addWeighted(self._layer, 0.18, self._dbg_img, 0.82, 0, dst=self._dbg_img) - cv2.rectangle(self._dbg_img, (0, y0), (W, y1), (180, 180, 80), 2) - cv2.putText(self._dbg_img, "Zona de Atuacao", (10, max(0, y0 - 10)), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (180, 180, 80), 2) + px_target_por_bico = np.add.reduceat( + col_sums_target, + edges[:-1], + ) - # --- bicos: desenha todos os retângulos ativos na layer e blend uma vez --- - self._layer.fill(0) - CORES = [(0,255,0), (255,0,0), (0,255,255), (255,128,0), (255,0,255), (0,128,255), (128,255,0), (0,0,255)] - for i in range(qtd_bicos): - x0 = int(i * largura_bico) - x1 = int((i + 1) * largura_bico) - cor = CORES[i % len(CORES)] - if atuacao_bicos.get(i, False): - cv2.rectangle(self._layer, (x0, y0), (x1, y1), cor, thickness=-1) - # um blend para todos os bicos ligados - cv2.addWeighted(self._layer, 0.15, self._dbg_img, 0.85, 0, dst=self._dbg_img) + alturas = int(y_bot - y_top) + larguras = np.diff(edges).astype(np.int32) + area_por_bico = alturas * larguras - # bordas + texto (rápido, mantém no loop) - for i in range(qtd_bicos): - x0 = int(i * largura_bico) - x1 = int((i + 1) * largura_bico) - cor = CORES[i % len(CORES)] - cv2.rectangle(self._dbg_img, (x0, y0), (x1, y1), cor, 1) - status = "ON" if atuacao_bicos.get(i, False) else "OFF" - cv2.putText(self._dbg_img, f"Bico {i} {status}", (x0 + 5, min(H-5, y1 + 20)), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, cor, 2) + with np.errstate(divide="ignore", invalid="ignore"): + frac_target_por_bico = np.where( + area_por_bico > 0, + px_target_por_bico / area_por_bico, + 0.0, + ).astype(np.float32) - # --- bboxes (escala pro debug HxW) --- - sx = W / float(self.resolucao[0]) - sy = H / float(self.resolucao[1]) - for det in detections: - x = int(det["bbox"][0] * sx); y = int(det["bbox"][1] * sy) - w = int(det["bbox"][2] * sx); h = int(det["bbox"][3] * sy) - bbox_cor = (0, 0, 255) - cv2.rectangle(self._dbg_img, (x, y), (x + w, y + h), bbox_cor, 2) - cv2.putText(self._dbg_img, f'ID:{det.get("id")} {det.get("descricao","erva")} {det.get("confianca",0):.2f}', - (x, max(0, y - 5)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, bbox_cor, 2) + atuacao_bicos, estat_debounce = self._atualizar_estado_bicos( + frac_target_por_bico=frac_target_por_bico, + cfg=cfg, + vel_norm=vel_norm, + ) - # --- HUD --- - cv2.putText(self._dbg_img, f"Ervas no radar: {'Sim' if self._ervas_no_radar else 'Nao'} ({(self._ervas_no_radar_percent * 100.0):.2f}%)", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0,255,0), 2) - cv2.putText(self._dbg_img, f"Dbg FPS: {dbg_fps:.1f}", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0,255,0), 2) + estatisticas = { + # Mantém nomes antigos por compatibilidade de payload. + # Semântica atual: fração de target/alvo pulverizável. + "frac_erva_por_bico": frac_target_por_bico, + "ema_frac_por_bico": self._ema_frac_bico.copy(), + "faixa": { + "y_top": int(y_top), + "y_bot": int(y_bot), + }, + "larguras": larguras, + "shift_px": int(shift), + } + estatisticas.update(estat_debounce) - # já está em BGR - cv2.imshow("Debug Weed Worker", self._dbg_img) - cv2.waitKey(1) - except Exception as e: - print(f"Erro ao mostrar debug: {e}") + return atuacao_bicos, estatisticas - def _mostrar_debug_bicos_overlay(self, overlay_bgr, detections, atuacao_bicos, config=None, show: bool = True, metricas_perf=None): - """ - Versão otimizada do debug: recebe o overlay BGR já montado - (RGB + segmentação) e só desenha faixa, bicos, bboxes e HUD. - """ - try: - dbg_fps = self._fps_update('_dbg_last_ts', '_dbg_fps_ema') + def _retornar_bicos_off( + self, + motivo: str, + y_top: int, + y_bot: int, + shift: int, + ): + self._reset_bicos_off() - if not self._mostrar_debug: - # print(f"FPS dbg: {dbg_fps:.2f}") - return + zeros_f = np.zeros(self.qtd_bicos, dtype=np.float32) - if overlay_bgr is None: - return + return {i: False for i in range(self.qtd_bicos)}, { + "motivo": motivo, + "frac_erva_por_bico": zeros_f, + "ema_frac_por_bico": self._ema_frac_bico.copy(), + "faixa": { + "y_top": int(y_top), + "y_bot": int(y_bot), + }, + "larguras": np.zeros(self.qtd_bicos, dtype=np.int32), + "shift_px": int(shift), + } - # --- cache/config --- - if config is None: - from weed_worker.config import load_seg_config - config = load_seg_config() + def _calcular_roi_vertical( + self, + h: int, + zona_inicio: float, + zona_altura: float, + vel_norm: float, + cfg: dict, + ): + # Convenção atual: + # zona_inicio e zona_altura são frações medidas a partir da parte inferior da imagem. + y_inicio = int((1.0 - zona_inicio) * h) + y_fim = int((1.0 - (zona_inicio + zona_altura)) * h) - qtd_bicos = int(config.get("qtd_bicos", 0)) - zona_inicio = float(config.get("faixa_atuacao_bicos", 0.2)) - faixa_atuacao = float(config.get("area_atuacao_bicos", 0.3)) + y_top = min(y_inicio, y_fim) + y_bot = max(y_inicio, y_fim) - # --- alocação única dos buffers de debug --- - if self._dbg_img is None or self._dbg_img.shape[:2] != (self._dbg_img_shape[1], self._dbg_img_shape[0]): - # _dbg_img_shape = (W, H) - self._dbg_img = np.empty((self._dbg_img_shape[1], self._dbg_img_shape[0], 3), dtype=np.uint8) - self._layer = np.zeros_like(self._dbg_img) + roi_shift_per_v = float(cfg.get("k_roi_shift_px_per_vnorm", 24.0)) + shift = int(roi_shift_per_v * self._clamp01(vel_norm)) - # --- copia/resize do overlay para _dbg_img --- - h_src, w_src = overlay_bgr.shape[:2] - W, H = self._dbg_img_shape # (W, H) + y_top = max(0, y_top - shift) + y_bot = max(y_top, min(h, y_bot - shift)) - if (w_src, h_src) != (W, H): - # redimensiona overlay para o tamanho de debug - cv2.resize(overlay_bgr, (W, H), dst=self._dbg_img, interpolation=cv2.INTER_AREA) - else: - # mesmo tamanho, só copia - self._dbg_img[...] = overlay_bgr + return int(y_top), int(y_bot), int(shift) - # a partir daqui, igualzinho ao método antigo, só usando _dbg_img como base + def _aplicar_filtros_opcionais( + self, + mask_target: np.ndarray, + cfg: dict, + ) -> np.ndarray: + usar_morf = bool(cfg.get("usar_morfologia", False)) + kernel_morf = int(cfg.get("kernel_morf", 3)) - largura_bico = W / float(max(qtd_bicos, 1)) - - # --- faixa de atuação --- - y_inicio = int((1.0 - zona_inicio) * H) - y_fim = int((1.0 - (zona_inicio + faixa_atuacao)) * H) - y0, y1 = min(y_inicio, y_fim), max(y_inicio, y_fim) - - self._layer.fill(0) - cv2.rectangle(self._layer, (0, y0), (W, y1), (220, 220, 100), thickness=-1) - cv2.addWeighted(self._layer, 0.18, self._dbg_img, 0.82, 0, dst=self._dbg_img) - cv2.rectangle(self._dbg_img, (0, y0), (W, y1), (180, 180, 80), 2) - cv2.putText(self._dbg_img, "Zona de Atuacao", (10, max(0, y0 - 10)), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (180, 180, 80), 2) - - # --- bicos --- - self._layer.fill(0) - CORES = [ (0,255,0), (255,0,0), (0,255,255), (255,128,0), (255,0,255), (0,128,255), (128,255,0), (0,0,255) ] - for i in range(qtd_bicos): - x0 = int(i * largura_bico) - x1 = int((i + 1) * largura_bico) - cor = CORES[i % len(CORES)] - if atuacao_bicos.get(i, False): - cv2.rectangle(self._layer, (x0, y0), (x1, y1), cor, thickness=-1) - - cv2.addWeighted(self._layer, 0.15, self._dbg_img, 0.85, 0, dst=self._dbg_img) - - # bordas + texto ON/OFF - for i in range(qtd_bicos): - x0 = int(i * largura_bico) - x1 = int((i + 1) * largura_bico) - cor = CORES[i % len(CORES)] - cv2.rectangle(self._dbg_img, (x0, y0), (x1, y1), cor, 1) - status = "ON" if atuacao_bicos.get(i, False) else "OFF" - cv2.putText( - self._dbg_img, - f"Bico {i} {status}", - (x0 + 5, min(H - 5, y1 + 20)), - cv2.FONT_HERSHEY_SIMPLEX, - 0.6, - cor, - 2 + if usar_morf and kernel_morf >= 3 and (kernel_morf & 1): + if self._morf_cache_k != kernel_morf: + self._morf_cache_k = kernel_morf + self._morf_kernel = cv2.getStructuringElement( + cv2.MORPH_RECT, + (kernel_morf, kernel_morf), ) - # --- bboxes (escala pro debug HxW) --- - # self.resolucao = (W_src_model, H_src_model) - sx = W / float(self.resolucao[0]) - sy = H / float(self.resolucao[1]) + mask_target = cv2.morphologyEx( + mask_target.astype(np.uint8), + cv2.MORPH_OPEN, + self._morf_kernel, + ).astype(bool) - for det in detections: - bx, by, bw, bh = det["bbox"] - x = int(bx * sx) - y = int(by * sy) - w = int(bw * sx) - h = int(bh * sy) - bbox_cor = (0, 0, 255) - cv2.rectangle(self._dbg_img, (x, y), (x + w, y + h), bbox_cor, 2) - cv2.putText( - self._dbg_img, - f'ID:{det.get("id")} {det.get("descricao","erva")} {det.get("confianca",0):.2f}', - (x, max(0, y - 5)), - cv2.FONT_HERSHEY_SIMPLEX, - 0.5, - bbox_cor, - 2 - ) - - # --- HUD --- - cv2.putText( - self._dbg_img, - f"Ervas no radar: {'Sim' if self._ervas_no_radar else 'Nao'} ({(self._ervas_no_radar_percent * 100.0):.2f}%)", - (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 0.8, - (0, 255, 0), - 2 - ) - - metricas_perf = metricas_perf or {} - fps_infer = float(metricas_perf.get("fps_infer") or 0.0) - infer_ms = float(metricas_perf.get("infer_ms") or 0.0) - infer_gpu_ms = float(metricas_perf.get("infer_gpu_ms") or 0.0) - fps_loop = float(metricas_perf.get("fps_loop") or 0.0) - cv2.putText( - self._dbg_img, - f"Dbg FPS: {dbg_fps:.1f}", - (10, 60), - cv2.FONT_HERSHEY_SIMPLEX, - 0.8, - (0, 255, 0), - 2 - ) - cv2.putText( - self._dbg_img, - f"Infer FPS: {fps_infer:.1f} | infer: {infer_ms:.1f}ms", - (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, - 0.75, - (0, 255, 255), - 2 - ) - cv2.putText( - self._dbg_img, - f"Loop FPS: {fps_loop:.1f} | GPU: {infer_gpu_ms:.1f}ms", - (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, - 0.75, - (0, 255, 255), - 2 + area_min_px = int(cfg.get("min_area_erva_px", 0)) + if area_min_px > 0: + num, labels, stats, _ = cv2.connectedComponentsWithStats( + mask_target.astype(np.uint8), + connectivity=8, ) - if (show): - cv2.imshow("Debug Weed Worker", self._dbg_img) - cv2.waitKey(1) + if num > 1: + areas = stats[:, cv2.CC_STAT_AREA] + valid_ids = np.where(areas >= area_min_px)[0] + valid_ids = valid_ids[valid_ids != 0] - return self._dbg_img + if len(valid_ids) > 0: + mask_target = np.isin(labels, valid_ids) + else: + mask_target = np.zeros_like(mask_target, dtype=bool) - except Exception as e: - print(f"Erro ao mostrar debug (overlay): {e}") - return None + return mask_target - def _fps_update(self, last_ts_attr: str, ema_attr: str, alpha: float = 0.2): - """Atualiza e retorna FPS (EMA) baseado no timestamp anterior salvo em self""" - import time - now = time.time() - last = getattr(self, last_ts_attr) - fps_ema = getattr(self, ema_attr) - if last is not None: - inst_fps = 1.0 / max(1e-6, (now - last)) - fps_ema = inst_fps if fps_ema is None else (1 - alpha) * fps_ema + alpha * inst_fps - setattr(self, last_ts_attr, now) - setattr(self, ema_attr, fps_ema) - return fps_ema if fps_ema is not None else 0.0 + def _atualizar_estado_bicos( + self, + frac_target_por_bico: np.ndarray, + cfg: dict, + vel_norm: float, + ): + thr_on = float(cfg.get("min_frac_erva_por_bico_on", 0.030)) + thr_off = float(cfg.get("min_frac_erva_por_bico_off", 0.015)) + + ema_alpha = float(cfg.get("ema_frac_bico", 0.35)) + ema_alpha = min(1.0, max(0.0, ema_alpha)) + + on_frames = int(cfg.get("on_frames_required", 2)) + off_frames = int(cfg.get("off_frames_required", 2)) + + vel_gain_local = float(cfg.get("erva_thresh_vel_gain_local", 0.6)) + scale = 1.0 + vel_gain_local * self._clamp01(vel_norm) + + thr_on_eff = thr_on * scale + thr_off_eff = thr_off * scale + + self._ema_frac_bico = ( + (1.0 - ema_alpha) * self._ema_frac_bico + + ema_alpha * frac_target_por_bico + ) + + cond_on = self._ema_frac_bico >= thr_on_eff + cond_off = self._ema_frac_bico < thr_off_eff + + self._on_cnt = np.where(cond_on, self._on_cnt + 1, 0) + self._off_cnt = np.where(cond_off, self._off_cnt + 1, 0) + + estado = self._estado_bico.copy() + estado = np.where(self._on_cnt >= on_frames, True, estado) + estado = np.where(self._off_cnt >= off_frames, False, estado) + + self._estado_bico = estado.astype(bool, copy=False) + + atuacao_bicos = { + i: bool(self._estado_bico[i]) + for i in range(self.qtd_bicos) + } + + self.ultimo_status_bicos = atuacao_bicos.copy() + + estat = { + "thr_on_eff": float(thr_on_eff), + "thr_off_eff": float(thr_off_eff), + "on_cnt": self._on_cnt.copy(), + "off_cnt": self._off_cnt.copy(), + "estado_bico": self._estado_bico.copy(), + } + + return atuacao_bicos, estat + + def _reset_bicos_off(self): + self._estado_bico[:] = False + self._on_cnt[:] = 0 + self._off_cnt[:] = 0 + self.ultimo_status_bicos = { + i: False for i in range(self.qtd_bicos) + } + + # ============================================================ + # Utils + # ============================================================ + + @staticmethod + def _clamp01(v: float) -> float: + return max(0.0, min(1.0, float(v))) \ No newline at end of file