ajustes no IMU e deteccao de obstaculos

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py

@@ -53,6 +53,16 @@ class IMUCamera(ModuloDiagnosticoBase):
                     self.imu_em_falha = False
 
                 if imuData is not None:
+                    # dt estimado por pacote
+                    now = time.time()
+                    if self.last_imu_ts is None:
+                        dt = 1.0 / max(1e-3, f_tick)   # fallback
+                    else:
+                        dt = max(0.0, now - self.last_imu_ts)
+                    self.last_imu_ts = now
+                    dt = min(dt, 0.05)  # clamp anti-bursts (<=50 ms)
+                    self.filtro_imu.Dt = float(dt)
+
                     for packet in imuData.packets:
                         accel = packet.acceleroMeter
                         gyro = packet.gyroscope
@@ -78,15 +88,6 @@ class IMUCamera(ModuloDiagnosticoBase):
                         yaw -= self.yaw_inicial
 
                         Rwb = r.as_matrix()
-                        # dt estimado por pacote
-                        now = time.time()
-                        if self.last_imu_ts is None:
-                            dt = 1.0 / max(1e-3, f_tick)   # fallback
-                        else:
-                            dt = max(0.0, now - self.last_imu_ts)
-                        self.last_imu_ts = now
-                        dt = min(dt, 0.05)  # clamp anti-bursts (<=50 ms)
-                        self.filtro_imu.Dt = float(dt)
 
                         # 1) aceleração no mundo
                         a_body   = np.array([ax, ay, az], dtype=np.float64)

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py

@@ -217,25 +217,27 @@ def _regras_taticas(contexto):
             blocked = bool(block.get("blocked")) if isinstance(block, dict) else False
             reason  = (block.get("reason") if isinstance(block, dict) else "none") or "none"
 
-            if blocked:
+            if blocked and reason == "blackout":
-                mostrar_log(f"🟥 Bloqueio detectado ({reason}). d_obs_min={d_obs_min if d_obs_min is not None else '–'} m. Parando.")
+                mostrar_log(f"🟥 Blackout de percepção. Parando.")
                 return _comando_direcional_parado(True)
 
-            if (d_obs_min is not None) and (d_obs_min <= dist_necessaria):
+            if blocked and reason == "obstacle":
-                mostrar_log(f"🟥 Obstáculo à {d_obs_min:.2f} m < distancia necessária {dist_necessaria:.2f} m. Parando.")
+                if (d_obs_min is not None) and (d_obs_min <= dist_necessaria):
-                return _comando_direcional_parado(True)
+                    mostrar_log(f"🟥 Obstáculo a {d_obs_min:.2f} m ≤ {dist_necessaria:.2f} m (necessária). Parando.")
+                    return _comando_direcional_parado(True)
+                else:
+                    # opcional: limitar velocidade para manter margem de frenagem
+                    # v_max_safe = sqrt(2*a*(dmin - margem)), se dmin existir
+                    if d_obs_min is not None and d_obs_min > margem_parada:
+                        v_max_safe = (2.0 * a_max_freio * max(0.0, d_obs_min - margem_parada)) ** 0.5
+                        contexto.setdefault("DirecionalHints", {})["v_max_sugerida_mps"] = float(v_max_safe)
 
-            # opcional: dica de veto lateral pro MPC (se você consumir)
+            # motivo narrow: não para, mas dá dica lateral
-            side_bias = None
+            if isinstance(block, dict) and reason == "narrow":
-            if isinstance(block, dict):
+                sb = (block.get("side_bias") or {}).get("value", 0.0)
-                sb = block.get("side_bias") or {}
-                side_bias = sb.get("value", None)
-            if side_bias is not None:
                 hints = contexto.setdefault("DirecionalHints", {})
-                if side_bias > 0.25:   # mais fechado à direita -> evite virar p/ direita
+                if sb > 0.25:  hints["vetar_direita"]  = True
-                    hints["vetar_direita"] = True
+                if sb < -0.25: hints["vetar_esquerda"] = True
-                elif side_bias < -0.25:  # mais fechado à esquerda -> evite virar p/ esquerda
-                    hints["vetar_esquerda"] = True
 
         # se chegou até aqui, pode seguir
         return _comando_direcional_parado(False)

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/utils.py

