diff --git a/AgroBase/.vs/AgroBase/FileContentIndex/5fbbf9bd-07b0-4a91-a81c-44b4df8f5196.vsidx b/AgroBase/.vs/AgroBase/FileContentIndex/2dd92849-35e7-4714-a5f2-81dac4ed8455.vsidx similarity index 81% rename from AgroBase/.vs/AgroBase/FileContentIndex/5fbbf9bd-07b0-4a91-a81c-44b4df8f5196.vsidx rename to AgroBase/.vs/AgroBase/FileContentIndex/2dd92849-35e7-4714-a5f2-81dac4ed8455.vsidx index 33bb56001..57fb5cd93 100644 Binary files a/AgroBase/.vs/AgroBase/FileContentIndex/5fbbf9bd-07b0-4a91-a81c-44b4df8f5196.vsidx and b/AgroBase/.vs/AgroBase/FileContentIndex/2dd92849-35e7-4714-a5f2-81dac4ed8455.vsidx differ diff --git a/AgroBase/.vs/AgroBase/v17/.suo b/AgroBase/.vs/AgroBase/v17/.suo index d0f031be9..58b4d5e52 100644 Binary files a/AgroBase/.vs/AgroBase/v17/.suo and b/AgroBase/.vs/AgroBase/v17/.suo differ diff --git a/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs b/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs index a11b6b96d..42eb2554d 100644 --- a/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs +++ b/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs @@ -1,7 +1,6 @@ using System; using System.Collections.Generic; using System.Drawing; -using System.Linq; using static AgroBase.Models.Enums; namespace AgroBase.Models.Operadores diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/__pycache__/camera_manager.cpython-311.pyc b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/__pycache__/camera_manager.cpython-311.pyc index 296fbd169..921a68053 100644 Binary files a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/__pycache__/camera_manager.cpython-311.pyc and b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/__pycache__/camera_manager.cpython-311.pyc differ diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/__pycache__/config.cpython-311.pyc b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/__pycache__/config.cpython-311.pyc index 112f93d3b..e57442a7b 100644 Binary files a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/__pycache__/config.cpython-311.pyc and b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/__pycache__/config.cpython-311.pyc differ 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 e296c016f..a478d5cbb 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 @@ -298,6 +298,8 @@ class CameraManager: def _realizar_analises(self): self._analise_segmentacao() + + self._analise_deteccao() if self._depth_frame_necessario: depth_frame_np, depth_timestamp, depth_res = self.get_depth_frame() @@ -308,7 +310,6 @@ class CameraManager: distancia_max_m = parametros_camera["distancia_maxima"] / 1000.0 self._analise_matriz_confianca(depth_frame_np, distancia_max_m, fov_h) - self._analise_deteccao() def _realizar_analises_async(self): executor = self._pool @@ -399,13 +400,14 @@ class CameraManager: try: 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 + if segmentacao is None: return + deteccoes = self._ultima_analise_deteccao.get("bboxes") vel = get_velocidade_atual_ms() t0 = time.time() #grid_conf = self._gerar_grid_confianca(depth_frame_np, segmentacao, dist_max) - grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, self.camera.modelo_ia_seg.get("classes")) + grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, self.camera.modelo_ia_seg.get("classes"), deteccoes=deteccoes) #self.mostrar_log(grid_conf) t1 = time.time() grid_conf["ultima_chamada"] = self._ultima_analise_matriz_confianca.get("ultima_chamada", t0) @@ -423,7 +425,9 @@ class CameraManager: #self._mostrar_debug_grid_confianca(self._ultimo_rgb_frame, grid_conf["matriz"], True, self._ultima_analise_segmentacao["mask_color"]) #key, vis = self.debug_show_visualworker(frame_bgr=self._ultimo_rgb_frame, grid=grid_conf, wait=1, text_mode="full", draw_grid=True, draw_cells=True, draw_legend=True) - #vis, metrics = self.debug_blockage_imshow(self._ultimo_rgb_frame, snapshot, velocidade_media=vel) + from visual_worker.config import load_seg_config + if load_seg_config().get("debug_visual", False): + vis, metrics = self.debug_blockage_imshow(self._ultimo_rgb_frame, snapshot, velocidade_media=vel) except Exception as e: self.mostrar_log(f"❌ Erro na geracao da matriz de confianca: {e}") finally: @@ -849,49 +853,80 @@ class CameraManager: def _construir_grid_confianca( self, depth_mm, # np.ndarray (H,W) em milímetros (0/NaN = inválido) - seg_ids_512x288, # np.ndarray (288,512) com IDs de classe por pixel + seg_ids_512x288, # np.ndarray (288,512) grid_ref, # np.ndarray (grid_h,) OU (grid_h, grid_w) em metros grid_shape=(15, 10), # (cols=15, rows=10) class_ids=None, # {'rua':X, 'cana':Y, 'obs':Z} - valid_mm=(300, 10000), # faixa válida do depth (ajuste conforme tua OAK) - range_m=(0.5, 5.0), # janela útil à frente (só p/ debug/checar) - min_valid_frac=0.30, # % mínimo de pixels válidos p/ aceitar mediana + valid_mm=(300, 10000), + range_m=(0.5, 5.0), + 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) - anom_tau_min=0.22, # tolerância mínima de “aproximação” (m) - anom_satur_m=0.50 # saturação da anomalia (m) + anom_tau_min=0.22, + anom_satur_m=0.50, + # -------- NOVO: 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) ): """ Retorna dict com arrays (grid_h, grid_w): - pct_rua, pct_cana, pct_obs, z_med (m), z_ref (m), - depth_valid_frac, conf, anom, custo, navegavel (0/1) + pct_rua, pct_cana, pct_obs, z_med, z_ref, depth_valid_frac, + conf, anom, custo, navegavel, + # --- NOVOS (debug/uso opcional) --- + det_cov_max, det_conf_max, det_score """ if class_ids is None: - # AJUSTE para os IDs reais do teu labelmap! class_ids = {'rua': 0, 'cana': 1, 'obs': 2} - grid_w, grid_h = grid_shape # (15, 10) + # ---------- defaults detecção ---------- + _det = { + # peso da penalização no custo + "w4": 0.25, # quão forte a detecção pesa no custo (0..1) + # limiar pra "bloquear" navegação só por detecção + "thr_det_block": 0.35, # se det_score >= isso, célula deixa de ser navegável + # mínimo de interseção da bbox com a célula pra considerar (fração da célula) + "min_cell_coverage": 0.10, + # confiança mínima da bbox pra considerar + "min_det_conf": 0.35, + # pesos por classe (se não souber o id, usa 1.0) + "class_weights": {}, # ex: {'person':1.0,'car':0.9,'dog':0.6} + # classes que vetam (tratadas como peso 1.0 e sem atenuação) + "veto_labels": set(["person"]), + # como combinar múltiplas bboxes na célula: "max" ou "sum_clamped" + "combine": "max", + # derrubar um pouco a conf_cell quando há detecção + "conf_drop_alpha": 0.15, # 0 = não derruba; 0.15 = derruba 15% * det_score + } + if det_params: + _det.update(det_params) + + grid_w, grid_h = grid_shape H0, W0 = depth_mm.shape - # --- 1) Reduz o depth para 512x288 preservando rótulos (sem blur de escala) --- + # --- 1) Resize depth para 512x288 --- d_small = cv2.resize(depth_mm, (512, 288), interpolation=cv2.INTER_NEAREST).astype(np.float32) - # marca inválidos d_small[(d_small < valid_mm[0]) | (d_small > valid_mm[1])] = np.nan - # --- 2) Bordas das células da grid 15x10 sobre a imagem 512x288 --- - x_edges = np.linspace(0, 512, grid_w + 1, dtype=int) # 16 bordas - y_edges = np.linspace(0, 288, grid_h + 1, dtype=int) # 11 bordas + # --- 2) Bordas da grid --- + x_edges = np.linspace(0, 512, grid_w + 1, dtype=int) + y_edges = np.linspace(0, 288, grid_h + 1, dtype=int) - # --- 3) Saídas (grid_h, grid_w) = (10, 15) --- + # --- 3) Saídas --- pct_rua = np.zeros((grid_h, grid_w), np.float32) pct_cana = np.zeros((grid_h, grid_w), np.float32) pct_obs = 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) - # --- 4) Garante grid_ref 2D em metros --- + # --- 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 + det_top_conf = np.zeros((grid_h, grid_w), np.float32) # conf da dominante + + # --- 4) grid_ref 2D --- if grid_ref.ndim == 1: - # grid_ref é por LINHA (grid_h,) 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) @@ -900,13 +935,16 @@ 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) Agregação por célula (150 células: tranquilo em tempo real) --- + # pré-slices por linha for j in range(grid_h): y0, y1 = y_edges[j], y_edges[j+1] - seg_row = seg_ids_512x288[y0:y1, :] # (rows, 512) - depth_row = d_small[y0:y1, :] # (rows, 512) + seg_row = seg_ids_512x288[y0:y1, :] + depth_row = d_small[y0:y1, :] + row_h = max(1, y1 - y0) for i in range(grid_w): x0, x1 = x_edges[i], x_edges[i+1] + col_w = max(1, x1 - x0) + seg_block = seg_row[:, x0:x1] depth_block = depth_row[:, x0:x1] @@ -922,46 +960,132 @@ class CameraManager: pct_cana[j, i] = n_cana / n pct_obs[j, i] = n_obs / n - # depth: mediana em metros + fração válida + # depth 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[j, i] = np.nanmedian(vals) / 1000.0 - # --- 6) Confiança por célula --- + # --- 6) Confiança --- t0, t1 = conf_params conf_seg = np.maximum.reduce([pct_rua, pct_cana, pct_obs]) conf_dep = np.clip((depth_valid_frac - t0) / (t1 - t0), 0.0, 1.0) conf_cell = 0.6 * conf_seg + 0.4 * conf_dep - # --- 7) Anomalia (obstáculo = mais perto que o esperado) --- - # ΔZ > 0 → medido está mais perto que a referência - delta = Z_ref - z_med # m - # z_med NaN → delta = 0 (sem evidência) + # --- 6b) Rasterizar detecções (opcional) --- + if deteccoes: + # percorre bboxes e projeta para grid + 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"]) + + # caixa em px (melhor usar bbox_px se já veio arredondado no teu pipeline) + 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 * 512, 0, 511)); x1p = int(np.clip(x1n * 512, 0, 512)) + y0p = int(np.clip(y0n * 288, 0, 287)); y1p = int(np.clip(y1n * 288, 0, 288)) + if x1p <= x0p or y1p <= y0p: + continue + + bbox_area = float((x1p - x0p) * (y1p - y0p)) + if bbox_area <= 1.0: + continue + + # descobre células que sobrepõem a bbox + # índices i (colunas) e j (linhas) 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): + y0, y1 = y_edges[j], y_edges[j+1] + for i in range(i0, i1+1): + x0, x1 = x_edges[i], x_edges[i+1] + # interseção + ix0 = max(x0, x0p); ix1 = min(x1, x1p) + iy0 = max(y0, y0p); iy1 = min(y1, y1p) + if ix1 <= ix0 or iy1 <= iy0: + continue + inter = float((ix1 - ix0) * (iy1 - iy0)) + + # cobertura em relação à célula (mais conservador que em relação à bbox) + cell_area = float((x1 - x0) * (y1 - y0)) + if cell_area <= 0: + continue + cov = inter / cell_area + + if cov < _det["min_cell_coverage"]: + continue + + # score local da detecção nesta célula + # se for classe vetada, zera atenuações + base = conf if not veto else 1.0 + s = base * 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] + s, 0.0, 1.0) + else: # "max" + det_score[j, i] = max(det_score[j, i], s) + + # critério: escolhe como dominante a de MAIOR (conf * cov) + 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)) + + # opcional: derruba um pouco a confiança onde há detecção + 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 (igual à tua) --- + 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) - # porta de tolerância mínima (tau) + confiança do depth anom = anom_raw * (delta > anom_tau_min).astype(np.float32) * conf_dep # --- 8) Custo e navegabilidade --- nao_rua = 1.0 - pct_rua w1, w2, w3 = w - custo = np.clip(w1 * nao_rua + w2 * anom + w3 * (1.0 - conf_cell), 0.0, 1.0) + custo = w1 * nao_rua + w2 * anom + w3 * (1.0 - conf_cell) - navegavel = (pct_rua >= 0.55) & (anom < 0.4) & (conf_cell >= 0.5) + # penalização por detecção (se houver) + if deteccoes: + custo = np.clip(custo + _det["w4"] * det_score, 0.0, 1.0) + + # regra de navegabilidade com detecção (bloqueia se score alto) + if deteccoes: + navegavel = (pct_rua >= 0.55) & (anom < 0.4) & (conf_cell >= 0.5) & (det_score < _det["thr_det_block"]) + else: + navegavel = (pct_rua >= 0.55) & (anom < 0.4) & (conf_cell >= 0.5) return { "pct_rua": pct_rua, "pct_cana": pct_cana, "pct_obs": pct_obs, - "z_med": z_med, # metros - "z_ref": Z_ref, # metros + "z_med": z_med, + "z_ref": Z_ref, "depth_valid_frac": depth_valid_frac, - "conf": conf_cell, # 0..1 - "anom": anom, # 0..1 - "custo": custo, # 0..1 - "navegavel": navegavel.astype(np.uint8) # 0/1 + "conf": np.clip(custo*0 + conf_cell, 0.0, 1.0), # garante 0..1 + "anom": np.clip(anom, 0.0, 1.0), + "custo": np.clip(custo, 0.0, 1.0), + "navegavel": navegavel.astype(np.uint8), + # ---- extras p/ debug/telemetria ---- + "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, } def _put_text_centered(self, img, text, cx, cy, font_scale=0.4, thickness=1, color=(255,255,255), outline=True): 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 5bd3ee81b..f3f3f00b5 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 @@ -72,7 +72,7 @@ def load_seg_config(force_reload=False): # "kernel_morf": 3 # } _CONFIG_CACHE = { - "debug_visual": False, + "debug_visual": True, "ia_roi_begin": 0.0, "ia_roi_size": 1.0, "ia_resolution": [512,288], 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 131b530da..7c113d4f5 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 @@ -107,7 +107,12 @@ class CostmapFuser: margem_parada=0.25, # m N_on=2, # frames p/ entrar N_off=5, # frames p/ sair - blackout_imediato=True + 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_det_consider=0.25 ): H, W = custo_f.shape c0, c1 = central_cols @@ -122,6 +127,15 @@ class CostmapFuser: mask_conf = (conf_f < thr_conf_low) unsafe = mask_anom | mask_cost | mask_conf + 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)} + # 2) Largura em colunas por linha width_need_m = robot_width_m + margin_m cols_need = np.empty(H, dtype=int) @@ -145,6 +159,12 @@ class CostmapFuser: 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 + + 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)) for j in it: a, b = central_window(j, cols_need[j]) @@ -183,6 +203,26 @@ class CostmapFuser: 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] @@ -238,18 +278,82 @@ class CostmapFuser: 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}." + f"cobertura_central_max={central_cov_max:.2f}{extra_det_txt}." ) elif (global_cov >= rho_block_global) and (conf_mean < 0.45): blocked_raw = True 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." + f"e confianca_media={conf_mean:.2f}<0.45{extra}." ) elif (central_cov_max > 0.45) and (left > 0.7 or right > 0.7): @@ -257,9 +361,12 @@ class CostmapFuser: 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}." + f"central_max={central_cov_max:.2f}{extra}." ) # 8) Persistência + decisão de parada @@ -382,6 +489,10 @@ class CostmapFuser: 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 + # 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)}" @@ -457,7 +568,12 @@ class CostmapFuser: use_persistence=True, velocidade_mps=velocidade_ms, a_max_freio=0.8, margem_parada=0.60, - N_on=2, N_off=5, blackout_imediato=True + N_on=2, N_off=5, blackout_imediato=True, + + det_score_f = det_score, + det_label_id = det_lbl, + det_strength = det_sdom, + thr_det_consider = 0.25 # só considera detecção acima desse score ) # incrementa seq