diff --git a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs index f06cb93bb..68f7384cb 100644 --- a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs +++ b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs @@ -142,11 +142,14 @@ namespace AgroBase.Forms return; } + var h = GPSUtils.DistanciaDoTrecho(simulacao.Select(x => new GPSModel() { Latitude = x.latitude, Longitude = x.longitude, OrientacaoReal = x.orientacao }).ToList()); + lblStatus.Text = $"Carro: {_Sensoriamento.Trajetoria.StatusCarro} ({_Sensoriamento.OperadorVisual.Analises.segmentacao.status_corredor})"; lblDirecao.Text = $"Direção: {_Sensoriamento.Trajetoria.DirecaoCaminho}"; lblDistanciaOperacao.Text = $"Distância Operação: {_Sensoriamento.Trajetoria.DistanciaPercorrida:F2} m de {_Sensoriamento.Trajetoria.DistanciaTotal:F2} m"; lblDistanciaFinal.Text = $"Distância Restante: {_Sensoriamento.Trajetoria.DistanciaRestante:F2} m"; - lblDistanciaLateral.Text = $"Esq: {_Sensoriamento.Trajetoria.DistanciaEsquerda:F2} m - Dir: {_Sensoriamento.Trajetoria.DistanciaDireita:F2} m {Variaveis.OperacaoEmAndamento.Controle.ErroLateral:F2} m"; + //lblDistanciaLateral.Text = $"Esq: {_Sensoriamento.Trajetoria.DistanciaEsquerda:F2} m - Dir: {_Sensoriamento.Trajetoria.DistanciaDireita:F2} m {Variaveis.OperacaoEmAndamento.Controle.ErroLateral:F2} m"; + lblDistanciaLateral.Text = $"h: {h:F2} m, p: {_Sensoriamento.Controle.SimulacaoMPC.Count}, s: {_Sensoriamento.Controle.DebugCustoMpc.Count}"; lblRua.Text = $"Corredor: " + (_Sensoriamento.Trajetoria.CorredorAtual.Dentro ? "Dentro" : "Fora"); lblMargem.Text = $"Margem: " + (_Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não"); lblProximoPonto.Text = $"Próximo Ponto: {_Sensoriamento.Trajetoria.ProximoPonto.idxPonto}"; @@ -260,6 +263,7 @@ namespace AgroBase.Forms { GPSModel novaPosicao = null; velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle); + Console.WriteLine($"RPM SP: {Variaveis.OperacaoEmAndamento.SimulacaoRpmControle} ({velocidadeCarroMs:F2} ms)"); Variaveis.OperacaoEmAndamento.DispMvd.Dados.VelocidadeMedia = velocidadeCarroMs; diff --git a/AgroBase/AgroBase/Models/Enums.cs b/AgroBase/AgroBase/Models/Enums.cs index 1c448aa84..a05358f0b 100644 --- a/AgroBase/AgroBase/Models/Enums.cs +++ b/AgroBase/AgroBase/Models/Enums.cs @@ -199,7 +199,8 @@ SaindoRua = 3, Manobrando = 4, Direcionando = 5, - RetornandoBase = 6 + RetornandoBase = 6, + Indefinido = 7 } public enum DirecaoCarroRua diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs index d0c76d638..aa711b65e 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs @@ -39,7 +39,7 @@ namespace AgroBase.Models.Operacoes public bool ImuParadaPorInclinacao { get; set; } public bool FrenagemAutomaticaAoParar { get; set; } - public bool RegistrarDadosPosProcessamento { get; set; } = false; + public bool RegistrarDadosPosProcessamento { get; set; } public int MovVelocidadeSErvasPercent { get; set; } public int MovRpmMax @@ -67,9 +67,8 @@ namespace AgroBase.Models.Operacoes public double AtuPercentualErvasBicoOn { get; set; } public double AtuPercentualErvasBicoOff { get; set; } - public bool MpcMatrizCusto { get; set; } = true; - public double MpcHorizonteMin { get; set; } = 1.0; - public double MpcHorizonteMax { get; set; } = 2.0; + public bool MpcMatrizCusto { get; set; } + public double MpcHorizonte { get; set; } = 4.0; } public class OperacaoParametrosDadosModel diff --git a/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs b/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs index 83751d61e..0bad3803c 100644 --- a/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs +++ b/AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs @@ -548,6 +548,7 @@ namespace AgroBase.Models.Operadores public double erro_lateral_pct { get; set; } public StatusCarroMapa status_corredor { get; set; } public StatusCarroMapa status_corredor_anterior { get; set; } + public VisualWorkerMessageSegmentacaoSemanticaDebugCorredorModel status_corredor_debug { get; set; } public List centros_corredor { get; set; } public VisualWorkerMessageSegmentacaoSemanticaModel Clone() @@ -561,11 +562,20 @@ namespace AgroBase.Models.Operadores erro_lateral_pct = erro_lateral_pct, status_corredor = status_corredor, status_corredor_anterior = status_corredor_anterior, + status_corredor_debug = status_corredor_debug, centros_corredor = new List(centros_corredor ?? new List()) }; } } + public class VisualWorkerMessageSegmentacaoSemanticaDebugCorredorModel + { + public double nav_global { get; set; } + public double nav_near { get; set; } + public double nav_mid { get; set; } + public double nav_far { get; set; } + } + public class VisualWorkerMessageMatrizConfiancaModel { public float ts { get; set; } diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index 57855143c..879c9f05c 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -184,6 +184,7 @@ namespace AgroBase.Services.Operadores ("camera_solo_id", Variaveis.OperacaoEmAndamento.DispSen?.Dados?.CamerasSolo?.FirstOrDefault()?.Id ?? ""), ("path_ia_model_ruas_seg", VersionamentoService.ArquivoModeloSegStreetDetector.CaminhoCompleto), ("path_ia_labelmap_ruas_seg", VersionamentoService.ArquivoLabelmapSegStreetDetector.CaminhoCompleto), + ("path_ia_norm_stats_ruas_seg", VersionamentoService.ArquivoModeloNormstatsWeedDetector.CaminhoCompleto), ("ia_backbone_ruas_seg", "nvidia/segformer-b0-finetuned-ade-512-512"), ("path_ia_model_ruas_det", VersionamentoService.ArquivoModeloDetStreetDetector.CaminhoCompleto), ("path_ia_model_ervas", VersionamentoService.ArquivoModeloWeedDetector.CaminhoCompleto), @@ -247,14 +248,13 @@ namespace AgroBase.Services.Operadores distanciaMargem = p.LarguraCorredor * 0.8 }) .ToList(), - horizonte_min = pControle.MpcHorizonteMin, - horizonte_max = pControle.MpcHorizonteMax, angulo_max_graus = pControle.DirAnguloMaximo, velocidade_min = Math.Round(FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeCErvasPercent), 4), velocidade_max = Math.Round(FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeSErvasPercent), 4), tempo_entre_comandos = (Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Enums.T_Code.Dir)?.DelayEnvioComando ?? 500) / 1000.0, + horizonte = pControle.MpcHorizonte, + usa_matriz_custo = pControle.MpcMatrizCusto, passos_atraso = 1, - usa_matriz_custo = pControle.MpcMatrizCusto } } ), diff --git a/AgroBase/AgroBase/Services/VersionamentoService.cs b/AgroBase/AgroBase/Services/VersionamentoService.cs index 382fd757d..be7c54cca 100644 --- a/AgroBase/AgroBase/Services/VersionamentoService.cs +++ b/AgroBase/AgroBase/Services/VersionamentoService.cs @@ -42,7 +42,7 @@ namespace AgroBase.Services } } } - public static VersaoArquivoModel ArquivoModeloDetStreetDetector + public static VersaoArquivoModel ArquivoModeloNormstatsStreetDetector { get { @@ -52,6 +52,16 @@ namespace AgroBase.Services } } } + public static VersaoArquivoModel ArquivoModeloDetStreetDetector + { + get + { + lock (_ArquivoLock) + { + return _ArquivosVersionados.Where(x => x.TipoArquivo == TipoArquivoVersionado.ModeloIA_StreetDetector).Skip(3).FirstOrDefault(); + } + } + } public static VersaoArquivoModel ArquivoModeloWeedDetector { get diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py index aa39abc0c..3145d01c6 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/segformer_runner.py @@ -1,3 +1,5 @@ +import json +import os import time from PIL import Image import cv2 @@ -6,8 +8,8 @@ import torch from transformers import SegformerForSemanticSegmentation class SegformerNavRunner: - IMAGENET_MEAN = torch.tensor([0.485, 0.456, 0.406]).view(3, 1, 1) - IMAGENET_STD = torch.tensor([0.229, 0.224, 0.225]).view(3, 1, 1) + IMAGENET_MEAN = [0.485, 0.456, 0.406] + IMAGENET_STD = [0.229, 0.224, 0.225] def __init__(self, seg_config, device="cuda"): from shared.utils import carregar_labelmap_completo @@ -20,6 +22,50 @@ class SegformerNavRunner: self.roi_tamanho = seg_config["ia_roi_size"] self.last_infer = None + # ========================== + # Normalização fixa (igual treino) + # ========================== + norm_mean = self.IMAGENET_MEAN + norm_std = self.IMAGENET_STD + + norm_stats_path = seg_config.get("ia_norm_stats_path") + if norm_stats_path and os.path.isfile(norm_stats_path): + with open(norm_stats_path, "r", encoding="utf-8") as f: + norm_stats = json.load(f) + + stats_channels = norm_stats.get("channels", []) + stats_mean = norm_stats.get("mean", []) + stats_std = norm_stats.get("std", []) + + print(f"[NORM] usando stats fixos de: {norm_stats_path}") + print(f"[NORM] channels={stats_channels}") + print(f"[NORM] mean={stats_mean}") + print(f"[NORM] std ={stats_std}") + + idx_by_name = {name: i for i, name in enumerate(stats_channels)} + + m_R = stats_mean[idx_by_name["R"]] + m_G = stats_mean[idx_by_name["G"]] + m_B = stats_mean[idx_by_name["B"]] + + s_R = stats_std[idx_by_name["R"]] + s_G = stats_std[idx_by_name["G"]] + s_B = stats_std[idx_by_name["B"]] + + norm_mean = [m_R, m_G, m_B] + norm_std = [s_R, s_G, s_B] + else: + print(f"[NORM] norm_stats.json não encontrado em {norm_stats_path}. " + f"Usando normalização dinâmica por frame.") + + self.set_norm_stats(norm_mean, norm_std) + + def set_norm_stats(self, mean, std): + mean = torch.tensor(mean, dtype=torch.float32).view(3, 1, 1) + std = torch.tensor(std, dtype=torch.float32).view(3, 1, 1).clamp_min(1e-6) + self._norm_mean = mean + self._norm_std = std + def _extract_state_dict(self, ckpt): """ Aceita: @@ -86,7 +132,7 @@ class SegformerNavRunner: return model def normalize_img(self, img: torch.Tensor) -> torch.Tensor: - return (img - self.IMAGENET_MEAN.to(img.device)) / self.IMAGENET_STD.to(img.device) + return (img - self._norm_mean.to(img.device)) / self._norm_std.to(img.device) def compute_roi_indices(self, H: int, zona_inicio: float, faixa_atuacao: float): y_inicio = int((1.0 - zona_inicio) * H) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py index 08dfa7607..7fadcc896 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py @@ -30,6 +30,7 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) ang_max = _operacao.get("Dir", {}).get("angulo_max", 30) status_ipb = StatusModulo(ContextoGlobalRedis.get_modulo(T_Code.Ipb).get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) + debug_mode = _operacao.get("debug_mode", False) status_carro = StatusCarroMapa.Parado reduzir_para_pulverizar = False @@ -47,7 +48,7 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) velocidade_sp = vel_com_ervas - if status_ipb != StatusModulo.OPERANTE: + if status_ipb != StatusModulo.OPERANTE and not debug_mode: velocidade_sp = vel_min elif status_carro == StatusCarroMapa.Parado: velocidade_sp = 0 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 2a2d5edc2..7d169499b 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 @@ -40,11 +40,11 @@ class ControladorMPC: 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_min = parametros_mpc.get("horizonte_min") - self.horizonte_max = parametros_mpc.get("horizonte_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_ @@ -670,11 +670,10 @@ class ControladorMPC: status_carro = contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value) idx_proximo_ponto_real = contexto.get("Carro", {}).get("IdxProximoPonto", 0) idx_ponto_alvo = contexto.get("Carro", {}).get("IdxPontoAlvo", 0) - Nmax = 20 if status_carro == StatusCarroMapa.CaminhandoRua.value else 60 + Nmax = 30 if status_carro == StatusCarroMapa.CaminhandoRua.value else 60 self.tempo_execucao_local = contexto.get("Carro", {}).get("TempoEntreComandos", 0.5) - self.passos_horizonte_local = max(1, math.floor(1 / self.tempo_execucao_local)) - self.passos_horizonte_local = 1 + #self.passos_horizonte_local = max(1, math.floor(1 / self.tempo_execucao_local)) # -------------------- Correção por latência (vida real) -------------------- # Mantém correção com 'self.dt' e 'velocidade' REAIS @@ -702,7 +701,7 @@ class ControladorMPC: idx_alvo_correcao = self._corrigir_pontos_visitados(x, y, self.visitados_execucao, idx_proximo_ponto_real) # -------------------- Planejamento: horizonte ESPACIAL fixo -------------------- - S_ALVO = self.horizonte_max + S_ALVO = float(self.horizonte) V_FLOOR = float(getattr(self, "_vmin_planejamento", 0.30)) DT_MIN = float(getattr(self, "_dt_pred_min", 0.05)) DT_MAX = float(getattr(self, "_dt_pred_max", 0.25)) @@ -711,25 +710,32 @@ class ControladorMPC: v_sim = max(velocidade, V_FLOOR) + # dt de controle real: tempo minimo entre comandos executados no robo + dt_ctrl = float(self.tempo_execucao_local) + dt_ctrl = max(DT_MIN, dt_ctrl) + # orçamento de avaliações (continua igual, para pacing) est_ms = max(1.0, float(getattr(self, "_t_est_passo_ms", 6.0))) exp_budget = max(4, int(ms_left() // est_ms)) - # >>> NOVO: limites teóricos de K impostos por DT_MAX/DT_MIN - K_lb = max(1, int(math.ceil(S_ALVO / max(1e-6, v_sim * DT_MAX)))) # mínimo p/ cobrir S - K_ub = max(K_lb, int(math.floor(S_ALVO / max(1e-6, v_sim * DT_MIN)))) # cima (granularidade) + # Horizonte em numero de comandos K, coerente com S_ALVO, v_sim e dt_ctrol + K_float = S_ALVO / max(1e-6, v_sim * dt_ctrl) + K_nom = int(max(1, math.ceil(K_float))) + K = min(Nmax, K_nom) - # >>> NOVO: NÃO deixe Nmax te travar abaixo do necessário - # se quiser ainda respeitar Nmax, aumente-o dinamicamente: - Nmax = max(Nmax, K_lb) - # teto “de intenção” (não crítico, só para não explodir absurdamente) - K_cap = min(Nmax, max(K_ALVO, K_lb)) + ## >>> NOVO: limites teóricos de K impostos por DT_MAX/DT_MIN + #K_lb = max(1, int(math.ceil(S_ALVO / max(1e-6, v_sim * DT_MAX)))) # mínimo p/ cobrir S + #K_ub = max(K_lb, int(math.floor(S_ALVO / max(1e-6, v_sim * DT_MIN)))) # cima (granularidade) + ## >>> NOVO: NÃO deixe Nmax te travar abaixo do necessário + ## se quiser ainda respeitar Nmax, aumente-o dinamicamente: + #Nmax = max(Nmax, K_lb) + ## teto “de intenção” (não crítico, só para não explodir absurdamente) + #K_cap = min(Nmax, max(K_ALVO, K_lb)) + ## >>> escolha K dentro do [K_lb, K_cap]; pacing do orçamento vem depois + #K = max(K_lb, min(K_cap, K_ALVO)) - # >>> escolha K dentro do [K_lb, K_cap]; pacing do orçamento vem depois - K = max(K_lb, min(K_cap, K_ALVO)) - - beam_traj_min = int(getattr(self, "_beam_traj_min", 3)) + beam_traj_min = int(self._beam_traj_min) # custo bruto para levar beam_traj_min trajetórias até K passos exp_min_full = beam_traj_min * K if exp_budget < exp_min_full: @@ -742,8 +748,9 @@ class ControladorMPC: return comando_anterior # dt para que K passos cubram S (agora garantido dentro de [DT_MIN, DT_MAX]) - dt_pred = (S_ALVO / (v_sim * K)) - dt_pred = max(DT_MIN, min(dt_pred, DT_MAX)) + #dt_pred = (S_ALVO / (v_sim * K)) + #dt_pred = max(DT_MIN, min(dt_pred, DT_MAX)) + dt_pred = dt_ctrl self.qtd_comandos_sucessivos = K @@ -1815,11 +1822,13 @@ class ControladorMPC: # omega e sub-stepping omega_const = self.gps_handler.calcular_omega(v_planejado, angulo_testado, tipo) - passos_local = max(1, int(getattr(self, "passos_horizonte_local", 1))) - dt_total = float(dt_pred) * passos_local - - SUB_MAX = float(getattr(self, "_dt_sub_max", 0.15)) # ~150 ms por subpasso (ajuste teu comentário) - n_subs = max(1, int(math.ceil(dt_total / SUB_MAX))) + #passos_local = max(1, int(getattr(self, "passos_horizonte_local", 1))) + #dt_total = float(dt_pred) * passos_local + #SUB_MAX = float(getattr(self, "_dt_sub_max", 0.15)) # ~150 ms por subpasso (ajuste teu comentário) + #n_subs = max(1, int(math.ceil(dt_total / SUB_MAX))) + dt_total = dt_pred + n_subs = max(1, self._n_subporpasso) + sub_dt = dt_total / n_subs dist_sub = v_planejado * sub_dt # custo por metro diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py index f09f7edc9..4ad3ae4f8 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py @@ -57,6 +57,7 @@ class StatusCarroMapa(IntEnum): Manobrando = 4 Direcionando = 5 RetornandoBase = 6 + Indefinido = 7 class T_Code(IntEnum): Vzo = -1 diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py index 74bfc3041..0b2949850 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py @@ -81,6 +81,7 @@ def load_seg_config(force_reload=False): } _CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ruas_seg") _CONFIG_CACHE["ia_labelmap_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_labelmap_ruas_seg") + _CONFIG_CACHE["ia_norm_stats_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_norm_stats_ruas_seg") _CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ruas_seg") return _CONFIG_CACHE diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py index 343d90374..4380d3da0 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py @@ -191,7 +191,7 @@ class SegmentacaoManager: lat_out = round(float(self._ema_lat), 3) mask_nav = (self.predictions == ClassesSegmentacao.NAVEGAVEL.value) - status_now, status_final, status_before, prob, debug = self.classificar_status_corredor(mask_nav) + status_now, status_final, status_before, debug = self.classificar_status_corredor(mask_nav) return { "timestamp": time.time(), @@ -201,6 +201,7 @@ class SegmentacaoManager: "erro_lateral_pct": lat_out, "status_corredor": status_final.value, "status_corredor_anterior": status_before.value, + "status_corredor_debug": debug, "centros_corredor": centros, "larguras_px": larguras, "confianca": round(float(score), 3), @@ -355,103 +356,300 @@ class SegmentacaoManager: def classificar_status_corredor(self, mask_nav: np.ndarray): """ mask_nav: (H,W) com 1 = navegável, 0 = não-navegável + Retorna: - status_now : StatusCarroMapa - status_final : StatusCarroMapa (igual ao now, sem histerese por enquanto) - probs : dict[StatusCarroMapa, float] (aqui só 1.0 pro escolhido) - debug : métricas pra log + status_now : StatusCarroMapa (instantâneo deste frame) + status_final : StatusCarroMapa (com histerese) + probs : dict[StatusCarroMapa, float] (one-hot) + debug : métricas pra log/diagnóstico """ + if mask_nav is None or mask_nav.size == 0: + status_now = StatusCarroMapa.Indefinido + self._status_hist.append((status_now, self._now())) + status_final = self._maioria_ultimos() + self._status_final_hist.append(status_final) + status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final + return status_now, status_final, status_before, { + "nav_global": 0.0, + "nav_near": 0.0, + "nav_mid": 0.0, + "nav_far": 0.0, + } + H, W = mask_nav.shape nav = (mask_nav > 0).astype(np.float32) - def faixa_mean(y0, y1): + def faixa_mean(y0: int, y1: int) -> float: fatia = nav[y0:y1, :] if fatia.size == 0: return 0.0 return float(fatia.mean()) - # corta em 3 faixas: far (topo), mid (meio), near (embaixo) + # 3 faixas verticais: far (topo), mid (meio), near (embaixo) y_far_top = 0 - y_far_bot = int(0.2 * H) + y_far_bot = int(0.25 * H) y_mid_top = y_far_bot - y_mid_bot = int(0.4 * H) + y_mid_bot = int(0.5 * H) y_near_top = y_mid_bot y_near_bot = H - nav_far = faixa_mean(y_far_top, y_far_bot) - nav_mid = faixa_mean(y_mid_top, y_mid_bot) - nav_near = faixa_mean(y_near_top, y_near_bot) - nav_global = float(nav.mean()) if nav.size > 0 else 0.0 + nav_far = faixa_mean(y_far_top, y_far_bot) + nav_mid = faixa_mean(y_mid_top, y_mid_bot) + nav_near = faixa_mean(y_near_top, y_near_bot) + nav_global = float(nav.mean()) - # limiares & delta - THR_NAV_ALTO = 0.95 # "quase tudo navegável" - THR_NAV_BAIXO = 0.30 # "quase nada navegável" - THR_NEAR_ALTO = 1.00 # near "100%" - DELTA = 0.05 # diferença mínima pra considerar > de verdade + # diferenças entre faixas (pra medir quão "desbalanceado" está) + d_nm = abs(nav_near - nav_mid) + d_mf = abs(nav_mid - nav_far) + d_nf = abs(nav_near - nav_far) + max_delta = max(d_nm, d_mf, d_nf) - def maior_que(a, b): - return a > min(b - DELTA, 1.0) - - def maior_igual_que(a, b): - return a >= min(b - DELTA, 1.0) + # ---- Blobs 2D na região distante (parede esquerda x direita) ---- + y_blob_top = 0 + y_blob_bot = int(0.35 * H) # um pouco mais profundo que o nav_far - status = None + faixa_obs = (mask_nav[y_blob_top:y_blob_bot, :] == 0).astype(np.uint8) # 1 = obstáculo - # 1) PARADO: quase todo frame não navegável - if nav_global < THR_NAV_BAIXO: + H_blob, W_blob = faixa_obs.shape + num_blobs_far = 0 + corridor_nav_far = 0.0 + corridor_width_frac = 0.0 + + if H_blob > 0 and W_blob > 0 and faixa_obs.max() > 0: + # connectedComponents espera 0/255 + faixa_obs_bin = (faixa_obs * 255).astype(np.uint8) + + num_labels, labels = cv2.connectedComponents(faixa_obs_bin) + + # ignora blobs muito pequenos (ruído) + MIN_AREA = 0.005 * H_blob * W_blob # 0.5% da área da faixa + blobs = [] + + for label in range(1, num_labels): # 0 é o fundo + ys, xs = np.where(labels == label) + area = len(xs) + if area < MIN_AREA: + continue + + x_min, x_max = xs.min(), xs.max() + y_min, y_max = ys.min(), ys.max() + blobs.append({ + "area": area, + "bbox": (x_min, y_min, x_max, y_max), + "x_center": float(xs.mean()), + }) + + num_blobs_far = len(blobs) + + if num_blobs_far >= 2: + # ordena da parede mais à esquerda pra mais à direita + blobs_sorted = sorted(blobs, key=lambda b: b["x_center"]) + left_blob = blobs_sorted[0] + right_blob = blobs_sorted[-1] + + # corredor é a região entre x_max da esquerda e x_min da direita + x_left = left_blob["bbox"][2] + 1 # x_max (inclusivo) -> +1 pra slice + x_right = right_blob["bbox"][0] # x_min + + if x_right > x_left: + corridor_width = x_right - x_left + corridor_width_frac = corridor_width / float(W) + + faixa_nav_corridor = (mask_nav[y_blob_top:y_blob_bot, x_left:x_right] > 0).astype(np.float32) + if faixa_nav_corridor.size > 0: + corridor_nav_far = float(faixa_nav_corridor.mean()) + + # ===== Limiares (podemos tunar depois) ===== + THR_BLOCKED = 0.30 # global bem baixo -> quase sem caminho + THR_OPEN = 0.90 # global bem alto -> mundo aberto + THR_DELTA_COR = 0.15 # diferença "significativa" entre faixas + THR_COR_FAR_LARGO = 0.65 # far ainda relativamente alto em corredor largo + THR_OPEN_MUITO_LIMPO = 0.90 # quase tudo navegável + THR_DELTA_OBST_PEQ = 0.20 # desbalance vertical máximo para considerar "campo aberto com obstáculo pequeno" + MIN_CORRIDOR_WIDTH_FRAC = 0.10 # corredor tem que ter pelo menos ~10% da largura + MAX_CORRIDOR_WIDTH_FRAC = 0.70 # evita chamar de corredor quando é um "campo" gigante + + # 1) Parado: quase não há caminho à frente (meio+frente mortos) + cond_parado = ( + nav_global <= THR_BLOCKED + and nav_mid < 0.20 + and nav_far < 0.10 + ) + + # 2) Direcionando: mundo aberto, sem corredor marcado + # 2.1 base: tudo alto e muito homogêneo + cond_direcionando_base = ( + nav_global >= THR_OPEN + and max_delta < 0.10 + ) + + # 2.2 modo "campo aberto com obstáculo pequeno": + # quase tudo navegável, e até o far é bem alto + cond_direcionando_obst_peq = ( + nav_global >= THR_OPEN_MUITO_LIMPO and # >= 0.90 + nav_near >= 0.95 and + nav_mid >= 0.80 and # um pouco mais permissivo + nav_far >= 0.50 and # aceita far um pouco mais fechado + num_blobs_far <= 1 # no máximo UMA parede grande + ) + + # 2.3 modo "campo aberto com parede na frente": + # chão bem navegável perto, sem corredor definido, e FAR quase todo bloqueado + cond_direcionando_frente_fe_chada = ( + nav_near >= 0.80 and # perto bem aberto + nav_mid >= 0.30 and # meio ainda razoável + nav_far <= 0.10 and # topo praticamente bloqueado (parede) + nav_global >= 0.50 and # ainda tem bastante área navegável no frame + num_blobs_far <= 1 # no máximo uma "parede", nada de corredor + ) + + # 2.3 modo "campo aberto com borda lateral": + # cena razoavelmente aberta, uma parede forte de um lado, mas sem corredor fechado + cond_direcionando_borda_lateral = ( + nav_global >= 0.60 and # já tem boa área navegável + nav_near >= 0.70 and + nav_mid >= 0.50 and + nav_far >= 0.40 and # far não está "morrendo", só mais sujo + nav_far <= 0.80 and # não é mundão 100% limpo + num_blobs_far == 1 # exatamente UMA parede grande + ) + + cond_direcionando_aberto = ( + nav_global >= 0.75 and + nav_near >= 0.70 and + nav_mid >= 0.70 and + nav_far >= 0.70 and + max_delta <= 0.12 and + num_blobs_far <= 1 + ) + + cond_direcionando = ( + cond_direcionando_base + or cond_direcionando_obst_peq + or cond_direcionando_frente_fe_chada + or cond_direcionando_borda_lateral + or cond_direcionando_aberto + ) + + # 3) CaminhandoRua: dentro do corredor "clássico" + cond_caminhando_base = ( + nav_near >= 0.55 and + nav_mid >= 0.25 and + nav_far <= 0.50 and + (nav_near - nav_far) >= 0.20 and + num_blobs_far >= 2 # precisa de DUAS paredes + ) + + # 3.1 CaminhandoRua em corredor mais largo, com parede só de um lado + # enquadra bem os casos: + # nav_global ~0.66–0.77, near ~0.75–0.88, mid ~0.57–0.70, far ~0.56–0.63 + cond_caminhando_largo = ( + nav_global >= 0.60 and + nav_near >= 0.75 and + nav_mid >= 0.50 and + nav_far >= 0.50 and + nav_far <= THR_COR_FAR_LARGO and + (nav_near - nav_far) >= THR_DELTA_COR and + num_blobs_far >= 2 # corredor largo, mas ainda corredor + ) + + cond_caminhando_multi_corredores = ( + nav_global >= 0.50 and # tem chão suficiente + nav_mid >= 0.55 and # meio bem limpo + nav_near >= 0.50 and # perto também ok + num_blobs_far >= 2 and # pelo menos duas "paredes" + corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and + corridor_width_frac <= MAX_CORRIDOR_WIDTH_FRAC and + corridor_nav_far >= 0.55 # corredor entre paredes bem navegável + ) + + cond_caminhando = ( + cond_caminhando_base + or cond_caminhando_largo + or cond_caminhando_multi_corredores + ) + + cond_entrando = ( + nav_global >= 0.75 and + nav_near >= 0.90 and + nav_mid >= 0.60 and + nav_far >= 0.40 and + nav_far <= 0.85 and + (nav_near - nav_far) >= 0.10 and # far mais fechado que near + num_blobs_far >= 2 and # duas paredes detectadas + corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and + corridor_nav_far >= 0.60 # corredor entre as paredes bem navegável + ) + + # 5) SaindoRua + cond_saindo_1 = ( + nav_near >= 0.50 and + nav_mid >= 0.25 and + nav_far >= 0.55 and + (nav_far - nav_mid) >= 0.10 and + nav_far >= nav_near - 0.15 + ) + + cond_saindo_2 = ( + nav_global >= 0.75 and + nav_near >= 0.70 and + nav_mid >= 0.60 and + nav_far >= 0.75 and + nav_far >= nav_mid + ) + + cond_saindo = cond_saindo_1 or cond_saindo_2 + + # ===== Decisão (ordem importa!) ===== + if cond_parado: status = StatusCarroMapa.Parado - - # 2) DIRECIONANDO: quase todo frame navegável - elif nav_global > THR_NAV_ALTO: + elif cond_direcionando: status = StatusCarroMapa.Direcionando - + elif cond_saindo: + status = StatusCarroMapa.SaindoRua + elif cond_entrando: + status = StatusCarroMapa.EntrandoRua + elif cond_caminhando: + status = StatusCarroMapa.CaminhandoRua else: - # 3) ENTRANDO RUA - cond_near_alto = nav_near >= THR_NEAR_ALTO - cond_near_gt_mid = maior_igual_que(nav_near, nav_mid) - cond_mid_gt_far = maior_que(nav_mid, nav_far) - - # 4) SAINDO RUA - cond_near_gt_mid2 = True or maior_que(nav_near, nav_mid) - cond_far_gt_mid = maior_que(nav_far, nav_mid) - - if cond_near_alto and cond_near_gt_mid and cond_mid_gt_far: - status = StatusCarroMapa.EntrandoRua - - elif cond_near_gt_mid2 and cond_far_gt_mid: - status = StatusCarroMapa.SaindoRua - - # 5) CAMINHANDO RUA (cone "normal" NEAR > MID > FAR) - elif cond_near_gt_mid and cond_mid_gt_far: - status = StatusCarroMapa.CaminhandoRua - - else: - # fallback: se ficar numa zona cinza, chama de Direcionando - status = StatusCarroMapa.Manobrando - - # monta probs "one-hot" - probs = {s: 0.0 for s in StatusCarroMapa} - probs[status] = 1.0 + status = StatusCarroMapa.Indefinido debug = { + "nav_global": nav_global, "nav_near": nav_near, "nav_mid": nav_mid, "nav_far": nav_far, - "nav_global": nav_global, - "THR_NAV_ALTO": THR_NAV_ALTO, - "THR_NAV_BAIXO": THR_NAV_BAIXO, - "THR_NEAR_ALTO": THR_NEAR_ALTO, + "d_nm": d_nm, + "d_mf": d_mf, + "d_nf": d_nf, + "max_delta": max_delta, + "num_blobs_far": num_blobs_far, + "corridor_nav_far": corridor_nav_far, + "corridor_width_frac": corridor_width_frac, + "cond_parado": cond_parado, + "cond_direcionando_base": cond_direcionando_base, + "cond_direcionando_obst_peq": cond_direcionando_obst_peq, + "cond_caminhando_base": cond_caminhando_base, + "cond_caminhando_largo": cond_caminhando_largo, + "cond_entrando": cond_entrando, + "cond_saindo": cond_saindo, + "THR_BLOCKED": THR_BLOCKED, + "THR_OPEN": THR_OPEN, + "THR_DELTA_COR": THR_DELTA_COR, + "THR_COR_FAR_LARGO": THR_COR_FAR_LARGO, + "THR_OPEN_MUITO_LIMPO": THR_OPEN_MUITO_LIMPO, } status_now = status - + #print(status_now.name, debug) + # Histerese temporal: mantém teu esquema de histórico self._status_hist.append((status_now, self._now())) status_final = self._maioria_ultimos() self._status_final_hist.append(status_final) status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final - return status_now, status_final, status_before, probs, debug + return status_now, status_final, status_before, debug @staticmethod def _now(): diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py index 91811729f..589e364e2 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py @@ -112,8 +112,8 @@ def load_seg_config(force_reload=False): _CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ervas") _CONFIG_CACHE["ia_labelmap_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_labelmap_ervas") - _CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ervas") _CONFIG_CACHE["ia_norm_stats_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_norm_stats_ervas") + _CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ervas") _CONFIG_CACHE["faixa_atuacao_bicos"] = dadosAtu.get("percent_vertical_deteccao", 0.7) _CONFIG_CACHE["area_atuacao_bicos"] = dadosAtu.get("height_area_deteccao", 0.1) diff --git a/AgroBase/OperationControl/Services/GpsService.cs b/AgroBase/OperationControl/Services/GpsService.cs index 56e766b47..0647357bd 100644 --- a/AgroBase/OperationControl/Services/GpsService.cs +++ b/AgroBase/OperationControl/Services/GpsService.cs @@ -1016,11 +1016,11 @@ namespace OperationControl.Services LoopRTK_Ntrip = true; // Configurações do NTRIP caster para RTK2Go - string host = "gps-ntrip.ibge.gov.br"; - int port = 2101; + string host = "gps-ntrip.ibge.gov.br"; + int port = 2101; string mountpoint = "EESC0"; - string username = "Zendion"; // Geralmente vazio para RTK2Go - string password = "vyNEF$5*"; // Geralmente vazio para RTK2Go + string username = "Zendion"; // Geralmente vazio para RTK2Go + string password = "QD&m1p60"; // Geralmente vazio para RTK2Go while (CorrecaoRTK_Ntrip && APIService.HasInternet) // Loop para reconectar em caso de falha { @@ -1265,12 +1265,20 @@ namespace OperationControl.Services amostras_pos = new List(1000); inicioProcesso = DateTime.UtcNow; + DateTime? inicioTentativaFix = DateTime.UtcNow; inicioFix = null; // Você já deve ter um leitor da COM que devolve linhas NMEA. // Abaixo, vamos supor um método async que lê GGA parseado. while (ProgressoGeral < 100) { + // Verifica se o timeout tentando aplicar fix ja excedeu + if (inicioTentativaFix.Value.AddMinutes(2) < DateTime.UtcNow && inicioFix is null) + { + Models.Variaveis.MostrarLog("Timeout ao tentar receber fix atraves do Ntrip..."); + break; + } + // Lê próxima sentença (bloqueante/assíncrono) var gga = await LerProximoGgaAsync(); // implemente no seu stack @@ -1280,6 +1288,7 @@ namespace OperationControl.Services if (!FixLiberado || !new List() { TiposCorrecaoGPS.RTKFixo }.Contains(gga.FixQuality)) { inicioFix = null; // reset + inicioTentativaFix = DateTime.UtcNow; continue; } @@ -1302,7 +1311,7 @@ namespace OperationControl.Services } // ===== 2.1) Ajuste a assinatura se quiser passar timeout e CT de fora - private async Task LerProximoGgaAsync(int timeoutMs = 5000, CancellationToken ct = default) + private async Task LerProximoGgaAsync(int timeoutMs = 5000, CancellationToken ct = default) { var sw = System.Diagnostics.Stopwatch.StartNew(); int startHb = System.Threading.Volatile.Read(ref _lastReadHeartbeat); @@ -1315,7 +1324,7 @@ namespace OperationControl.Services if (sw.ElapsedMilliseconds >= timeoutMs) //throw new TimeoutException("Timeout aguardando nova leitura GGA."); - return new GgaFix(DateTime.UtcNow, 0, 0, 0, TiposCorrecaoGPS.SemCorrecao); + return null; await Task.Delay(75, ct).ConfigureAwait(false); }