@@ -2,9 +2,10 @@ import base64
 import math
 import cv2
 import numpy as np
-import cupy as cp
 
 from manager_worker.config import mostrar_log
+from shared.contexto_global_redis import ContextoGlobalRedis
+from shared.enums import StatusModulo, T_Code
 
 
 
@@ -206,3 +207,18 @@ def resize_keep_width(img: np.ndarray, new_w: int, min_h: int) -> np.ndarray:
         new_h = min_h
     return cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA)
 
+# ----------------------------
+# Helpers equipamento
+# ----------------------------
+def get_velocidade_atual_ms():
+    velocidade = 0.0
+    try:
+        velocidade = ContextoGlobalRedis.get_contexto().get("Gerais", {}).get("velocidade_ms", 0.0)
+        if velocidade == 0:
+            imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
+            if imu is not None and imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) == StatusModulo.OPERANTE.value:
+                velocidade = imu.get("vel_mps", 0.0)
+    except Exception as e:
+        mostrar_log(f"Erro ao consultar velocidade atual: {e}")
+    finally:
+        return velocidade

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py

@@ -14,7 +14,7 @@ from visual_worker.processamento.radar_top_down import Radar2DManager
 from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao, SegmentacaoManager
 from visual_worker.processamento.costmap_fuser import CostmapFuser, unpack_snapshot
 from shared.enums import StatusModulo, T_Code, CameraFrameType
-from shared.utils import analisar_linhas_por_profundidade, decode_image_base64, encode_image_base64, fazer_overlay
+from shared.utils import analisar_linhas_por_profundidade, decode_image_base64, encode_image_base64, fazer_overlay, get_velocidade_atual_ms
 from shared.gps_handler import GPSHandler
 from camera_worker.camera_oak import CameraOak
 from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
@@ -402,6 +402,8 @@ class CameraManager:
             segmentacao = self._ultima_analise_segmentacao.get("classes")
             if segmentacao is None: return 
 
+            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.classes)
@@ -409,7 +411,7 @@ class CameraManager:
             t1 = time.time()
             grid_conf["ultima_chamada"] = self._ultima_analise_matriz_confianca.get("ultima_chamada", t0)
             self._calcular_performance(t0, t1, grid_conf)
-            snapshot = self.data_fuser.update(grid_conf, ts=t1)
+            snapshot = self.data_fuser.update(grid_conf, ts=t1, velocidade_ms=vel)
             self._ultima_analise_matriz_confianca = grid_conf
             #self._ultima_analise_segmentacao["corredor_perfil"] = self.segmentacao_manager.calcular_perfil_corredor(matriz, fov_h)
             ContextoGlobalRedis.atualizar_ctx_dict(
@@ -422,11 +424,6 @@ 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)
 
-            vel = 0.0
-            imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
-            if imu is not None and imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) == StatusModulo.OPERANTE.value:
-                vel = imu.get("vel_mps", 0.0)
-
             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}")

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py

