diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py index 552996709..00c551ee8 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py +++ b/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) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/__pycache__/direcional.cpython-311.pyc b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/__pycache__/direcional.cpython-311.pyc index 1e3631b3b..f818f29e2 100644 Binary files a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/__pycache__/direcional.cpython-311.pyc and b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/__pycache__/direcional.cpython-311.pyc differ diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py index 2ccc21e9b..f150180b5 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py +++ b/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: - mostrar_log(f"🟥 Bloqueio detectado ({reason}). d_obs_min={d_obs_min if d_obs_min is not None else '–'} m. Parando.") + if blocked and reason == "blackout": + 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): - mostrar_log(f"🟥 Obstáculo à {d_obs_min:.2f} m < distancia necessária {dist_necessaria:.2f} m. Parando.") - return _comando_direcional_parado(True) + if blocked and reason == "obstacle": + if (d_obs_min is not None) and (d_obs_min <= dist_necessaria): + 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) - side_bias = None - if isinstance(block, dict): - sb = block.get("side_bias") or {} - side_bias = sb.get("value", None) - if side_bias is not None: + # motivo narrow: não para, mas dá dica lateral + if isinstance(block, dict) and reason == "narrow": + sb = (block.get("side_bias") or {}).get("value", 0.0) hints = contexto.setdefault("DirecionalHints", {}) - if side_bias > 0.25: # mais fechado à direita -> evite virar p/ direita - hints["vetar_direita"] = True - elif side_bias < -0.25: # mais fechado à esquerda -> evite virar p/ esquerda - hints["vetar_esquerda"] = True + if sb > 0.25: hints["vetar_direita"] = True + if sb < -0.25: hints["vetar_esquerda"] = True # se chegou até aqui, pode seguir return _comando_direcional_parado(False) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/__pycache__/utils.cpython-311.pyc b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/__pycache__/utils.cpython-311.pyc index e5b2bd253..37c34f295 100644 Binary files a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/__pycache__/utils.cpython-311.pyc and b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/__pycache__/utils.cpython-311.pyc differ diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/utils.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/utils.py index 5123a8076..0977e797a 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/utils.py +++ b/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 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 9cac9da3d..b8d2cac27 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 1066fb91d..9ce2ba711 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 bf7427a54..0b6f2fdf5 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 @@ -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 @@ -401,6 +401,8 @@ 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 + + vel = get_velocidade_atual_ms() t0 = time.time() #grid_conf = self._gerar_grid_confianca(depth_frame_np, segmentacao, dist_max) @@ -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}") 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 c5d1a587d..993d80c7b 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 @@ -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 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 5b679f2d4..b3f34abd7 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 @@ -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”) - # mede cobertura do lado esquerdo vs direito dentro do intervalo [c0,c1) + # 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)) - # 7) Decisão de bloqueio e razão - blocked = False + # 7) Decisão "raw" (sem persistência) + 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 - reason = "blackout" # visão ruim / depth ruim geral - # opcional: “narrow” se central ok mas laterais muito ruins + blocked_raw = True + reason = "blackout" 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 }