diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index 56578c0b4..8b709955d 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -114,22 +114,41 @@ namespace AgroBase.Models private int MaxPontosEntradaRecuperacaoTransicao { get; set; } = 12; private int ConfirmacoesMinimasEntradaProximoCorredor { get; set; } = 2; - // V11 - watchdog de progresso da trajetória. + // V14 - recuperação por ancora futura na transição entre corredores. + // Se LigacaoEntrada for perdida por uma curva real mais aberta que a + // Dubins nominal, podemos provar a passagem usando um segmento futuro + // da segunda metade da manobra, desde que pose, heading e continuidade + // temporal sejam compatíveis. A visita normal continua estrita. + private int LookAheadPontosRecuperacaoTransicao { get; set; } = 3; + private double ToleranciaLateralRecuperacaoTransicaoM { get; set; } = 1.25; + private double ErroOrientacaoMaximoRecuperacaoTransicaoGraus { get; set; } = 35.0; + private double ToleranciaParametroRecuperacaoTransicao { get; set; } = 0.25; + private int SaltoMaximoAncoraRecuperacaoTransicao { get; set; } = 3; + + // V12 - watchdog de progresso da trajetória. // - // Filosofia: - // - NÃO julga saúde por repetição de comando/ângulo. Uma rua reta pode - // operar por muito tempo com ângulo 0 e velocidade constante. - // - julga progresso FÍSICO + GEOMÉTRICO: o rover precisa estar andando - // e, ao mesmo tempo, reduzir a distância restante / distância ao - // próximo ponto ou avançar o prefixo autoritativo. - // - Suspeita é passiva. Reaquisição apenas limita a velocidade e dá - // tempo para as recuperações estritas já existentes agirem. - // - se mesmo assim não houver progresso, a saída é falha fechada: - // velocidade 0 + operação não liberada. + // Princípios: + // - avanço autoritativo de idxPonto SEMPRE é progresso; + // - enquanto o idx não muda, aproximar-se de ProximoPonto é progresso; + // - afastar-se persistentemente de ProximoPonto é evidência de perda; + // - pausa/freio/velocidade zero NÃO apagam a investigação em andamento; + // - troca de StatusCarroMapa NÃO é progresso e NÃO reinicia a janela; + // - o watchdog NÃO escreve liberado/status/velocidade diretamente no Redis; + // - ao concluir que a navegação é insegura, solicita op.DefinirPausa(true); + // - contexto_global_redis continua sendo a única autoridade de liberação, + // status e travamento operacional; + // - a retomada humana (Pausa true -> false) rearma uma janela nova sobre + // a pose/índice atuais. Se continuar ruim, o watchdog pausa novamente. + // + // Observação importante: + // PontoAtual é o último ponto autoritativamente visitado. Portanto a + // distância geométrica usada como alvo é ProximoPonto. A distância a + // PontoAtual tende naturalmente a CRESCER quando o rover progride. public bool WatchdogProgressoHabilitado { get; set; } = true; public double WatchdogVelocidadeMinimaMs { get; set; } = 0.12; - public double WatchdogProgressoRestanteMinimoM { get; set; } = 0.22; public double WatchdogProgressoAlvoMinimoM { get; set; } = 0.15; + public double WatchdogAfastamentoAlvoSuspeitaM { get; set; } = 0.60; + public double WatchdogAfastamentoAlvoReaquisicaoM { get; set; } = 1.00; public double WatchdogTempoMinimoSuspeitaS { get; set; } = 2.50; public double WatchdogTempoMinimoReaquisicaoS { get; set; } = 3.75; public double WatchdogTempoMaximoReaquisicaoS { get; set; } = 4.00; @@ -138,6 +157,7 @@ namespace AgroBase.Models public double WatchdogDistanciaMinimaMovimentoM { get; set; } = 0.15; public double WatchdogAnguloMinimoInversaoGraus { get; set; } = 3.0; public int WatchdogInversoesParaReaquisicao { get; set; } = 4; + public double WatchdogIntervaloTraceS { get; set; } = 1.0; #endregion @@ -159,14 +179,29 @@ namespace AgroBase.Models [JsonProperty] public double WatchdogDistanciaFisicaJanelaM { get; private set; } = 0.0; + [JsonProperty] + public double WatchdogTempoAtivoJanelaS { get; private set; } = 0.0; + + [JsonProperty] + public double WatchdogDistanciaAlvoAtualM { get; private set; } = double.NaN; + + [JsonProperty] + public double WatchdogMelhorDistanciaAlvoM { get; private set; } = double.NaN; + + [JsonProperty] + public double WatchdogAfastamentoAlvoM { get; private set; } = 0.0; + [JsonProperty] public int WatchdogInversoesAnguloJanela { get; private set; } = 0; - [JsonIgnore] - private DateTime _watchdogInicioJanelaUtc = DateTime.MinValue; + [JsonProperty] + public bool WatchdogPausado { get; private set; } = false; + + [JsonProperty] + public bool WatchdogPausaSolicitada { get; private set; } = false; [JsonIgnore] - private DateTime _watchdogInicioReaquisicaoUtc = DateTime.MinValue; + private double _watchdogTempoAtivoInicioReaquisicaoS = 0.0; [JsonIgnore] private double _watchdogDistanciaFisicaInicioReaquisicaoM = 0.0; @@ -190,7 +225,19 @@ namespace AgroBase.Models private double _watchdogDistanciaRestanteRefM = double.NaN; [JsonIgnore] - private double _watchdogDistanciaProximoRefM = double.NaN; + private double _watchdogDistanciaAlvoRefM = double.NaN; + + [JsonIgnore] + private double _watchdogDistanciaAlvoAnteriorM = double.NaN; + + [JsonIgnore] + private double _watchdogLatAlvoRef = double.NaN; + + [JsonIgnore] + private double _watchdogLonAlvoRef = double.NaN; + + [JsonIgnore] + private int _watchdogAfastamentosConsecutivos = 0; [JsonIgnore] private double _watchdogUltimoAnguloComandoGraus = 0.0; @@ -211,10 +258,16 @@ namespace AgroBase.Models private double _watchdogUltimaLongitudeGps = double.NaN; [JsonIgnore] - private DateTime _watchdogUltimaReducaoVelocidadeUtc = DateTime.MinValue; + private DateTime _watchdogPausaSolicitadaEmUtc = DateTime.MinValue; [JsonIgnore] - private DateTime _watchdogUltimaReafirmacaoParadaUtc = DateTime.MinValue; + private DateTime _watchdogUltimoTickUtc = DateTime.MinValue; + + [JsonIgnore] + private DateTime _watchdogUltimoTraceUtc = DateTime.MinValue; + + [JsonIgnore] + private string _watchdogMotivoPausa = string.Empty; public DateTime UltimaAtualizacaoDados { get; set; } = DateTime.MinValue; public bool RetornandoBase { get; set; } @@ -226,6 +279,9 @@ namespace AgroBase.Models [JsonIgnore] private int _confirmacoesCandidatoTransicao = 0; + [JsonIgnore] + private int _idxPontoCandidatoTransicao = -1; + [JsonIgnore] private GPSModel _amostraGpsCandidatoTransicao; @@ -1029,18 +1085,80 @@ namespace AgroBase.Models } } - private void ReiniciarJanelaWatchdog( - EstadoWatchdogTrajetoria novoEstado, - string motivo, - bool registrarRecuperacao = false + private void RegistrarEventoWatchdog(string mensagem) + { + Variaveis.MostrarLog("[TRJ/WATCHDOG][EVENT] " + mensagem); + } + + private void RegistrarTraceWatchdog( + DateTime agoraUtc, + string regime, + double velocidadeComandadaMs, + double passoFisico, + double distanciaProximo ) { - var estadoAnterior = WatchdogEstado; + double intervalo = Math.Max(0.25, WatchdogIntervaloTraceS); + if (_watchdogUltimoTraceUtc != DateTime.MinValue && + (agoraUtc - _watchdogUltimoTraceUtc).TotalSeconds < intervalo) + { + return; + } - _watchdogInicioJanelaUtc = DateTime.UtcNow; - _watchdogInicioReaquisicaoUtc = DateTime.MinValue; - _watchdogDistanciaFisicaInicioReaquisicaoM = 0.0; + _watchdogUltimoTraceUtc = agoraUtc; + var controle = op?.Controle; + bool manualFlag = op?.Parametros?.Controle?.ControleManualAcionado == true; + + double melhoraAlvo = + ValorFinito(_watchdogDistanciaAlvoRefM) && ValorFinito(distanciaProximo) + ? _watchdogDistanciaAlvoRefM - distanciaProximo + : double.NaN; + + double melhoraRestante = + ValorFinito(_watchdogDistanciaRestanteRefM) && ValorFinito(DistanciaRestante) + ? _watchdogDistanciaRestanteRefM - DistanciaRestante + : double.NaN; + + Variaveis.MostrarLog( + $"[TRJ/WATCHDOG][TRACE] estado={WatchdogEstado}, pausado={WatchdogPausado}, " + + $"pausaOp={op?.Sensoriamento?.Operacao?.Pausa ?? false}, pausaWatchdog={WatchdogPausaSolicitada}, regime={regime}, " + + $"op={op?.Sensoriamento?.Operacao?.StatusOperacaoAtual}, carro={StatusAtual}, " + + $"auto={op?.Parametros?.ControleAutomatico ?? false}, manualFlag={manualFlag}, " + + $"velPct={controle?.PercentualVelocidadeSP ?? 0:F1}, vCmd={velocidadeComandadaMs:F3}m/s, " + + $"freio={controle?.EmFreio ?? false}, idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + + $"corr={CorredorAtual?.Idx ?? -1}, qtd={_TrajetoriaFixa?.Count ?? 0}, " + + $"dAlvo={distanciaProximo:F2}m, melhor={WatchdogMelhorDistanciaAlvoM:F2}m, " + + $"afast={WatchdogAfastamentoAlvoM:F2}m, melhoraAlvo={melhoraAlvo:F2}m, " + + $"dRest={DistanciaRestante:F2}m, melhoraRest={melhoraRestante:F2}m, " + + $"fisico={WatchdogDistanciaFisicaJanelaM:F2}m, tAtivo={WatchdogTempoAtivoJanelaS:F2}s, " + + $"gpsStep={passoFisico:F3}m, ang={controle?.Angulo ?? 0:F1}deg, " + + $"inversoes={WatchdogInversoesAnguloJanela}, motivo={WatchdogMotivo}" + ); + } + + private void AtualizarTempoAtivoWatchdog(DateTime agoraUtc) + { + if (_watchdogUltimoTickUtc == DateTime.MinValue) + { + _watchdogUltimoTickUtc = agoraUtc; + return; + } + + double dt = (agoraUtc - _watchdogUltimoTickUtc).TotalSeconds; + _watchdogUltimoTickUtc = agoraUtc; + + if (!ValorFinito(dt) || dt <= 0) + return; + + // Breakpoint, suspensão de thread ou travamento momentâneo não podem + // transformar um único ciclo em vários segundos de falta de progresso. + dt = Math.Min(dt, 0.50); + WatchdogTempoAtivoJanelaS += dt; + } + + private void AtualizarReferenciasWatchdog() + { _watchdogIdxPontoAtualRef = PontoAtual?.idxPonto ?? -1; _watchdogIdxProximoPontoRef = ProximoPonto?.idxPonto ?? -1; _watchdogIdxCorredorRef = CorredorAtual?.Idx ?? -1; @@ -1048,16 +1166,46 @@ namespace AgroBase.Models _watchdogStatusRef = StatusAtual; _watchdogDistanciaRestanteRefM = DistanciaRestante; - _watchdogDistanciaProximoRefM = + + double distanciaAlvo = (GPSPosicaoAtual != null && ProximoPonto?.Posicao != null) ? GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, ProximoPonto.Posicao) : double.NaN; + _watchdogDistanciaAlvoRefM = distanciaAlvo; + _watchdogDistanciaAlvoAnteriorM = distanciaAlvo; + WatchdogDistanciaAlvoAtualM = distanciaAlvo; + WatchdogMelhorDistanciaAlvoM = distanciaAlvo; + WatchdogAfastamentoAlvoM = 0.0; + _watchdogAfastamentosConsecutivos = 0; + + _watchdogLatAlvoRef = ProximoPonto?.Posicao?.Latitude ?? double.NaN; + _watchdogLonAlvoRef = ProximoPonto?.Posicao?.Longitude ?? double.NaN; + } + + private void ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria novoEstado, + string motivo, + bool registrarRecuperacao = false + ) + { + EstadoWatchdogTrajetoria estadoAnterior = WatchdogEstado; + WatchdogDistanciaFisicaJanelaM = 0.0; + WatchdogTempoAtivoJanelaS = 0.0; WatchdogInversoesAnguloJanela = 0; + WatchdogPausado = false; + _watchdogMotivoPausa = string.Empty; + _watchdogUltimoTickUtc = DateTime.UtcNow; + _watchdogTempoAtivoInicioReaquisicaoS = 0.0; + _watchdogDistanciaFisicaInicioReaquisicaoM = 0.0; + + AtualizarReferenciasWatchdog(); _watchdogUltimoAnguloComandoGraus = op?.Controle?.Angulo ?? 0.0; - _watchdogUltimoAnguloValido = Math.Abs(_watchdogUltimoAnguloComandoGraus) >= WatchdogAnguloMinimoInversaoGraus; + _watchdogUltimoAnguloValido = + Math.Abs(_watchdogUltimoAnguloComandoGraus) >= + WatchdogAnguloMinimoInversaoGraus; WatchdogEstado = novoEstado; WatchdogMotivo = motivo ?? string.Empty; @@ -1067,27 +1215,27 @@ namespace AgroBase.Models estadoAnterior != EstadoWatchdogTrajetoria.Desarmado) { string msg = - $"[TRJ/WATCHDOG] Progresso recuperado. " + - $"estado={estadoAnterior}->Normal, " + - $"idx={_watchdogIdxPontoAtualRef}, " + - $"status={StatusAtual}."; + $"Progresso recuperado. estado={estadoAnterior}->Normal, " + + $"idx={_watchdogIdxPontoAtualRef}, prox={_watchdogIdxProximoPontoRef}, " + + $"status={StatusAtual}, motivo={motivo}."; - Variaveis.MostrarLog(msg); + RegistrarEventoWatchdog(msg); op?.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Operante, 100, - msg + "[TRJ/WATCHDOG] " + msg ); } } - private void DesarmarWatchdog(string motivo, bool limparParadaSegura) + private void DesarmarWatchdog(string motivo) { - if (WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura && - !limparParadaSegura) + if (WatchdogEstado != EstadoWatchdogTrajetoria.Desarmado) { - return; + RegistrarEventoWatchdog( + $"{WatchdogEstado}->Desarmado | motivo={motivo}" + ); } ReiniciarJanelaWatchdog( @@ -1097,6 +1245,62 @@ namespace AgroBase.Models ); } + public void RearmarWatchdogProgresso(string motivo = "rearme explícito") + { + EstadoWatchdogTrajetoria anterior = WatchdogEstado; + + // Rearme explícito mexe apenas na memória interna do watchdog. + // NÃO remove Pausa da operação. A retomada operacional continua + // sendo responsabilidade do fluxo humano/ContextoGlobalRedis. + WatchdogPausaSolicitada = false; + _watchdogPausaSolicitadaEmUtc = DateTime.MinValue; + + ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria.Desarmado, + motivo, + registrarRecuperacao: false + ); + + RegistrarEventoWatchdog( + $"REARME EXPLICITO | {anterior}->Desarmado | motivo={motivo}" + ); + } + + private void PausarWatchdog(string motivo) + { + DateTime agoraUtc = DateTime.UtcNow; + _watchdogUltimoTickUtc = agoraUtc; + + if (WatchdogPausado && _watchdogMotivoPausa == (motivo ?? string.Empty)) + return; + + WatchdogPausado = true; + _watchdogMotivoPausa = motivo ?? string.Empty; + + RegistrarEventoWatchdog( + $"PAUSE estado={WatchdogEstado}, idx={PontoAtual?.idxPonto}, " + + $"prox={ProximoPonto?.idxPonto}, fisico={WatchdogDistanciaFisicaJanelaM:F2}m, " + + $"tAtivo={WatchdogTempoAtivoJanelaS:F2}s, motivo={_watchdogMotivoPausa}" + ); + } + + private void RetomarWatchdogSeNecessario() + { + if (!WatchdogPausado) + return; + + string motivoAnterior = _watchdogMotivoPausa; + WatchdogPausado = false; + _watchdogMotivoPausa = string.Empty; + _watchdogUltimoTickUtc = DateTime.UtcNow; + + RegistrarEventoWatchdog( + $"RESUME estado={WatchdogEstado}, idx={PontoAtual?.idxPonto}, " + + $"prox={ProximoPonto?.idxPonto}, fisicoPreservado={WatchdogDistanciaFisicaJanelaM:F2}m, " + + $"tAtivoPreservado={WatchdogTempoAtivoJanelaS:F2}s, pausaAnterior={motivoAnterior}" + ); + } + private void EntrarSuspeitaWatchdog(string motivo) { if (WatchdogEstado == EstadoWatchdogTrajetoria.Suspeita || @@ -1110,48 +1314,21 @@ namespace AgroBase.Models WatchdogMotivo = motivo; string msg = - $"[TRJ/WATCHDOG] SUSPEITA de falta de progresso. " + - $"idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + + $"Normal->Suspeita | idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + $"status={StatusAtual}, fisico={WatchdogDistanciaFisicaJanelaM:F2}m, " + + $"tAtivo={WatchdogTempoAtivoJanelaS:F2}s, dAlvo={WatchdogDistanciaAlvoAtualM:F2}m, " + + $"melhor={WatchdogMelhorDistanciaAlvoM:F2}m, afast={WatchdogAfastamentoAlvoM:F2}m, " + $"inversoes={WatchdogInversoesAnguloJanela}, motivo={motivo}"; - Variaveis.MostrarLog(msg); + RegistrarEventoWatchdog(msg); op?.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Alerta, 80, - msg + "[TRJ/WATCHDOG] " + msg ); } - private void LimitarVelocidadeReaquisicaoWatchdog() - { - if ((DateTime.UtcNow - _watchdogUltimaReducaoVelocidadeUtc).TotalSeconds < 0.50) - return; - - var parametrosControle = op?.Parametros?.Controle; - var controle = op?.Controle; - - if (parametrosControle == null || controle == null) - return; - - double velocidadeSeguraPercent = parametrosControle.MovVelocidadeCErvasPercent; - - if (!ValorFinito(velocidadeSeguraPercent) || - velocidadeSeguraPercent <= 0 || - controle.PercentualVelocidadeSP <= velocidadeSeguraPercent + 0.1) - { - return; - } - - RedisService.AtualizarCampos( - CtxKey.DadosControle, - ("velocidade_sp", velocidadeSeguraPercent) - ); - - _watchdogUltimaReducaoVelocidadeUtc = DateTime.UtcNow; - } - private void EntrarReaquisicaoWatchdog(string motivo) { if (WatchdogEstado == EstadoWatchdogTrajetoria.Reaquisicao || @@ -1162,106 +1339,211 @@ namespace AgroBase.Models WatchdogEstado = EstadoWatchdogTrajetoria.Reaquisicao; WatchdogMotivo = motivo; - _watchdogInicioReaquisicaoUtc = DateTime.UtcNow; + _watchdogTempoAtivoInicioReaquisicaoS = WatchdogTempoAtivoJanelaS; _watchdogDistanciaFisicaInicioReaquisicaoM = WatchdogDistanciaFisicaJanelaM; - // Não troca status, não muda geometria e não marca waypoint. - // As recuperações estritas do fluxo normal continuam soberanas. - // A única intervenção é limitar a velocidade ao perfil seguro que - // já existe no equipamento, caso o Python esteja mandando mais. - LimitarVelocidadeReaquisicaoWatchdog(); - string msg = - $"[TRJ/WATCHDOG] REAQUISICAO armada. " + - $"idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + + $"Suspeita->Reaquisicao | idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + $"status={StatusAtual}, fisico={WatchdogDistanciaFisicaJanelaM:F2}m, " + - $"inversoes={WatchdogInversoesAnguloJanela}, motivo={motivo}"; + $"tAtivo={WatchdogTempoAtivoJanelaS:F2}s, dAlvo={WatchdogDistanciaAlvoAtualM:F2}m, " + + $"afast={WatchdogAfastamentoAlvoM:F2}m, inversoes={WatchdogInversoesAnguloJanela}, " + + $"motivo={motivo}"; - Variaveis.MostrarLog(msg); + RegistrarEventoWatchdog(msg); op?.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Alerta, 70, - msg + "[TRJ/WATCHDOG] " + msg ); } private void AcionarParadaSeguraWatchdog(string motivo) { - if (WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura) + if (WatchdogPausaSolicitada && + WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura) + { return; + } WatchdogEstado = EstadoWatchdogTrajetoria.ParadaSegura; - WatchdogMotivo = motivo; + WatchdogMotivo = motivo ?? "Watchdog de trajetória solicitou pausa"; + WatchdogPausado = true; + WatchdogPausaSolicitada = true; + _watchdogMotivoPausa = "pausa de segurança solicitada pelo watchdog"; + _watchdogPausaSolicitadaEmUtc = DateTime.UtcNow; + _watchdogUltimoTickUtc = _watchdogPausaSolicitadaEmUtc; - RedisService.AtualizarCampos( - CtxKey.DadosControle, - ("velocidade_sp", 0) - ); + // IMPORTANTE: + // o watchdog não escreve liberado/status/velocidade_sp. + // Ele apenas solicita a variável operacional de alto nível. + // ContextoGlobalRedis permanece como única autoridade para decidir + // liberação, status, motivos e travamento dos controles. + op?.DefinirPausa(true); - RedisService.AtualizarCampos( - CtxKey.DadosOperacao, - ("liberado", false), - ("motivo_nao_liberado", "Watchdog de trajetória: " + motivo) - ); + bool pausaAplicada = op?.Sensoriamento?.Operacao?.Pausa == true; - _watchdogUltimaReafirmacaoParadaUtc = DateTime.UtcNow; + // Não existe fallback escrevendo campos derivados no Redis. + // Se por qualquer anomalia a própria OperacaoModel não refletir a + // pausa, deixamos isso explícito no EVENT/TRACE para diagnóstico. + if (!pausaAplicada) + { + RegistrarEventoWatchdog( + "FALHA AO APLICAR PAUSA LOCAL via OperacaoModel.DefinirPausa(true)." + ); + } string msg = - $"[TRJ/WATCHDOG] PARADA SEGURA. " + - $"idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + + $"PAUSA DE SEGURANCA SOLICITADA | idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + $"status={StatusAtual}, fisico={WatchdogDistanciaFisicaJanelaM:F2}m, " + - $"inversoes={WatchdogInversoesAnguloJanela}, motivo={motivo}"; + $"tAtivo={WatchdogTempoAtivoJanelaS:F2}s, dAlvo={WatchdogDistanciaAlvoAtualM:F2}m, " + + $"melhor={WatchdogMelhorDistanciaAlvoM:F2}m, afast={WatchdogAfastamentoAlvoM:F2}m, " + + $"inversoes={WatchdogInversoesAnguloJanela}, pausaAplicada={pausaAplicada}, motivo={WatchdogMotivo}"; - Variaveis.MostrarLog(msg); + RegistrarEventoWatchdog(msg); op?.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Falha, 0, - msg + "[TRJ/WATCHDOG] " + msg ); } - private void ReafirmarParadaSeguraWatchdog() + private bool WatchdogAlvoMudouSemAvancoIndice() { - if ((DateTime.UtcNow - _watchdogUltimaReafirmacaoParadaUtc).TotalSeconds < 0.50) - return; + if (ProximoPonto?.Posicao == null) + return false; - RedisService.AtualizarCampos( - CtxKey.DadosControle, - ("velocidade_sp", 0) + if (!ValorFinito(_watchdogLatAlvoRef) || !ValorFinito(_watchdogLonAlvoRef)) + return false; + + return + Math.Abs(ProximoPonto.Posicao.Latitude - _watchdogLatAlvoRef) > ToleranciaCoordenada || + Math.Abs(ProximoPonto.Posicao.Longitude - _watchdogLonAlvoRef) > ToleranciaCoordenada; + } + + private void RegistrarProgressoWatchdog(string motivo) + { + EstadoWatchdogTrajetoria anterior = WatchdogEstado; + + ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria.Normal, + motivo, + registrarRecuperacao: + anterior == EstadoWatchdogTrajetoria.Suspeita || + anterior == EstadoWatchdogTrajetoria.Reaquisicao ); - - _watchdogUltimaReafirmacaoParadaUtc = DateTime.UtcNow; } private void AtualizarWatchdogProgressoTrajetoria() { - bool operacaoEmAndamento = - op?.Sensoriamento?.Operacao?.StatusOperacaoAtual == - StatusOperacao.EmAndamento; + DateTime agoraUtc = DateTime.UtcNow; bool controleAutomatico = op?.Parametros?.ControleAutomatico ?? false; - bool controleManualAcionado = op?.Parametros?.Controle?.ControleManualAcionado == true; + var dadosOperacao = op?.Sensoriamento?.Operacao; + var statusOperacao = dadosOperacao?.StatusOperacaoAtual; + bool pausaOperacao = dadosOperacao?.Pausa == true; - // Fora da operação automática, o operador tem autoridade para - // reposicionar/rearmar o equipamento. Uma nova entrada no modo - // automático começa com janela limpa. - if (!WatchdogProgressoHabilitado || - !operacaoEmAndamento || - !controleAutomatico || - controleManualAcionado) + bool operacaoEncerrada = + statusOperacao == StatusOperacao.NaoIniciado || + statusOperacao == StatusOperacao.Concluido; + + // ============================================================ + // PAUSA SOLICITADA PELO PRÓPRIO WATCHDOG + // ============================================================ + // A Pausa é o latch operacional. Enquanto ela permanecer true, + // o watchdog não disputa velocidade/status/liberação com o + // ContextoGlobalRedis e não acumula nova evidência. + if (WatchdogPausaSolicitada) { - DesarmarWatchdog( - "watchdog fora do regime automático", - limparParadaSegura: !operacaoEmAndamento || controleManualAcionado + if (pausaOperacao) + { + WatchdogEstado = EstadoWatchdogTrajetoria.ParadaSegura; + WatchdogPausado = true; + _watchdogUltimoTickUtc = agoraUtc; + + RegistrarTraceWatchdog( + agoraUtc, + "PAUSA_WATCHDOG_AGUARDANDO_RETOMADA", + 0.0, + 0.0, + WatchdogDistanciaAlvoAtualM + ); + return; + } + + // A transição Pausa true -> false é a autorização humana de + // retomada. Não reaproveitamos a evidência geométrica antiga: + // o operador pode ter reposicionado fisicamente o rover. + EstadoWatchdogTrajetoria anterior = WatchdogEstado; + WatchdogPausaSolicitada = false; + _watchdogPausaSolicitadaEmUtc = DateTime.MinValue; + + bool podeArmarAgora = + WatchdogProgressoHabilitado && + controleAutomatico && + !operacaoEncerrada && + !TrajeotiraConcluida && + _TrajetoriaFixaDefinida && + GPSPosicaoAtual != null && + GPSPosicaoAnterior != null && + PontoAtual != null && + ProximoPonto != null && + !ReferenceEquals(PontoAtual, ProximoPonto); + + ReiniciarJanelaWatchdog( + podeArmarAgora + ? EstadoWatchdogTrajetoria.Normal + : EstadoWatchdogTrajetoria.Desarmado, + "retomada humana após pausa de segurança; nova referência de progresso", + registrarRecuperacao: false + ); + + string msgRetomada = + $"RETOMADA HUMANA APOS PAUSA | {anterior}->{WatchdogEstado} | " + + $"idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + + $"dAlvo={WatchdogDistanciaAlvoAtualM:F2}m, status={StatusAtual}"; + + RegistrarEventoWatchdog(msgRetomada); + op?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Operante, + 100, + "[TRJ/WATCHDOG] " + msgRetomada + ); + + // Um ciclo limpo antes de voltar a acumular evidência. + return; + } + + // Pausa externa/base: congela a investigação sem apagá-la. + // Ao remover a pausa, RetomarWatchdogSeNecessario preserva a janela. + if (pausaOperacao) + { + PausarWatchdog("operação pausada externamente"); + RegistrarTraceWatchdog( + agoraUtc, + "PAUSA_OPERACAO", + 0.0, + 0.0, + WatchdogDistanciaAlvoAtualM ); return; } - if (WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura) + if (!WatchdogProgressoHabilitado) { - ReafirmarParadaSeguraWatchdog(); + DesarmarWatchdog("watchdog desabilitado por configuração"); + return; + } + + if (!controleAutomatico || operacaoEncerrada || TrajeotiraConcluida) + { + DesarmarWatchdog( + TrajeotiraConcluida + ? "trajetória concluída" + : "fora do regime automático definitivo" + ); return; } @@ -1270,66 +1552,138 @@ namespace AgroBase.Models GPSPosicaoAnterior == null || PontoAtual == null || ProximoPonto == null || - ReferenceEquals(PontoAtual, ProximoPonto) || - TrajeotiraConcluida || - StatusAtual == StatusCarroMapa.Parado) + ReferenceEquals(PontoAtual, ProximoPonto)) { - DesarmarWatchdog("trajetória sem condição de monitoramento", limparParadaSegura: false); + PausarWatchdog("trajetória/GPS sem condição de monitoramento"); return; } - var controle = op?.Controle; - if (controle == null || controle.EmFreio) - { - DesarmarWatchdog("movimento freado", limparParadaSegura: false); - return; - } - - double velocidadeComandadaMs = - FuncoesMatematicas.CalculaVelocidadeMsPercentual( - controle.PercentualVelocidadeSP - ); - - if (!ValorFinito(velocidadeComandadaMs) || - velocidadeComandadaMs < WatchdogVelocidadeMinimaMs) - { - DesarmarWatchdog("velocidade abaixo do limiar do watchdog", limparParadaSegura: false); - return; - } - - if (!WatchdogNovaAmostraGps()) - return; - - DateTime agoraUtc = DateTime.UtcNow; - int idxAtual = PontoAtual.idxPonto; int idxProximo = ProximoPonto.idxPonto; int idxCorredor = CorredorAtual?.Idx ?? PontoAtual.idxCorredor; int qtdPontos = _TrajetoriaFixa?.Count ?? 0; + // Nova rota substituiu a anterior. Sem Pausa solicitada pelo watchdog, + // isso reancora o supervisor na nova missão. Se o watchdog havia pedido + // Pausa, o bloco acima mantém a operação aguardando decisão humana. + bool novaTrajetoria = + _watchdogQtdPontosRef >= 0 && + ( + qtdPontos != _watchdogQtdPontosRef || + idxAtual < _watchdogIdxPontoAtualRef + ); + + if (novaTrajetoria) + { + EstadoWatchdogTrajetoria anterior = WatchdogEstado; + ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria.Normal, + "nova trajetória/reinício de prefixo autoritativo", + registrarRecuperacao: false + ); + RegistrarEventoWatchdog( + $"NOVA TRAJETORIA | {anterior}->Normal | idx={idxAtual}, prox={idxProximo}, qtd={qtdPontos}" + ); + return; + } + + // Primeiro ciclo válido da rota. + if (_watchdogIdxPontoAtualRef < 0 || _watchdogQtdPontosRef < 0) + { + ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria.Normal, + "watchdog armado sobre referência autoritativa inicial", + registrarRecuperacao: false + ); + RegistrarEventoWatchdog( + $"ARMADO | idx={idxAtual}, prox={idxProximo}, corr={idxCorredor}, qtd={qtdPontos}" + ); + return; + } + + // Regra nº 1: avanço do índice autoritativo é progresso inequívoco. + if (idxAtual > _watchdogIdxPontoAtualRef) + { + RegistrarProgressoWatchdog( + $"idx autoritativo avançou {_watchdogIdxPontoAtualRef}->{idxAtual}" + ); + return; + } + + // Se o alvo mudou sem o prefixo avançar, não chamamos isso de + // progresso. Apenas reancoramos a geometria e deixamos um evento + // explícito para diagnóstico. + if (idxProximo != _watchdogIdxProximoPontoRef || WatchdogAlvoMudouSemAvancoIndice()) + { + RegistrarEventoWatchdog( + $"ALVO MUDOU SEM AVANCO | idx={idxAtual}, prox={_watchdogIdxProximoPontoRef}->{idxProximo}, " + + $"status={_watchdogStatusRef}->{StatusAtual}. Reancorando alvo sem zerar evidência física." + ); + + _watchdogIdxProximoPontoRef = idxProximo; + _watchdogIdxCorredorRef = idxCorredor; + _watchdogStatusRef = StatusAtual; + _watchdogLatAlvoRef = ProximoPonto.Posicao.Latitude; + _watchdogLonAlvoRef = ProximoPonto.Posicao.Longitude; + + double dNovoAlvo = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, ProximoPonto.Posicao); + _watchdogDistanciaAlvoRefM = dNovoAlvo; + _watchdogDistanciaAlvoAnteriorM = dNovoAlvo; + WatchdogDistanciaAlvoAtualM = dNovoAlvo; + WatchdogMelhorDistanciaAlvoM = dNovoAlvo; + WatchdogAfastamentoAlvoM = 0.0; + _watchdogAfastamentosConsecutivos = 0; + } + + var controle = op?.Controle; + double velocidadeComandadaMs = + controle == null + ? 0.0 + : FuncoesMatematicas.CalculaVelocidadeMsPercentual( + controle.PercentualVelocidadeSP + ); + + bool operacaoEmAndamento = statusOperacao == StatusOperacao.EmAndamento; + + string motivoPausa = null; + if (controle == null) + motivoPausa = "controle indisponível"; + else if (controle.EmFreio) + motivoPausa = "movimento freado"; + else if (!ValorFinito(velocidadeComandadaMs) || velocidadeComandadaMs < WatchdogVelocidadeMinimaMs) + motivoPausa = $"velocidade abaixo do limiar ({velocidadeComandadaMs:F3}m/s)"; + + // StatusOperacao e StatusCarroMapa NÃO pausam o watchdog quando há + // comando real de movimento. Se a máquina estiver andando quando + // aparece Aguardando/Parametrizando/Parado, isso é justamente uma + // condição que queremos enxergar, não esconder. + + // ControleManualAcionado NÃO desarma mais o watchdog. Essa flag já + // apareceu stale em operação real. Ela continua visível no TRACE. + if (motivoPausa != null) + { + PausarWatchdog(motivoPausa); + RegistrarTraceWatchdog(agoraUtc, "PAUSE", velocidadeComandadaMs, 0.0, WatchdogDistanciaAlvoAtualM); + return; + } + + RetomarWatchdogSeNecessario(); + + if (!WatchdogNovaAmostraGps()) + { + RegistrarTraceWatchdog(agoraUtc, "SEM_NOVA_AMOSTRA_GPS", velocidadeComandadaMs, 0.0, WatchdogDistanciaAlvoAtualM); + return; + } + + AtualizarTempoAtivoWatchdog(agoraUtc); + double distanciaProximo = GPSUtils.DistanciaEntrePontos( GPSPosicaoAtual, ProximoPonto.Posicao ); - bool referenciaMudou = - _watchdogInicioJanelaUtc == DateTime.MinValue || - _watchdogIdxPontoAtualRef != idxAtual || - _watchdogIdxProximoPontoRef != idxProximo || - _watchdogIdxCorredorRef != idxCorredor || - _watchdogQtdPontosRef != qtdPontos || - _watchdogStatusRef != StatusAtual; - - if (referenciaMudou) - { - ReiniciarJanelaWatchdog( - EstadoWatchdogTrajetoria.Normal, - "referência autoritativa avançou ou mudou", - registrarRecuperacao: true - ); - return; - } + WatchdogDistanciaAlvoAtualM = distanciaProximo; double passoFisico = GPSUtils.DistanciaEntrePontos( @@ -1352,14 +1706,14 @@ namespace AgroBase.Models if (passoFisico > limiteSaltoGps) { - // Um salto GNSS não pode ser interpretado nem como progresso - // nem como ausência de progresso. Reinicia a janela e espera - // novas amostras coerentes. - ReiniciarJanelaWatchdog( - EstadoWatchdogTrajetoria.Normal, - $"salto GNSS ignorado ({passoFisico:F2}m)", - registrarRecuperacao: false + // Não apagamos a investigação por causa de uma única amostra + // ruim. Apenas ignoramos esse passo físico. + RegistrarEventoWatchdog( + $"SALTO GPS IGNORADO | passo={passoFisico:F2}m, limite={limiteSaltoGps:F2}m, " + + $"estadoPreservado={WatchdogEstado}, idx={idxAtual}, prox={idxProximo}" ); + _watchdogUltimoTickUtc = agoraUtc; + RegistrarTraceWatchdog(agoraUtc, "SALTO_GPS", velocidadeComandadaMs, passoFisico, distanciaProximo); return; } @@ -1387,72 +1741,93 @@ namespace AgroBase.Models _watchdogUltimoAnguloValido = true; } - double melhoraRestante = - ValorFinito(_watchdogDistanciaRestanteRefM) && ValorFinito(DistanciaRestante) - ? _watchdogDistanciaRestanteRefM - DistanciaRestante + // Regra nº 2: enquanto o idx não muda, uma nova melhor distância + // até ProximoPonto é progresso. Usamos histerese de 15 cm para + // não transformar jitter GNSS em progresso infinito. + if (!ValorFinito(WatchdogMelhorDistanciaAlvoM)) + WatchdogMelhorDistanciaAlvoM = distanciaProximo; + + double melhoriaDesdeReferencia = + ValorFinito(_watchdogDistanciaAlvoRefM) + ? _watchdogDistanciaAlvoRefM - distanciaProximo : 0.0; - double melhoraProximo = - ValorFinito(_watchdogDistanciaProximoRefM) && ValorFinito(distanciaProximo) - ? _watchdogDistanciaProximoRefM - distanciaProximo - : 0.0; - - bool progressoGeometrico = - melhoraRestante >= WatchdogProgressoRestanteMinimoM || - melhoraProximo >= WatchdogProgressoAlvoMinimoM; - - if (progressoGeometrico) + if (melhoriaDesdeReferencia >= WatchdogProgressoAlvoMinimoM) { - ReiniciarJanelaWatchdog( - EstadoWatchdogTrajetoria.Normal, - $"progresso geométrico confirmado (rest={melhoraRestante:F2}m, alvo={melhoraProximo:F2}m)", - registrarRecuperacao: true + RegistrarProgressoWatchdog( + $"aproximação do alvo confirmou progresso: " + + $"dRef={_watchdogDistanciaAlvoRefM:F2}->{distanciaProximo:F2}m" ); return; } - double tempoJanelaS = - Math.Max( - 0.0, - (agoraUtc - _watchdogInicioJanelaUtc).TotalSeconds - ); + // Tendência de afastamento: a melhor distância alcançada é memória + // monotônica. Ir embora do alvo não consegue mais ser apagado por + // uma oscilação pequena ou troca de status. + if (ValorFinito(_watchdogDistanciaAlvoAnteriorM) && + distanciaProximo > _watchdogDistanciaAlvoAnteriorM + 0.03) + { + _watchdogAfastamentosConsecutivos++; + } + else if (ValorFinito(_watchdogDistanciaAlvoAnteriorM) && + distanciaProximo < _watchdogDistanciaAlvoAnteriorM - 0.03) + { + _watchdogAfastamentosConsecutivos = 0; + } - // Caso separado: existe comando de movimento há bastante tempo, - // mas o rover praticamente não saiu do lugar. Isso é mais parecido - // com obstáculo/falha mecânica do que com erro de waypoint. - if (tempoJanelaS >= WatchdogTempoMaximoSemMovimentoS && + _watchdogDistanciaAlvoAnteriorM = distanciaProximo; + WatchdogMelhorDistanciaAlvoM = + Math.Min(WatchdogMelhorDistanciaAlvoM, distanciaProximo); + WatchdogAfastamentoAlvoM = + Math.Max(0.0, distanciaProximo - WatchdogMelhorDistanciaAlvoM); + + double distanciaMinimaSuspeita = ObterDistanciaMinimaSuspeitaWatchdogM(); + + bool afastamentoForte = + WatchdogAfastamentoAlvoM >= WatchdogAfastamentoAlvoSuspeitaM && + _watchdogAfastamentosConsecutivos >= 3; + + bool faltaProgressoPorDistancia = + WatchdogTempoAtivoJanelaS >= WatchdogTempoMinimoSuspeitaS && + WatchdogDistanciaFisicaJanelaM >= distanciaMinimaSuspeita; + + // Caso separado: há comando ativo, mas quase nenhum deslocamento. + if (WatchdogTempoAtivoJanelaS >= WatchdogTempoMaximoSemMovimentoS && WatchdogDistanciaFisicaJanelaM < WatchdogDistanciaMinimaMovimentoM) { AcionarParadaSeguraWatchdog( - $"movimento comandado por {tempoJanelaS:F1}s, mas deslocamento físico foi apenas " + - $"{WatchdogDistanciaFisicaJanelaM:F2}m" + $"movimento comandado por {WatchdogTempoAtivoJanelaS:F1}s ativos, " + + $"mas deslocamento físico foi apenas {WatchdogDistanciaFisicaJanelaM:F2}m" ); return; } - double distanciaMinimaSuspeita = - ObterDistanciaMinimaSuspeitaWatchdogM(); - if (WatchdogEstado == EstadoWatchdogTrajetoria.Normal && - tempoJanelaS >= WatchdogTempoMinimoSuspeitaS && - WatchdogDistanciaFisicaJanelaM >= distanciaMinimaSuspeita) + (faltaProgressoPorDistancia || afastamentoForte)) { EntrarSuspeitaWatchdog( - $"percorreu {WatchdogDistanciaFisicaJanelaM:F2}m sem reduzir a rota restante nem o próximo alvo" + afastamentoForte + ? $"distância ao alvo cresce persistentemente: melhor={WatchdogMelhorDistanciaAlvoM:F2}m, " + + $"atual={distanciaProximo:F2}m, afastamento={WatchdogAfastamentoAlvoM:F2}m" + : $"percorreu {WatchdogDistanciaFisicaJanelaM:F2}m em {WatchdogTempoAtivoJanelaS:F1}s ativos " + + $"sem avançar idx nem melhorar o alvo em {WatchdogProgressoAlvoMinimoM:F2}m" ); } bool muitasInversoes = - WatchdogInversoesAnguloJanela >= - WatchdogInversoesParaReaquisicao; + WatchdogInversoesAnguloJanela >= WatchdogInversoesParaReaquisicao; - double distanciaMinimaReaquisicao = - distanciaMinimaSuspeita * 1.55; + double distanciaMinimaReaquisicao = distanciaMinimaSuspeita * 1.55; + + bool afastamentoCritico = + WatchdogAfastamentoAlvoM >= WatchdogAfastamentoAlvoReaquisicaoM && + _watchdogAfastamentosConsecutivos >= 4; bool evidenciaForteReaquisicao = - tempoJanelaS >= WatchdogTempoMinimoReaquisicaoS && + WatchdogTempoAtivoJanelaS >= WatchdogTempoMinimoReaquisicaoS && ( WatchdogDistanciaFisicaJanelaM >= distanciaMinimaReaquisicao || + afastamentoCritico || ( muitasInversoes && WatchdogDistanciaFisicaJanelaM >= distanciaMinimaSuspeita * 1.20 @@ -1463,45 +1838,56 @@ namespace AgroBase.Models evidenciaForteReaquisicao) { EntrarReaquisicaoWatchdog( - muitasInversoes - ? "falta de progresso + inversões repetidas do comando direcional" - : "falta de progresso persistente apesar de deslocamento físico" + afastamentoCritico + ? "afastamento persistente e crítico do próximo ponto" + : muitasInversoes + ? "falta de progresso + inversões repetidas do comando direcional" + : "falta de progresso persistente apesar de deslocamento físico" ); } - if (WatchdogEstado != EstadoWatchdogTrajetoria.Reaquisicao) - return; - - LimitarVelocidadeReaquisicaoWatchdog(); - - double tempoReaquisicaoS = - Math.Max( - 0.0, - (agoraUtc - _watchdogInicioReaquisicaoUtc).TotalSeconds - ); - - double distanciaFisicaReaquisicaoM = - Math.Max( - 0.0, - WatchdogDistanciaFisicaJanelaM - - _watchdogDistanciaFisicaInicioReaquisicaoM - ); - - bool excedeuDistanciaReaquisicao = - distanciaFisicaReaquisicaoM >= - WatchdogDistanciaMaximaReaquisicaoM; - - bool excedeuTempoReaquisicao = - tempoReaquisicaoS >= WatchdogTempoMaximoReaquisicaoS && - distanciaFisicaReaquisicaoM >= 0.40; - - if (excedeuDistanciaReaquisicao || excedeuTempoReaquisicao) + if (WatchdogEstado == EstadoWatchdogTrajetoria.Reaquisicao) { - AcionarParadaSeguraWatchdog( - $"reaquisição não convergiu: tempo={tempoReaquisicaoS:F1}s, " + - $"deslocamento={distanciaFisicaReaquisicaoM:F2}m" - ); + double tempoReaquisicaoAtivoS = + Math.Max( + 0.0, + WatchdogTempoAtivoJanelaS - + _watchdogTempoAtivoInicioReaquisicaoS + ); + + double distanciaFisicaReaquisicaoM = + Math.Max( + 0.0, + WatchdogDistanciaFisicaJanelaM - + _watchdogDistanciaFisicaInicioReaquisicaoM + ); + + bool excedeuDistanciaReaquisicao = + distanciaFisicaReaquisicaoM >= + WatchdogDistanciaMaximaReaquisicaoM; + + bool excedeuTempoReaquisicao = + tempoReaquisicaoAtivoS >= WatchdogTempoMaximoReaquisicaoS && + distanciaFisicaReaquisicaoM >= 0.40; + + if (excedeuDistanciaReaquisicao || excedeuTempoReaquisicao) + { + AcionarParadaSeguraWatchdog( + $"reaquisição não convergiu: tempoAtivo={tempoReaquisicaoAtivoS:F1}s, " + + $"deslocamento={distanciaFisicaReaquisicaoM:F2}m, " + + $"dAlvo={distanciaProximo:F2}m, afast={WatchdogAfastamentoAlvoM:F2}m" + ); + return; + } } + + RegistrarTraceWatchdog( + agoraUtc, + "ATIVO", + velocidadeComandadaMs, + passoFisico, + distanciaProximo + ); } [JsonProperty] @@ -3090,15 +3476,13 @@ namespace AgroBase.Models } /* - * Recuperação forte de transição: + * V14 - recuperação forte de transição por âncora futura: * - * Se o rover cortou parte da curva planejada, os pontos finais - * do corredor atual podem continuar pendentes mesmo depois de o - * veículo já estar fisicamente dentro do corredor seguinte. - * - * Nesse caso é correto fechar o prefixo anterior, mas somente - * quando posição, corredor geométrico, sentido local e duas - * amostras GNSS independentes confirmarem a nova entrada. + * Se a curva física abrir e perder LigacaoEntrada, não exigimos + * mais que esse próprio ponto seja recuperável antes de olhar + * adiante. Procuramos apenas na segunda metade imediata da manobra + * e exigimos proximidade lateral, heading de look-ahead compatível + * e duas amostras GNSS independentes. */ if (TentarRecuperarTransicaoPorEntradaConfirmada( idxCorredorAtual, @@ -3203,6 +3587,7 @@ namespace AgroBase.Models { if (GPSPosicaoAtual == null || PontoAtual == null || + ProximoPonto == null || pontosProximoCorredor == null || pontosProximoCorredor.Count < 2) { @@ -3211,15 +3596,57 @@ namespace AgroBase.Models } /* - * A proximidade de um ponto isolado não basta, principalmente - * porque corredores agrícolas adjacentes podem estar a 1,5 m. - * A geometria completa precisa identificar o corredor seguinte - * como o corredor fisicamente mais próximo do centro do rover. + * V14 - a recuperação só existe na FRONTEIRA IMEDIATA entre os + * corredores. Isto evita usar uma geometria futura como atalho em + * qualquer outro trecho da missão. + * + * Caso real de campo: + * PontoAtual = LigacaoSaida (corredor N) + * ProximoPonto = LigacaoEntrada (corredor N+1) + * + * Se a curva física abriu um pouco, LigacaoEntrada pode nunca + * entrar na janela estrita de 0,80 m, embora o rover já esteja + * seguindo a segunda metade correta da Dubins. + */ + bool fronteiraImediata = + PontoAtual.idxCorredor == idxCorredorAtual && + ProximoPonto.idxCorredor == idxProximoCorredor; + + bool contextoDeManobra = + PontoAtual.PontoLigacao || + PontoAtual.PontoBorda || + PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores || + ProximoPonto.PontoLigacao || + ProximoPonto.Tipo == TipoPontoRua.CruvaEntreCorredores; + + if (!fronteiraImediata || !contextoDeManobra) + { + ResetarCandidatoTransicaoGeometrica(); + return false; + } + + /* + * Se a geometria dos corredores afirma explicitamente que o rover + * está DENTRO de outro corredor que não seja o próximo, não existe + * autorização para recuperar. Na cabeceira é normal retornar -1; + * nesse caso a própria geometria futura da Dubins é a evidência. */ int idxCorredorGeometrico = VerificaEquipamentoDentroCorredor(GPSPosicaoAtual); - if (idxCorredorGeometrico != idxProximoCorredor) + if (idxCorredorGeometrico >= 0 && + idxCorredorGeometrico != idxProximoCorredor) + { + ResetarCandidatoTransicaoGeometrica(); + return false; + } + + double orientacaoRobo = + GPSUtils.NormalizarAngulo( + GPSPosicaoAtual.AnguloCarroDefinido + ); + + if (!ValorFinito(orientacaoRobo)) { ResetarCandidatoTransicaoGeometrica(); return false; @@ -3227,25 +3654,46 @@ namespace AgroBase.Models int limite = Math.Min( pontosProximoCorredor.Count - 1, - MaxPontosEntradaRecuperacaoTransicao + Math.Max(1, MaxPontosEntradaRecuperacaoTransicao) ); - int idxUltimoRecuperavel = -1; - int quantidadePontosNovoCorredor = 0; - bool entradaFisicaConfirmada = false; - double maiorErroOrientacaoGraus = 0.0; + int melhorIdx = -1; + int melhorIndiceLocal = -1; + double melhorScore = double.MaxValue; + double melhorLateral = double.PositiveInfinity; + double melhorErroHeading = double.PositiveInfinity; + double melhorParametro = double.NaN; + string melhorTipo = ""; for (int i = 0; i < limite; i++) { var ponto = pontosProximoCorredor[i]; var proximo = pontosProximoCorredor[i + 1]; - if (ponto.idxCorredor != idxProximoCorredor || + if (ponto?.Posicao == null || + proximo?.Posicao == null || + ponto.idxCorredor != idxProximoCorredor || proximo.idxCorredor != idxProximoCorredor) { - break; + continue; } + /* + * Só aceitamos a segunda metade da manobra / entrada real. + * Não procuramos arbitrariamente no interior inteiro da rua. + */ + bool trechoElegivel = + ponto.Tipo == TipoPontoRua.LigacaoEntrada || + ponto.Tipo == TipoPontoRua.CruvaEntreCorredores || + ponto.Tipo == TipoPontoRua.BordaEntrada || + ponto.Tipo == TipoPontoRua.Rua || + proximo.Tipo == TipoPontoRua.CruvaEntreCorredores || + proximo.Tipo == TipoPontoRua.BordaEntrada || + proximo.Tipo == TipoPontoRua.Rua; + + if (!trechoElegivel) + continue; + ponto.AtualizarPropriedades( _gpsAtualCiclo, _gpsAnteriorCiclo, @@ -3264,53 +3712,143 @@ namespace AgroBase.Models GPSPosicaoAtual ); - double erroOrientacaoGraus; - - if (!PodeRecuperarPontoDeixadoParaTras( - ponto, - proximo.Posicao, - progresso, - out erroOrientacaoGraus - )) + if (!ValorFinito(progresso.avancoLongitudinalM) || + !ValorFinito(progresso.distanciaLateralM) || + !ValorFinito(progresso.parametroSegmento)) { - /* - * O prefixo é contínuo: se este ponto ainda não ficou - * para trás, nenhum ponto posterior pode ser confirmado. - */ - break; + continue; } - idxUltimoRecuperavel = ponto.idxPonto; - quantidadePontosNovoCorredor++; - maiorErroOrientacaoGraus = Math.Max( - maiorErroOrientacaoGraus, - erroOrientacaoGraus + double tolParametro = + Math.Max(0.05, ToleranciaParametroRecuperacaoTransicao); + + double toleranciaLateral = Math.Min( + ToleranciaLateralRecuperacaoTransicaoM, + ObterToleranciaLateralRecuperacao(ponto) + 0.20 ); - /* - * Ultrapassar apenas LigacaoEntrada ainda pode significar - * que o rover está executando a curva. Para fechar o corredor - * anterior ele precisa alcançar BordaEntrada ou um ponto Rua. - */ - if (ponto.Tipo == TipoPontoRua.BordaEntrada || - ponto.Tipo == TipoPontoRua.Rua) + if (progresso.distanciaLateralM > toleranciaLateral || + progresso.parametroSegmento < -tolParametro || + progresso.parametroSegmento > 1.0 + tolParametro || + progresso.avancoLongitudinalM < -0.20) { - entradaFisicaConfirmada = true; + continue; + } + + /* + * Heading de LOOK-AHEAD, e não bearing para um waypoint. + * Em curva usamos alguns pontos futuros para representar a + * direção em que a própria centerline está evoluindo. + */ + int idxLookAhead = Math.Min( + pontosProximoCorredor.Count - 1, + i + Math.Max(1, LookAheadPontosRecuperacaoTransicao) + ); + + var pontoLookAhead = pontosProximoCorredor[idxLookAhead]; + GPSModel fimHeading = + pontoLookAhead?.Posicao ?? + proximo.Posicao; + + double orientacaoLookAhead = + GPSUtils.NormalizarAngulo( + GPSUtils.CalcularOrientacao( + ponto.Posicao, + fimHeading + ) + ); + + if (!ValorFinito(orientacaoLookAhead)) + continue; + + double erroHeading = Math.Abs( + GPSUtils.CalcularDiferencaAngulo( + orientacaoLookAhead, + orientacaoRobo + ) + ); + + if (!ValorFinito(erroHeading) || + erroHeading > ErroOrientacaoMaximoRecuperacaoTransicaoGraus) + { + continue; + } + + /* + * Se o primeiro ponto ainda é a própria LigacaoEntrada, + * exigimos que o centro do rover já tenha avançado ao menos + * 20% no segmento que sai dela. Um ponto exatamente antes da + * ligação não pode ser promovido só por estar lateralmente perto. + */ + bool comprovouPassagemDaLigacao = + ponto.Tipo != TipoPontoRua.LigacaoEntrada || + i > 0 || + progresso.parametroSegmento >= 0.20; + + if (!comprovouPassagemDaLigacao) + continue; + + /* + * Fora do polígono/centro do corredor (-1), uma âncora Rua é + * aceita apenas se estiver realmente muito próxima da linha. + * Curva/Borda continuam válidas na cabeceira, onde "Dentro" + * naturalmente ainda pode ser falso. + */ + if (idxCorredorGeometrico < 0 && + ponto.Tipo == TipoPontoRua.Rua && + progresso.distanciaLateralM > 0.60) + { + continue; + } + + double score = + progresso.distanciaLateralM + + erroHeading * 0.015 + + i * 0.015; + + if (score < melhorScore) + { + melhorScore = score; + melhorIdx = ponto.idxPonto; + melhorIndiceLocal = i; + melhorLateral = progresso.distanciaLateralM; + melhorErroHeading = erroHeading; + melhorParametro = progresso.parametroSegmento; + melhorTipo = ponto.Tipo.ToString(); } } - if (!entradaFisicaConfirmada || - idxUltimoRecuperavel <= PontoAtual.idxPonto) + if (melhorIdx <= PontoAtual.idxPonto) { ResetarCandidatoTransicaoGeometrica(); return false; } - if (_idxCorredorCandidatoTransicao != idxProximoCorredor) + /* + * Confirmação temporal. O candidato pode avançar 1-3 pontos entre + * amostras enquanto o rover se move; isto ainda representa a mesma + * segunda metade da curva. Saltos grandes ou regressões reiniciam + * a confirmação para não costurar evidências diferentes. + */ + bool mesmaEvidencia = + _idxCorredorCandidatoTransicao == idxProximoCorredor && + _idxPontoCandidatoTransicao >= 0 && + melhorIdx >= _idxPontoCandidatoTransicao && + melhorIdx - _idxPontoCandidatoTransicao <= + Math.Max(1, SaltoMaximoAncoraRecuperacaoTransicao); + + if (!mesmaEvidencia) { _idxCorredorCandidatoTransicao = idxProximoCorredor; + _idxPontoCandidatoTransicao = melhorIdx; _confirmacoesCandidatoTransicao = 0; _amostraGpsCandidatoTransicao = null; + _timestampPosCandidatoTransicao = double.NaN; + _momentoCandidatoTransicao = DateTime.MinValue; + } + else + { + _idxPontoCandidatoTransicao = melhorIdx; } if (RegistrarNovaAmostraGpsCandidatoTransicao()) @@ -3318,6 +3856,16 @@ namespace AgroBase.Models _confirmacoesCandidatoTransicao++; } + Variaveis.MostrarLog( + "[TRJ/RECUP-TRANSICAO] Ancora futura candidata | " + + $"corredor={idxCorredorAtual}->{idxProximoCorredor}, " + + $"idx={melhorIdx}, tipo={melhorTipo}, local={melhorIndiceLocal}, " + + $"lateral={melhorLateral:F2}m, heading={melhorErroHeading:F1}graus, " + + $"t={melhorParametro:F2}, geom_corr={idxCorredorGeometrico}, " + + $"confirmacoes={_confirmacoesCandidatoTransicao}/" + + $"{ConfirmacoesMinimasEntradaProximoCorredor}" + ); + if (_confirmacoesCandidatoTransicao < ConfirmacoesMinimasEntradaProximoCorredor) { @@ -3327,18 +3875,28 @@ namespace AgroBase.Models int idxPontoAtualAntes = PontoAtual.idxPonto; /* - * Aqui MarcarVisitadoAte é intencional: a entrada comprovada no - * corredor imediatamente seguinte demonstra que os pontos ainda - * pendentes da curva ficaram para trás, mesmo que a curva real - * tenha sido mais fechada que a curva planejada. + * A âncora futura prova geometricamente que o prefixo até o início + * daquele segmento ficou para trás. Isto inclui uma LigacaoEntrada + * perdida, mas NÃO marca o ponto final do segmento nem pontos mais + * adiante que ainda não foram comprovados. */ - MarcarVisitadoAte(idxUltimoRecuperavel); + MarcarVisitadoAte(melhorIdx); ResetarCandidatoTransicaoGeometrica(); DefinirPontoAtual(); AtualizarCorredorAtual(); DefinirProximoPonto(); + string mensagem = + "[TRJ] Transição recuperada por âncora futura da manobra. " + + $"corredor={idxCorredorAtual}->{idxProximoCorredor}, " + + $"idx={idxPontoAtualAntes}->{melhorIdx}, " + + $"tipo={melhorTipo}, lateral={melhorLateral:F2}m, " + + $"erro_heading_lookahead={melhorErroHeading:F1}graus, " + + $"pendentes_anterior={pontosRestantesCorredorAtual}"; + + Variaveis.MostrarLog(mensagem); + Variaveis .OperacaoEmAndamento .Sensoriamento? @@ -3346,13 +3904,7 @@ namespace AgroBase.Models T_Code.Trj, StatusModulo.Alerta, 100, - "[TRJ] Transição recuperada por entrada confirmada " + - "no corredor seguinte. " + - $"corredor={idxCorredorAtual}->{idxProximoCorredor}, " + - $"idx={idxPontoAtualAntes}->{idxUltimoRecuperavel}, " + - $"pendentes_anterior={pontosRestantesCorredorAtual}, " + - $"pontos_novo={quantidadePontosNovoCorredor}, " + - $"erro_heading_max={maiorErroOrientacaoGraus:F1}graus" + mensagem ); return true; @@ -3412,6 +3964,7 @@ namespace AgroBase.Models private void ResetarCandidatoTransicaoGeometrica() { _idxCorredorCandidatoTransicao = -1; + _idxPontoCandidatoTransicao = -1; _confirmacoesCandidatoTransicao = 0; _amostraGpsCandidatoTransicao = null; _timestampPosCandidatoTransicao = double.NaN; diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index bb7358558..175e184ad 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -2,6 +2,7 @@ using AgroBase.Models.Modules; using AgroBase.Services; using System; using System.Collections.Generic; +using System.Collections.Concurrent; using System.Drawing.Imaging; using System.Drawing; using System.Linq; @@ -35,6 +36,168 @@ namespace AgroBase.Models public static bool DebugMode { get; set; } = false; public static bool Fechando { get; set; } = false; + // ============================================================================ + // LOG GLOBAL DE SESSÃO DO AGROBASE + // ============================================================================ + // Toda chamada a Variaveis.MostrarLog(...) continua indo ao console e é + // enfileirada para persistência em um arquivo único por execução do processo. + // O thread que chamou MostrarLog NUNCA faz escrita normal de disco. + // Assim, logging não entra no caminho crítico do controle. + private static readonly object _logSessaoLock = new object(); + private static readonly ConcurrentQueue _logSessaoFila = new ConcurrentQueue(); + private static readonly AutoResetEvent _logSessaoSignal = new AutoResetEvent(false); + private static StreamWriter _logSessaoWriter; + private static Thread _logSessaoThread; + private static string _caminhoLogSessao; + private static bool _logSessaoFalhou; + private static bool _logSessaoHookRegistrado; + private static volatile bool _logSessaoEncerrando; + + public static string CaminhoLogSessao + { + get { return _caminhoLogSessao ?? string.Empty; } + } + + private static void GarantirLogSessao() + { + if (_logSessaoThread != null || _logSessaoFalhou) + return; + + lock (_logSessaoLock) + { + if (_logSessaoThread != null || _logSessaoFalhou) + return; + + try + { + string pasta = Path.Combine(CaminhoLogsDispositivos, "Sessoes"); + Directory.CreateDirectory(pasta); + + DateTime agora = DateTime.Now; + int pid = Process.GetCurrentProcess().Id; + string nome = $"AgroBase_{agora:yyyyMMdd_HHmmss_fff}_PID{pid}.log"; + + _caminhoLogSessao = Path.Combine(pasta, nome); + _logSessaoWriter = new StreamWriter(_caminhoLogSessao, true); + _logSessaoWriter.AutoFlush = false; + _logSessaoEncerrando = false; + + _logSessaoWriter.WriteLine("============================================================"); + _logSessaoWriter.WriteLine("AGROBASE SESSION LOG"); + _logSessaoWriter.WriteLine($"Inicio: {agora:yyyy-MM-dd HH:mm:ss.fff}"); + _logSessaoWriter.WriteLine($"PID: {pid}"); + _logSessaoWriter.WriteLine($"Aplicacao: {NomeAplicacao}"); + _logSessaoWriter.WriteLine($"Versao: {Versao}"); + _logSessaoWriter.WriteLine($"Machine: {Environment.MachineName}"); + _logSessaoWriter.WriteLine("============================================================"); + _logSessaoWriter.Flush(); + + _logSessaoThread = new Thread(ProcessarFilaLogSessao); + _logSessaoThread.IsBackground = true; + _logSessaoThread.Name = "AgroBase.SessionLogger"; + _logSessaoThread.Start(); + + if (!_logSessaoHookRegistrado) + { + _logSessaoHookRegistrado = true; + AppDomain.CurrentDomain.ProcessExit += (sender, args) => EncerrarLogSessao(); + } + } + catch (Exception ex) + { + _logSessaoFalhou = true; + try + { + _logSessaoWriter?.Dispose(); + _logSessaoWriter = null; + Debug.WriteLine("[AGROBASE/LOGGER] Falha ao iniciar log de sessão: " + ex.Message); + } + catch { } + } + } + } + + private static void ProcessarFilaLogSessao() + { + try + { + while (!_logSessaoEncerrando || !_logSessaoFila.IsEmpty) + { + bool escreveu = false; + string linha; + + while (_logSessaoFila.TryDequeue(out linha)) + { + _logSessaoWriter?.WriteLine(linha); + escreveu = true; + } + + if (escreveu) + _logSessaoWriter?.Flush(); + + if (!_logSessaoEncerrando) + _logSessaoSignal.WaitOne(500); + } + + _logSessaoWriter?.Flush(); + } + catch (Exception ex) + { + _logSessaoFalhou = true; + try + { + Debug.WriteLine("[AGROBASE/LOGGER] Falha no writer de sessão: " + ex.Message); + } + catch { } + } + } + + public static void EncerrarLogSessao() + { + Thread thread; + + lock (_logSessaoLock) + { + if (_logSessaoThread == null && _logSessaoWriter == null) + return; + + try + { + _logSessaoFila.Enqueue( + $"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff} " + + $"[T{Thread.CurrentThread.ManagedThreadId}] [AGROBASE] [SESSION] Encerrando log." + ); + } + catch { } + + _logSessaoEncerrando = true; + _logSessaoSignal.Set(); + thread = _logSessaoThread; + } + + try + { + if (thread != null && thread != Thread.CurrentThread) + thread.Join(1500); + } + catch { } + + lock (_logSessaoLock) + { + try + { + _logSessaoWriter?.Flush(); + _logSessaoWriter?.Dispose(); + } + catch { } + finally + { + _logSessaoWriter = null; + _logSessaoThread = null; + } + } + } + public static string CaminhoSistema { get; set; } = AppDomain.CurrentDomain.BaseDirectory; public static string NomeAplicacao { @@ -784,7 +947,37 @@ namespace AgroBase.Models public static void MostrarLog(string Mensagem) { - Console.WriteLine($"[AGROBASE] {Mensagem}"); + string mensagem = Mensagem ?? string.Empty; + string linhaConsole = $"[AGROBASE] {mensagem}"; + + // Mantém o comportamento histórico do console. + Console.WriteLine(linhaConsole); + + // A thread chamadora apenas enfileira. Escrita/flush ficam no logger. + try + { + GarantirLogSessao(); + + if (_logSessaoFalhou || _logSessaoThread == null) + return; + + string linhaArquivo = + $"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff} " + + $"[T{Thread.CurrentThread.ManagedThreadId}] " + + linhaConsole; + + _logSessaoFila.Enqueue(linhaArquivo); + _logSessaoSignal.Set(); + } + catch (Exception ex) + { + // Logging nunca pode impactar controle/movimento. + try + { + Debug.WriteLine("[AGROBASE/LOGGER] Falha ao enfileirar log: " + ex.Message); + } + catch { } + } } @@ -825,8 +1018,9 @@ namespace AgroBase.Models public static double LarguraDireitaCm { get; } = 53.0; // 22 public static double ComprimentoFrenteCm { get; } = 90.0; // 7 public static double ComprimentoTrasCm { get; } = 40.0; // 107 - public static double DistanciaEntreEixosCm { get; set; } = 92.0; - public static double BitolaRodasCm { get; set; } = 75.0; + public static double DistanciaEntreEixosCm { get; } = 92.0; + public static double BitolaRodasCm { get; } = 75.0; + public static double KuDirecional { get; } = 2.65; public static double LeverArmFrontalCm { get; set; } = 37.0; // cm public static double LeverArmLateralCm { get; set; } = 0.0; // cm public static double LarguraEquipamentoMm @@ -1179,7 +1373,7 @@ namespace AgroBase.Models // ------------------------------------------------------------ // Deve ser o MESMO valor usado em: // mpc_v16 -> cinematica_4ws_ku - double Ku = 2.5; + double Ku = KuDirecional; double kappaEf = kappaGeom / diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index 480b35931..73047fdd4 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -252,6 +252,7 @@ namespace AgroBase.Services.Operadores ("serial_number", VariaveisEquipamento.Parametros.serial_number), ("pct_vel_min", VariaveisEquipamento.PercentualVelMin), ("largura", VariaveisEquipamento.LarguraEquipamentoMm / 1000.0), + ("ku_direcional", VariaveisEquipamento.KuDirecional), ("distancia_entre_eixos", VariaveisEquipamento.DistanciaEntreEixosCm / 100.0), ("qtd_bicos", VariaveisEquipamento.QuantidadeBicosPulverizadores), ("percentual_reservatorio_min", VariaveisEquipamento.PercentualReservatorioMin), @@ -389,6 +390,7 @@ namespace AgroBase.Services.Operadores tempo_entre_comandos = (op.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Enums.T_Code.Dir)?.DelayEnvioComando ?? 500) / 1000.0, horizonte = pControle.MpcHorizonte, passos_atraso = 1, + ku_direcional = VariaveisEquipamento.KuDirecional, } } ), 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 fbdda1442..ef81e9b3a 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 @@ -518,6 +518,7 @@ def _montar_contexto_direcional(*, operacao, controle, contexto_global, equipame "Equipamento": { "largura": _float(equipamento.get("largura", 0.85), 0.85), "entre_eixos": _float(equipamento.get("distancia_entre_eixos", 0.92), 0.92), + "ku_direcional": _float(equipamento.get("ku_direcional", 2.65), 2.65), "imu_roll_direita_sinal": _float(equipamento.get("imu_roll_direita_sinal", 1.0), 1.0), "dir_angulo_direita_sinal": _float(equipamento.get("dir_angulo_direita_sinal", 1.0), 1.0), "imu_dir_roll_min_aux": _float(equipamento.get("imu_dir_roll_min_aux", 6.0), 6.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 58c9d20b1..699d5ba54 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 @@ -450,6 +450,21 @@ class ControladorMPC: 1.0, min_value=0.25, max_value=2.0, ) + # V18 - handoff antecipado para o Pure Pursuit. + # + # SaindoRua ja nasce alguns metros antes da BordaSaida para reduzir a + # velocidade. Nessa fase o MPC pesado nao deve mais antecipar a curva: + # o mesmo Pure Pursuit que executa a Dubins assume cedo, segue reto + # enquanto a centerline ainda e reta e comeca a esterçar naturalmente + # quando o look-ahead passa a enxergar a ferradura. + # + # A busca por mudanca de corredor evita ativar esse handoff no fim do + # ultimo corredor, onde nao existe uma manobra Dubins seguinte. + self._dubins_handoff_busca_frente_m = _safe_float( + parametros_mpc.get("dubins_handoff_busca_frente_m", 8.0), + 8.0, min_value=2.0, max_value=20.0, + ) + # ------------------------------------------------------------------ # PATH TRACKING V2 - referência geométrica da passada # ------------------------------------------------------------------ @@ -527,6 +542,39 @@ class ControladorMPC: 3.0, min_value=0.5, max_value=8.0, ) + # ------------------------------------------------------------------ + # V19 - ENTRY CAPTURE pós-Dubins + # ------------------------------------------------------------------ + # O campo mostrou que, ao terminar a ferradura, o rover pode entrar na + # nova passada com 20-70 cm de cross-track e ainda 5-12 graus de yaw. + # A política antiga priorizava yaw e só depois liberava o crab, fazendo + # o rover consumir alguns metros dentro da rua antes de centralizar. + # + # Agora o fim do Pure Pursuit arma uma janela curta "centro primeiro": + # - se o erro lateral ainda é relevante e |heading| <= 15 graus, + # MovimentoDiagonal recebe prioridade mesmo sem heading perfeito; + # - heading muito grande continua com Arco/Dianteira normalmente; + # - assim que |e_lat| <= 10 cm, a política normal volta a mandar; + # - existe limite de distância para o latch nunca permanecer indefinido. + self._entry_capture_enabled = bool( + parametros_mpc.get("entry_capture_enabled", True) + ) + self._entry_capture_heading_max_graus = _safe_float( + parametros_mpc.get("entry_capture_heading_max_graus", 15.0), + 15.0, min_value=3.0, max_value=25.0, + ) + self._entry_capture_lateral_release_m = _safe_float( + parametros_mpc.get("entry_capture_lateral_release_m", 0.10), + 0.10, min_value=0.03, max_value=0.30, + ) + self._entry_capture_max_dist_m = _safe_float( + parametros_mpc.get("entry_capture_max_dist_m", 4.0), + 4.0, min_value=1.0, max_value=10.0, + ) + self._entry_capture_active = False + self._entry_capture_dist_m = 0.0 + self._dubins_pp_ativo_ciclo_anterior = False + self._lateral_deadband_m = _safe_float( parametros_mpc.get("lateral_deadband_m", 0.05), 0.05, min_value=0.0, max_value=0.30, @@ -699,9 +747,14 @@ class ControladorMPC: # é o oposto do comando de alto nível (efeito empilhadeira). # # O fator Ku preserva a mesma correção dinâmica usada pelo simulador C#. + #equipamento = _as_dict(_as_dict(contexto).get("Equipamento", {})) + #self._cinematica_4ws_ku = _safe_float( + # equipamento.get("ku_direcional", 2.65), + # 2.65, min_value=0.0, max_value=20.0, + #) self._cinematica_4ws_ku = _safe_float( - parametros_mpc.get("cinematica_4ws_ku", 2.5), - 2.5, min_value=0.0, max_value=20.0, + parametros_mpc.get("ku_direcional", 2.65), + 2.65, min_value=0.0, max_value=20.0, ) self._cinematica_entre_eixos_m = _safe_float( parametros_mpc.get("cinematica_entre_eixos_m", 0.94), @@ -1366,17 +1419,53 @@ class ControladorMPC: return traj - def _usar_seguidor_dubins(self, status_carro, idx_base): - """V12: cabeceira estrutural usa Pure Pursuit dedicado em vez da arvore MPC. + def _ha_transicao_corredor_adiante(self, idx_base): + """Confirma que existe uma troca de corredor logo adiante na centerline. - O C# ja coloca Status=Manobrando quando o ponto atual/proximo pertence - a geometria de manobra. Para o primeiro A/B de campo/simulador usamos - esse contrato diretamente, evitando depender do valor numerico privado - de TipoPontoRua no Python. + Usado somente para o handoff antecipado SaindoRua -> Pure Pursuit. + A verificacao e geometrica/estrutural, nao altera progresso visitado. + """ + if not self.pontos_info or self._s_nodes is None or len(self._s_nodes) == 0: + return False + + n = len(self.pontos_info) + idx = max(0, min(_safe_int(idx_base, 0), n - 1)) + corredor_base = self._idx_corredor(idx) + if corredor_base < 0: + return False + + s_base = float(self._s_nodes[idx]) + limite_m = max(2.0, float(self._dubins_handoff_busca_frente_m)) + + for j in range(idx + 1, n): + if float(self._s_nodes[j]) - s_base > limite_m: + break + + corredor_j = self._idx_corredor(j) + if corredor_j >= 0 and corredor_j != corredor_base: + return True + + return False + + def _usar_seguidor_dubins(self, status_carro, idx_base): + """V18: Pure Pursuit assume a cabeceira antes da BordaSaida. + + - Manobrando: contrato ja validado em campo. + - SaindoRua: assume antecipadamente SOMENTE se ha uma troca real de + corredor logo adiante na centerline. Assim o MPC pesado deixa de + "prometer" uma curva antes do handoff e o PP chega contextualizado + desde o trecho reto de aproximacao. """ if not bool(self._seguidor_dubins_enabled): return False - return _status_in(status_carro, [StatusCarroMapa.Manobrando]) + + if _status_in(status_carro, [StatusCarroMapa.Manobrando]): + return True + + if _status_in(status_carro, [StatusCarroMapa.SaindoRua]): + return self._ha_transicao_corredor_adiante(idx_base) + + return False def _comando_seguidor_dubins( self, contexto, comando_anterior, x, y, theta, velocidade, status_carro, @@ -1425,7 +1514,11 @@ class ControladorMPC: "erro_heading_caminho_graus": abs(float(math.degrees(dbg["erro_heading"]))), "debug_custo": {}, "debug_seguidor_dubins": { - "modo": "pure_pursuit_4ws", + "modo": ( + "pure_pursuit_4ws_antecipado" + if _status_in(status_carro, [StatusCarroMapa.SaindoRua]) + else "pure_pursuit_4ws" + ), "lookahead_m": float(dbg["lookahead_m"]), "distancia_alvo_m": float(dbg["distancia_alvo_m"]), "alpha_graus": float(math.degrees(dbg["alpha"])), @@ -2489,6 +2582,31 @@ class ControladorMPC: e_head_deg = abs(math.degrees(erro_heading_rad)) tipo_prev = _movimento_from_value(tipo_preferido) + # V19 - captura lateral imediatamente após a ferradura. + # + # Este override é propositalmente ANTES da política normal de + # CaminhandoRua/EntrandoRua. O estado é armado apenas pela saída real do + # seguidor Dubins, portanto não altera a centralização nominal de uma rua + # comum. Enquanto ativo, o crab pode começar com heading moderadamente + # errado, preservando a prioridade de manter o rover entre as linhas. + if ( + bool(getattr(self, "_entry_capture_active", False)) + and _status_in( + status_carro, + [ + StatusCarroMapa.EntrandoRua, + StatusCarroMapa.CaminhandoRua, + StatusCarroMapa.Direcionando, + ], + ) + and abs(e_lat) > float(self._entry_capture_lateral_release_m) + and e_head_deg <= float(self._entry_capture_heading_max_graus) + ): + return ( + TipoMovimentoDirecional.MovimentoDiagonal, + self._angulo_diagonal_tracking(e_lat, v), + ) + if _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]): return self._referencia_supervisor_corredor( e_lat, @@ -3774,11 +3892,21 @@ class ControladorMPC: velocidade, ) - # V12 - cabeceira Dubins: a rota ja esta pronta. Em Manobrando nao - # abrimos a arvore MPC; usamos Pure Pursuit 4WS mirando um ponto - # fisico adiante na propria curva. O heading/tangente fica apenas - # como telemetria e nao alivia o comando antes da hora. - if self._usar_seguidor_dubins(status_carro, idx_alvo_correcao): + # V18 - handoff antecipado da cabeceira. SaindoRua ja opera lento e, + # quando existe troca de corredor logo adiante, entrega o volante ao + # mesmo Pure Pursuit 4WS usado em Manobrando. Enquanto a centerline + # ainda e reta o comando permanece praticamente reto; quando o alvo + # entra na Dubins a curva nasce sem a previsao concorrente do MPC. + usando_pp_dubins = self._usar_seguidor_dubins( + status_carro, idx_alvo_correcao + ) + if usando_pp_dubins: + # V19: enquanto PP possui o volante apenas memorizamos que a + # próxima saída dele deve armar o Entry Capture. + self._dubins_pp_ativo_ciclo_anterior = True + self._entry_capture_active = False + self._entry_capture_dist_m = 0.0 + return self._comando_seguidor_dubins( contexto=contexto, comando_anterior=comando_anterior, @@ -3791,6 +3919,77 @@ class ControladorMPC: ponto_alvo_real=ponto_alvo_real, ) + # ------------------------------------------------------------ + # V19 - arma/atualiza ENTRY CAPTURE somente no ciclo REAL. + # ------------------------------------------------------------ + status_entry_ok = _status_in( + status_carro, + [ + StatusCarroMapa.EntrandoRua, + StatusCarroMapa.CaminhandoRua, + StatusCarroMapa.Direcionando, + ], + ) + + if bool(getattr(self, "_dubins_pp_ativo_ciclo_anterior", False)): + self._dubins_pp_ativo_ciclo_anterior = False + if bool(self._entry_capture_enabled) and status_entry_ok: + self._entry_capture_active = True + self._entry_capture_dist_m = 0.0 + _log( + "[MPC/V19 ENTRY] Capture armado após Dubins | " + f"status={_enum_name(status_carro, StatusCarroMapa)}" + ) + + if bool(getattr(self, "_entry_capture_active", False)): + if not status_entry_ok: + self._entry_capture_active = False + self._entry_capture_dist_m = 0.0 + _log( + "[MPC/V19 ENTRY] Capture encerrado por mudança de estado | " + f"status={_enum_name(status_carro, StatusCarroMapa)}" + ) + else: + dt_entry = _safe_float( + self.tempo_execucao_local, + 0.5, + min_value=0.05, + max_value=1.5, + ) + self._entry_capture_dist_m += ( + abs(float(velocidade)) * dt_entry + ) + + try: + ref_entry = self._referencia_caminho_lookahead( + x, y, idx_alvo_correcao, velocidade, status_carro + ) + e_lat_entry = abs(float(ref_entry["e_lat"])) + theta_entry = float(ref_entry["theta_ref"]) + e_head_entry = abs(math.degrees( + self._erro_heading_caminho(theta, theta_entry) + )) + except Exception: + e_lat_entry = float("inf") + e_head_entry = float("inf") + + if e_lat_entry <= float(self._entry_capture_lateral_release_m): + self._entry_capture_active = False + _log( + "[MPC/V19 ENTRY] Capture concluído | " + f"e_lat={e_lat_entry:.2f}m | " + f"e_head={e_head_entry:.1f}deg | " + f"dist={self._entry_capture_dist_m:.2f}m" + ) + elif self._entry_capture_dist_m >= float(self._entry_capture_max_dist_m): + self._entry_capture_active = False + _log( + "[MPC/V19 ENTRY] Capture expirado por distância | " + f"e_lat={e_lat_entry:.2f}m | " + f"e_head={e_head_entry:.1f}deg | " + f"dist={self._entry_capture_dist_m:.2f}m" + ) + # -------------------- Planejamento: horizonte ESPACIAL fixo -------------------- S_ALVO = float(self.horizonte) V_FLOOR = float(getattr(self, "_vmin_planejamento", 0.30))