@@ -32,7 +32,6 @@ def iniciar_camera_manager(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

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/costmap_fuser.py

@@ -42,6 +42,12 @@ class CostmapFuser:
         self.buf_zref = []   # opcional
         self.buf_ts = []
 
+        self._blk_state = {
+            "on": 0, "off": 0, "latched": False,
+            "reason": "none", "dmin": None,
+            "last_decision": "LIVRE"
+        }
+
         self.seq = 0
 
     def _stack(self, lst, fallback_val=0.0):
@@ -112,7 +118,7 @@ class CostmapFuser:
         # metros por coluna
         return (row_width / float(self.grid_w)).astype(np.float32)             # (H,)
 
-    def update(self, grid_dict, ts=None):
+    def update(self, grid_dict, ts=None, velocidade_ms=0.0):
         """
         grid_dict deve conter (grid_h,grid_w): "custo","conf","anom","navegavel"
         opcional: "z_ref" (m) 2D
@@ -189,6 +195,15 @@ class CostmapFuser:
         # escala X por linha (m/col)
         row_scale_x = self._row_scale_x(row_dist)                  # (H,) ou None
 
+        block = self._compute_blockage_metrics(
+            custo_f, anom_f, conf_f, nav_f,
+            row_dist_m, row_scale_x, self.central_cols,
+            use_persistence=True,
+            velocidade_mps=velocidade_ms,
+            a_max_freio=0.8, margem_parada=0.25,
+            N_on=2, N_off=5, blackout_imediato=True
+        )
+
         # incrementa seq
         self.seq += 1
 
@@ -210,13 +225,14 @@ class CostmapFuser:
                 "near_is_bottom": bool(self.near_is_bottom),
             },
             "y_range_m": [float(self.y_range_m[0]), float(self.y_range_m[1])],
-            #"d_obs_min": (None if d_obs_min is None else float(d_obs_min)),
             "row_dist_m": row_dist_m.tolist(),
             "row_scale_x_m": row_scale_x.tolist(),
             "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()
+            "nav_mask": nav_f.astype(np.uint8).ravel().tolist(),
+            "d_obs_min": block["d_obs_min"],
+            "block": block
         }
         
         # (opcional) incluir z_ref_u8 pra debug/visualização
@@ -224,16 +240,6 @@ class CostmapFuser:
         #     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()
 
-        block = self._compute_blockage_metrics(
-            custo_f, anom_f, conf_f, nav_f,
-            row_dist_m, row_scale_x,
-            central_cols=self.central_cols,
-            robot_width_m=self.robot_width_m,  # defina no __init__ ou config
-            margin_m=0.12,
-        )
-        snap["block"] = block
-        snap["d_obs_min"] = block["d_obs_min"]  # mantém campo raiz por compatibilidade
-
         return snap
     
     def _compute_blockage_metrics(
@@ -243,57 +249,69 @@ class CostmapFuser:
         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
+        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.25,     # m
+        N_on=2,                 # frames p/ entrar
+        N_off=5,                # frames p/ sair
+        blackout_imediato=True
     ):
         """
         Retorna dict com:
         - d_obs_min (m) ou None
-        - blocked (bool)
+        - blocked (bool)  -> se use_persistence=False: igual ao "raw"; se True: já com persistência
+        - blocked_raw (bool)
         - reason ('obstacle','blackout','narrow','none')
         - coverage: {'central_max':..., 'global':...}
         - side_bias: {'value': -1..+1, 'left_frac':..., 'right_frac':...}
-        - j_block (índice da linha que bloqueia) ou None
+        - j_block (índice) ou None
+        - decision: { 'parar': bool, 'dist_necessaria': float, 'v_max_sugerida_mps': float|None,
+                    'frames_on':int, 'frames_off':int, 'N_on':int, 'N_off':int }
         """
 
+        import numpy as np
+
         H, W = custo_f.shape
-        c0, c1 = central_cols  # intervalo central sugerido pelo seu snap
+        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  # fallback
 
-        # 1) Máscaras “inseguras”
+        # 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
 
-        # 2) Largura em colunas por linha (corredor = robô + margem)
+        # 2) Largura em colunas por linha
         width_need_m = robot_width_m + margin_m
         cols_need = []
         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)))
             if sx is None or sx <= 1e-6:
-                cols_need.append(c1 - c0)  # fallback
+                cols_need.append(c1 - c0)
             else:
                 ncols = int(np.ceil(width_need_m / sx))
                 cols_need.append(max(1, min(W, ncols)))
         cols_need = np.asarray(cols_need, dtype=int)
 
-        # 3) Varredura por linha: janela central com largura cols_need[j]
+        # 3) Varredura central
         def central_window(j, ncols):
-            # centra no meio de [c0,c1)
             mid = (c0 + c1) // 2
             half = ncols // 2
             a = max(0, mid - half)
             b = min(W, a + ncols)
-            # ajusta se estourou esquerda/direita
             a = max(0, b - ncols)
             return a, b
 
         coverage_central = np.zeros(H, np.float32)
         j_block = None
-        for j in (range(H-1, -1, -1) if near_is_bottom else range(H)):  # começa pelo "mais perto"
+        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])
             unsafe_row = unsafe[j, a:b]
             coverage = unsafe_row.mean() if (b > a) else 1.0
@@ -302,45 +320,109 @@ class CostmapFuser:
                 j_block = j
                 break
 
-        # 4) Cobertura global (fallback diagnóstico)
+        # 4) Cobertura global
         global_cov = unsafe.mean()
 
-        # 5) d_obs_min em metros
+        # 5) Distância do 1º bloqueio
         d_obs_min = None if j_block is None else float(row_dist_m[j_block])
 
-        # 6) Viés lateral (onde está mais “fechado”)
+        # 6) Viés lateral
-        # mede cobertura do lado esquerdo vs direito dentro do intervalo [c0,c1)
         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))
 
-        # 7) Decisão de bloqueio e razão
+        # 7) Decisão "raw" (sem persistência)
-        blocked = False
+        blocked_raw = False
         reason  = "none"
         if j_block is not None:
-            blocked = True
+            blocked_raw = True
             reason = "obstacle"
         elif global_cov >= rho_block_global and (conf_f.mean() < 0.45):
-            blocked = True
+            blocked_raw = True
-            reason = "blackout"  # visão ruim / depth ruim geral
+            reason = "blackout"
-        # opcional: “narrow” se central ok mas laterais muito ruins
         elif coverage_central.max() > 0.45 and (left > 0.7 or right > 0.7):
             reason = "narrow"
 
+        # 8) Persistência / histerese + decisão de parada (opcional)
+        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
+        }
+
+        blocked_out = blocked_raw  # default: compatível com antes
+
+        if use_persistence:
+            # calcula distância necessária se tivermos velocidade
+            v = float(max(0.0, velocidade_mps or 0.0))
+            dist_freio      = max(0.30, (v*v) / max(1e-9, 2.0 * a_max_freio))
+            dist_necessaria = dist_freio + margem_parada
+            decision["dist_necessaria"] = float(dist_necessaria)
+
+            # regra "stop_now" crua (antes do debounce)
+            stop_now = False
+            v_max_sug = None
+
+            if blocked_raw and reason == "blackout":
+                stop_now = True if blackout_imediato else False
+
+            if blocked_raw and reason == "obstacle":
+                if (d_obs_min is not None) and np.isfinite(d_obs_min) and (d_obs_min <= dist_necessaria):
+                    stop_now = True
+                else:
+                    if (d_obs_min is not None) and np.isfinite(d_obs_min) and (d_obs_min > margem_parada):
+                        v_max_sug = float(np.sqrt(max(0.0, 2.0 * a_max_freio * (d_obs_min - margem_parada))))
+
+            # histerese
+            st = self._blk_state
+            if stop_now:
+                st["on"]  += 1
+                st["off"]  = 0
+            else:
+                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
+
+            # saída persistente
+            decision.update({
+                "parar": bool(st["latched"]),
+                "v_max_sugerida_mps": v_max_sug,
+                "frames_on": int(st["on"]),
+                "frames_off": int(st["off"])
+            })
+
+            # quando usamos persistência, o 'blocked' exposto passa a refletir a decisão debounced:
+            blocked_out = bool(st["latched"])
+
+            # guarda debug
+            st["reason"] = reason
+            st["dmin"] = (None if d_obs_min is None else float(d_obs_min))
+            st["last_decision"] = "PARAR" if st["latched"] else "LIVRE"
+
         return {
             "d_obs_min": d_obs_min,
-            "blocked": bool(blocked),
+            "blocked": bool(blocked_out),          # <- já com persistência se habilitada
+            "blocked_raw": bool(blocked_raw),      # <- útil pra debug/HUD
             "reason": reason,
             "coverage": {
                 "central_max": float(coverage_central.max()),
                 "global": float(global_cov),
             },
             "side_bias": {
-                "value": side_bias_val,   # <0 = mais fechado à esquerda; >0 = direita
+                "value": side_bias_val,
                 "left_frac": float(left),
                 "right_frac": float(right),
             },
             "j_block": (None if j_block is None else int(j_block)),
+            "decision": decision
         }