From fec398d5a41dd33f65d8303257834af8169b4786 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Mon, 8 Jun 2026 07:36:41 -0300 Subject: [PATCH] ajustes mpc e raw_processor_core --- AgroBase/AgroBase/Services/SerialService.cs | 2 +- .../manager_worker/modulos/direcional.py | 308 +++++- .../workers/manager_worker/modulos/mpc.py | 981 ++++++++++++------ .../oak-fcc-3/calibration/module_params.json | 20 +- .../oak-fcc-3/core/raw_processor_core.py | 393 ++++++- Python/OAK/visual_worker/camera_manager.py | 241 ----- Python/OAK/visual_worker/enums.py | 38 - Python/OAK/visual_worker/filtros.py | 0 Python/OAK/visual_worker/main.py | 173 --- Python/OAK/visual_worker/mqtt_handler.py | 51 - .../visual_worker/processamento/corredor.py | 54 - .../visual_worker/processamento/obstaculos.py | 227 ---- .../visual_worker/processamento/visao3d.py | 48 - Python/OAK/visual_worker/utils.py | 47 - 14 files changed, 1375 insertions(+), 1208 deletions(-) delete mode 100644 Python/OAK/visual_worker/camera_manager.py delete mode 100644 Python/OAK/visual_worker/enums.py delete mode 100644 Python/OAK/visual_worker/filtros.py delete mode 100644 Python/OAK/visual_worker/main.py delete mode 100644 Python/OAK/visual_worker/mqtt_handler.py delete mode 100644 Python/OAK/visual_worker/processamento/corredor.py delete mode 100644 Python/OAK/visual_worker/processamento/obstaculos.py delete mode 100644 Python/OAK/visual_worker/processamento/visao3d.py delete mode 100644 Python/OAK/visual_worker/utils.py diff --git a/AgroBase/AgroBase/Services/SerialService.cs b/AgroBase/AgroBase/Services/SerialService.cs index b211799fb..3cb2b6814 100644 --- a/AgroBase/AgroBase/Services/SerialService.cs +++ b/AgroBase/AgroBase/Services/SerialService.cs @@ -38,7 +38,7 @@ namespace AgroBase.Services new DispositivoDetalhesModel() { Dispositivo = T_Code.Npc, - Endereco = EthernetService.ObterIpAtual(null), + Endereco = EthernetService.ObterIpAtual(VariaveisEquipamento.Parametros.rover_interface), Versao = Variaveis.Versao, Mod_ID = "" } 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 6e248ff75..483be8c19 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 @@ -1,4 +1,5 @@ import time +import numpy as np from shared.enums import ModoOperacao, StatusCarroMapa, StatusModulo, T_Code, TipoMovimentoDirecional, TiposControladorDirecional from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey @@ -17,46 +18,81 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool): _equipamento = ContextoGlobalRedis.get_equipamento() _trajetoria = _contexto.get("Trajetoria", {}) - usar_dados_sonar = _controle.get("dir_auxilio_sonar", False) + usar_auxilio_visual = _controle.get("dir_auxilio_sonar", False) dados_visual_worker = None - if usar_dados_sonar: + if usar_auxilio_visual: _snr = ContextoGlobalRedis.get_modulo(T_Code.Snr) - visual_worker_operante = (_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) == StatusModulo.OPERANTE.value + visual_worker_operante = ( + _snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) + == StatusModulo.OPERANTE.value + ) + _dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}) _snapshot_vw = _dados_vw.get("matriz_confianca") + custo = None + conf = None + anom = None nav = None dists = None escalas = None block = None - if not _snapshot_vw: - visual_worker_atualizado = False - else: - visual_worker_atualizado = (time.time() - _snapshot_vw.get("ts", 0.0) <= 1.0) + + max_idade_ms = 1000.0 + visual_worker_atualizado = False + + if _snapshot_vw: + ts_snapshot = float(_snapshot_vw.get("ts", 0.0) or 0.0) + idade_ms = (time.time() - ts_snapshot) * 1000.0 if ts_snapshot > 0 else None + visual_worker_atualizado = idade_ms is not None and idade_ms <= max_idade_ms + if visual_worker_atualizado: custo, conf, anom, nav = unpack_snapshot(_snapshot_vw) dists = _snapshot_vw.get("row_dist_m", None) escalas = _snapshot_vw.get("row_scale_x_m", None) block = _snapshot_vw.get("block", None) - matriz_valida = ( - visual_worker_operante - and visual_worker_atualizado - and _snapshot_vw is not None - and custo is not None - and nav is not None - and dists is not None - and escalas is not None - and block is not None + + costmap_direcional = _montar_costmap_direcional( + habilitado=usar_auxilio_visual, + operante=visual_worker_operante, + atualizado=visual_worker_atualizado, + snapshot=_snapshot_vw, + custo=custo, + nav=nav, + dists=dists, + escalas=escalas, + conf=conf, + anom=anom, + block=block, + max_idade_ms=max_idade_ms ) + + matriz_valida = bool(costmap_direcional.get("valido", False)) + dados_visual_worker = { - "Operante": visual_worker_operante, - "Atualizado": visual_worker_atualizado, + "Operante": visual_worker_operante, + "Atualizado": visual_worker_atualizado, + + # Contrato novo, será o principal daqui pra frente. + "CostMapDirecional": costmap_direcional, + + # Compatibilidade temporária com o MPC atual. "MatrizCusto": { - "Valida": matriz_valida, - "Custo": custo, - "Navegavel": nav, - "DistanciasRef": dists, + "Valida": matriz_valida, + "EmUso": matriz_valida, + "Custo": custo, + "Navegavel": nav, + "DistanciasRef": dists, "EscalasX": escalas, + + # extras úteis mesmo no contrato antigo + "Confianca": conf, + "Anomalia": anom, + "Bloqueio": block, + "Ts": costmap_direcional.get("ts", 0.0), + "IdadeMs": costmap_direcional.get("idade_ms", None), + "MotivoInvalido": costmap_direcional.get("motivo_invalido", ""), + "MotivosInvalidos": costmap_direcional.get("motivos_invalidos", []) }, } @@ -280,3 +316,231 @@ def _montar_comando_retorno(comando, latencia: float = -1): if latencia > -1: comando["latencia"] = latencia return comando + + + +def _shape_matriz(m): + try: + if m is None: + return {"linhas": 0, "colunas": 0} + s = getattr(m, "shape", None) + if s is None or len(s) < 2: + return {"linhas": 0, "colunas": 0} + return {"linhas": int(s[0]), "colunas": int(s[1])} + except Exception: + return {"linhas": 0, "colunas": 0} + +def _mesmo_shape(a, b): + try: + if a is None or b is None: + return False + return tuple(a.shape) == tuple(b.shape) + except Exception: + return False + +def _len_igual_shape_linhas(v, matriz): + try: + if v is None or matriz is None: + return False + return len(v) == int(matriz.shape[0]) + except Exception: + return False + +def _resumo_matriz(custo, nav, conf=None, anom=None, block=None): + resumo = { + "frac_navegavel": None, + "frac_bloqueado": None, + "custo_min": None, + "custo_max": None, + "custo_medio": None, + "confianca_media": None, + "anomalia_media": None, + } + + try: + if nav is not None: + nav_np = np.asarray(nav, dtype=bool) + resumo["frac_navegavel"] = float(np.mean(nav_np)) + resumo["frac_bloqueado"] = float(1.0 - np.mean(nav_np)) + + if block is not None: + block_np = np.asarray(block, dtype=bool) + resumo["frac_bloqueado"] = float(np.mean(block_np)) + + if custo is not None: + custo_np = np.asarray(custo, dtype=np.float32) + if custo_np.size > 0: + resumo["custo_min"] = float(np.nanmin(custo_np)) + resumo["custo_max"] = float(np.nanmax(custo_np)) + resumo["custo_medio"] = float(np.nanmean(custo_np)) + + if conf is not None: + conf_np = np.asarray(conf, dtype=np.float32) + if conf_np.size > 0: + resumo["confianca_media"] = float(np.nanmean(conf_np)) + + if anom is not None: + anom_np = np.asarray(anom, dtype=np.float32) + if anom_np.size > 0: + resumo["anomalia_media"] = float(np.nanmean(anom_np)) + + except Exception as e: + mostrar_log(f"⚠️ Erro ao gerar resumo do CostMapDirecional: {e}") + + return resumo + +def _montar_costmap_direcional( + *, + habilitado, + operante, + atualizado, + snapshot, + custo, + nav, + dists, + escalas, + conf=None, + anom=None, + block=None, + max_idade_ms=1000.0 +): + try: + ts = 0.0 + idade_ms = None + seq = None + fonte = "visual_worker" + + if snapshot: + ts = float(snapshot.get("ts", 0.0) or 0.0) + idade_ms = (time.time() - ts) * 1000.0 if ts > 0 else None + seq = snapshot.get("seq", snapshot.get("id", None)) + fonte = snapshot.get("fonte", snapshot.get("origem", "visual_worker")) + + shape = _shape_matriz(custo) + + motivos_invalidos = [] + + if not habilitado: + motivos_invalidos.append("auxilio_visual_desabilitado") + if not operante: + motivos_invalidos.append("visual_worker_nao_operante") + if not atualizado: + motivos_invalidos.append("snapshot_desatualizado") + if snapshot is None: + motivos_invalidos.append("snapshot_ausente") + if custo is None: + motivos_invalidos.append("matriz_custo_ausente") + if nav is None: + motivos_invalidos.append("matriz_navegavel_ausente") + if dists is None: + motivos_invalidos.append("row_dist_m_ausente") + if escalas is None: + motivos_invalidos.append("row_scale_x_m_ausente") + if block is None: + motivos_invalidos.append("block_ausente") + + if custo is not None and nav is not None and not _mesmo_shape(custo, nav): + motivos_invalidos.append("shape_custo_navegavel_incompativel") + + if custo is not None and conf is not None and not _mesmo_shape(custo, conf): + motivos_invalidos.append("shape_custo_confianca_incompativel") + + if custo is not None and anom is not None and not _mesmo_shape(custo, anom): + motivos_invalidos.append("shape_custo_anomalia_incompativel") + + if custo is not None and block is not None and not _mesmo_shape(custo, block): + motivos_invalidos.append("shape_custo_block_incompativel") + + if custo is not None and dists is not None and not _len_igual_shape_linhas(dists, custo): + motivos_invalidos.append("row_dist_m_tamanho_incompativel") + + if custo is not None and escalas is not None and not _len_igual_shape_linhas(escalas, custo): + motivos_invalidos.append("row_scale_x_m_tamanho_incompativel") + + valido = len(motivos_invalidos) == 0 + + # Por enquanto, este contrato ainda descreve a convenção atual: + # row_dist_m corresponde ao vetor físico em ordem crescente, + # mas a matriz atual ainda pode estar no padrão legado/reverso. + # Na próxima etapa, o MPC normaliza isso. + contrato = { + "versao": 2, + + "habilitado": bool(habilitado), + "valido": bool(valido), + "em_uso": bool(valido), + "motivo_invalido": "" if valido else ";".join(motivos_invalidos), + "motivos_invalidos": motivos_invalidos, + + "ts": ts, + "idade_ms": idade_ms, + "max_idade_ms": float(max_idade_ms), + "seq": seq, + + "frame": "robo", + "origem": fonte, + + "shape": shape, + + "geometria": { + "row_dist_m": dists, + "row_scale_x_m": escalas, + + # Importante: deixa explícito para o MPC não depender de magia escondida. + # Se depois o VisualWorker já mandar matriz física direta, mudamos aqui. + "row_order": "near_to_far", + "matrix_row_order": "legacy_reversed", + "col_order": "left_to_right", + "x_zero": "centro", + "y_zero": "frente_robo", + "unidade": "m" + }, + + "matrizes": { + "custo": custo, + "navegavel": nav, + "confianca": conf, + "anomalia": anom, + "bloqueio": block + }, + + "normalizacao": { + "custo_min": 0.0, + "custo_max": 1.0, + "custo_baixo_melhor": True, + "navegavel_true_livre": True, + "bloqueio_true_ocupado": True, + "confianca_max_melhor": True, + "anomalia_max_pior": True + }, + + "politica": { + "usar_bloqueio_duro": True, + "limiar_bloqueio_frac": 0.20, + "limiar_hot_stop_frac": 0.45, + "peso_visual": 1.0, + "distancia_min_avaliacao_m": 0.60 + }, + + "debug": { + "resumo": _resumo_matriz(custo, nav, conf=conf, anom=anom, block=block) + } + } + + return contrato + + except Exception as e: + mostrar_log(f"❌ Erro ao montar CostMapDirecional: {e}") + return { + "versao": 2, + "habilitado": bool(habilitado), + "valido": False, + "em_uso": False, + "motivo_invalido": f"erro_montagem_contrato:{e}", + "motivos_invalidos": [f"erro_montagem_contrato:{e}"], + "matrizes": {}, + "geometria": {}, + "politica": {}, + "debug": {} + } + diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py index b543342d4..98b7ecdbf 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py @@ -1,6 +1,11 @@ + +from __future__ import annotations + import heapq import math import time +from typing import Any, Callable, Dict, Iterable, List, Optional, Sequence, Tuple + import numpy as np from shared.enums import StatusCarroMapa, TipoMovimentoDirecional @@ -8,16 +13,120 @@ from manager_worker.config import mostrar_log from shared.gps_handler import GPSHandler from shared.contexto_global_redis import ContextoGlobalRedis + +def _log(msg: str) -> None: + """Log protegido: nunca deixa uma falha de logging derrubar o MPC.""" + try: + mostrar_log(msg) + except Exception: + pass + + +def _as_dict(value: Any) -> dict: + return value if isinstance(value, dict) else {} + + +def _safe_len(value: Any) -> int: + try: + return len(value) if value is not None else 0 + except Exception: + return 0 + + +def _safe_float(value: Any, default: float = 0.0, *, min_value: float | None = None, max_value: float | None = None) -> float: + try: + out = float(value) + if not math.isfinite(out): + out = float(default) + except Exception: + out = float(default) + if min_value is not None: + out = max(float(min_value), out) + if max_value is not None: + out = min(float(max_value), out) + return out + + +def _safe_int(value: Any, default: int = 0, *, min_value: int | None = None, max_value: int | None = None) -> int: + try: + out = int(value) + except Exception: + out = int(default) + if min_value is not None: + out = max(int(min_value), out) + if max_value is not None: + out = min(int(max_value), out) + return out + + +def _enum_value(value: Any, default: Any) -> Any: + return getattr(value, "value", value if value is not None else default) + + +def _enum_name(value: Any, enum_cls: Any, default: str = "Desconhecido") -> str: + try: + return enum_cls(value).name + except Exception: + try: + return value.name + except Exception: + return default + + +def _status_in(status_value: Any, statuses: Sequence[StatusCarroMapa]) -> bool: + status_raw = _enum_value(status_value, status_value) + return any(status_raw == s.value or status_value == s for s in statuses) + + +def _movimento_from_value(value: Any, default: TipoMovimentoDirecional = TipoMovimentoDirecional.RodasDianteiras) -> TipoMovimentoDirecional: + try: + return TipoMovimentoDirecional(value) + except Exception: + try: + if isinstance(value, TipoMovimentoDirecional): + return value + except Exception: + pass + return default + + +def _safe_motivos(motivos: Any) -> list: + if motivos is None: + return [] + if isinstance(motivos, list): + return motivos + if isinstance(motivos, tuple): + return list(motivos) + return [str(motivos)] + _mpc = None _iniciado = False def inicializar(parametros, mapa, p_ref, forcar): + """Inicializa ou recria o controlador MPC sem derrubar a instância anterior em caso de erro.""" global _mpc, _iniciado - mostrar_log(f"[MPC] Inicializar chamado. iniciado: {_iniciado}, len_pontosinfo: {(len(_mpc.pontos_info) if _mpc is not None else 0)}, len_mapa: {len(mapa)}, forcar: {forcar}") - if not _iniciado or (_mpc is not None and len(_mpc.pontos_info) != len(mapa)) or forcar: - _mpc = ControladorMPC(parametros_mpc=parametros, pontos_mapa=mapa, p_ref=p_ref) - mostrar_log(f"MPC iniciado! Trajetoria com {len(_mpc.pontos_info)} pontos") + + mapa = list(mapa or []) + parametros = _as_dict(parametros) + + len_atual = _safe_len(getattr(_mpc, "pontos_info", [])) if _mpc is not None else 0 + len_mapa = _safe_len(mapa) + precisa_recriar = (not _iniciado) or (_mpc is not None and len_atual != len_mapa) or bool(forcar) + + _log(f"[MPC] Inicializar chamado. iniciado: {_iniciado}, len_pontosinfo: {len_atual}, len_mapa: {len_mapa}, forcar: {forcar}") + + if not precisa_recriar: + return + + try: + novo_mpc = ControladorMPC(parametros_mpc=parametros, pontos_mapa=mapa, p_ref=p_ref) + _mpc = novo_mpc _iniciado = True + _log(f"[MPC] Iniciado. Trajetoria com {_safe_len(_mpc.pontos_info)} pontos") + except Exception as e: + _log(f"❌ Erro ao inicializar MPC. Instância anterior preservada: {e}") + if _mpc is None: + _iniciado = False def get_mpc(): global _mpc @@ -32,57 +141,81 @@ def comando_parado(): class ControladorMPC: def __init__(self, parametros_mpc, pontos_mapa, p_ref): + """Cria o controlador com defaults explícitos e validações leves. + + Compatibilidade preservada: + - mesmos parâmetros de entrada; + - mesmos atributos públicos usados pelo restante do worker; + - mesma estrutura de `pontos_info` esperada pelo MPC. + """ + parametros_mpc = _as_dict(parametros_mpc) + self.pontos_info = list(pontos_mapa or []) + + if not isinstance(p_ref, (list, tuple)) or len(p_ref) < 2: + raise ValueError("p_ref inválido. Esperado (lat0, lon0).") + + self.lat0 = _safe_float(p_ref[0]) + self.lon0 = _safe_float(p_ref[1]) + self.gps_handler = GPSHandler(self.lat0, self.lon0) + + self.ultima_atualizacao = None + self.dt = 0.0 + + self.angulo_max_graus = _safe_float(parametros_mpc.get("angulo_max_graus", 30.0), 30.0, min_value=0.1, max_value=89.0) + self.velocidade_min = _safe_float(parametros_mpc.get("velocidade_min", 0.2), 0.2, min_value=0.0) + self.velocidade_max = _safe_float(parametros_mpc.get("velocidade_max", 2.0), 2.0, min_value=max(self.velocidade_min, 1e-6)) + self.horizonte = _safe_float(parametros_mpc.get("horizonte", 2.0), 2.0, min_value=0.1) + + self._beam_traj_min = _safe_int(parametros_mpc.get("beam_traj_min", 3), 3, min_value=1, max_value=20) + self._beam_topN_max = _safe_int(parametros_mpc.get("beam_topN_max", 5), 5, min_value=1, max_value=50) + self._beam_topN_min = _safe_int(parametros_mpc.get("beam_topN_min", 2), 2, min_value=1, max_value=self._beam_topN_max) + self._n_subporpasso = _safe_int(parametros_mpc.get("n_subporpasso", 1), 1, min_value=1, max_value=10) + + self._np_bool = np.bool_ + self._lut_pronto = False + self._lut = {} + self._lut_tipo = {} + self._lut_tipo_sig = {} + self._lut_base_sig = None + + self.visitados_execucao = np.zeros(len(self.pontos_info), dtype=self._np_bool) if self.pontos_info else np.zeros(0, dtype=self._np_bool) + try: - self.gps_handler = GPSHandler(p_ref[0], p_ref[1]) - self.lat0 = p_ref[0] - self.lon0 = p_ref[1] - #self.visualizador = VisualizadorTrajetoria() - - self.ultima_atualizacao = None - self.dt = 0 - - #print(parametros_mpc) - self.angulo_max_graus = parametros_mpc.get("angulo_max_graus") - self.velocidade_min = parametros_mpc.get("velocidade_min") - self.velocidade_max = parametros_mpc.get("velocidade_max") - self.horizonte = parametros_mpc.get("horizonte") - self._beam_traj_min = parametros_mpc.get("beam_traj_min", 3) # nº mínimo de trajetórias completas que queremos - self._beam_topN_max = parametros_mpc.get("beam_topN_max", 5) # limite superior de candidatos ativos - self._beam_topN_min = parametros_mpc.get("beam_topN_min", 2) # limite inferior - self._n_subporpasso = 1 - - # otimizacoes - self._np_bool = np.bool_ - self._lut_pronto = False - self._lut = {} - - self.pontos_info = pontos_mapa - self.visitados_execucao = np.zeros(len(self.pontos_info), dtype=self._np_bool) if pontos_mapa else [] - - P = np.array([p["xy"] for p in self.pontos_info], dtype=np.float32) - M = np.array([p.get("distanciaMargem", 0.7) for p in self.pontos_info], dtype=np.float32) - d = np.sqrt(np.sum(np.diff(P, axis=0)**2, axis=1, dtype=np.float32)) - s = np.concatenate(([0.0], np.cumsum(d))) - self._p_xy = P; self._p_margem = M; self._p_s = s - - self._t_est_passo_ms = 6.0 # chute inicial: tempo por expansão (um _simular_passo) - self._ema_alpha = 0.2 # suavização da EMA - self._mpc_sla_ms = 200.0 # alvo 5 Hz - self._mpc_headroom_ms = 25.0 # respiro p/ IO/log/atuadores - self.debug_cost_vis = False - - self._cl_xy = None # Nx2 - self._seg_p0 = None # (N-1)x2 - self._seg_v = None # (N-1)x2 - self._seg_L = None # (N-1,) - self._seg_t_hat = None # (N-1)x2 unit tangent - self._seg_n_hat = None # (N-1)x2 unit normal (esquerda) - self._s_nodes = None # (N,) - self._mg_nodes = None # (N,) - self._precompute_centerline() - + if self.pontos_info: + P = np.array([p.get("xy", (0.0, 0.0)) for p in self.pontos_info], dtype=np.float32) + M = np.array([p.get("distanciaMargem", 0.7) for p in self.pontos_info], dtype=np.float32) + if P.ndim != 2 or P.shape[1] != 2: + raise ValueError("pontos_mapa deve conter campo 'xy' com dois valores por ponto.") + d = np.sqrt(np.sum(np.diff(P, axis=0) ** 2, axis=1, dtype=np.float32)) if len(P) > 1 else np.array([], dtype=np.float32) + s = np.concatenate(([0.0], np.cumsum(d))) + else: + P = np.zeros((0, 2), dtype=np.float32) + M = np.zeros((0,), dtype=np.float32) + s = np.zeros((0,), dtype=np.float32) except Exception as e: - mostrar_log(f"Erro ao inicializar __init__ MPC: {e}") + raise ValueError(f"Mapa inválido para o MPC: {e}") from e + + self._p_xy = P + self._p_margem = M + self._p_s = s + + self._t_est_passo_ms = _safe_float(parametros_mpc.get("t_est_passo_ms", 6.0), 6.0, min_value=1.0) + self._t_est_candidato_ms = _safe_float(parametros_mpc.get("t_est_candidato_ms", 4.0), 4.0, min_value=1.0) + self._ema_alpha = _safe_float(parametros_mpc.get("ema_alpha", 0.2), 0.2, min_value=0.01, max_value=1.0) + self._mpc_sla_ms = _safe_float(parametros_mpc.get("mpc_sla_ms", 200.0), 200.0, min_value=50.0) + self._mpc_headroom_ms = _safe_float(parametros_mpc.get("mpc_headroom_ms", 25.0), 25.0, min_value=0.0) + self.debug_cost_vis = bool(parametros_mpc.get("debug_cost_vis", False)) + + self._cl_xy = None + self._seg_p0 = None + self._seg_v = None + self._seg_L = None + self._seg_t_hat = None + self._seg_n_hat = None + self._s_nodes = None + self._mg_nodes = None + self._tick_id = 0 + self._precompute_centerline() def _precompute_centerline(self): cl = np.array([p["xy"] for p in self.pontos_info], dtype=float) # N x 2 @@ -151,34 +284,199 @@ class ControladorMPC: return e_lat, s_proj, i, proj_i, theta_path, margem except Exception as e: - mostrar_log(f"Erro ao calcular erro lateral: {e}") + _log(f"Erro ao calcular erro lateral: {e}") + return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7 - def _ensure_luts(self, dados_matriz_custo, largura_equip, L): - if not dados_matriz_custo or dados_matriz_custo.get("Custo") is None: + def _get_costmap_direcional(self, contexto): + """ + Lê o contrato VisualWorker.CostMapDirecional e devolve uma estrutura interna + normalizada para uso do MPC. + + Regra interna após normalização: + - custo[i, :] corresponde à distância row_dist_m[i] + - row_dist_m está em ordem crescente/near_to_far + - row_scale_x_m[i] corresponde à mesma linha i da matriz + - navegavel True = livre + - bloqueio True = ocupado + """ + try: + vw = contexto.get("VisualWorker") or {} + cm = vw.get("CostMapDirecional") or {} + + if not cm or not cm.get("valido", False): + return { + "valido": False, + "em_uso": False, + "motivo_invalido": cm.get("motivo_invalido", "costmap_ausente_ou_invalido"), + } + + matrizes = cm.get("matrizes", {}) or {} + geom = cm.get("geometria", {}) or {} + politica = cm.get("politica", {}) or {} + normalizacao = cm.get("normalizacao", {}) or {} + + custo = matrizes.get("custo", None) + nav = matrizes.get("navegavel", None) + conf = matrizes.get("confianca", None) + anom = matrizes.get("anomalia", None) + block = matrizes.get("bloqueio", None) + + row_dist_m = geom.get("row_dist_m", None) + row_scale_x_m = geom.get("row_scale_x_m", None) + + if custo is None or nav is None or row_dist_m is None or row_scale_x_m is None: + return { + "valido": False, + "em_uso": False, + "motivo_invalido": "campos_obrigatorios_ausentes", + } + + custo = np.asarray(custo, dtype=np.float32) + nav = np.asarray(nav, dtype=bool) + row_dist_m = np.asarray(row_dist_m, dtype=np.float32) + row_scale_x_m = np.asarray(row_scale_x_m, dtype=np.float32) + + if custo.ndim != 2 or nav.shape != custo.shape: + return { + "valido": False, + "em_uso": False, + "motivo_invalido": "shape_custo_navegavel_invalido", + } + + H, W = custo.shape + + if row_dist_m.size != H or row_scale_x_m.size != H: + return { + "valido": False, + "em_uso": False, + "motivo_invalido": "geometria_incompativel_com_shape", + } + + conf_np = None if conf is None else np.asarray(conf, dtype=np.float32) + anom_np = None if anom is None else np.asarray(anom, dtype=np.float32) + block_np = None if block is None else np.asarray(block, dtype=bool) + + if conf_np is not None and conf_np.shape != custo.shape: + conf_np = None + + if anom_np is not None and anom_np.shape != custo.shape: + anom_np = None + + if block_np is not None and block_np.shape != custo.shape: + block_np = None + + matrix_row_order = geom.get("matrix_row_order", "aligned") + row_order = geom.get("row_order", "near_to_far") + + # Normalização de linhas. + # Objetivo: depois daqui, matriz[i] corresponde a row_dist_m[i]. + if matrix_row_order == "legacy_reversed": + custo = custo[::-1, :] + nav = nav[::-1, :] + if conf_np is not None: + conf_np = conf_np[::-1, :] + if anom_np is not None: + anom_np = anom_np[::-1, :] + if block_np is not None: + block_np = block_np[::-1, :] + + # Se as distâncias vierem far_to_near, inverte tudo para near_to_far. + if row_order == "far_to_near": + row_dist_m = row_dist_m[::-1] + row_scale_x_m = row_scale_x_m[::-1] + custo = custo[::-1, :] + nav = nav[::-1, :] + if conf_np is not None: + conf_np = conf_np[::-1, :] + if anom_np is not None: + anom_np = anom_np[::-1, :] + if block_np is not None: + block_np = block_np[::-1, :] + + # Garante ordem crescente por segurança. + ordem = np.argsort(row_dist_m) + row_dist_m = row_dist_m[ordem] + row_scale_x_m = row_scale_x_m[ordem] + custo = custo[ordem, :] + nav = nav[ordem, :] + if conf_np is not None: + conf_np = conf_np[ordem, :] + if anom_np is not None: + anom_np = anom_np[ordem, :] + if block_np is not None: + block_np = block_np[ordem, :] + + # Se existe bloqueio explícito, ele também fecha navegabilidade. + if block_np is not None and normalizacao.get("bloqueio_true_ocupado", True): + nav = np.logical_and(nav, ~block_np) + + return { + "valido": True, + "em_uso": bool(cm.get("em_uso", True)), + "versao": cm.get("versao", 2), + "ts": cm.get("ts", 0.0), + "idade_ms": cm.get("idade_ms", None), + "origem": cm.get("origem", "visual_worker"), + + "custo": custo, + "navegavel": nav, + "confianca": conf_np, + "anomalia": anom_np, + "bloqueio": block_np, + + "row_dist_m": row_dist_m, + "row_scale_x_m": row_scale_x_m, + + "shape": { + "linhas": int(H), + "colunas": int(W), + }, + + "politica": { + "usar_bloqueio_duro": politica.get("usar_bloqueio_duro", True), + "limiar_bloqueio_frac": float(politica.get("limiar_bloqueio_frac", 0.20)), + "limiar_hot_stop_frac": float(politica.get("limiar_hot_stop_frac", 0.45)), + "peso_visual": float(politica.get("peso_visual", 1.0)), + "distancia_min_avaliacao_m": float(politica.get("distancia_min_avaliacao_m", 0.60)), + }, + + "debug": cm.get("debug", {}), + } + + except Exception as e: + mostrar_log(f"❌ Erro ao normalizar CostMapDirecional: {e}") + return { + "valido": False, + "em_uso": False, + "motivo_invalido": f"erro_normalizacao:{e}", + } + + def _ensure_luts(self, dados_costmap, largura_equip, L): + if not dados_costmap or dados_costmap.get("custo") is None: return - C = dados_matriz_custo["Custo"] - escx = dados_matriz_custo.get("EscalasX", []) - dist = dados_matriz_custo.get("DistanciasRef", []) - dy = getattr(self, "dy", 0.02) + C = dados_costmap["custo"] + escx = dados_costmap.get("row_scale_x_m", []) + dist = dados_costmap.get("row_dist_m", []) + dy = getattr(self, "dy", 0.02) base_sig = ( - C.shape, len(escx), len(dist), + C.shape, + tuple(np.round(np.asarray(dist, dtype=np.float32), 3)), + tuple(np.round(np.asarray(escx, dtype=np.float32), 4)), round(float(largura_equip), 3), round(float(L), 3), round(float(dy), 4), ) if getattr(self, "_lut_base_sig", None) != base_sig: - # reset do cache base if not hasattr(self, "_lut"): self._lut = {} - self._lut_pronto = False # <- IMPORTANTE - self._preparar_luts(dados_matriz_custo, largura_equip) + self._lut_pronto = False + self._preparar_luts(dados_costmap, largura_equip) self._lut_base_sig = base_sig - # invalida por-tipo self._lut_tipo = {} self._lut_tipo_sig = {} @@ -209,57 +507,55 @@ class ControladorMPC: ) self._lut_tipo_sig[tipo_key] = sig - def _preparar_luts(self, dados_matriz_custo, largura_equip): + def _preparar_luts(self, dados_costmap, largura_equip): if getattr(self, "_lut_pronto", False): return - # --- distâncias em ordem ASCENDENTE para mapear y -> índice físico --- - dist_asc = np.asarray(dados_matriz_custo["DistanciasRef"], dtype=np.float32) # [H] crescente - H = dist_asc.size - self.y_min = float(dist_asc.min()); self.y_max = float(dist_asc.max()) - self.dy = getattr(self, "dy", 0.02) # mantenha consistente com a assinatura + dist_asc = np.asarray(dados_costmap["row_dist_m"], dtype=np.float32) + escx = np.asarray(dados_costmap["row_scale_x_m"], dtype=np.float32) + C = np.asarray(dados_costmap["custo"], dtype=np.float32) + + H, W = C.shape + + self.y_min = float(dist_asc.min()) + self.y_max = float(dist_asc.max()) + self.dy = getattr(self, "dy", 0.02) - # tabela y->índice na MATRIZ (que está em ordem REVERTIDA) n = int(self.y_max / self.dy) + 2 lut_y = np.empty(n, dtype=np.int32) - j = 0 # ponteiro no vetor ascendente + j = 0 for k in range(n): y = k * self.dy while j + 1 < H and dist_asc[j + 1] <= y: j += 1 - ir = min(j, H - 1) # índice físico (ascendente) - idx = H - 1 - ir # índice na matriz (reversa) - lut_y[k] = idx - self._lut["y2idx"] = lut_y + lut_y[k] = min(j, H - 1) - # --- escalas/offsets por linha (na ordem da MATRIZ: reversa) --- - escx_rev = np.asarray(dados_matriz_custo["EscalasX"][::-1], dtype=np.float32) # [H] - W = int(dados_matriz_custo["Custo"].shape[1]) - - # footprint em colunas: garanta cobertura central mesmo para colunas pares offsets = np.empty((H, 2), dtype=np.int32) - for i, sx in enumerate(escx_rev): + for i, sx in enumerate(escx): + sx = max(float(sx), 1e-6) cols = int(np.ceil(largura_equip / sx)) - # centraliza footprint: para cols pares, deixa 1 a mais à direita + cols = max(1, cols) + halfL = cols // 2 halfR = cols - 1 - halfL offsets[i] = (-halfL, halfR) - self._lut["offsets"] = offsets - self._lut["escala_x"] = escx_rev + + self._lut["y2idx"] = lut_y + self._lut["offsets"] = offsets + self._lut["escala_x"] = escx self._lut["H"] = H self._lut["W"] = W self._lut["dist_asc"] = dist_asc - self._lut["dist_rev"] = dist_asc[::-1] - # NADA de construir LUT por tipo aqui — isso é função do _ensure_lut_geom_por_tipo self._lut_pronto = True def _build_lut_geom_por_tipo(self, dados, tipo, ang_min=-30.0, ang_max=30.0, passo=1.0, L=1.0, k_r=0.5, crab_in_phase=False, beta_crab_deg=4.0): - C = dados["Custo"]; W = int(C.shape[1]) - escx = np.asarray(dados["EscalasX"][::-1], dtype=np.float32) # [H] - dist = np.asarray(dados["DistanciasRef"][::-1], dtype=np.float32) # [H] - offs = self._lut["offsets"] # [H, 2] + C = dados["custo"] + W = int(C.shape[1]) + escx = np.asarray(dados["row_scale_x_m"], dtype=np.float32) + dist = np.asarray(dados["row_dist_m"], dtype=np.float32) + offs = self._lut["offsets"] H = dist.size ang_grid_deg = np.arange(ang_min, ang_max + 1e-6, passo, dtype=np.float32) # [K] @@ -343,7 +639,8 @@ class ControladorMPC: d_norm = (erro_pos / d_sat) return (d_norm, erro_pos) except Exception as e: - mostrar_log(f"Erro ao calcular erro de posicao: {e}") + _log(f"Erro ao calcular erro de posicao: {e}") + return 1.0, float("inf") def _calcula_erro_orientacao(self, x, y, theta, ponto_alvo, angulo_caminho, peso_proximo_ponto=0.5): try: @@ -356,7 +653,8 @@ class ControladorMPC: erro_ori_sim = np.degrees(erro_ori_sim) return (o_norm, erro_ori_sim) except Exception as e: - mostrar_log(f"Erro ao calcular erro de orientacao: {e}") + _log(f"Erro ao calcular erro de orientacao: {e}") + return 1.0, 180.0 # UTILS @@ -367,7 +665,8 @@ class ControladorMPC: return i return len(visitados) - 1 except Exception as e: - mostrar_log(f"Erro ao consultar proximo ponto nao visitado: {e}") + _log(f"Erro ao consultar proximo ponto nao visitado: {e}") + return 0 def _corrigir_pontos_visitados(self, x, y, pontos_visitados, idx_atual=-1, limite_max_avanco=5.0, limite_max_pontos: int = 10, velocidade: float = -1, dt: float = -1, look_ahead: bool = True): if (idx_atual > -1): @@ -464,7 +763,12 @@ class ControladorMPC: return (peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, custo_movimento) except Exception as e: - mostrar_log(f"Erro ao calcular pesos de movimento: {e}") + _log(f"Erro ao calcular pesos de movimento: {e}") + return (1.2, 2.2, 1.0, 0.05, 1.4, 0.0, { + TipoMovimentoDirecional.MovimentoArco: 0.0, + TipoMovimentoDirecional.RodasDianteiras: 0.0, + TipoMovimentoDirecional.MovimentoDiagonal: 2.0, + }) def corrigir_pose_por_latencia( self, @@ -540,7 +844,8 @@ class ControladorMPC: try: return (a + np.pi) % (2*np.pi) - np.pi except Exception as e: - mostrar_log(f"Erro no wrap_pi: {e}") + _log(f"Erro no wrap_pi: {e}") + return 0.0 def _circ_mean(self, a, b, wa=0.5): wb = 1.0 - wa @@ -633,11 +938,15 @@ class ControladorMPC: return comando except Exception as e: - mostrar_log(f"❌ Erro ao processar compute MPC: {e}") + _log(f"❌ Erro ao processar compute MPC: {e}") + return self._comando_fallback_hot_stop(comando_anterior or {}, motivos=[f"Erro ao processar compute MPC: {e}"], latencia=0.0) def _processar_mpc_receding(self, contexto, comando_anterior): + contexto = _as_dict(contexto) + comando_anterior = _as_dict(comando_anterior) now = time.perf_counter t0 = now() + pos_latencia = 0.0 # --- SLAs de execução --- SLA_MS = getattr(self, "_mpc_sla_ms", 200.0) # 200 ms ⇒ alvo 5 Hz @@ -784,6 +1093,8 @@ class ControladorMPC: #mostrar_log(f"idx_proximo_ponto_C#: {idx_proximo_ponto_real}, idx_ponto_alvo_C#: {idx_ponto_alvo}, idx_proximo_ponto_Python: {idx_alvo_correcao}") + dados_costmap_ciclo = self._get_costmap_direcional(contexto) + # Loop de K comandos (receding); cada comando simula dt_pred com v_sim #print("COMECANDO...") for passo in range(self.qtd_comandos_sucessivos): @@ -810,35 +1121,27 @@ class ControladorMPC: #if idx_alvo < len(self.pontos_info) - 1: idx_alvo += 1 ponto_alvo = self.pontos_info[idx_alvo] - tipos, angs, custos_candidatos = self._gerar_angulos_candidatos_receding( - ponto_alvo, (x_atual, y_atual, theta_atual), (x, y, theta), - contexto, deadline, 15.0 + pares_candidatos, custos_candidatos = self._gerar_angulos_candidatos_receding( + ponto_alvo, + (x_atual, y_atual, theta_atual), + (x, y, theta), + contexto, + deadline, + 15.0, + dados_costmap=dados_costmap_ciclo ) + if ms_left() <= 0 or exp_budget <= 0: break - # ---------- seleção leve (ordena por heurística) ---------- - ang_array = np.asarray(angs, dtype=np.float32) - graus_r = np.round(np.degrees(ang_array), 2) - from itertools import chain - - pairs_tipo, pairs_ang, pairs_heur = [], [], [] - for tipo in tipos: - tv = tipo.value - h_tmp = np.array( - [float(custos_candidatos.get((tv, float(g)), 0.0)) for g in graus_r], - dtype=np.float32 - ) - pairs_tipo.append([tipo] * ang_array.size) - pairs_ang.append(ang_array) - pairs_heur.append(h_tmp) - - if not pairs_tipo: + if not pares_candidatos: continue - tipos_flat = list(chain.from_iterable(pairs_tipo)) - angs_flat = np.concatenate(pairs_ang, axis=0) - heur_flat = np.concatenate(pairs_heur, axis=0) + # ---------- seleção leve: agora ordena pares reais já validados ---------- + heur_flat = np.asarray( + [float(p.get("custo_heuristico", 0.0)) for p in pares_candidatos], + dtype=np.float32 + ) if heur_flat.size == 0: continue @@ -870,9 +1173,11 @@ class ControladorMPC: break i = int(ord_idx[k]) - tipo_k = tipos_flat[i] - ang_k = float(angs_flat[i]) - heur_k = float(heur_flat[i]) + par_k = pares_candidatos[i] + + tipo_k = par_k["tipo"] + ang_k = float(par_k["angulo"]) + heur_k = float(par_k.get("custo_heuristico", 0.0)) t_c_ini = now() try: @@ -949,7 +1254,9 @@ class ControladorMPC: else: idx_cmd = 0 if len(_comandos) == 1 else 1 tipo_final, angulo_final = _comandos[idx_cmd] - debug_custo[f"{tipo_final.value}_{np.degrees(angulo_final):.2f}"]["melhor"] = True + key_debug = f"{tipo_final.value}_{np.degrees(angulo_final):.2f}" + if key_debug in debug_custo: + debug_custo[key_debug]["melhor"] = True simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(melhor["trajetoria"]) or []) simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta)) else: @@ -978,20 +1285,32 @@ class ControladorMPC: # -------------------- Debug opcional da matriz de custo -------------------- self._tick_id = getattr(self, "_tick_id", 0) + 1 - if (getattr(self, "debug_cost_vis", False) and contexto.get("VisualWorker", {}).get("MatrizCusto", {}).get("EmUso", False) and (self._tick_id % 10 == 0)): - dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {}) + dados_costmap_dbg = dados_costmap_ciclo + if ( + getattr(self, "debug_cost_vis", False) + and dados_costmap_dbg.get("em_uso", False) + and (self._tick_id % 10 == 0) + ): deadline_dbg = time.perf_counter() + 0.03 - tipos_dbg, angs_dbg, _ = self._gerar_angulos_candidatos_receding( + pares_dbg, _ = self._gerar_angulos_candidatos_receding( ponto_alvo=self.pontos_info[idx_alvo_correcao], ponto_atual=(x, y, theta), ponto_ref=(x, y, theta), contexto=contexto, deadline=deadline_dbg, margem_ms=8.0, + dados_costmap=dados_costmap_dbg ) - angs_dbg_deg = [round(float(np.degrees(a)), 2) for a in angs_dbg] + + tipos_dbg = [] + angs_dbg_deg = [] + + for p in pares_dbg: + if p["tipo"] not in tipos_dbg: + tipos_dbg.append(p["tipo"]) + angs_dbg_deg.append(round(float(p["angulo_deg"]), 2)) self.debug_plot_matriz_custo( - dados_matriz_custo, + dados_costmap_dbg, tipos_dbg, angs_dbg_deg, max_linhas=3, @@ -1015,23 +1334,26 @@ class ControladorMPC: return hot_stop def _comando_fallback_hot_stop(self, comando_anterior, motivos, latencia=0): - _cmd = { - "enviar_comando": True, - "parada_necessaria": True, - "erro": True, - "latencia": latencia, - "angulo": comando_anterior.get("angulo", 0), - "tipo": comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value), + comando_anterior = _as_dict(comando_anterior) + tipo_seguro = _movimento_from_value( + comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value) + ).value + return { + "enviar_comando": True, + "parada_necessaria": True, + "erro": True, + "latencia": _safe_float(latencia, 0.0, min_value=0.0), + "angulo": _safe_float(comando_anterior.get("angulo", 0.0), 0.0), + "tipo": tipo_seguro, "simulacao": [], - "erro_lateral": 0, - "erro_orientacao": 0, + "erro_lateral": 0.0, + "erro_orientacao": 0.0, "debug_custo": {}, "candidatos_testados": 0, - "motivos": motivos + "motivos": _safe_motivos(motivos), } - return _cmd - def _gerar_angulos_candidatos_receding(self, ponto_alvo, ponto_atual, ponto_ref, contexto, deadline, margem_ms): + def _gerar_angulos_candidatos_receding(self, ponto_alvo, ponto_atual, ponto_ref, contexto, deadline, margem_ms, dados_costmap=None): """ Gera candidatos de (tipo, ângulo) respeitando o deadline do ciclo: 1) Ângulo ideal @@ -1040,7 +1362,17 @@ class ControladorMPC: 4) Ranking rápido (custo prefixado + distância ao ideal) e corte por orçamento 5) Filtro pesado/ranqueamento com matriz (usa 'permitidos' p/ pular combos barrados) 6) Uniformização em torno do ideal - Retorna: (tipos_final, angulos_final(rad), custos_candidatos{(tipo.value,graus_2c)->custo}) + Retorna: + pares_final: lista de candidatos já validados: + { + "tipo": TipoMovimentoDirecional, + "angulo": float rad, + "angulo_deg": float, + "custo_heuristico": float + } + + custos_candidatos: + dict[(tipo.value, angulo_deg)] = custo """ try: now = time.perf_counter @@ -1073,7 +1405,7 @@ class ControladorMPC: # 2) Tipos válidos conforme contexto CARRO = contexto.get("Carro", {}) status = CARRO.get("Status", 0) - if status in [StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando]: + if _status_in(status, [StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando]): tipos_validos = [TipoMovimentoDirecional.MovimentoArco] elif abs(e_ori) > 15.0: tipos_validos = [TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.MovimentoArco] @@ -1083,10 +1415,9 @@ class ControladorMPC: tipos_validos = [TipoMovimentoDirecional.RodasDianteiras] # 3) Flags de matriz - usar_dados_sonar = contexto.get("VisualWorker") is not None - dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {}) if usar_dados_sonar else {} - matriz_custo_valida = bool(dados_matriz_custo.get("Valida", False)) - filtrar_matriz = usar_dados_sonar and matriz_custo_valida + if dados_costmap is None: + dados_costmap = self._get_costmap_direcional(contexto) + filtrar_matriz = bool(dados_costmap.get("valido", False) and dados_costmap.get("em_uso", False)) # 4) Geração bruta (graus) angulos_raw = self._gerar_candidatos_brutos(angulo_ideal_deg, filtrar_matriz) @@ -1100,7 +1431,7 @@ class ControladorMPC: if filtrar_matriz and (deadline is None or (deadline - now()) * 1000.0 > margem_ms): largura = float(contexto.get("Equipamento", {}).get("largura", 0.85)) L = float(contexto.get("Equipamento", {}).get("entre_eixos", 0.94)) - self._ensure_luts(dados_matriz_custo, largura, L) + self._ensure_luts(dados_costmap, largura, L) ang_min = -float(self.angulo_max_graus) ang_max = +float(self.angulo_max_graus) @@ -1109,18 +1440,18 @@ class ControladorMPC: for t in tipos_validos: if t == TipoMovimentoDirecional.MovimentoArco: self._ensure_lut_geom_por_tipo( - dados_matriz_custo, largura, t, L=L, + dados_costmap, largura, t, L=L, ang_min=ang_min, ang_max=ang_max, passo=passo, k_r=0.5, crab_in_phase=False ) elif t == TipoMovimentoDirecional.MovimentoDiagonal: self._ensure_lut_geom_por_tipo( - dados_matriz_custo, largura, t, L=L, + dados_costmap, largura, t, L=L, ang_min=ang_min, ang_max=ang_max, passo=passo, crab_in_phase=True) # beta será por-ângulo else: self._ensure_lut_geom_por_tipo( - dados_matriz_custo, largura, t, L=L, + dados_costmap, largura, t, L=L, ang_min=ang_min, ang_max=ang_max, passo=passo ) @@ -1128,7 +1459,7 @@ class ControladorMPC: ang_all = list(angulos_raw) # snapshot antes do filtro masks = [] for t in tipos_validos: - mask_t, _ = self._quick_gate_by_lut_tipo(angulos_raw, dados_matriz_custo, t, max_linhas=3) + mask_t, _ = self._quick_gate_by_lut_tipo(angulos_raw, dados_costmap, t, max_linhas=3) masks.append(mask_t) if masks: mask_any = np.logical_or.reduce(masks) @@ -1154,7 +1485,7 @@ class ControladorMPC: # Score(grau) = λ * |g - ideal| + (1-λ) * custo_quick (média nas últimas linhas, melhor tipo) if filtrar_matriz and angulos_raw: try: - C = np.asarray(dados_matriz_custo.get("Custo"), dtype=np.float32) + C = np.asarray(dados_costmap.get("custo"), dtype=np.float32) if C is not None and C.size > 0: pref_cost = np.cumsum(C, axis=1) H, W = C.shape @@ -1204,29 +1535,33 @@ class ControladorMPC: mostrar_log(f"⚠️ Ranking rápido falhou: {e}") # 5) Filtro pesado / matriz (usa 'permitidos' para pular combos bloqueados) - tipos_final, angulos_final, custos_candidatos = self._filtrar_candidatos_validos_por_matriz( + pares_final, custos_candidatos = self._filtrar_candidatos_validos_por_matriz( filtrar_matriz=filtrar_matriz, tipos=tipos_validos, - angulos_deg=angulos_raw, # graus + angulos_deg=angulos_raw, angulo_ideal_rad=angulo_ideal_rad, p_atual=ponto_atual, p_ref=ponto_ref, contexto=contexto, + dados_costmap=dados_costmap, deadline=deadline, margem_ms=float(margem_ms), - permitidos=permitidos, # <- NOVO (pode ser None) + permitidos=permitidos, e_lat=e_lat, e_ori=e_ori ) - # 6) Uniformização ao redor do ideal (rad) com N limite - angulos_final = self._uniformizar_angulos_candidatos(angulos_final, angulo_ideal_rad, N=10) - - return tipos_final, angulos_final, custos_candidatos + pares_final = self._uniformizar_pares_candidatos( + pares_final, + angulo_ideal_rad, + N=10 + ) + + return pares_final, custos_candidatos except Exception as e: mostrar_log(f"❌ Erro ao gerar candidatos receding com contexto visual: {e}") - return [], [], {} + return [], {} def _gerar_candidatos_brutos(self, angulo_ideal_deg, filtrar_matriz_custo): try: @@ -1286,6 +1621,7 @@ class ControladorMPC: p_atual, p_ref, contexto, + dados_costmap, deadline: float | None = None, margem_ms: float = 20.0, permitidos=None, @@ -1320,32 +1656,46 @@ class ControladorMPC: peso_angulo_ideal = float(ContextoGlobalRedis.get_pesos_mpc().get("ideal", 1.4)) # ---- dados da matriz / ensure LUTs (idempotente) - dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {}) if filtrar_matriz else {} - custo_grid = dados_matriz_custo.get("Custo", None) + custo_grid = dados_costmap.get("custo", None) if filtrar_matriz else None # conversões básicas angulos_deg = [float(a) for a in angulos_deg] angulos_rad = np.deg2rad(np.asarray(angulos_deg, dtype=np.float32)) graus_2c = np.round(np.asarray(angulos_deg, dtype=np.float32), 2) + pares_validos: list = [] + custos_candidatos: dict = {} + # fallback sem matriz ou sem tempo if (not filtrar_matriz) or (custo_grid is None) or (np.size(custo_grid) == 0) or (ms_left() <= 0.0): custos_candidatos = {} erro = ang_err(angulos_rad, float(angulo_ideal_rad)) / np.pi custo_ideal = peso_angulo_ideal * erro for tipo in tipos: - for g2, c in zip(graus_2c, custo_ideal): - # respeita 'permitidos' se existir - if permitidos is not None and (tipo.value, float(g2)) not in permitidos: + for ang_rad, g2, c in zip(angulos_rad, graus_2c, custo_ideal): + g2 = float(g2) + chave = (tipo.value, g2) + + if permitidos is not None and chave not in permitidos: continue - custos_candidatos[(tipo.value, float(g2))] = float(c) - # retorna o conjunto original (em rad) - return tipos, angulos_rad.astype(float).tolist(), custos_candidatos + + custo = float(c) + custos_candidatos[chave] = custo + + pares_validos.append({ + "tipo": tipo, + "angulo": float(ang_rad), + "angulo_deg": g2, + "custo_heuristico": custo, + "origem": "fallback_sem_matriz" + }) + + return pares_validos, custos_candidatos # garante LUTs base (idempotente); algumas avaliações usam offsets/escala largura = float(contexto.get("Equipamento", {}).get("largura", 0.85)) L = float(contexto.get("Equipamento", {}).get("entre_eixos", 0.94)) - self._ensure_luts(dados_matriz_custo, largura, L) + self._ensure_luts(dados_costmap, largura, L) # ---- distância de simulação curta (penúltima linha, min 0.6 m) velocidade = float(contexto.get("Carro", {}).get("Velocidade", 1.0)) @@ -1384,9 +1734,6 @@ class ControladorMPC: escala_sim *= 0.6 distancia_sim_m = max(0.6, distancia_sim_m_base * escala_sim) - tipos_validos: list = [] - angulos_validos: list = [] - custos_candidatos: dict = {} melhor_custo_local = float("inf") # ---- laço por tipo com orçamento próprio @@ -1434,7 +1781,7 @@ class ControladorMPC: t_c_ini = now() try: traj = self._simular_trajetoria_curta(p_atual, tipo, ang_rad, velocidade, distancia_sim_m) - custo, valido = self._avaliar_trajetoria_matriz_custo(tipo, ang_rad, traj, p_ref, dados_matriz_custo) + custo, valido = self._avaliar_trajetoria_matriz_custo(tipo, ang_rad, traj, p_ref, dados_costmap) #print(f"avaliando {tipo} {np.degrees(ang_rad):.2f} - custo: {custo}, valido: {valido}") if valido: custo_total = float(custo) + float(c_ideal) @@ -1443,9 +1790,16 @@ class ControladorMPC: if (custo_total > melhor_custo_local * 1.20) and (ms_left() < 2 * self._t_est_candidato_ms): pass else: - tipos_validos.append(tipo) - angulos_validos.append(ang_rad) - custos_candidatos[(tipo.value, g2)] = custo_total + chave = (tipo.value, g2) + custos_candidatos[chave] = custo_total + + pares_validos.append({ + "tipo": tipo, + "angulo": float(ang_rad), + "angulo_deg": float(g2), + "custo_heuristico": float(custo_total), + "origem": "matriz" + }) if custo_total < melhor_custo_local: melhor_custo_local = custo_total except Exception as e: @@ -1457,24 +1811,34 @@ class ControladorMPC: self._t_est_candidato_ms = (1.0 - alpha) * self._t_est_candidato_ms + alpha * dt_ms # ---- fail-safe: se nada válido, devolve top heurístico (sem matriz) - if not tipos_validos: - #print("sem tipos validos") + if not pares_validos: custos_candidatos = {} + fallback_idx = ordem_global[:max(3, Kdyn_total)] + for tipo in tipos: - for idx in ordem_global[:max(3, Kdyn_total)]: + for idx in fallback_idx: g2 = float(graus_2c[idx]) - if permitidos is not None and (tipo.value, g2) not in permitidos: + chave = (tipo.value, g2) + + if permitidos is not None and chave not in permitidos: continue - custos_candidatos[(tipo.value, g2)] = float(custo_ideal[idx]) - # devolve ângulos (rad) correspondentes - ang_out = [float(angulos_rad[idx]) for idx in ordem_global[:max(3, Kdyn_total)]] - return tipos, ang_out, custos_candidatos - #print(tipos_validos, angulos_validos, custos_candidatos) - return tipos_validos, angulos_validos, custos_candidatos + + custo = float(custo_ideal[idx]) + custos_candidatos[chave] = custo + + pares_validos.append({ + "tipo": tipo, + "angulo": float(angulos_rad[idx]), + "angulo_deg": g2, + "custo_heuristico": custo, + "origem": "fallback_heuristico" + }) + + return pares_validos, custos_candidatos except Exception as e: mostrar_log(f"❌ Erro ao filtrar candidatos por matriz: {e}") - return [], [], {} + return [], {} def _simular_trajetoria_curta(self, p_atual, tipo, angulo, velocidade, dist_min): """ @@ -1521,10 +1885,11 @@ class ControladorMPC: mostrar_log(f"❌ Erro na simulação curta: {e}") return [] - def _avaliar_trajetoria_matriz_custo(self, tipo, ang, trajetoria, p_ref, dados_matriz_custo): + def _avaliar_trajetoria_matriz_custo(self, tipo, ang, trajetoria, p_ref, dados_costmap): try: - matriz_custo = dados_matriz_custo.get("Custo", None) - matriz_nav = dados_matriz_custo.get("Navegavel", None) + matriz_custo = dados_costmap.get("custo", None) + matriz_nav = dados_costmap.get("navegavel", None) + politica = dados_costmap.get("politica", {}) if matriz_custo is None or matriz_nav is None or matriz_custo.size == 0 or not trajetoria: return float("inf"), False @@ -1535,7 +1900,12 @@ class ControladorMPC: H, W = matriz_custo.shape # --- prefixos por frame (navegável e custo) --- - blocked = (~matriz_nav).astype(np.int32) # 1 se bloqueado + usar_bloqueio_duro = bool(politica.get("usar_bloqueio_duro", True)) + if usar_bloqueio_duro: + blocked = (~matriz_nav).astype(np.int32) + else: + blocked = np.zeros_like(matriz_nav, dtype=np.int32) + pref_blk = np.cumsum(blocked, axis=1) # [H,W] pref_cst = np.cumsum(matriz_custo.astype(np.float32), axis=1) # [H,W] @@ -1557,10 +1927,10 @@ class ControladorMPC: # --- ignora pontos muito próximos (ex.: < 0.6 m) --- # usa dist mínima do LUT (asc) se existir, senão 0.6 - y0 = 0.6 + y0 = float(politica.get("distancia_min_avaliacao_m", 0.60)) dist_asc = self._lut.get("dist_asc", None) if dist_asc is not None and len(dist_asc) > 0: - y0 = max(0.6, float(dist_asc[0])) + y0 = max(y0, float(dist_asc[0])) mask_far = y_rel >= y0 if not np.any(mask_far): @@ -1571,7 +1941,7 @@ class ControladorMPC: # --- mapeia y_rel -> idx de linha via LUT --- ky = np.clip((y_rel / self.dy).astype(np.int32), 0, lut_y.size - 1) - idx = lut_y[ky] # [M] índices de linha na ordem da matriz (reversa) + idx = lut_y[ky] # [M] índices de linha alinhados com row_dist_m crescente # --- coluna central por ponto --- sx = escx[idx] # [M] escala por linha @@ -1613,13 +1983,17 @@ class ControladorMPC: #invalido = bool(np.any(blocked_any)) frac_bloq = float(np.mean(blocked_any.astype(np.float32))) - if frac_bloq >= 0.45: + limiar_hot_stop = float(politica.get("limiar_hot_stop_frac", 0.45)) + limiar_bloqueio = float(politica.get("limiar_bloqueio_frac", 0.20)) + peso_visual = float(politica.get("peso_visual", 1.0)) + + if frac_bloq >= limiar_hot_stop: return float("inf"), False - invalido = frac_bloq > 0.20 + invalido = frac_bloq > limiar_bloqueio penal_bloq = 0.5 * frac_bloq - return custo_medio + penal_bloq, (not invalido) + return (custo_medio + penal_bloq) * peso_visual, (not invalido) except Exception as e: mostrar_log(f"❌ Erro ao avaliar trajetoria na matriz: {e}") @@ -1832,15 +2206,41 @@ class ControladorMPC: x_sim, y_sim, theta_sim = x, y, theta self.x_ant, self.y_ant = 0.0, 0.0 - # chave para custo heurístico vindo do VisualWorker (por ângulo/tipo) - chave = (tipo.value, round(float(np.degrees(angulo_testado)), 2)) - custo_visual_worker = float(custos_candidatos.get(chave, 0.0)) - simulacoes = [] prev_ang = float(comando_anterior.get("angulo", 0.0)) prev_tipo = comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value) pontos_visitados = visitados.copy() + # chave para custo heurístico vindo do VisualWorker (por ângulo/tipo) + grau_chave = round(float(np.degrees(angulo_testado)), 2) + chave = (tipo.value, grau_chave) + custo_visual_worker = 0.0 + + if custos_candidatos: + if chave in custos_candidatos: + custo_visual_worker = float(custos_candidatos[chave]) + else: + melhor_chave = None + menor_delta = float("inf") + + for (tv, g), c in custos_candidatos.items(): + if tv != tipo.value: + continue + + try: + delta_g = abs(float(g) - grau_chave) + except Exception: + continue + + if delta_g < menor_delta: + menor_delta = delta_g + melhor_chave = (tv, g) + + if melhor_chave is not None and menor_delta <= 0.05: + custo_visual_worker = float(custos_candidatos.get(melhor_chave, 0.0)) + else: + custo_visual_worker = 0.0 + # omega e sub-stepping omega_const = self.gps_handler.calcular_omega(v_planejado, angulo_testado, tipo) @@ -2067,95 +2467,7 @@ class ControladorMPC: return x_sim, y_sim, theta_sim - def _prefiltrar_angulos_por_matriz_quick( - self, - angulos_deg, # iterable de graus - p_atual, # (x, y, theta) do candidato atual - p_ref, # (x,y,theta) referência (pode não usar aqui) - dados_matriz_custo, # dict com Custo/Navegavel/EscalasX/DistanciasRef - largura_equip, # metros - max_linhas=3, # quantas linhas à frente checar (as mais distantes) - deadline=None, margem_ms=5.0 - ): - """ - Retorna um np.ndarray(bool) mask do mesmo tamanho de angulos_deg indicando - quais ângulos passam no gate rápido da matriz (todas as linhas checadas navegáveis). - """ - # Se não tem matriz, tudo liberado - custo = dados_matriz_custo.get("Custo", None) - nav = dados_matriz_custo.get("Navegavel", None) - if custo is None or nav is None or nav.size == 0: - a = np.asarray(angulos_deg, dtype=np.float32) - return np.ones(a.shape, dtype=bool) - - now = time.perf_counter - def time_ok(): - return True if deadline is None else (deadline - now())*1000.0 > margem_ms - - # LUTs - lut_y = self._lut["y2idx"] - offs = self._lut["offsets"] # shape: [H, 2] - escx = self._lut["escala_x"] # shape: [H] - H, W = nav.shape - - # linhas a checar: pegue as últimas (mais “pra frente”) - dist_ref = np.asarray(dados_matriz_custo.get("DistanciasRef", []), dtype=np.float32) - if dist_ref.size == 0: - a = np.asarray(angulos_deg, dtype=np.float32) - return np.ones(a.shape, dtype=bool) - - # usamos as linhas com maiores distâncias (últimas do array, assumindo crescente) - linhas = np.arange(max(0, dist_ref.size - max_linhas), dist_ref.size, dtype=int) - - # converte angulos pra rad e calcula tan (uma vez) - a_deg = np.asarray(angulos_deg, dtype=np.float32) - a_rad = np.deg2rad(a_deg) - tan_a = np.tan(a_rad) - - # posição atual do robô no referencial do robô é (0,0,theta=0). - # A projeção do centro à distância y: x ≈ y * tan(a) - # Para cada linha selecionada, fazemos o gate de footprint. - mask = np.ones(a_deg.shape, dtype=bool) - - for li in linhas: - if not time_ok(): - break - - # distância y desta linha (em metros) - y = float(dist_ref[li]) - if y <= 0.0: - continue - - # coluna central para cada ângulo - # j_c = int((x_rel + larg_total/2) / escala_x), onde x_rel ≈ y * tan(a) - x_rel = y * tan_a - idx = int(lut_y[min(int(y / getattr(self, "dy", 0.02)), lut_y.size - 1)]) # fallback por segurança - sx = float(escx[idx]) - larg_total = sx * W - j_c = np.floor((x_rel + larg_total * 0.5) / sx).astype(np.int32) - - # fatia footprint na coluna - offL, offR = offs[idx] - j0 = np.clip(j_c + offL, 0, W - 1) - j1 = np.clip(j_c + offR, 0, W - 1) - - # checa navegabilidade por ângulo (tudo True na fatia) - # (fazemos com loop leve sobre ângulos para evitar broadcasting 2D grande; A << W) - ok = [] - row_nav = nav[idx] # 1D view - for c0, c1 in zip(j0, j1): - if c0 > c1: - c0, c1 = c1, c0 - ok.append(bool(row_nav[c0:c1+1].all())) - mask &= np.array(ok, dtype=bool) - - # early-exit: se já tudo falso, pode sair - if not mask.any(): - break - - return mask - - def _quick_gate_by_lut_tipo(self, angulos_deg, dados_matriz_custo, tipo, max_linhas=3): + def _quick_gate_by_lut_tipo(self, angulos_deg, dados_costmap, tipo, max_linhas=3): """ Gate rápido por LUT: - retorna mask (K,) indicando se o ângulo passa em TODAS as 'max_linhas' finais, @@ -2169,8 +2481,8 @@ class ControladorMPC: K = len(angulos_deg) return np.ones(K, dtype=bool), np.full(K, np.inf, dtype=np.float32) - C = dados_matriz_custo.get("Custo", None) - NAV = dados_matriz_custo.get("Navegavel", None) + C = dados_costmap.get("custo", None) + NAV = dados_costmap.get("navegavel", None) if C is None or NAV is None or C.size == 0: K = len(angulos_deg) return np.ones(K, dtype=bool), np.full(K, np.inf, dtype=np.float32) @@ -2254,13 +2566,13 @@ class ControladorMPC: return float(dist_asc[idx]) - def _dbg_cost_rows_and_cols(self, dados_matriz_custo, tipo, angulos_deg, max_linhas=3): + def _dbg_cost_rows_and_cols(self, dados_costmap, tipo, angulos_deg, max_linhas=3): """ Retorna (rows, j0, j1) para desenhar footprints por ângulo. rows: (R,) linhas escolhidas (últimas R) j0/j1: (R,K) colunas esquerda/direita por (linha, ângulo) """ - C = dados_matriz_custo.get("Custo") + C = dados_costmap.get("custo") if C is None or C.size == 0: return None, None, None H, W = C.shape @@ -2275,7 +2587,7 @@ class ControladorMPC: return rows, j0, j1 def debug_plot_matriz_custo( - self, dados_matriz_custo, tipos, angulos_deg, *, + self, dados_costmap, tipos, angulos_deg, *, max_linhas=3, mostra_nav=True, titulo="debug_cost.png", highlight=None ): """ @@ -2286,7 +2598,8 @@ class ControladorMPC: """ try: import matplotlib.pyplot as plt - C = dados_matriz_custo.get("Custo"); NAV = dados_matriz_custo.get("Navegavel") + C = dados_costmap.get("custo") + NAV = dados_costmap.get("navegavel") if C is None or C.size == 0: return C = np.asarray(C, dtype=np.float32) @@ -2331,7 +2644,7 @@ class ControladorMPC: # distância aproximada dessa linha (se disponível) dtxt = "" if dist_asc is not None and len(dist_asc) == H: - # rows são os últimos índices; em nosso LUT, índice maior = mais perto + # rows são os últimos índices; com row_dist_m crescente, índice maior = mais longe dtxt = f" (≈ {dist_asc[r]:.2f} m)" ax.set_title(f"linha {int(r)}{dtxt}") @@ -2383,3 +2696,73 @@ class ControladorMPC: except Exception as e: mostrar_log(f"[DEBUG] falha no debug_plot_matriz_custo: {e}") + + def _uniformizar_pares_candidatos(self, pares_validos, angulo_ideal_rad, N=10): + """ + Reduz a lista de candidatos preservando o par (tipo, ângulo). + Nunca separa tipo de ângulo, evitando recombinação inválida depois. + """ + try: + if not pares_validos: + return [] + + def _ang_dist(par): + return abs(self._wrap_pi(float(par["angulo"]) - float(angulo_ideal_rad))) + + # Remove duplicados exatos por (tipo, grau) + best_by_key = {} + for par in pares_validos: + tipo = par.get("tipo") + ang_deg = round(float(par.get("angulo_deg", np.degrees(par.get("angulo", 0.0)))), 2) + key = (tipo.value if hasattr(tipo, "value") else tipo, ang_deg) + + custo = float(par.get("custo_heuristico", float("inf"))) + atual = best_by_key.get(key) + if atual is None or custo < float(atual.get("custo_heuristico", float("inf"))): + par["angulo_deg"] = ang_deg + best_by_key[key] = par + + pares = list(best_by_key.values()) + + if len(pares) <= N: + return sorted(pares, key=lambda p: (float(p.get("custo_heuristico", 0.0)), _ang_dist(p))) + + # Sempre preserva o mais próximo do ideal + pares_ord_ideal = sorted(pares, key=_ang_dist) + selecionados = [pares_ord_ideal[0]] + + # Depois prioriza menor custo heurístico, mantendo diversidade angular por tipo + pares_ord_custo = sorted( + pares, + key=lambda p: ( + float(p.get("custo_heuristico", 0.0)), + _ang_dist(p) + ) + ) + + margem_graus = 1.0 + + for par in pares_ord_custo: + if len(selecionados) >= N: + break + + tipo = par["tipo"] + ang_deg = float(par["angulo_deg"]) + + ja_existe = False + for sel in selecionados: + mesmo_tipo = sel["tipo"] == tipo + muito_perto = abs(float(sel["angulo_deg"]) - ang_deg) < margem_graus + if mesmo_tipo and muito_perto: + ja_existe = True + break + + if not ja_existe: + selecionados.append(par) + + return selecionados[:N] + + except Exception as e: + mostrar_log(f"❌ Erro ao uniformizar pares candidatos: {e}") + return pares_validos[:N] + diff --git a/Python/OAK/datasets/oak-fcc-3/calibration/module_params.json b/Python/OAK/datasets/oak-fcc-3/calibration/module_params.json index 1024c8bfd..9a40746ff 100644 --- a/Python/OAK/datasets/oak-fcc-3/calibration/module_params.json +++ b/Python/OAK/datasets/oak-fcc-3/calibration/module_params.json @@ -9,8 +9,22 @@ "sensor_height": 800, "bayer_pattern": "BGGR", "rgb_processing": { - "mode": "linear_demosaic", - "demosaic_algorithm": "bilinear" + "mode": "bayer_planes", + "demosaic_algorithm": "ea", + "enhancement": { + "enabled": true, + "backend": "hybrid", + "preset": "soft", + "apply_to_preview_input": false, + "use_u8_pipeline": true, + "auto_stretch": { + "enabled": false, + "low_pct": 0.2, + "high_pct": 99.8 + }, + "clip_output": true, + "save_debug": false + } }, "camera_settings": { "rgb": { @@ -239,7 +253,7 @@ }, "crop_valid_common": true, "resize_after_crop": true, - "target_size": [1024,640] + "target_size": [640, 400] }, "radiometric_config": { "enabled": false, diff --git a/Python/OAK/datasets/oak-fcc-3/core/raw_processor_core.py b/Python/OAK/datasets/oak-fcc-3/core/raw_processor_core.py index 24cceafb5..5f3367a79 100644 --- a/Python/OAK/datasets/oak-fcc-3/core/raw_processor_core.py +++ b/Python/OAK/datasets/oak-fcc-3/core/raw_processor_core.py @@ -237,6 +237,40 @@ class RawProcessorCore: self.rgb_processing_config = { "mode": "linear_demosaic", # "linear_demosaic", "linear_demosaic_half" ou "bayer_planes" "demosaic_algorithm": "ea", # "ea" ou "bilinear" + + # Pós-processamento RGB opcional, aplicado logo após o decode/debayer + # e antes da fusão com RE/NIR. + # + # backend aceitos: + # "none" / None / false + # "hybrid" / "hybrid:balanced" / "hybrid_balanced" + # "hybrid_soft", "hybrid_strong" + # + # Observação: + # - Por padrão fica desligado para manter compatibilidade total. + # - Para o experimento atual, use backend="hybrid" e preset="balanced". + "enhancement": { + "enabled": False, + "backend": "none", + "preset": "balanced", # "soft", "balanced", "strong" + "apply_to_preview_input": False, + + # Mantém o pipeline parecido com o script de preview/regens: + # float01 -> uint8 -> OpenCV -> float01. + "use_u8_pipeline": True, + + # Desligado por padrão para preservar escala radiométrica. + # Se quiser reproduzir exatamente o visual do script de previews, + # pode ligar este auto_stretch. + "auto_stretch": { + "enabled": False, + "low_pct": 0.2, + "high_pct": 99.8 + }, + + "clip_output": True, + "save_debug": True + } } self.fusion_config = { @@ -389,6 +423,7 @@ class RawProcessorCore: self.last_decode_perf = {} self._last_decode_perf_log_ts = 0.0 + self.last_rgb_enhancement_result = None if calibration_json_path: self.load_config_json(calibration_json_path) @@ -553,6 +588,301 @@ class RawProcessorCore: return rgb[:, :, 0], rgb[:, :, 1], rgb[:, :, 2] + + # ============================================================ + # RGB ENHANCEMENT / REGEN BACKENDS + # ============================================================ + + def _get_rgb_enhancement_config(self) -> dict: + """ + Resolve a configuração de pós-processamento RGB dentro de rgb_processing. + + Contrato recomendado no module_params.json: + + "rgb_processing": { + "mode": "bayer_planes", + "demosaic_algorithm": "ea", + "enhancement": { + "enabled": true, + "backend": "hybrid", + "preset": "balanced", + "use_u8_pipeline": true, + "auto_stretch": { + "enabled": false, + "low_pct": 0.2, + "high_pct": 99.8 + }, + "clip_output": true + } + } + + Compatibilidade: + - também aceita rgb_processing.enhancement_backend = "hybrid:balanced" + - também aceita rgb_processing.enhancement_preset = "balanced" + """ + rgb_cfg = getattr(self, "rgb_processing_config", {}) or {} + enh = rgb_cfg.get("enhancement", {}) or {} + + if not isinstance(enh, dict): + enh = {} + + # Atalhos opcionais no nível de rgb_processing. + if "enhancement_backend" in rgb_cfg and "backend" not in enh: + enh["backend"] = rgb_cfg.get("enhancement_backend") + if "enhancement_preset" in rgb_cfg and "preset" not in enh: + enh["preset"] = rgb_cfg.get("enhancement_preset") + if "enhancement_enabled" in rgb_cfg and "enabled" not in enh: + enh["enabled"] = bool(rgb_cfg.get("enhancement_enabled")) + + backend = enh.get("backend", "none") + backend_norm, preset_norm = self._resolve_rgb_enhancement_backend_and_preset( + backend=backend, + preset=enh.get("preset", "balanced"), + ) + + enabled = bool(enh.get("enabled", False)) + if backend_norm in ("none", "", "off", "disabled"): + enabled = False + + auto_stretch = enh.get("auto_stretch", {}) or {} + if not isinstance(auto_stretch, dict): + auto_stretch = {} + + return { + "enabled": enabled, + "backend": backend_norm, + "preset": preset_norm, + "apply_to_preview_input": bool(enh.get("apply_to_preview_input", False)), + "use_u8_pipeline": bool(enh.get("use_u8_pipeline", True)), + "auto_stretch": { + "enabled": bool(auto_stretch.get("enabled", False)), + "low_pct": float(auto_stretch.get("low_pct", 0.2)), + "high_pct": float(auto_stretch.get("high_pct", 99.8)), + }, + "clip_output": bool(enh.get("clip_output", True)), + "save_debug": bool(enh.get("save_debug", True)), + } + + def _resolve_rgb_enhancement_backend_and_preset(self, backend, preset="balanced") -> tuple[str, str]: + """ + Aceita strings amigáveis: + none + hybrid + hybrid:balanced + hybrid_balanced + hybrid-soft + """ + if backend is None or backend is False: + return "none", "balanced" + + b = str(backend).strip().lower() + p = str(preset or "balanced").strip().lower() + + if b in ("", "none", "off", "false", "disabled", "raw"): + return "none", "balanced" + + # hybrid:balanced + if ":" in b: + parts = [x.strip() for x in b.split(":") if x.strip()] + if len(parts) >= 1: + b = parts[0] + if len(parts) >= 2: + p = parts[1] + + # hybrid_balanced / hybrid-balanced + for sep in ("_", "-"): + if b.startswith(f"hybrid{sep}"): + p = b.split(sep, 1)[1] + b = "hybrid" + + if p not in ("soft", "balanced", "strong"): + p = "balanced" + + if b not in ("hybrid",): + raise ValueError( + f"rgb_processing.enhancement.backend inválido: {backend}. " + "Use 'none' ou 'hybrid'." + ) + + return b, p + + def _rgb_float01_to_u8_for_enhancement(self, rgb: np.ndarray, cfg: dict) -> np.ndarray: + """ + Converte RGB float01 para uint8. + Opcionalmente aplica auto_stretch por percentil para reproduzir melhor + o visual dos scripts de preview/regens. + + Por padrão auto_stretch fica desligado, porque preservar a escala do tensor + tende a ser mais seguro para treino/inferência. + """ + x = np.asarray(rgb, dtype=np.float32) + + auto = (cfg or {}).get("auto_stretch", {}) or {} + if bool(auto.get("enabled", False)): + low_pct = float(auto.get("low_pct", 0.2)) + high_pct = float(auto.get("high_pct", 99.8)) + + lo = np.percentile(x, low_pct) + hi = np.percentile(x, high_pct) + + if hi <= lo + 1e-6: + lo = float(np.min(x)) + hi = float(np.max(x)) + + x = (x - float(lo)) / max(float(hi - lo), 1e-6) + + x = np.clip(x, 0.0, 1.0) + return np.clip(x * 255.0 + 0.5, 0, 255).astype(np.uint8) + + def _gray_world_wb_u8(self, rgb_u8: np.ndarray, strength: float = 0.55) -> np.ndarray: + img = rgb_u8.astype(np.float32) + means = img.reshape(-1, 3).mean(axis=0) + target = float(means.mean()) + gains = target / np.maximum(means, 1e-6) + gains = np.clip(gains, 0.60, 1.70) + gains = 1.0 + (gains - 1.0) * float(strength) + return np.clip(img * gains[None, None, :], 0, 255).astype(np.uint8) + + def _apply_gamma_u8(self, rgb_u8: np.ndarray, gamma: float = 0.94) -> np.ndarray: + x = rgb_u8.astype(np.float32) / 255.0 + y = np.power(np.clip(x, 0.0, 1.0), float(gamma)) + return np.clip(y * 255.0 + 0.5, 0, 255).astype(np.uint8) + + def _clahe_luminance_u8(self, rgb_u8: np.ndarray, clip_limit: float = 1.7, tile_grid_size: int = 8) -> np.ndarray: + lab = cv2.cvtColor(rgb_u8, cv2.COLOR_RGB2LAB) + l, a, b = cv2.split(lab) + clahe = cv2.createCLAHE( + clipLimit=float(clip_limit), + tileGridSize=(int(tile_grid_size), int(tile_grid_size)), + ) + l2 = clahe.apply(l) + return cv2.cvtColor(cv2.merge([l2, a, b]), cv2.COLOR_LAB2RGB) + + def _denoise_fast_u8(self, rgb_u8: np.ndarray, strength: float = 3.0) -> np.ndarray: + bgr = cv2.cvtColor(rgb_u8, cv2.COLOR_RGB2BGR) + out = cv2.bilateralFilter( + bgr, + d=5, + sigmaColor=float(strength) * 12.0, + sigmaSpace=3.0, + ) + return cv2.cvtColor(out, cv2.COLOR_BGR2RGB) + + def _unsharp_u8(self, rgb_u8: np.ndarray, sigma: float = 0.9, amount: float = 0.85, threshold: int = 2) -> np.ndarray: + img = rgb_u8.astype(np.float32) + blur = cv2.GaussianBlur(img, (0, 0), sigmaX=float(sigma), sigmaY=float(sigma)) + sharp = img + float(amount) * (img - blur) + + if int(threshold) > 0: + diff = np.max(np.abs(img - blur), axis=2) + mask = diff >= int(threshold) + out = img.copy() + out[mask] = sharp[mask] + else: + out = sharp + + return np.clip(out, 0, 255).astype(np.uint8) + + def _local_contrast_u8(self, rgb_u8: np.ndarray, sigma: float = 9.0, amount: float = 0.14) -> np.ndarray: + img = rgb_u8.astype(np.float32) + blur = cv2.GaussianBlur(img, (0, 0), sigmaX=float(sigma), sigmaY=float(sigma)) + return np.clip(img + float(amount) * (img - blur), 0, 255).astype(np.uint8) + + def _hybrid_enhance_u8(self, rgb_u8: np.ndarray, preset: str = "balanced") -> np.ndarray: + preset = str(preset or "balanced").lower() + + if preset == "soft": + wb, gamma, clahe, den, lc, us = 0.35, 0.96, 1.35, 1.8, 0.08, 0.55 + elif preset == "strong": + wb, gamma, clahe, den, lc, us = 0.65, 0.90, 2.25, 3.2, 0.22, 1.25 + else: + wb, gamma, clahe, den, lc, us = 0.50, 0.94, 1.75, 2.5, 0.14, 0.85 + + out = self._gray_world_wb_u8(rgb_u8, strength=wb) + out = self._apply_gamma_u8(out, gamma=gamma) + out = self._clahe_luminance_u8(out, clip_limit=clahe, tile_grid_size=8) + out = self._denoise_fast_u8(out, strength=den) + out = self._local_contrast_u8(out, sigma=9.0, amount=lc) + out = self._unsharp_u8(out, sigma=0.85, amount=us, threshold=2) + return out + + def apply_rgb_enhancement_to_hwc_float01( + self, + rgb: np.ndarray, + stage: str = "after_decode", + source_kind: str = "raw", + ) -> np.ndarray: + """ + Aplica pós-processamento RGB opcional em imagem HWC float32 0..1. + + Local correto no pipeline: + - depois do debayer/decode RGB; + - depois do rgb_calibration; + - antes de flatfield native/fusão/crop/resize final. + + Isso garante que treinamento e inferência usem exatamente a mesma transformação + quando ambos usam o mesmo module_params.json. + """ + cfg = self._get_rgb_enhancement_config() + self.last_rgb_enhancement_result = { + "enabled": bool(cfg.get("enabled", False)), + "applied": False, + "stage": stage, + "source_kind": source_kind, + "config": cfg, + "warnings": [], + } + + if not cfg.get("enabled", False): + return rgb + + if source_kind == "preview" and not cfg.get("apply_to_preview_input", False): + self.last_rgb_enhancement_result["warnings"].append("skipped_preview_input") + return rgb + + if rgb is None or not isinstance(rgb, np.ndarray) or rgb.ndim != 3 or rgb.shape[2] != 3: + self.last_rgb_enhancement_result["warnings"].append( + f"invalid_rgb_shape:{None if rgb is None else rgb.shape}" + ) + return rgb + + t0 = time.perf_counter() + + backend = cfg.get("backend", "none") + preset = cfg.get("preset", "balanced") + + if backend == "hybrid": + if bool(cfg.get("use_u8_pipeline", True)): + rgb_u8 = self._rgb_float01_to_u8_for_enhancement(rgb, cfg) + out_u8 = self._hybrid_enhance_u8(rgb_u8, preset=preset) + out = out_u8.astype(np.float32) / 255.0 + else: + # Caminho defensivo. Hoje mantemos o u8 como padrão porque é + # exatamente o mesmo tipo de operação usado no script visual. + rgb_u8 = self._rgb_float01_to_u8_for_enhancement(rgb, cfg) + out_u8 = self._hybrid_enhance_u8(rgb_u8, preset=preset) + out = out_u8.astype(np.float32) / 255.0 + else: + raise ValueError(f"RGB enhancement backend não suportado: {backend}") + + if bool(cfg.get("clip_output", True)): + np.clip(out, 0.0, 1.0, out=out) + + dt_ms = (time.perf_counter() - t0) * 1000.0 + + self.last_rgb_enhancement_result.update({ + "applied": True, + "backend": backend, + "preset": preset, + "time_ms": float(dt_ms), + "input_shape": list(rgb.shape), + "output_shape": list(out.shape), + "output_dtype": str(out.dtype), + }) + + return out.astype(np.float32, copy=False) + + def build_training_rgb( self, raw16: np.ndarray, @@ -585,8 +915,19 @@ class RawProcessorCore: g = g * float(gains.get("G", 1.0)) b = b * float(gains.get("B", 1.0)) - chw = np.stack([r, g, b], axis=0).astype(np.float32) - chw = np.clip(chw, 0.0, 1.0) + rgb_hwc = np.stack([r, g, b], axis=2).astype(np.float32) + np.clip(rgb_hwc, 0.0, 1.0, out=rgb_hwc) + + # Pós-processamento RGB opcional. + # Aplicado aqui para o caminho de treino/offline que chama build_training_rgb(). + rgb_hwc = self.apply_rgb_enhancement_to_hwc_float01( + rgb_hwc, + stage="build_training_rgb.after_decode", + source_kind="raw", + ) + + chw = np.transpose(rgb_hwc, (2, 0, 1)).astype(np.float32, copy=False) + np.clip(chw, 0.0, 1.0, out=chw) if output_dtype == "float32": return chw @@ -639,9 +980,34 @@ class RawProcessorCore: cam_id = meta.get("cam_id") or meta.get("camera_id") or meta.get("id") or role if role == "rgb": + # Caminho offline: se o RGB veio como RAW16 Bayer 2D, + # monta RGB de treino usando a mesma configuração de rgb_processing. + if isinstance(data, np.ndarray) and data.ndim == 2: + rgb_chw = self.build_training_rgb( + data, + output_dtype="float32", + bit_depth=bit_depth, + ) + rgb_img = np.transpose(rgb_chw, (1, 2, 0)).astype(np.float32, copy=False) + + # Compatibilidade: se já veio HWC RGB processado. + elif isinstance(data, np.ndarray) and data.ndim == 3 and data.shape[2] == 3: + rgb_img = data.astype(np.float32) + if rgb_img.max() > 1.5: + rgb_img /= 255.0 + rgb_img = np.clip(rgb_img, 0.0, 1.0) + + rgb_img = self.apply_rgb_enhancement_to_hwc_float01( + rgb_img, + stage="decode_bins_cameras.preview_or_processed_input", + source_kind="preview", + ) + else: + raise RuntimeError(f"RGB offline inválido em {cam_id}: shape={getattr(data, 'shape', None)}") + decoded[cam_id] = { "name": "RGB", - "image": data.astype(np.float32) / max_val, + "image": rgb_img, "meta": meta, } @@ -798,6 +1164,14 @@ class RawProcessorCore: else: raise ValueError(f"rgb_processing.mode inválido: {rgb_mode}") + # Pós-processamento RGB opcional. + # Este é o caminho principal de inferência RAW_BRUTO. + rgb_hwc = self.apply_rgb_enhancement_to_hwc_float01( + rgb_hwc, + stage="decode_stream_cameras.after_raw_decode", + source_kind="raw", + ) + decoded[cam_id] = { "name": "RGB", "role": "rgb", @@ -812,10 +1186,21 @@ class RawProcessorCore: rgb = data[:, :, ::-1].astype(np.float32) / 255.0 + rgb = np.clip(rgb, 0.0, 1.0) + + # Por padrão não mexe em preview/RGB já processado. + # Se quiser aplicar também neste caminho, use: + # rgb_processing.enhancement.apply_to_preview_input=true + rgb = self.apply_rgb_enhancement_to_hwc_float01( + rgb, + stage="decode_stream_cameras.preview_input", + source_kind="preview", + ) + decoded[cam_id] = { "name": "RGB", "role": "rgb", - "image": np.clip(rgb, 0.0, 1.0), + "image": rgb, "meta": cam_meta, } diff --git a/Python/OAK/visual_worker/camera_manager.py b/Python/OAK/visual_worker/camera_manager.py deleted file mode 100644 index e7e7313c5..000000000 --- a/Python/OAK/visual_worker/camera_manager.py +++ /dev/null @@ -1,241 +0,0 @@ -# camera_manager.py - -import depthai as dai -import cv2 -import numpy as np -import time - -# Parâmetros da câmera -RGB_WIDTH, RGB_HEIGHT = 640, 480 -DEPTH_WIDTH, DEPTH_HEIGHT = 320, 240 -FX = 440.0 # distância focal em pixels (aproximado) -BASELINE = 0.075 # distância entre câmeras estéreo (em metros) - -# Variáveis globais -device = None -device_info = None -rgb_queue = None -depth_queue = None -ultimo_frame_depth = None -timestamp_ultimo_depth_frame = None -ultimo_frame_rgb = None -timestamp_ultimo_rgb_frame = None - -def iniciar_camera(index=0): - global device, device_info, rgb_queue, depth_queue - - print("Iniciando câmera OAK-D Lite...") - - pipeline = dai.Pipeline() - - # RGB - cam_rgb = pipeline.create(dai.node.ColorCamera) - cam_rgb.setPreviewSize(RGB_WIDTH, RGB_HEIGHT) - cam_rgb.setInterleaved(False) - cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB) - - xout_rgb = pipeline.create(dai.node.XLinkOut) - xout_rgb.setStreamName("rgb") - cam_rgb.preview.link(xout_rgb.input) - - # Profundidade - mono_left = pipeline.create(dai.node.MonoCamera) - mono_right = pipeline.create(dai.node.MonoCamera) - stereo = pipeline.create(dai.node.StereoDepth) - - mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P) - mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P) - mono_left.setBoardSocket(dai.CameraBoardSocket.LEFT) - mono_right.setBoardSocket(dai.CameraBoardSocket.RIGHT) - - stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) - - mono_left.out.link(stereo.left) - mono_right.out.link(stereo.right) - - xout_depth = pipeline.create(dai.node.XLinkOut) - xout_depth.setStreamName("depth") - stereo.depth.link(xout_depth.input) - - # Dispositivo - available_devices = dai.Device.getAllAvailableDevices() - if index >= len(available_devices): - raise ValueError(f"Câmera de índice {index} não encontrada.") - - device_info = available_devices[index] - device = dai.Device(pipeline, device_info) - - rgb_queue = device.getOutputQueue(name="rgb", maxSize=1, blocking=False) - depth_queue = device.getOutputQueue(name="depth", maxSize=1, blocking=False) - - print(f"Câmera iniciada: {device_info.name} (ID: {device_info.getMxId()})") - - get_camera_calib() - -def get_camera_calib(): - global FX, BASELINE - calib = device.readCalibration() - - # Obtem matriz intrínseca da câmera LEFT (com resolução padrão 640x400) - intrinsics = calib.getCameraIntrinsics(dai.CameraBoardSocket.LEFT, 640, 400) - FX = intrinsics[0][0] # fx - BASELINE = calib.getBaselineDistance() / 1000.0 # de mm → m - - print(f"[CALIB] FX: {FX:.2f} px, BASELINE: {BASELINE:.4f} m") - -def get_rgb_frame(): - global ultimo_frame_rgb, timestamp_ultimo_rgb_frame - if rgb_queue is None: - return None, None - frame = rgb_queue.tryGet() - if frame is not None: - ultimo_frame_rgb = frame.getCvFrame() - timestamp_ultimo_rgb_frame = time.time() - return ultimo_frame_rgb, timestamp_ultimo_rgb_frame - -def get_heatmap_frame(): - frame = get_depth_frame() - if frame is not None: - return gerar_heatmap(frame), timestamp_ultimo_depth_frame - return None, None - -def gerar_heatmap(depth_frame): - # Normaliza e aplica colormap - normalized = cv2.normalize(depth_frame, None, 0, 255, cv2.NORM_MINMAX) - heatmap = cv2.applyColorMap(normalized.astype(np.uint8), cv2.COLORMAP_JET) - return heatmap - -def get_depth_frame(): - global ultimo_frame_depth, timestamp_ultimo_depth_frame - if depth_queue is None: - return None, None - - frame = depth_queue.tryGet() - if frame is not None: - ultimo_frame_depth = frame.getFrame() - timestamp_ultimo_depth_frame = time.time() - return ultimo_frame_depth, timestamp_ultimo_depth_frame - -def get_status_dispositivo(): - from depthai import UsbSpeed - - try: - memory = device.getDdrMemoryUsage() - memory_info = { - "used": memory.used, - "remaining": memory.remaining, - "total": memory.total - } - except: - memory_info = None - - try: - temp = device.getChipTemperature() - temp_info = { - "css": temp.css, - "mss": temp.mss, - "upa": temp.upa, - "dss": temp.dss - } - except: - temp_info = None - - try: - info = device.getDeviceInfo() - protocol = str(info.protocol) - except: - protocol = None - - try: - bootloader = str(device.getBootloaderVersion()) - except: - bootloader = None - - try: - usb_speed = str(device.getUsbSpeed().name) - except: - usb_speed = None - - try: - pipeline_running = device.isPipelineRunning() - except: - pipeline_running = None - - try: - cameras = [sensor.name for sensor in device.getConnectedCameras()] - except: - cameras = None - - return { - "id": device_info.getMxId(), - "name": device_info.name, - "state": device_info.state.name, - "usb_speed": usb_speed, - "available_camera_sensors": cameras, - "version": protocol, - "bootloader_version": bootloader, - "is_pipeline_running": pipeline_running, - "memory_usage": memory_info, - "temperature": temp_info - } - -def analisar_obstaculos(velocidade=0.0, refinar=False): - from processamento.obstaculos import analisar_macro_grid, analisar_micro_grid - global timestamp_ultimo_depth_frame - - frame = get_depth_frame() - - if frame is None: - if ultimo_frame_depth is None: - return { "erro": "Sem frame disponível", "timestamp": None } - frame = ultimo_frame_depth - - macro = analisar_macro_grid(frame, velocidade) - - if macro["precisa_micro"] and refinar: - micro = analisar_micro_grid(frame) - micro["timestamp"] = timestamp_ultimo_depth_frame - return micro - - macro["timestamp"] = timestamp_ultimo_depth_frame - return macro - -def analisar_corredor(): - from processamento.corredor import estimar_largura_corredor - global timestamp_ultimo_depth_frame # precisa garantir que fx e baseline existam - - frame = get_depth_frame() - - if frame is None: - if ultimo_frame_depth is None: - return { "erro": "Sem frame disponível", "timestamp": None } - frame = ultimo_frame_depth - - largura_mm, pos_central, pontos_debug = estimar_largura_corredor(frame, FX, BASELINE) - - return { - "largura_corredor_mm": float(largura_mm) if largura_mm is not None else None, - "posicao_central_fracao": float(pos_central) if pos_central is not None else None, - "timestamp": timestamp_ultimo_depth_frame, - "pontos_debug": [ [int(x), float(y)] for x, y in pontos_debug ] - } - -def analisar_obstaculos_3d(): - from processamento.visao3d import detectar_obstaculos_em_frente - global timestamp_ultimo_depth_frame - - frame = get_depth_frame() - - if frame is None: - if ultimo_frame_depth is None: - return { "erro": "Sem frame disponível", "timestamp": None } - frame = ultimo_frame_depth - - lista = detectar_obstaculos_em_frente(frame, FX, BASELINE) - - return { - "obstaculos_detectados": lista, - "timestamp": timestamp_ultimo_depth_frame - } - - diff --git a/Python/OAK/visual_worker/enums.py b/Python/OAK/visual_worker/enums.py deleted file mode 100644 index 6d0475ae4..000000000 --- a/Python/OAK/visual_worker/enums.py +++ /dev/null @@ -1,38 +0,0 @@ -from enum import IntEnum - -class Comando(IntEnum): - PING = 1 - GET_RGB_FRAME = 2 - GET_HEATMAP_FRAME = 3 - GET_OBSTACULOS = 4 - GET_STATUS_DISPOSITIVO = 5 - CALIBRAR_GRADES = 6 - GET_MAPA_PROFUNDIDADE = 7 - GET_LARGURA_CORREDOR = 8 - GET_OBSTACULOS_3D = 9 - -class TipoComando(IntEnum): - TX = 1 - RX = 2 - -class TipoDeteccao(IntEnum): - SEGURO = 1 - OBSTACULO = 2 - DEPRESSAO = 3 - -class TipoRegiaoRadar(IntEnum): - SOLO = 1 - AEREO = 2 - -class Direcao(IntEnum): - PARADO = 0 - FRENTE = 1 - TRAS = 2 - ESQUERDA = 3 - DIREITA = 4 - CIMA = 5 - BAIXO = 6 - ESQUERDABAIXO = 7 - ESQUERDACIMA = 8 - DIREITABAIXO = 9 - DIREITACIMA = 10 \ No newline at end of file diff --git a/Python/OAK/visual_worker/filtros.py b/Python/OAK/visual_worker/filtros.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/Python/OAK/visual_worker/main.py b/Python/OAK/visual_worker/main.py deleted file mode 100644 index 585029c86..000000000 --- a/Python/OAK/visual_worker/main.py +++ /dev/null @@ -1,173 +0,0 @@ -import sys -import os -import time -import json -from queue import Queue -import threading - -from enums import Comando, TipoComando - -from mqtt_handler import enviar_mensagem_mqtt - -sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))) - - -# ───────────────────────────── -# 🔹 Fila de Mensagens para Processamento de Comandos -# ───────────────────────────── -fila_comandos = Queue() -def worker(): - while True: - topico, payload = fila_comandos.get() - try: - processar_comando(topico, payload) - except Exception as e: - print("❌ Erro:", e) - fila_comandos.task_done() -# 🔥 Cria 4 workers -for _ in range(4): - threading.Thread(target=worker, daemon=True).start() - - -# ───────────────────────────── -# 🔹 Parâmetros de Execução -# ───────────────────────────── -mqtt_topic = sys.argv[1] if len(sys.argv) > 1 else "agrobot/operador/sonar" -camera_index = int(sys.argv[2]) if len(sys.argv) > 2 else 0 - - -# ───────────────────────────── -# 🔹 Callback de Comando MQTT -# ───────────────────────────── -def ao_receber_comando(topico, payload): - fila_comandos.put((topico, payload)) - -def processar_comando(topico, payload): - try: - comando = json.loads(payload.decode()) - tipo_cmd = int(comando.get("tipo_cmd")) - codigo = int(comando.get("cmd")) - - # Ignora mensagens enviadas por si proprio - if (tipo_cmd == TipoComando.RX): - return - - resposta = {} - - if codigo == Comando.PING: - resposta = { - "ping": { - "timestamp": time.time(), - "status": True - } - } - - elif codigo == Comando.GET_STATUS_DISPOSITIVO: - from camera_manager import get_status_dispositivo - resposta = { - "device_status": get_status_dispositivo() - } - - elif codigo == Comando.GET_RGB_FRAME: - from camera_manager import get_rgb_frame - frame, timestamp = get_rgb_frame() - base64_img = None - if frame is not None: - from utils import encode_image_base64 - base64_img = encode_image_base64(frame) - resposta = { - "timestamp": timestamp, - "frame": base64_img - } - - elif codigo == Comando.GET_HEATMAP_FRAME: - from camera_manager import get_heatmap_frame - frame, timestamp = get_heatmap_frame() - base64_img = None - if frame is not None: - from utils import encode_image_base64 - base64_img = encode_image_base64(frame) - resposta = { - "timestamp": timestamp, - "frame": base64_img - } - - elif codigo == Comando.CALIBRAR_GRADES: - from camera_manager import get_depth_frame - from processamento.obstaculos import calibrar_grades - sucesso = calibrar_grades(get_depth_frame, n_frames=50) - resposta = { - "calibragem": { - "timestamp": time.time(), - "status": sucesso - } - } - - elif codigo == Comando.GET_OBSTACULOS: - from camera_manager import analisar_obstaculos - velocidade = float(comando.get("velocidade", 0.0)) # padrão = 0.0 m/s, parado - precisao_micro = float(comando.get("precisao_micro", False)) - resposta = { - "obstaculos": analisar_obstaculos(velocidade=velocidade, refinar=precisao_micro) - } - - elif codigo == Comando.GET_LARGURA_CORREDOR: - from camera_manager import analisar_corredor - resposta = { - "corredor": analisar_corredor() - } - - elif codigo == Comando.GET_MAPA_PROFUNDIDADE: - from camera_manager import get_depth_frame - from utils import gerar_mapa_profundidade - - depth, timestamp = get_depth_frame() - if depth is not None: - resolucao = str(comando.get("resolucao", "10x10")) - try: - linhas, colunas = map(int, resolucao.lower().split("x")) - except: - linhas, colunas = 10, 10 - - grid = gerar_mapa_profundidade(depth, linhas, colunas) - grid["timestamp"] = timestamp - resposta = { - "mapa_profundidade": grid - } - - elif codigo == Comando.GET_OBSTACULOS_3D: - from camera_manager import analisar_obstaculos_3d - resposta = { - "obstaculos_3d": analisar_obstaculos_3d() - } - - enviar_mensagem_mqtt(codigo, resposta) - - except Exception as e: - print("❌ Erro ao processar comando MQTT:", e) - -# ───────────────────────────── -# 🔹 Execução Principal -# ───────────────────────────── -if __name__ == "__main__": - print("🚀 Iniciando Operário Visual com OAK-D Lite...") - - # 1. Inicia a câmera - from camera_manager import iniciar_camera - iniciar_camera(camera_index) - - from mqtt_handler import iniciar_mqtt, enviar_mensagem_script_carregado - - # 2. Inicia MQTT e registra callback - iniciar_mqtt(mqtt_topic, ao_receber_comando) - - print(f"✅ Escutando comandos no tópico: {mqtt_topic}") - - enviar_mensagem_script_carregado() - - # 3. Loop principal - try: - while True: - time.sleep(1) - except KeyboardInterrupt: - print("Encerrando...") diff --git a/Python/OAK/visual_worker/mqtt_handler.py b/Python/OAK/visual_worker/mqtt_handler.py deleted file mode 100644 index 363cf8b57..000000000 --- a/Python/OAK/visual_worker/mqtt_handler.py +++ /dev/null @@ -1,51 +0,0 @@ -import paho.mqtt.client as mqtt -import uuid -import json -import time - -from enums import TipoComando - -mqtt_client = None -mqtt_topic = None - -def iniciar_mqtt(topico, funcao_callback, broker="localhost", porta=1883): - """ - Inicia conexão MQTT e escuta o tópico. - :param tópico: string com o nome do tópico - :param funcao_callback: função que será chamada ao receber mensagem - """ - global mqtt_client, mqtt_topic - - mqtt_client = mqtt.Client(f"visual_worker_{uuid.uuid4()}") - mqtt_topic = topico - - def on_connect(client, userdata, flags, rc): - if rc == 0: - print(f"[MQTT] Conectado ao broker em {broker}:{porta}") - client.subscribe(mqtt_topic) - else: - print("[MQTT] Falha ao conectar, código de retorno:", rc) - - def on_message(client, userdata, msg): - try: - funcao_callback(msg.topic, msg.payload) - except Exception as e: - print("[MQTT] Erro no callback:", e) - - mqtt_client.on_connect = on_connect - mqtt_client.on_message = on_message - - mqtt_client.connect(broker, porta) - mqtt_client.loop_start() - -def enviar_mensagem_script_carregado(): - mqtt_client.publish(mqtt_topic, "OK") - -def enviar_mensagem_mqtt(comando, objeto): - mensagem = { - "momento": time.time(), - "tipo_cmd": TipoComando.RX, - "cmd": comando, - "obj": objeto - } - mqtt_client.publish(mqtt_topic, json.dumps(mensagem)) diff --git a/Python/OAK/visual_worker/processamento/corredor.py b/Python/OAK/visual_worker/processamento/corredor.py deleted file mode 100644 index 29318b239..000000000 --- a/Python/OAK/visual_worker/processamento/corredor.py +++ /dev/null @@ -1,54 +0,0 @@ -import numpy as np -import cv2 - -def estimar_largura_corredor(depth_frame, fx, baseline, largura_mm_max=2000): - """ - Estima a largura do corredor com base no mapa de profundidade. - - Parâmetros: - - depth_frame: imagem de profundidade (em disparidades) - - fx: distância focal da câmera (pixels) - - baseline: distância entre as câmeras estéreo (metros) - - largura_mm_max: largura máxima esperada do corredor (em mm) - - Retorna: - - largura_estimada (em mm) - - posicao_central (0 a 1 → fração da imagem) - - lista de pontos com (x, distancia_mm) - """ - - altura, largura = depth_frame.shape - faixa_inicio = int(altura * 0.45) - faixa_fim = int(altura * 0.55) - - # Recorte central horizontal - faixa = depth_frame[faixa_inicio:faixa_fim, :] - - # Média por coluna - profundidade_media = np.median(faixa, axis=0) - - # Converte de disparidade para distância (mm) - with np.errstate(divide='ignore'): # evita divisão por zero - distancias_mm = (fx * baseline * 1000) / profundidade_media - distancias_mm = np.clip(distancias_mm, 0, largura_mm_max) - - # Aplica filtro para remover ruído (média móvel simples) - distancias_mm_suave = cv2.blur(distancias_mm.reshape(1, -1).astype(np.float32), (15, 1)).flatten() - - # Detecta bordas esquerda e direita: picos de distância próxima - limite = 2500 # Limite máximo para considerar obstáculo lateral (em mm) - idx_validos = np.where((distancias_mm_suave > 100) & (distancias_mm_suave < limite))[0] - - if len(idx_validos) < 2: - return None, None, [] - - esquerda = idx_validos[0] - direita = idx_validos[-1] - - largura_estimada = abs(distancias_mm_suave[direita] - distancias_mm_suave[esquerda]) - posicao_central = (esquerda + direita) / 2 / largura - - # Para visualização/debug - pontos_debug = [(int(x), float(distancias_mm_suave[x])) for x in range(0, largura, 10)] - - return largura_estimada, posicao_central, pontos_debug diff --git a/Python/OAK/visual_worker/processamento/obstaculos.py b/Python/OAK/visual_worker/processamento/obstaculos.py deleted file mode 100644 index 12f5b9c38..000000000 --- a/Python/OAK/visual_worker/processamento/obstaculos.py +++ /dev/null @@ -1,227 +0,0 @@ -# obstaculos.py - -import numpy as np -import time -from scipy import stats - -from visual_worker.enums import Direcao, TipoDeteccao, TipoRegiaoRadar - -LIMIAR_OBSTACULO = 1000 # mm -LIMIAR_DEPRESSAO = 2500 # mm - -buffer_macro = None -buffer_micro = None -ref_macro = None -ref_micro = None - -MACRO_ROWS = 3 -MACRO_COLS = 4 -MICRO_ROWS = 10 -MICRO_COLS = 10 - - - -def analisar_macro_grid(depth_frame, velocidade): - altura, largura = depth_frame.shape - row_h = altura // MACRO_ROWS - col_w = largura // MACRO_COLS - - LIMIAR_OBSTACULO = calcular_limiar_dinamico(velocidade) - LIMIAR_DEPRESSAO = 2500 # Pode virar dinâmico depois - - zonas_ocupadas = [] - dist_min = 9999 - frontal_bloqueado = False - laterais = { Direcao.ESQUERDA: False, Direcao.DIREITA: False } - zona_livre = Direcao.FRENTE - - for i in range(MACRO_ROWS): - for j in range(MACRO_COLS): - y1, y2 = i * row_h, (i + 1) * row_h - x1, x2 = j * col_w, (j + 1) * col_w - - area = depth_frame[y1:y2, x1:x2] - validos = area[(area > 0) & (area < 10000)] - if validos.size == 0: - continue - - media = np.mean(validos) - erro = media - ref_macro[i][j] - dist_min = min(dist_min, media) - - tipo = TipoDeteccao.SEGURO - if erro < LIMIAR_OBSTACULO: - tipo = TipoDeteccao.OBSTACULO - elif erro > LIMIAR_DEPRESSAO: - tipo = TipoDeteccao.DEPRESSAO - - if tipo != TipoDeteccao.SEGURO: - regiao = TipoRegiaoRadar.SOLO if i == MACRO_ROWS - 1 else TipoRegiaoRadar.AEREO - - zonas_ocupadas.append({ - "linha": i, - "coluna": j, - "distancia": round(float(media) / 1000, 2), - "tipo": tipo, - "regiao": regiao - }) - - if i == MACRO_ROWS - 1 and j in [1, 2]: - frontal_bloqueado = True - if j == 0: - laterais[Direcao.ESQUERDA] = True - if j == MACRO_COLS - 1: - laterais[Direcao.DIREITA] = True - - if not zonas_ocupadas: - zona_livre = Direcao.FRENTE - elif all(z["coluna"] < MACRO_COLS // 2 for z in zonas_ocupadas): - zona_livre = Direcao.DIREITA - elif all(z["coluna"] >= MACRO_COLS // 2 for z in zonas_ocupadas): - zona_livre = Direcao.ESQUERDA - else: - zona_livre = Direcao.PARADO - - return { - "alerta": len(zonas_ocupadas) > 0, - "frontal_bloqueado": frontal_bloqueado, - "laterais": laterais, - "zona_livre": zona_livre, - "distancia_minima": round(float(dist_min) / 1000, 2), - "detalhes": zonas_ocupadas, - "precisa_micro": frontal_bloqueado - } - -def analisar_micro_grid(depth_frame): - altura, largura = depth_frame.shape - row_h = altura // MICRO_ROWS - col_w = largura // MICRO_COLS - - detalhes = [] - dist_min = 9999 - - for i in range(MICRO_ROWS): - for j in range(MICRO_COLS): - y1, y2 = i * row_h, (i + 1) * row_h - x1, x2 = j * col_w, (j + 1) * col_w - - area = depth_frame[y1:y2, x1:x2] - validos = area[(area > 0) & (area < 10000)] - if validos.size == 0: - continue - - media = np.mean(validos) - erro = media - ref_macro[i][j] - dist_min = min(dist_min, media) - - tipo = TipoDeteccao.SEGURO - if erro < LIMIAR_OBSTACULO: - tipo = TipoDeteccao.OBSTACULO - elif erro > LIMIAR_DEPRESSAO: - tipo = TipoDeteccao.DEPRESSAO - - if tipo != TipoDeteccao.SEGURO: - regiao = TipoRegiaoRadar.SOLO if i >= MICRO_ROWS * 0.6 else TipoRegiaoRadar.AEREO - - detalhes.append({ - "linha": i, - "coluna": j, - "distancia": round(float(media) / 1000, 2), - "tipo": tipo, - "regiao": regiao - }) - - return { - "alerta": len(detalhes) > 0, - "quantidade": len(detalhes), - "distancia_minima": round(float(dist_min) / 1000, 2), - "detalhes": detalhes - } - - -def calcular_limiar_dinamico(velocidade): - # Exemplo: de 0.6m a 1.5m dependendo da velocidade - return int(min(max(600 + velocidade * 800, 600), 1500)) - -def gerar_ref(buffer): - rows = len(buffer) - cols = len(buffer[0]) - ref_out = np.zeros((rows, cols), dtype=np.float32) - - for i in range(rows): - for j in range(cols): - valores = buffer[i][j] - if len(valores) == 0: - ref_out[i][j] = 0 # ou -1 para representar célula vazia - continue - - # Tenta usar moda - moda = stats.mode(valores, keepdims=True).mode - if moda.size > 0: - ref_out[i][j] = moda[0] - else: - ref_out[i][j] = np.median(valores) - - return ref_out - -def calibrar_macro(frame): - global buffer_macro - altura, largura = frame.shape - row_h = altura // MACRO_ROWS - col_w = largura // MACRO_COLS - - for i in range(MACRO_ROWS): - for j in range(MACRO_COLS): - y1, y2 = i * row_h, (i + 1) * row_h - x1, x2 = j * col_w, (j + 1) * col_w - - area = frame[y1:y2, x1:x2] - validos = area[(area > 0) & (area < 10000)] - if validos.size > 0: - buffer_macro[i][j].extend(validos.tolist()) - -def calibrar_micro(frame): - global buffer_micro - altura, largura = frame.shape - row_h = altura // MICRO_ROWS - col_w = largura // MICRO_COLS - - for i in range(MICRO_ROWS): - for j in range(MICRO_COLS): - y1, y2 = i * row_h, (i + 1) * row_h - x1, x2 = j * col_w, (j + 1) * col_w - - area = frame[y1:y2, x1:x2] - validos = area[(area > 0) & (area < 10000)] - if validos.size > 0: - buffer_micro[i][j].extend(validos.tolist()) - -def calibrar_grades(capturar_depth_frame, n_frames=50): - global buffer_macro, buffer_micro, ref_macro, ref_micro - - buffer_macro = [[[] for _ in range(MACRO_COLS)] for _ in range(MACRO_ROWS)] - buffer_micro = [[[] for _ in range(MICRO_COLS)] for _ in range(MICRO_ROWS)] - - print(f"📡 Iniciando calibração de {n_frames} frames...") - - total_validos = 0 - for i in range(n_frames): - frame = capturar_depth_frame() - if frame is not None: - calibrar_macro(frame) - calibrar_micro(frame) - total_validos += 1 - print(f"✔️ Frame {i+1}/{n_frames} calibrado.") - else: - print(f"⚠️ Frame {i+1}/{n_frames} inválido, ignorado.") - time.sleep(0.03) # pausa leve pra estabilizar captura - - if total_validos == 0: - print("❌ Nenhum frame válido recebido. Calibração cancelada.") - return False - - ref_macro = gerar_ref(buffer_macro) - ref_micro = gerar_ref(buffer_micro) - - print("✅ Calibração concluída com sucesso!") - return True diff --git a/Python/OAK/visual_worker/processamento/visao3d.py b/Python/OAK/visual_worker/processamento/visao3d.py deleted file mode 100644 index 17de6eeb2..000000000 --- a/Python/OAK/visual_worker/processamento/visao3d.py +++ /dev/null @@ -1,48 +0,0 @@ -import numpy as np - -def detectar_obstaculos_em_frente(depth_frame, fx, baseline, faixa_altura=(0.45, 0.55), max_distancia_mm=2500): - """ - Detecta obstáculos com base no perfil de profundidade frontal. - - Retorna uma lista de obstáculos com: - - posição_percentual (0 à 100) - - distancia (em metros) - - largura_aproximada (em px) - """ - - altura, largura = depth_frame.shape - y1 = int(altura * faixa_altura[0]) - y2 = int(altura * faixa_altura[1]) - - faixa = depth_frame[y1:y2, :] - - profundidade_media = np.median(faixa, axis=0) - with np.errstate(divide='ignore'): - distancias_mm = (fx * baseline * 1000) / profundidade_media - distancias_mm = np.clip(distancias_mm, 0, max_distancia_mm) - - # Simplifica usando limiar: onde há objetos "próximos" - mascara = (distancias_mm > 100) & (distancias_mm < max_distancia_mm) - - obstaculos = [] - inicio = None - - for x in range(largura): - if mascara[x]: - if inicio is None: - inicio = x - elif inicio is not None: - fim = x - centro = (inicio + fim) // 2 - largura_px = fim - inicio - distancia = np.min(distancias_mm[inicio:fim]) - posicao_pct = 100 * centro / largura - - obstaculos.append({ - "posicao": round(float(posicao_pct), 1), - "distancia": round(float(distancia) / 1000, 2), - "largura_px": largura_px - }) - inicio = None - - return obstaculos diff --git a/Python/OAK/visual_worker/utils.py b/Python/OAK/visual_worker/utils.py deleted file mode 100644 index 2096d4270..000000000 --- a/Python/OAK/visual_worker/utils.py +++ /dev/null @@ -1,47 +0,0 @@ -# utils.py - -import cv2 -import base64 -import numpy as np - -def encode_image_base64(frame): - """ - Codifica uma imagem (np.ndarray) como string base64 JPEG. - """ - ret, buffer = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 80]) - if not ret: - return None - base64_str = base64.b64encode(buffer).decode('utf-8') - return base64_str - -def gerar_mapa_profundidade(depth_frame, linhas=10, colunas=10): - altura, largura = depth_frame.shape - h = altura // linhas - w = largura // colunas - - grid = [] - resposta = { - "resolucao": f"{linhas}x{colunas}", - "unidade": "cm", - "grid": grid - } - - if depth_frame is None: - return resposta - - for i in range(linhas): - linha = [] - for j in range(colunas): - y1, y2 = i * h, (i + 1) * h - x1, x2 = j * w, (j + 1) * w - celula = depth_frame[y1:y2, x1:x2] - validos = celula[(celula > 0) & (celula < 10000)] - if validos.size == 0: - linha.append(None) - else: - media_cm = np.mean(validos) / 100.0 - linha.append(round(float(media_cm), 2)) - grid.append(linha) - - resposta["grid"] = grid - return resposta