diff --git a/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs b/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs index a13b1c8d1..26a085bad 100644 --- a/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs +++ b/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs @@ -54,6 +54,7 @@ namespace AgroBase.Models.Operadores { public StatusCarroMapa StatusCarro { get; set; } public double ErroLateral { get; set; } + public double ErroAngular { get; set; } public bool ObstaculoDetectado { get; set; } = false; public bool DeveParar { get; set; } = false; @@ -67,7 +68,8 @@ namespace AgroBase.Models.Operadores var Sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.OperadorVisual; ObstaculoDetectado = (Sonar?.Analises?.matriz_confianca?.block?.decision?.parar ?? false); StatusCarro = Sonar?.Analises?.segmentacao?.status_corredor ?? StatusCarroMapa.Indefinido; - ErroLateral = Sonar?.Analises?.segmentacao?.erro_angular ?? 0; + ErroLateral = Sonar?.Analises?.segmentacao?.erro_lateral_pct ?? 0; + ErroAngular = Sonar?.Analises?.segmentacao?.erro_angular ?? 0; if (!(Variaveis.OperacaoEmAndamento.Parametros?.Controle?.OakParadaPorObstaculo ?? false)) { @@ -102,6 +104,7 @@ namespace AgroBase.Models.Operadores { StatusCarro = StatusCarro, ErroLateral = ErroLateral, + ErroAngular = ErroAngular, ObstaculoDetectado = ObstaculoDetectado, DeveParar = DeveParar, EnviarComandoParada = EnviarComandoParada, diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/ponte_ip.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/ponte_ip.py index ae85f235f..f51cfcd75 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/ponte_ip.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/ponte_ip.py @@ -15,7 +15,7 @@ from shared.enums import StatusModulo, T_Code from health_worker.modulos.base import ModuloDiagnosticoBase class ModuloIPBribge(ModuloDiagnosticoBase): - def __init__(self, window_fast: int = 6, window_slow: int = 20): + def __init__(self, window_fast: int = 8, window_slow: int = 24): self.t_code = T_Code.Ipb self.nome = "IP_Brigde" self.timeout = 5 @@ -61,8 +61,8 @@ class ModuloIPBribge(ModuloDiagnosticoBase): self._mqtt_backoff_max_s = 30.0 self._mqtt_connected_ts = 0.0 self.last_heartbeat_ts = 0.0 - self._heartbeat_timeout_s = 10.0 - self._heartbeat_grace_after_connect_s = 15.0 + self._heartbeat_timeout_s = 7.0 + self._heartbeat_grace_after_connect_s = 10.0 self.sub = False self._mqtt_future = None self._mqtt_executor = ThreadPoolExecutor(max_workers=1) @@ -80,9 +80,9 @@ class ModuloIPBribge(ModuloDiagnosticoBase): self._last_ping_loss_pct = 100.0 # parâmetros do ping - self._ping_timeout_ms = 400 # recomendado: 300–500 - self._ping_count = 2 # recomendado: 1 (janela já suaviza) - self._ping_min_interval = 0.6 # não precisa pingar a cada 100ms + self._ping_timeout_ms = 500 # recomendado: 300–500 + self._ping_count = 1 # recomendado: 1 (janela já suaviza) + self._ping_min_interval = 0.5 # não precisa pingar a cada 100ms self._hb_last_rx_monotonic = 0.0 self._last_ping_sample_applied_ts = 0.0 @@ -697,8 +697,8 @@ class ModuloIPBribge(ModuloDiagnosticoBase): # opcional: um pequeno piso e teto bw_max = max(0.35, min(bw_max, 10.0)) - bw_safe = bw_max * 0.68 - bw_hard = bw_max * 0.82 + bw_safe = bw_max * 0.60 + bw_hard = bw_max * 0.78 # garantias mínimas bw_safe = max(0.20, bw_safe) @@ -1124,11 +1124,11 @@ class ModuloIPBribge(ModuloDiagnosticoBase): self._cycles_recovered = min(self._cycles_recovered, 30) prev_state = self.link_state - if self._cycles_critical >= 8: + if self._cycles_critical >= 10: self.link_state = "CRITICAL" - elif self._cycles_degraded >= 12: + elif self._cycles_degraded >= 14: self.link_state = "DEGRADED" - elif self._cycles_recovered >= 20: + elif self._cycles_recovered >= 24: self.link_state = "OK" # entrou em DEGRADED agora 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 7a31d6b43..159050278 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 @@ -82,7 +82,8 @@ class CameraManager: self._timestamp_analise = None self.grid_ref_shape = (15, 10) - self.gerar_grid_ref() + 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, fuse_method="max", block_thr=0.7, 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) self.seg_runner = SegformerNavRunner(seg_config) @@ -120,18 +121,97 @@ class CameraManager: self.atualizar_saude_camera() def gerar_grid_ref(self, n_frames=50): - self.grid_ref = self._gerar_grid_referencia_geometrico() + try: + grid = self._gerar_grid_referencia_geometrico() + + if grid is None or len(grid) != self.grid_ref_shape[1]: + raise ValueError("grid_ref inválido ou com tamanho incorreto") + + grid = np.asarray(grid, dtype=np.float32) + + if not np.all(np.isfinite(grid)): + raise ValueError("grid_ref contém NaN/Inf") + + if np.any(grid <= 0): + raise ValueError("grid_ref contém valores <= 0") + + # 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()}") + + self.grid_ref = grid + + except Exception as e: + self.mostrar_log(f"Erro ao gerar grid_ref: {e}") + # fallback linear seguro + self.grid_ref = np.linspace(0.5, 5.0, self.grid_ref_shape[1], dtype=np.float32) def _gerar_grid_referencia_geometrico(self, angulo_inclinacao_graus=28.91, altura_camera_m=0.74): - grid_w, grid_h = self.grid_ref_shape - def dist_grid_calibrado(grid_h, i, fov, incl, altura): - alpha_v = ((i + 0.5) / grid_h - 0.5) * np.radians(fov) # (i + 0.5) = centro da linha, (i + 0.0) = baixo da linha, (i + 1.0) = topo da linha - gamma = np.radians(incl) + alpha_v - d = (altura / np.tan(gamma)) #* 1000.0 + 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 - 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] # ordena de baixo->cima como você queria - return d # <-- ndarray, não list + + 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() + + 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...") @@ -346,6 +426,8 @@ class CameraManager: else: depth_frame_np = self._ultimo_depth_frame + imu_roll = ContextoGlobalRedis.get_modulo(T_Code.Imu).get("roll_seg", 0) + self.grid_ref = self._ajustar_grid_ref_por_pitch(pitch_graus=imu_roll) self._analise_matriz_confianca(depth_frame_np) @@ -356,7 +438,8 @@ class CameraManager: t0 = time.time() predictions, ts, res = self.get_segmentation_predictions() if ts == self._ts_segmentacao_anterior: return - fps = 1.0 / (ts - self._ts_segmentacao_anterior) + dt = max(1e-6, ts - self._ts_segmentacao_anterior) if self._ts_segmentacao_anterior else 0.0 + fps = (1.0 / dt) if dt > 0 else 0.0 self._ts_segmentacao_anterior = ts if predictions is not None: analise_segmentacao, log = self.segmentacao_manager.segmentar(predictions) @@ -391,7 +474,8 @@ class CameraManager: t0 = time.time() dets, ts, meta = self.get_detections() if ts == self._ts_deteccao_anterior: return - fps = 1.0 / (ts - self._ts_deteccao_anterior) + dt = max(1e-6, ts - self._ts_deteccao_anterior) if self._ts_deteccao_anterior else 0.0 + fps = (1.0 / dt) if dt > 0 else 0.0 self._ts_deteccao_anterior = ts if dets is not None: t1 = time.time() @@ -545,13 +629,31 @@ class CameraManager: if depth_frame_np is None or depth_frame_np.size == 0: return segmentacao = self._ultima_analise_segmentacao.get("classes") if segmentacao is None: return - deteccoes = self._ultimo_detections + deteccoes = (self._ultima_analise_deteccao or {}).get("bboxes", []) + 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, + } vel = get_velocidade_atual_ms() t0 = time.time() - grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, deteccoes=deteccoes) + grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, deteccoes=deteccoes, det_params=det_params) #self.mostrar_log(grid_conf) + if grid_conf is None: + return t1 = time.time() grid_conf["ultima_chamada"] = self._ultima_analise_matriz_confianca.get("ultima_chamada", t0) self._calcular_performance(t0, t1, grid_conf) @@ -573,43 +675,57 @@ class CameraManager: #self.mostrar_log("Matriz de confianca concluida") def _construir_grid_confianca( - self, - depth_mm, # np.ndarray (H,W) em milímetros (0/NaN = inválido) ou None se não usar depth + 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=15, rows=10) + 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 p/ mapear %depth_valido -> conf_depth - w=(0.6, 0.3, 0.1), # pesos do custo: classe, anom, (1-conf) + 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, - # -------- detecções -------- - deteccoes=None, # lista de dicts: {label_id,label,conf,bbox_norm:[x0n,y0n,x1n,y1n],bbox_px:[x0,y0,x1,y1]} - det_params=None, # dict com hiperparâmetros (ver defaults abaixo) - usar_depth: bool = True # NOVO: se False, ignora depth e anomalia + deteccoes=None, # lista de dicts + det_params=None, + usar_depth: bool = True, + anom_tau_up=0.22, + anom_satur_up_m=0.50, + anom_tau_down=0.18, + anom_satur_down_m=0.40, + anom_down_weight=0.70, ): """ - 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 + Constrói uma grid de confiança/risco por célula. - Se usar_depth=False, os campos z_med/depth_valid_frac/anom serão pouco informativos - (z_med NaN, depth_valid_frac=0, anom=0) e a confiança/conf/custo - serão baseados apenas em segmentação + detecções. + 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: - # ---------- defaults detecção ---------- + # -------------------------------------------------- + # 0) Defaults de detecção alinhados com nova lógica + # -------------------------------------------------- _det = { - "w4": 0.25, # quão forte a detecção pesa no custo (0..1) - "thr_det_block": 0.35, # se det_score >= isso, célula deixa de ser navegável + "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.35, - "class_weights": {}, # ex: {'person':1.0,'car':0.9,'dog':0.6} - "veto_labels": set(["person"]), - "combine": "max", # "max" ou "sum_clamped" - "conf_drop_alpha": 0.15, # 0 = não derruba; 0.15 = derruba 15% * det_score + "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) @@ -617,30 +733,41 @@ class CameraManager: grid_w, grid_h = grid_shape H1, W1 = seg_ids.shape - # --- 1) Depth: opcional --- + # -------------------------------------------------- + # 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 --- + # -------------------------------------------------- + # 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) + # -------------------------------------------------- + # 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) - # --- 3b) mapas da detecção (debug/uso) --- - det_cov_max = np.zeros((grid_h, grid_w), np.float32) # cobertura máxima (0..1) - det_conf_max = np.zeros((grid_h, grid_w), np.float32) # conf máx (0..1) - det_score = np.zeros((grid_h, grid_w), np.float32) # score combinado (0..1) - det_top_label_id = -np.ones((grid_h, grid_w), np.int32) # -1 = nenhuma + # 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) - # --- 4) grid_ref 2D --- 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}") @@ -650,27 +777,28 @@ class CameraManager: 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: seg + (depth se ativo) --- + # -------------------------------------------------- + # 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, :] + 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]) + 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 - # % por classe - 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 + 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 - # depth (apenas se usando) if depth_row is not None: depth_block = depth_row[:, x0:x1] vals = depth_block[~np.isnan(depth_block)] @@ -679,28 +807,35 @@ class CameraManager: if valid >= max(int(min_valid_frac * n), 1): z_med[j, i] = np.nanmedian(vals) / 1000.0 # mm -> m - # --- 6) Confiança da célula --- + # -------------------------------------------------- + # 6) Confiança da célula + # -------------------------------------------------- t0, t1 = conf_params - conf_seg = np.maximum.reduce([pct_navegavel, pct_nao_navegavel]) + 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) / (t1 - t0), 0.0, 1.0) - conf_cell = 0.6 * conf_seg + 0.4 * conf_dep + 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: - # sem depth: confiança baseada só na segmentação conf_dep = np.zeros_like(conf_seg, dtype=np.float32) conf_cell = conf_seg.copy() - # --- 6b) Rasterizar detecções (opcional) --- + # -------------------------------------------------- + # 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", "")) - w_class = _det["class_weights"].get(label, 1.0) - veto = (label in _det["veto_labels"]) + 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"]: @@ -711,6 +846,7 @@ class CameraManager: 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 @@ -720,98 +856,147 @@ class CameraManager: # 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")) + 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")) + 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]) + 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) - 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)) + 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 - base = conf if not veto else 1.0 - s = base * cov * w_class + score_local = conf * cov * w_class - det_cov_max[j, i] = max(det_cov_max[j, i], cov) + 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] + s, 0.0, 1.0) + 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], s) + det_score[j, i] = max(det_score[j, i], score_local) - keyval = conf * cov - if keyval > det_top_conf[j, i]: - det_top_conf[j, i] = keyval - det_top_label_id[j, i] = int(det.get("label_id", -1)) + # 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 - if _det["conf_drop_alpha"] > 0.0: - conf_cell = np.clip(conf_cell * (1.0 - _det["conf_drop_alpha"] * det_score), 0.0, 1.0) - - # --- 7) Anomalia de solo --- + # -------------------------------------------------- + # 8) Anomalia física (positiva e negativa) + # -------------------------------------------------- if usar_depth and d_small is not None: - delta = Z_ref - z_med - delta = np.where(np.isnan(z_med), 0.0, np.maximum(delta, 0.0)) - anom_raw = np.clip(delta / anom_satur_m, 0.0, 1.0) - anom = anom_raw * (delta > anom_tau_min).astype(np.float32) * conf_dep + 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) + ) + + # 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) - # --- 8) Custo e navegabilidade --- + # -------------------------------------------------- + # 9) Custo suave + # -------------------------------------------------- nao_navegavel = 1.0 - pct_navegavel w1, w2, w3 = w - custo = w1 * nao_navegavel + w2 * anom + w3 * (1.0 - conf_cell) - if deteccoes: - custo = np.clip(custo + _det["w4"] * det_score, 0.0, 1.0) + custo_base = ( + w1 * nao_navegavel + + w2 * anom + + w3 * (1.0 - conf_cell) + ) - if deteccoes: - navegavel = ( - (pct_navegavel >= 0.55) & - (anom < 0.4) & - (conf_cell >= 0.5) & - (det_score < _det["thr_det_block"]) - ) - else: - navegavel = ( - (pct_navegavel >= 0.55) & - (anom < 0.4) & - (conf_cell >= 0.5) - ) + # 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": pct_navegavel, - "pct_nao_navegavel": pct_nao_navegavel, + "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": depth_valid_frac, + "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": det_cov_max, - "det_conf_max": det_conf_max, - "det_score": det_score, + + "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": det_top_conf, + "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, 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 82a9d6f5a..280f50714 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 @@ -10,8 +10,7 @@ class CostmapFuser: 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", # "q0.7" (quantil) ou "max" - block_thr=0.7, # célula bloqueia se custo > thr + 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? @@ -19,23 +18,31 @@ class CostmapFuser: robot_width=0.84 ): self.grid_w, self.grid_h = grid_shape - self.K = int(K) - self.M = int(M) - self.block_thr = float(block_thr) + + self.K = max(1, int(K)) + self.M = max(1, min(int(M), self.K)) + + # legado / fallback geral self.fuse_method = fuse_method + + # 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.y_range_m = y_range_m self.near_is_bottom = near_is_bottom self.fov_h_rad = fov_h_rad - self.robot_width_m = robot_width + self.robot_width_m = float(robot_width) if central_cols is None: - # 3 colunas centrais mid = self.grid_w // 2 self.central_cols = (max(0, mid - 1), min(self.grid_w - 1, mid + 1)) else: - self.central_cols = central_cols + self.central_cols = (int(central_cols[0]), int(central_cols[1])) - # ring-buffers + # ring-buffers principais self.buf_custo = [] self.buf_conf = [] self.buf_anom = [] @@ -44,9 +51,17 @@ class CostmapFuser: 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 = { - "on": 0, "off": 0, "latched": False, - "reason": "none", "dmin": None, + "on": 0, + "off": 0, + "latched": False, + "reason": "none", + "dmin": None, "last_decision": "LIVRE" } @@ -60,31 +75,70 @@ class CostmapFuser: 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) - def _fuse_array(self, stack, method, q=0.7, avg=False): - """Fusão temporal de um stack (T, H, W).""" + 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.shape[0] == 0: - return np.zeros((self.grid_h, self.grid_w), np.float32) - if avg: - return stack.mean(axis=0).astype(np.float32) + 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) - # quantil - return np.quantile(stack, q, 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}") - - def _rows_near_to_far(self): - """Ordem das linhas do mais perto ao mais longe.""" - try: - if self.near_is_bottom: - # j = grid_h-1 (baixo) é mais perto - return range(self.grid_h - 1, -1, -1) - # j=0 (topo) é mais perto - return range(0, self.grid_h, 1) - except Exception as e: - mostrar_log(f"Erro no _rows_near_far: {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.""" @@ -104,298 +158,347 @@ class CostmapFuser: 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( self, - custo_f, anom_f, conf_f, nav_f, zmed_f, + 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.65, thr_conf_low=0.35, - rho_block_central=0.70, rho_block_global=0.40, - near_is_bottom=True, - # ------ NOVOS (opcionais) ------ - use_persistence=False, - velocidade_mps=None, # m/s; se None, decisão não usa distância de frenagem - a_max_freio=0.8, # m/s² - margem_parada=0.5, # m - N_on=2, # frames p/ entrar - N_off=3, # frames p/ sair - blackout_imediato=True, - det_score_f=None, # (H,W) 0..1 - det_label_id=None, # (H,W) int, -1 = none - det_strength=None, # (H,W) 0..1 ~ conf*cov dominante + 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 ): try: H, W = custo_f.shape + + # ----------------------------- + # 0) Preparação / defaults + # ----------------------------- c0, c1 = central_cols - c0 = max(0, min(W-1, int(c0))) - c1 = max(0, min(W, int(c1))) + 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 # fallback + c0, c1 = W // 3, 2 * W // 3 - # 1) Máscaras inseguras - mask_anom = (anom_f >= thr_anom_block) - mask_cost = (custo_f >= thr_cost_block) - mask_conf = (conf_f < thr_conf_low) - unsafe = mask_anom | mask_cost | mask_conf + if det_veto_labels is None: + det_veto_labels = {"person"} - has_det = None labelmap_det = None - if (det_score_f is not None): - has_det = (det_score_f >= float(thr_det_consider)) - from visual_worker.config import load_det_config - labelmap_det = load_det_config().get("classes") - if labelmap_det is not None: - labelmap_det = {i: name for i, name in enumerate(labelmap_det)} + 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 - # 2) Largura em colunas por linha + # 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 (width_need_m / max(1, (c1 - c0))) + 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] = (c1 - c0) + 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): - try: - 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 - except Exception as e: - mostrar_log(f"Erro no central_window: {e}") + 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) - # 3) Varredura central (cobertura e "existe obstáculo?") - coverage_central = np.zeros(H, np.float32) - exists_unsafe_central = np.zeros(H, np.bool_) # <- NOVO: existe ao menos 1 px inseguro na janela j_block = None + j_det_block = None - det_exists_central = np.zeros(H, np.bool_) - det_min_z_central = np.full(H, np.nan, np.float32) - det_best_label = -np.ones(H, np.int32) - det_best_strength = np.zeros(H, np.float32) + it = range(H - 1, -1, -1) if near_is_bottom else range(H) - 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] - coverage = unsafe_row.mean() if (b > a) else 1.0 - coverage_central[j] = coverage - exists_unsafe_central[j] = bool(unsafe_row.any()) if (b > a) else True - if (j_block is None) and (coverage >= rho_block_central): + 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 - # 4) Cobertura global - global_cov = unsafe.mean() + if (j_det_block is None) and (cov_detcrit > 0.0): + j_det_block = j - # --- 4.5) Sanitiza zmed_f --- - # copia para não mexer no buffer original - zmed = np.array(zmed_f, dtype=np.float32, copy=True) - # trata inválidos: <=0, nan, inf -> nan - invalid = (~np.isfinite(zmed)) | (zmed <= 0.0) - zmed[invalid] = np.nan + # 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] - # --- 5) Distâncias usando Z real (zmed_f) --- - d_block_line_m = None if j_block is None else float(row_dist_m[j_block]) # fallback geométrico antigo - d_block_line_m_z = None # NOVO: Z real na linha-bloqueio + # 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 - # primeira linha (mais próxima) onde há QUALQUER insegurança central (com Z real) j_obs_true_min = None - d_obs_true_min_m = None # fallback geométrico - d_obs_true_min_m_z = None # NOVO: Z real + j_det_true_min = None - # varre do perto -> longe conforme 'near_is_bottom' - it2 = (range(H-1, -1, -1) if near_is_bottom else range(H)) + it2 = range(H - 1, -1, -1) if near_is_bottom else range(H) for j in it2: - # janela central nessa linha a, b = central_window(j, cols_need[j]) if b <= a: continue - if has_det is not None: - det_win = has_det[j, a:b] - if np.any(det_win): - det_exists_central[j] = True - # força dominante (strength) e rótulo dominante nessa janela - 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 na janela - k = np.argmax(str_win) - det_best_strength[j] = float(str_win.ravel()[k]) - det_best_label[j] = int(lbl_win.ravel()[k]) - # distância usando zmed somente onde há detecção - if zmed_f is not None: - z_win = zmed_f[j, a:b] - z_sel = z_win[det_win] - z_sel = z_sel[np.isfinite(z_sel) & (z_sel > 0)] - if z_sel.size: - det_min_z_central[j] = float(np.min(z_sel)) - - # máscara de insegurança na janela - bad = unsafe[j, a:b] - - if np.any(bad): - z_slice = zmed[j, a:b] - z_bad = z_slice[bad] - - if z_bad.size: - z_bad_f = z_bad[np.isfinite(z_bad)] - if z_bad_f.size: - z_min = float(np.min(z_bad_f)) # ok, só finitos + 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 + j_obs_true_min = j d_obs_true_min_m_z = z_min - d_obs_true_min_m = float(row_dist_m[j]) - # se esta é a linha-bloqueio (cobertura >= rho), também calcule o Z dessa linha - if (j_block is not None) and (j == j_block): - if np.any(unsafe[j, a:b]): - z_block = zmed[j, a:b][unsafe[j, a:b]] - if z_block.size > 0: - z_bmin = np.nanmin(z_block) - if np.isfinite(z_bmin): - d_block_line_m_z = float(z_bmin) + # 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 - # 6) Viés lateral - left = unsafe[:, c0:(c0+c1)//2].mean() if (c1-c0) >= 2 else 0.0 - right = unsafe[:, (c0+c1)//2:c1].mean() if (c1-c0) >= 2 else 0.0 - side_bias_val = float(np.clip((right - left) / max(1e-6, (right + left)), -1.0, 1.0)) + 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) Decisão "raw" (sem persistência) + detalhes + # ----------------------------- + # 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)) + + # ----------------------------- + # 8) Classificação raw + # ----------------------------- def _fmt_m(x): - try: - return "-" if (x is None or not np.isfinite(x)) else f"{float(x):.2f} m" - except Exception as e: - mostrar_log(f"Erro no _fmt_m: {e}") + return "-" if (x is None or not np.isfinite(x)) else f"{float(x):.2f} m" - conf_mean = float(conf_f.mean()) - central_cov_max = float(coverage_central.max()) + 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 = "none" + blocked_raw = False + reason = "free" reason_detail = "Caminho livre." - # qual distância vamos considerar como "mais conservadora" - # (pixel inseguro mais perto vs linha que bloqueia) - def _min_non_none(a, b): - try: - 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 - except Exception as e: - mostrar_log(f"Erro no _min_non_none: {e}") + central_unsafe_max = float(np.max(coverage_central_unsafe)) + central_conf_max = float(np.max(coverage_central_conf)) + central_detcrit_max = float(np.max(coverage_central_detcrit)) - if j_block is not None: - # houve uma linha com cobertura central >= rho_block_central + # 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"Detecção crítica na faixa central: {lbl_txt}{strength_txt}, " + f"d={_fmt_m(d_used)}, cobertura_detcrit_max={central_detcrit_max:.2f}." + ) + + # 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) - extra_det_txt = "" - if has_det is not None: - # prioriza a linha de bloqueio; se não houver, a primeira linha com detecção - j_det = None - if (j_block is not None) and det_exists_central[j_block]: - j_det = j_block - else: - for jj in it2: # perto -> longe (ou longe->perto dependendo de near_is_bottom) - if det_exists_central[jj]: - j_det = jj - break - if j_det is not None: - d_det = det_min_z_central[j_det] - # pega janela central nessa linha - a, b = central_window(j_det, cols_need[j_det]) - labels = {} - if det_label_id is not None and det_strength is not None: - lbls = det_label_id[j_det, a:b].ravel() - strs = det_strength[j_det, a:b].ravel() - for lid, s in zip(lbls, strs): - if lid < 0: - continue - if s < thr_det_consider: - continue - # guarda o maior score visto pra essa classe - if lid not in labels or s > labels[lid]: - labels[lid] = s - # traduz para nomes - if labels: - parts = [] - for lid, s in labels.items(): - if labelmap_det is not None and lid < len(labelmap_det): - nm = labelmap_det[lid] - else: - nm = f"label#{lid}" - parts.append(f"{nm}({s:.2f})") - lbl_txt = ", ".join(parts) - else: - lbl_txt = "objeto" - d_det_txt = "-" if (d_det is None or not np.isfinite(d_det)) else f"{d_det:.2f} m" - extra_det_txt = f" | deteccoes: {lbl_txt} a {d_det_txt}" - reason_detail = ( - f"Janela central bloqueada (p>={rho_block_central:.2f}). " - f"d*={_fmt_m(d_used)} [linha={_fmt_m(d_block_line_m_z)}; pixel={_fmt_m(d_obs_true_min_m_z)}]; " - f"cobertura_central_max={central_cov_max:.2f}{extra_det_txt}." + f"Faixa central bloqueada por anomalia/custo suportado " + f"(cov={central_unsafe_max:.2f} >= {rho_block_central:.2f}), " + f"d*={_fmt_m(d_used)} [linha={_fmt_m(d_block_line_m_z)}; pixel={_fmt_m(d_obs_true_min_m_z)}]." ) - elif (global_cov <= rho_block_global) and (conf_mean < 0.45): - blocked_raw = True + # 8.3 percepção degradada / blackout + elif (global_cov_conf >= rho_block_global) and (conf_mean < thr_conf_low): + blocked_raw = False reason = "blackout" - extra = "" - if has_det is not None and np.any(det_exists_central): - # lista até 2 rótulos distintos mais fortes (opcional) - labs = [] - if (det_best_label is not None) and (labelmap_det is not None): - # pega top-2 por força - idxs = np.argsort(-det_best_strength) # desc força - seen = set() - for k in idxs: - lid = int(det_best_label[k]) - if lid < 0: - continue - if lid in seen: - continue - seen.add(lid) - labs.append(labelmap_det.get(lid, f"label#{lid}")) - if len(labs) >= 2: - break - if labs: - extra = f" (deteccoes vistas: {', '.join(labs)})" - else: - extra = " (deteccoes presentes)" reason_detail = ( - f"Percepcao degradada: cobertura_global={global_cov:.2f}<={rho_block_global:.2f} " - f"e confianca_media={conf_mean:.2f}<0.45{extra}." + f"Percepção degradada: frac_conf_baixa={global_cov_conf:.2f}, " + f"conf_media={conf_mean:.2f}. " + f"Isto indica incerteza perceptiva, não obstáculo confirmado." ) - elif (central_cov_max > 0.45) and (left > 0.7 or right > 0.7): - # corredor 'estreito': laterais muito ruins, mesmo sem bloquear de fato a janela central + # 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) - extra = "" - if has_det is not None and np.any(det_exists_central): - extra = " (detecções na faixa central)" reason_detail = ( - f"Corredor estreito: lateral {lado} muito fechada (frac={lado_frac:.2f}), " - f"central_max={central_cov_max:.2f}{extra}." + f"Corredor estreito: lateral {lado} fechada (frac={lado_frac:.2f}), " + f"central_unsafe_max={central_unsafe_max:.2f}." ) - # 8) Persistência + decisão de parada + # ----------------------------- + # 9) Decisão dinâmica com persistência + # ----------------------------- decision = { "parar": False, "dist_necessaria": None, @@ -403,89 +506,158 @@ class CostmapFuser: "frames_on": 0, "frames_off": 0, "N_on": N_on, - "N_off": N_off + "N_off": N_off, + "mode": "free" } - blocked_out = status_seg == StatusCarroMapa.Parado or blocked_raw + + 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_freio = (v * v) / max(1e-9, 2.0 * a_max_freio) dist_necessaria = dist_freio + margem_parada decision["dist_necessaria"] = float(dist_necessaria) - d_for_stop = _min_non_none(d_block_line_m_z, d_obs_true_min_m_z) + 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) stop_now = False v_max_sug = None + mode = "free" - if blocked_raw and reason == "blackout": - stop_now = True if blackout_imediato else False + if reason == "detected_critical": + mode = "stop" + stop_now = True if (d_stop_ref is None or d_stop_ref <= dist_necessaria + 0.3) else True - if blocked_raw and reason == "obstacle": - if (d_for_stop is not None) and np.isfinite(d_for_stop) and (d_for_stop <= dist_necessaria): + 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: - # ainda não precisa parar: sugere v_max segura se tivermos alguma medida de distância - d_ref = _min_non_none(d_block_line_m_z, d_obs_true_min_m_z) - if (d_ref is not None) and np.isfinite(d_ref) and (d_ref > margem_parada): - v_max_sug = float(np.sqrt(max(0.0, 2.0 * a_max_freio * (d_ref - margem_parada)))) + 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)))) - if (d_for_stop is not None) and np.isfinite(d_for_stop) and (d_for_stop <= dist_necessaria): - stop_now = True + elif reason == "blackout": + mode = "caution" + stop_now = bool(blackout_imediato) + + elif reason == "narrow": + mode = "slowdown" + stop_now = False - # histerese - 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 + 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 = status_seg == StatusCarroMapa.Parado or bool(st["latched"]) + 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"]) + "frames_off": int(st["off"]), + "mode": mode }) st["reason"] = reason - # guarda também as duas distâncias pra debug - 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_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 { - # mantém o nome antigo, mas esclarece nos campos abaixo: - "d_block_line_m": d_block_line_m_z, # distância da LINHA que bloqueia (cobertura ≥ ρ) - "d_obs_true_min_m": d_obs_true_min_m_z, # menor distância com QUALQUER insegurança central - "blocked": bool(blocked_out), # com persistência se habilitada + "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(coverage_central.max()), - "global": float(global_cov), + "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_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), + "decision": decision } + except Exception as e: - mostrar_log("Erro no _compute_blockage_metrics") - + mostrar_log(f"Erro no _compute_blockage_metrics: {e}") + 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, + "j_obs_true_min": None, + "j_det_true_min": None, + "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" + } + } + def _row_scale_x(self, row_dist_m): """metros por célula em X para cada linha, dado FOV_H.""" try: @@ -496,37 +668,71 @@ class CostmapFuser: # metros por coluna return (row_width / float(self.grid_w)).astype(np.float32) # (H,) except Exception as e: - mostrar_log("Erro no _row_scale_x") + mostrar_log(f"Erro no _row_scale_x: {e}") + return None def update(self, grid_dict, ts=None, velocidade_ms=0.0, status_seg=StatusCarroMapa.Direcionando): """ - grid_dict deve conter (grid_h,grid_w): "custo","conf","anom","navegavel" - opcional: "z_ref" (m) 2D - Retorna snapshot pronto pra serializar e gravar no Redis. + Atualiza o estado temporal da costmap fundida e retorna um snapshot serializável. + + 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.perf_counter() + + # ----------------------------- + # 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) # 0/1 - zref = grid_dict.get("z_ref", None) + 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) + + 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) - det_lbl = grid_dict.get("det_top_label_id", None) - det_sdom = grid_dict.get("det_top_conf", None) # nosso conf*cov dominante + 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) - # valida shape (H,W) = (grid_h,grid_w) H, W = custo.shape - assert (H, W) == (self.grid_h, self.grid_w), f"grid {H,W} != {(self.grid_h,self.grid_w)}" + if (H, W) != (self.grid_h, self.grid_w): + raise ValueError(f"grid {H, W} != {(self.grid_h, self.grid_w)}") - # push no ring-buffer + # ----------------------------- + # 2) Buffers temporais + # ----------------------------- self.buf_custo.append(custo) self.buf_conf.append(conf) self.buf_anom.append(anom) @@ -535,82 +741,149 @@ class CostmapFuser: 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 - if 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) + 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) - # empilha + 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_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) - # fusão - q = 0.7 - if self.fuse_method.startswith("q"): - try: - q = float(self.fuse_method[1:]) - except Exception: - q = 0.7 - custo_f = self._fuse_array(S_custo, "q", q=q) - anom_f = self._fuse_array(S_anom, "q", q=q) - else: - custo_f = self._fuse_array(S_custo, "max") - anom_f = self._fuse_array(S_anom, "max") + 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 - conf_f = self._fuse_array(S_conf, method="q", q=0.5, avg=True) # média - # navegável: M-de-N (soma >= M) + 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), np.uint8) + nav_f = np.zeros((self.grid_h, self.grid_w), dtype=np.uint8) - # z_ref fundido (opcional) + # 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): - # pega a última não-nula for z in reversed(self.buf_zref): if z is not None: zref_f = z break - # z_med fundido (opcional) zmed_f = None if any(z is not None for z in self.buf_zmed): - # pega a última não-nula + # 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 - # distâncias por linha (m), usando z_ref se houver; senão, mapeamento linear y_range_m - row_dist = self._row_distances(zref_f).astype(np.float32) # shape (grid_h,) - row_dist_m = row_dist.astype(np.float32) - - # escala X por linha (m/col) - row_scale_x = self._row_scale_x(row_dist) # (H,) ou None + # ----------------------------- + # 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, anom_f, conf_f, nav_f, zmed_f, - row_dist_m, row_scale_x, self.central_cols, + 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=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=True, + a_max_freio=0.5, + margem_parada=0.60, + N_on=2, + N_off=3, + blackout_imediato=False, - det_score_f = det_score, - det_label_id = det_lbl, - det_strength = det_sdom, - thr_det_consider = 0.25, + 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 ) - # incrementa seq + # ----------------------------- + # 8) Seq + # ----------------------------- self.seq += 1 - # empacota para JSON (u8 para leveza) + # ----------------------------- + # 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 = { @@ -621,30 +894,75 @@ class CostmapFuser: "fuse": { "K": int(self.K), "M": int(self.M), - "method": self.fuse_method, - "block_thr": float(self.block_thr), + "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(), + "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), + "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 } - - # (opcional) incluir z_ref_u8 pra debug/visualização - # if zref_f is not None: - # zref_u8 = np.clip(zref_f / self.y_range_m[1] * 255.0, 0, 255).astype(np.uint8) - # snap["zref_u8"] = zref_u8.ravel().tolist() return snap + 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 method is None: + method = "mean" + + method = str(method).lower().strip() + + if method.startswith("q"): + try: + q = float(method[1:]) + except Exception: + q = default_q + return self._fuse_array(stack, method="q", q=q) + + if method in ("mean", "avg", "media"): + return self._fuse_array(stack, method="mean") + + if method in ("median", "mediana"): + return self._fuse_array(stack, method="median") + + if method in ("ema", "ewma"): + # alpha maior = responde mais rápido ao presente + return self._fuse_array(stack, method="ema", alpha=0.60) + + if method == "max": + return self._fuse_array(stack, method="max") + + # fallback seguro + return self._fuse_array(stack, method="mean") @@ -663,9 +981,5 @@ def unpack_snapshot(snap): nav = nav_u8.astype(bool) return custo, conf, anom, nav -def thr_u8(snap): - thr = float(snap["fuse"]["block_thr"]) # ex.: 0.7 - return int(round(thr * 255.0)) # 0.7 -> 179 - def mostrar_log(mensagem): print(f"[COSTMAP_FUSER] {mensagem}")