diff --git a/AgroBase/AgroBase/Models/MapasModel.cs b/AgroBase/AgroBase/Models/MapasModel.cs index a485a0476..ae9cb907a 100644 --- a/AgroBase/AgroBase/Models/MapasModel.cs +++ b/AgroBase/AgroBase/Models/MapasModel.cs @@ -436,77 +436,105 @@ namespace AgroBase.Models public void PopularTrajetoriaMapa(MapaFeatureCollectionModel dados = null) { if (dados != null) - { mapaService.DefinirMapa(dados, mapaService.NomeArquivos ?? "Mapa"); - } - if (mapaService.DadosMapa == null || mapaService.DadosMapa.features == null) + if (mapaService.DadosMapa?.features == null) + throw new InvalidOperationException("Mapa sem features para montar a trajetória."); + + var ids = (RuasPercorrer ?? new List()) + .Select(x => x?.Trim()) + .Where(x => !string.IsNullOrWhiteSpace(x)) + .ToList(); + + if (ids.Count < 2) + throw new InvalidOperationException("Selecione ao menos duas ruas, na ordem de percurso."); + + if (ids.Distinct(StringComparer.Ordinal).Count() != ids.Count) + throw new InvalidOperationException("A seleção contém IDs de rua repetidos."); + + var ruasMontadas = new List>(); + + foreach (string idSelecionado in ids) { - return; - } + var correspondencias = mapaService.DadosMapa.features + .Where(x => + x?.properties != null && + string.Equals( + x.properties.Id?.Trim(), + idSelecionado, + StringComparison.Ordinal + ) + ) + .ToList(); - TrajetoriaMapa = new List>(); - - double orientacaoReferencia = double.NaN; - - foreach (string idSelecionado in RuasPercorrer) - { - MapaFeatureModel feature = mapaService.DadosMapa.features - .FirstOrDefault(x => - x != null && - x.properties != null && - string.Equals(x.properties?.Id?.Trim(), idSelecionado?.Trim(), StringComparison.Ordinal) + if (correspondencias.Count != 1) + { + throw new InvalidOperationException( + $"A rua '{idSelecionado}' possui {correspondencias.Count} correspondências no mapa; esperado=1." ); - - if (feature?.geometry?.coordinates == null || feature.geometry.coordinates.Count < 2) - { - continue; } - List rua = new List(); + var coordenadas = correspondencias[0].geometry?.coordinates; - foreach (List ponto in feature.geometry.coordinates) + if (coordenadas == null || coordenadas.Count < 2) + throw new InvalidOperationException($"A rua '{idSelecionado}' não possui geometria navegável."); + + var rua = new List(); + + for (int i = 0; i < coordenadas.Count; i++) { + List ponto = coordenadas[i]; + if (ponto == null || ponto.Count < 2) + throw new InvalidOperationException($"Coordenada incompleta na rua '{idSelecionado}', ponto {i + 1}."); + + double longitude = ponto[0]; + double latitude = ponto[1]; + + bool valida = + !double.IsNaN(latitude) && + !double.IsInfinity(latitude) && + !double.IsNaN(longitude) && + !double.IsInfinity(longitude) && + latitude >= -90 && latitude <= 90 && + longitude >= -180 && longitude <= 180; + + if (!valida) + throw new InvalidOperationException($"Coordenada inválida na rua '{idSelecionado}', ponto {i + 1}."); + + rua.Add(new GPSModel { - continue; - } - - rua.Add(new GPSModel { Longitude = ponto[0], Latitude = ponto[1] }); + Latitude = latitude, + Longitude = longitude + }); } - if (rua.Count < 2) - continue; - - double orientacaoRua = GPSUtils.CalcularOrientacao(rua[rua.Count - 1], rua[rua.Count - 2]); - - if (double.IsNaN(orientacaoReferencia)) - { - orientacaoReferencia = orientacaoRua; - } - else - { - double anguloOposto = (orientacaoRua + 180.0) % 360.0; - double erro = Math.Abs(CalcularErroAngular(anguloOposto, orientacaoReferencia)); - if (erro <= 45.0) - { - rua.Reverse(); - } - } - - TrajetoriaMapa.Add(rua); + ruasMontadas.Add(rua); } - if (!TrajetoriaMapa.Any()) - return; - OperacaoModel op = Variaveis.OperacaoEmAndamento; if (op == null) - return; + throw new InvalidOperationException("Operação indisponível para instalar a trajetória."); - op.Trajetoria = new TrajetoriaMapaOperacaoModel(TrajetoriaMapa); - op.Trajetoria.ProjetarTrajetoriaFixa(); + var trajetoriaAnterior = op.Trajetoria; + var mapaAnterior = TrajetoriaMapa; + var novaTrajetoria = new TrajetoriaMapaOperacaoModel(ruasMontadas); + + try + { + // A projeção consulta op.Trajetoria internamente; por isso a nova + // instância é instalada dentro da transação e restaurada na falha. + op.Trajetoria = novaTrajetoria; + novaTrajetoria.ProjetarTrajetoriaFixa(); + TrajetoriaMapa = ruasMontadas; + } + catch + { + op.Trajetoria = trajetoriaAnterior; + TrajetoriaMapa = mapaAnterior; + throw; + } } private static double CalcularErroAngular(double atual, double referencia) diff --git a/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs b/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs index 08d2a70c4..1499c2d32 100644 --- a/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs +++ b/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs @@ -496,27 +496,42 @@ namespace AgroBase.Models.Modules LigadoAlterado || FreioAlterado || ReafirmacaoNecessaria; - - OidFuncCode modoControle = - Freado ? OidFuncCode.ComandoCorrenteFreio : - ComandoLigado ? OidFuncCode.ComandoVelocidade : - OidFuncCode.ComandoCorrente; + + OidFuncCode modoControle; + + if (Freado) + { + // TESTE DIAGNÓSTICO: + // mantém toda a lógica EmFreio ativa, + // mas NÃO aplica frenagem elétrica nos OID. + modoControle = OidFuncCode.ComandoCorrente; + } + else + { + modoControle = + ComandoLigado + ? OidFuncCode.ComandoVelocidade + : OidFuncCode.ComandoCorrente; + } if (EnviaComando) { float setPoint = 0; + if (modoControle == OidFuncCode.ComandoCorrenteFreio) { setPoint = (float)CorrenteFreio; } - else { + else if (modoControle == OidFuncCode.ComandoVelocidade) + { int mx = - Sentido_SP == Sentido.Horario ? 1 : - Sentido_SP == Sentido.Antihorario ? -1 : - 0; + Sentido_SP == Sentido.Horario ? 1 : + Sentido_SP == Sentido.Antihorario ? -1 : + 0; + setPoint = RPM_SP * mx; } - + //velocidade = (Mod_ID.Contains("T") ? velocidade : velocidade * 0.7f); OIDCanService.ComandoControle(_EnderecoCAN_Tx, _EnderecoCAN_Rx, modoControle, setPoint); @@ -584,6 +599,7 @@ namespace AgroBase.Models.Modules public SensorServoModel Servo => Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Servos?.FirstOrDefault(x => x.Inicializado && x.ID.Contains(Mod_ID)); private double VelocidadeAtual => FuncoesMatematicas.CalculaVelocidadePercentualMs(Variaveis.OperacaoEmAndamento.Sensoriamento?.Movimentacao?.VelocidadeMediaMs ?? 0); private bool StatusControleEmFreio => Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.Modulo_ID == Mod_ID)?.MovMotor?.Leitura?.Freado ?? false; + private bool StatusComandoFreio => Variaveis.OperacaoEmAndamento.Controle?.EmFreio ?? false; private int? AnguloLido => (int?)Servo?.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.ServoAnguloLeitura)?.atual?.valor; // histerese de velocidade @@ -622,7 +638,8 @@ namespace AgroBase.Models.Modules return; } - bool emFreio = StatusControleEmFreio; + //bool emFreio = StatusControleEmFreio; + bool emFreio = StatusComandoFreio; bool transicao = (UltimoControle != emFreio); if (anguloInicial == -1) anguloInicial = AnguloInicial; diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index c8d4bb0f4..62c550ee4 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -1,4 +1,4 @@ -using AgroBase.Models.Operadores; +using AgroBase.Models.Operadores; using AgroBase.Services; using AgroBase.Services.Operadores; using BlackSharp.Core.Extensions; @@ -6,6 +6,7 @@ using Newtonsoft.Json; using Newtonsoft.Json.Linq; using System; using System.Collections.Generic; +using System.Globalization; using System.Linq; using static AgroBase.Models.CorredorTrajetoriaModel; using static AgroBase.Models.Enums; @@ -16,14 +17,41 @@ namespace AgroBase.Models { public TrajetoriaMapaOperacaoModel(List> RuasMapa) { - RuasPlantacao = new List>(RuasMapa); + RuasPlantacao = ClonarRuasMapa(RuasMapa); AutonomiaCorredor = new AutonomiaCorredorModel(); } + /* + * Todas as mutacoes da trajetoria passam por este monitor. + * O Monitor do C# e reentrante; por isso os fluxos publicos podem + * chamar outros fluxos publicos da classe sem deadlock. + */ + [JsonIgnore] + private readonly object _syncTrajetoria = new object(); + + /* + * Um ciclo inteiro deve enxergar o mesmo par atual/anterior. + * Sem isso, PontoTrajetoriaModel podia comparar o snapshot de + * Sensoriamento.Gps com PenultimaLeitura de outra amostra. + */ + [JsonIgnore] + private GPSModel _gpsAtualCiclo; + + [JsonIgnore] + private GPSModel _gpsAnteriorCiclo; + + [JsonIgnore] + private int _loopEmExecucao; + + private const double ToleranciaCoordenada = 1e-12; + private const double DistanciaMaximaSaltoMapaM = 25.0; + #region PARAMETROS public double AnguloAberturaCurva { get; set; } = 25; // Angulo usado para deslocar o ponto de curva - public double DistanciaProjecaoRua => (VariaveisEquipamento.DistanciaEntreEixosCm / 100.0 / 2.0) + Variaveis.OperacaoEmAndamento?.Parametros?.Controle?.DistanciaManobra ?? 3.0; // Distancia para projetar o primeiro ponto para fora do corredor + public double DistanciaProjecaoRua => + (VariaveisEquipamento.DistanciaEntreEixosCm / 100.0 / 2.0) + + (Variaveis.OperacaoEmAndamento?.Parametros?.Controle?.DistanciaManobra ?? 3.0); // Distancia para projetar o primeiro ponto para fora do corredor public static double DistanciaEntrePontos { get; set; } = 0.8; // Distancia entre os pontos dentro do corredor public static double DistanciaEntrePontosCurva { get; set; } = 0.25; // Distancia entre os pontos durante a curva entre corredores private double DistanciaManobraEntreRuas { get; set; } = 3.0; // Distancia máxima para gerar a curva de conexão entre os corredores @@ -40,7 +68,11 @@ namespace AgroBase.Models private int MaxTentativasRecuperacaoTrajetoria { get; set; } = 3; private double ToleranciaLateralRecuperacaoM { get; set; } = 1.5; private double ToleranciaLongitudinalAntesPontoM { get; set; } = 0.10; + private double ErroOrientacaoMaximoRecuperacaoGraus { get; set; } = 45.0; + private double ErroOrientacaoMaximoRecuperacaoEstruturalGraus { get; set; } = 35.0; private int MaxPontosRecuperadosPorCiclo { get; set; } = 8; + private int MaxPontosEntradaRecuperacaoTransicao { get; set; } = 12; + private int ConfirmacoesMinimasEntradaProximoCorredor { get; set; } = 2; #endregion @@ -49,16 +81,86 @@ namespace AgroBase.Models public AutonomiaCorredorModel AutonomiaCorredor { get; set; } [JsonIgnore] - private GPSModel GPSPosicaoAtual => GPSService.UltimaLeitura?.Clone(); + private int _idxCorredorCandidatoTransicao = -1; + + [JsonIgnore] + private int _confirmacoesCandidatoTransicao = 0; + + [JsonIgnore] + private GPSModel _amostraGpsCandidatoTransicao; + + [JsonIgnore] + private double _timestampPosCandidatoTransicao = double.NaN; + + [JsonIgnore] + private DateTime _momentoCandidatoTransicao = DateTime.MinValue; + + [JsonIgnore] + private GPSModel GPSPosicaoAtual => + _gpsAtualCiclo ?? GPSService.GetSnapshot(); + + private GPSModel GPSPosicaoAnterior => + _gpsAnteriorCiclo ?? GPSService.GetPreviousSnapshot(); + + private (GPSModel atual, GPSModel anterior) ObterParGpsCoerente() + { + GPSModel atualAntes = null; + GPSModel anterior = null; + GPSModel atualDepois = null; + + /* + * GPSService ainda nao oferece o par atomico em uma unica API. + * O retry evita aceitar um Penultima pertencente a outra amostra + * quando uma nova GGA chega entre as duas leituras. + */ + for (int tentativa = 0; tentativa < 3; tentativa++) + { + atualAntes = GPSService.GetSnapshot(); + anterior = GPSService.GetPreviousSnapshot(); + atualDepois = GPSService.GetSnapshot(); + + double tsAntes = atualAntes?.TimestampPos?.valor ?? double.NaN; + double tsDepois = atualDepois?.TimestampPos?.valor ?? double.NaN; + + if ( + !double.IsNaN(tsAntes) && + !double.IsNaN(tsDepois) && + Math.Abs(tsAntes - tsDepois) <= ToleranciaCoordenada + ) + { + break; + } + } + + return (atualDepois ?? atualAntes, anterior); + } + + private void AplicarSnapshotGpsCiclo((GPSModel atual, GPSModel anterior) par) + { + _gpsAtualCiclo = par.atual; + _gpsAnteriorCiclo = par.anterior; + } + + private void LimparSnapshotGpsCiclo() + { + _gpsAtualCiclo = null; + _gpsAnteriorCiclo = null; + } [JsonProperty] public double TempoEntreLeituras { get; private set; } private void AtualizarTempoEntreLeituras() { DateTime leitura_gps = GPSPosicaoAtual?.Momento ?? DateTime.MinValue; - double t_min = (1.0 / GPSService.TaxaAmostragemHz); + double t_nominal = 1.0 / Math.Max(1, GPSService.TaxaAmostragemHz); double dt = (leitura_gps - UltimaAtualizacaoDados).TotalSeconds; - TempoEntreLeituras = Math.Min(t_min, dt); + + if (UltimaAtualizacaoDados == DateTime.MinValue || dt < 0 || double.IsNaN(dt) || double.IsInfinity(dt)) + { + dt = 0; + } + + TempoEntreLeituras = Math.Max(0, Math.Min(t_nominal * 2.0, dt)); UltimaAtualizacaoDados = leitura_gps; } @@ -68,8 +170,11 @@ namespace AgroBase.Models { get { + if (RuasPlantacao == null || RuasPlantacao.Count == 0) + return new GPSModel(); + // Verifica se o índice é válido e se a rua não está vazia - if (CorredorAtual != null && CorredorAtual.Idx < RuasPlantacao.Count && RuasPlantacao[CorredorAtual.Idx].Count > 0) + if (CorredorAtual != null && CorredorAtual.Idx >= 0 && CorredorAtual.Idx < RuasPlantacao.Count && RuasPlantacao[CorredorAtual.Idx].Count > 0) { return RuasPlantacao[CorredorAtual.Idx][0]; // Acessa diretamente o primeiro ponto da rua } @@ -78,15 +183,18 @@ namespace AgroBase.Models return RuasPlantacao[0][0]; } - return new GPSModel(); // Pode ser null se preferir + return new GPSModel(); } } public GPSModel UltimoPontoRuaMapa { get { + if (RuasPlantacao == null || RuasPlantacao.Count == 0) + return new GPSModel(); + // Verifica se o índice é válido e se a rua não está vazia - if (CorredorAtual != null && CorredorAtual.Idx < RuasPlantacao.Count && RuasPlantacao[CorredorAtual.Idx].Count > 0) + if (CorredorAtual != null && CorredorAtual.Idx >= 0 && CorredorAtual.Idx < RuasPlantacao.Count && RuasPlantacao[CorredorAtual.Idx].Count > 0) { return RuasPlantacao[CorredorAtual.Idx][RuasPlantacao[CorredorAtual.Idx].Count - 1]; // Acessa diretamente o ultimo ponto da rua } @@ -95,13 +203,16 @@ namespace AgroBase.Models return RuasPlantacao[0][RuasPlantacao[0].Count - 1]; } - return new GPSModel(); // Pode ser null se preferir + return new GPSModel(); } } public int ExtremoMaisProximoMapa { get { + if (GPSPosicaoAtual == null || PrimeiroPontoRuaMapa == null || UltimoPontoRuaMapa == null) + return 0; + double distPP = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, PrimeiroPontoRuaMapa); double distUP = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, UltimoPontoRuaMapa); @@ -124,7 +235,7 @@ namespace AgroBase.Models private void AtualizarTrajetoriaJanela() { _TrajetoriaJanela = new List(); - + if (!_TrajetoriaFixaDefinida) return; for (int i = idxInicialJanela; i <= idxFinalJanela; i++) @@ -133,10 +244,43 @@ namespace AgroBase.Models } } public List _Corredores { get; set; } - public List TrajetoriaFixa => _TrajetoriaFixa?.ConvertAll(x => x.Posicao); + public List TrajetoriaFixa + { + get + { + lock (_syncTrajetoria) + return _TrajetoriaFixa?.ConvertAll(x => x.Posicao); + } + } public List _TrajetoriaDinamica { get; private set; } - public List TrajetoriaDinamica => _TrajetoriaDinamica?.ConvertAll(x => x.Posicao); + public List TrajetoriaDinamica + { + get + { + lock (_syncTrajetoria) + return _TrajetoriaDinamica?.ConvertAll(x => x.Posicao); + } + } public void AtualizarTrajetoriaDinamica() + { + var parGps = ObterParGpsCoerente(); + + lock (_syncTrajetoria) + { + AplicarSnapshotGpsCiclo(parGps); + + try + { + AtualizarTrajetoriaDinamicaCore(); + } + finally + { + LimparSnapshotGpsCiclo(); + } + } + } + + private void AtualizarTrajetoriaDinamicaCore() { // Cria a lista com a última leitura do robô var trajetoria = new List() @@ -145,7 +289,7 @@ namespace AgroBase.Models { Posicao = GPSPosicaoAtual, LarguraCorredor = 1.0, - idxCorredor = CorredorAtual?.Idx ?? 0, + idxCorredor = CorredorAtual?.Idx ?? 0, Visitado = true } }; @@ -173,9 +317,14 @@ namespace AgroBase.Models public static double DistanciaMaximaEntreLeituras { get; private set; } public void AtualizarDistanciaMaximaEntreLeituras() { - double velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP); - double distanciaMaxima = (velocidadeCarroMs / GPSService.TaxaAmostragemHz); - DistanciaMaximaEntreLeituras = distanciaMaxima; + double percentualVelocidade = Variaveis.OperacaoEmAndamento?.Controle?.PercentualVelocidadeSP ?? 0; + double velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(percentualVelocidade); + double distanciaMaxima = velocidadeCarroMs / Math.Max(1, GPSService.TaxaAmostragemHz); + + DistanciaMaximaEntreLeituras = + double.IsNaN(distanciaMaxima) || double.IsInfinity(distanciaMaxima) + ? 0 + : Math.Max(0, distanciaMaxima); } private int idxInicialJanela = 0; @@ -278,8 +427,14 @@ namespace AgroBase.Models // Verifica se o corredor atual existe if (!_CorredorAtualDefinido) return 0; + if (RuasPlantacao == null || idxRua < 0 || idxRua >= RuasPlantacao.Count) + return 0; + List rua = new List(RuasPlantacao[idxRua]); + if (rua.Count < 2) + return 0; + double d0 = double.MaxValue; int idx = -1; for (int i = 0; i < rua.Count; i++) @@ -290,12 +445,11 @@ namespace AgroBase.Models d0 = dist; idx = i; } - else - { - break; - } } + if (idx < 0) + return 0; + GPSModel p0 = rua[idx - (idx > 0 ? 1 : 0)]; GPSModel p1 = rua[idx + (idx < (rua.Count - 1) ? 1 : 0)]; int idxPa = rua.IndexOf(p0); @@ -316,14 +470,14 @@ namespace AgroBase.Models rua = novaRua; - int qtdPontosRua = Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua)); - rua = GPSUtils.InterpolarRota(rua, qtdPontosRua); + int qtdPontosRua = Math.Max(2, Convert.ToInt32(Math.Ceiling(GPSUtils.DistanciaDoTrecho(rua))) + 1); + rua = InterpolarRotaPorDistancia(rua, Math.Max(2, qtdPontosRua)); // Calcula a menor distância entre o robô e a rua double distancia = GPSUtils.CalcularMenorDistanciaAteTrecho(posicaoAtual, rua); // Calcula o vetor entre os pontos da rua e o vetor entre o ponto A e o GPS - double crossProduct = + double crossProduct = (pontoB.Longitude - pontoA.Longitude) * (posicaoAtual.Latitude - pontoA.Latitude) - (pontoB.Latitude - pontoA.Latitude) * (posicaoAtual.Longitude - pontoA.Longitude); @@ -355,13 +509,17 @@ namespace AgroBase.Models int idxUltimoVisitado = -1; - for (int i = _TrajetoriaFixa.Count - 1; i >= 0; i--) + /* + * O progresso valido e sempre um prefixo continuo. + * Um ponto futuro marcado por ruido ou por um estado legado + * nunca pode fazer PontoAtual saltar um corredor. + */ + for (int i = 0; i < _TrajetoriaFixa.Count; i++) { - if (_TrajetoriaFixa[i].Visitado) - { - idxUltimoVisitado = i; + if (!_TrajetoriaFixa[i].Visitado) break; - } + + idxUltimoVisitado = i; } if (idxUltimoVisitado >= 0) @@ -469,9 +627,17 @@ namespace AgroBase.Models { if (CorredorAtual != null) { - if ((RetornandoBase && CorredorAtual.Dentro) || (!RetornandoBase && Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual == StatusOperacao.EmAndamento)) + bool operacaoEmAndamento = + Variaveis.OperacaoEmAndamento?.Sensoriamento?.Operacao?.StatusOperacaoAtual == + StatusOperacao.EmAndamento; + + if ((RetornandoBase && CorredorAtual.Dentro) || (!RetornandoBase && operacaoEmAndamento)) { - PontoAtual.AtualizarPropriedades(); + PontoAtual.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas) { StatusAtual = StatusCarroMapa.Direcionando; @@ -480,7 +646,17 @@ namespace AgroBase.Models { StatusAtual = StatusCarroMapa.Manobrando; } - else if (!CorredorAtual.Dentro && PontoAtual.Tipo == TipoPontoRua.Rua && ProximoPonto.Tipo == TipoPontoRua.Rua && ProximoPonto.OrientacaoAtual > 7.0) + else if ( + !CorredorAtual.Dentro && + PontoAtual.Tipo == TipoPontoRua.Rua && + ProximoPonto.Tipo == TipoPontoRua.Rua && + Math.Abs( + GPSUtils.CalcularDiferencaAngulo( + ProximoPonto.OrientacaoAtual, + GPSPosicaoAtual?.AnguloCarroDefinido ?? ProximoPonto.OrientacaoAtual + ) + ) > 7.0 + ) { StatusAtual = StatusCarroMapa.Manobrando; } @@ -518,8 +694,29 @@ namespace AgroBase.Models public double DistanciaPercorrida { get; private set; } public void AtualizarDistanciaPercorrida(double distancia) { - DistanciaPercorrida += distancia; - CorredorAtual?.AtualizarDistancias(distancia); + if (double.IsNaN(distancia) || double.IsInfinity(distancia) || distancia <= 0) + return; + + /* + * Um salto isolado de GNSS nao pode inflar progresso e consumo. + * O HealthWorker decide se o rover pode mover; esta protecao + * preserva somente a coerencia das metricas da trajetoria. + */ + double limiteSalto = Math.Max(2.0, DistanciaMaximaEntreLeituras * 5.0 + 0.50); + + if (distancia > limiteSalto) + { + Variaveis.MostrarLog( + $"[TRJ] Incremento de distancia rejeitado: {distancia:F2} m (limite {limiteSalto:F2} m)." + ); + return; + } + + lock (_syncTrajetoria) + { + DistanciaPercorrida += distancia; + CorredorAtual?.AtualizarDistancias(distancia); + } } [JsonProperty] public double DistanciaRestante { get; private set; } @@ -532,7 +729,9 @@ namespace AgroBase.Models public double PercentualTrajetoria { get; private set; } public void AtualizarPercentualTrajetoria() { - PercentualTrajetoria = DistanciaTotal > 0 ? (DistanciaPercorrida / DistanciaTotal) * 100.0 : 0; + PercentualTrajetoria = DistanciaTotal > 0 + ? FuncoesMatematicas.Clamp((DistanciaPercorrida / DistanciaTotal) * 100.0, 0, 100) + : 0; } [JsonProperty] public string TempoEstimadoOperacao { get; private set; } @@ -542,6 +741,13 @@ namespace AgroBase.Models { var op = Variaveis.OperacaoEmAndamento; + if (op?.Sensoriamento == null || op?.Parametros?.Controle == null) + { + TempoEstimadoOperacao = "00:00:00"; + TempoEstimadoRestante = "00:00:00"; + return; + } + if (!velocidadeSemErvasMs.HasValue) { velocidadeSemErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(op.Parametros.Controle.MovVelocidadeSErvasPercent) * 0.8; @@ -551,9 +757,14 @@ namespace AgroBase.Models velocidadeComErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(op.Parametros.Controle.MovVelocidadeCErvasPercent); } - // Supondo que metade do percurso tem ervas e a outra metade não tem - double distanciaComErvas = DistanciaTotal * op.Sensoriamento.Atuador.PercentualErvasTerreno; - double distanciaSemErvas = DistanciaTotal * (100 - op.Sensoriamento.Atuador.PercentualErvasTerreno); + double percentualErvas = FuncoesMatematicas.Clamp( + op?.Sensoriamento?.Atuador?.PercentualErvasTerreno ?? 0, + 0, + 100 + ) / 100.0; + + double distanciaComErvas = DistanciaTotal * percentualErvas; + double distanciaSemErvas = DistanciaTotal * (1.0 - percentualErvas); // Tempo em segundos para cada parte do percurso double tempoSemErvas = velocidadeSemErvasMs.Value > 0 ? distanciaSemErvas / velocidadeSemErvasMs.Value : 0; @@ -563,30 +774,46 @@ namespace AgroBase.Models double tempoTotalSegundos = tempoSemErvas + tempoComErvas; // Conversão para horas, minutos e segundos - TimeSpan tempoTotal = TimeSpan.FromSeconds(tempoTotalSegundos); - DateTime total = new DateTime(tempoTotal.Ticks); - - TempoEstimadoOperacao = total.ToString("HH:mm:ss"); + TempoEstimadoOperacao = FormatarDuracao(tempoTotalSegundos); // Distâncias (em metros) double distanciaRestante = DistanciaRestante; // Calcular a distância restante sem ervas e com ervas - double distanciaSemErvasRestante = distanciaRestante * (100 - op.Sensoriamento.Atuador.PercentualErvasTerreno); - double distanciaComErvasRestante = distanciaRestante * op.Sensoriamento.Atuador.PercentualErvasTerreno; + double distanciaSemErvasRestante = distanciaRestante * (1.0 - percentualErvas); + double distanciaComErvasRestante = distanciaRestante * percentualErvas; // Calcular o tempo restante para as áreas sem ervas e com ervas - double tempoRestanteSemErvas = distanciaSemErvasRestante / velocidadeSemErvasMs.Value; - double tempoRestanteComErvas = distanciaComErvasRestante / velocidadeComErvasMs.Value; + double tempoRestanteSemErvas = velocidadeSemErvasMs.Value > 0 + ? distanciaSemErvasRestante / velocidadeSemErvasMs.Value + : 0; + + double tempoRestanteComErvas = velocidadeComErvasMs.Value > 0 + ? distanciaComErvasRestante / velocidadeComErvasMs.Value + : 0; // Tempo total restante em segundos double tempoTotalRestanteSegundos = tempoRestanteSemErvas + tempoRestanteComErvas; // Conversão para horas, minutos e segundos - TimeSpan tempoTotalRestante = TimeSpan.FromSeconds(tempoTotalRestanteSegundos); - DateTime totalRestante = new DateTime(tempoTotalRestante.Ticks); + TempoEstimadoRestante = FormatarDuracao(tempoTotalRestanteSegundos); + } - TempoEstimadoRestante = totalRestante.ToString("HH:mm:ss"); + private static string FormatarDuracao(double segundos) + { + if (double.IsNaN(segundos) || double.IsInfinity(segundos) || segundos < 0) + return "00:00:00"; + + TimeSpan duracao = TimeSpan.FromSeconds(Math.Min(segundos, TimeSpan.MaxValue.TotalSeconds)); + long horasTotais = (long)Math.Floor(duracao.TotalHours); + + return string.Format( + CultureInfo.InvariantCulture, + "{0:00}:{1:00}:{2:00}", + horasTotais, + duracao.Minutes, + duracao.Seconds + ); } [JsonProperty] public double DistanciaErroMapaPlantacao { get; private set; } = -1; @@ -601,14 +828,24 @@ namespace AgroBase.Models var op = Variaveis.OperacaoEmAndamento; + if (op?.Sensoriamento == null) + return; + if (PrimeiroPontoDentroMapa == null) PrimeiroPontoDentroMapa = GPSPosicaoAtual; var OpVisual = op.Sensoriamento?.OperadorVisual ?? new VisualWorkerModel(); - StatusModulo OpVisualStatus = op.Sensoriamento.OperadorSaude.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado; + StatusModulo OpVisualStatus = op.Sensoriamento?.OperadorSaude?.ModulosSaude? + .FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado; if (OpVisualStatus != StatusModulo.Operante) return; var DadosSegmentacao = OpVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel(); + double agoraUnix = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0; + double idade = agoraUnix - DadosSegmentacao.timestamp; + + if (idade < 0 || idade > 2.0) + return; + bool CanaNoRadar = new List() { StatusCarroMapa.EntrandoRua, StatusCarroMapa.CaminhandoRua, StatusCarroMapa.SaindoRua }.Contains(DadosSegmentacao.status_corredor); if (!CanaNoRadar) return; @@ -649,7 +886,11 @@ namespace AgroBase.Models if (ultimo == null) return false; - ultimo.AtualizarPropriedades(); + ultimo.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); int idxUltimoVisitado = ObterIdxUltimoVisitado(); bool pertoDoFimPorIndice = idxUltimoVisitado >= _TrajetoriaFixa.Count - 3; @@ -691,8 +932,7 @@ namespace AgroBase.Models private void AtualizarErroCombinado() { var op = Variaveis.OperacaoEmAndamento; - var sensoriamento = op?.Sensoriamento; - var gps = sensoriamento?.Gps; + var gps = GPSPosicaoAtual; if (op?.Parametros?.Controle == null || gps == null || PontoAtual?.Posicao == null || ProximoPonto?.Posicao == null) { @@ -815,15 +1055,32 @@ namespace AgroBase.Models private void MarcarPontosIntermediariosNaoVisitados() { - if (!_TrajetoriaFixaDefinida || PontoAtual == null) + if (!_TrajetoriaFixaDefinida) return; - int idxFinal = Math.Max(0, Math.Min(PontoAtual.idxPonto, _TrajetoriaFixa.Count - 1)); + bool encontrouPrimeiroNaoVisitado = false; + bool corrigiuEstadoOrfao = false; - for (int i = 0; i <= idxFinal; i++) + for (int i = 0; i < _TrajetoriaFixa.Count; i++) { if (!_TrajetoriaFixa[i].Visitado) - _TrajetoriaFixa[i].Visitado = true; + { + encontrouPrimeiroNaoVisitado = true; + continue; + } + + if (encontrouPrimeiroNaoVisitado) + { + _TrajetoriaFixa[i].Visitado = false; + corrigiuEstadoOrfao = true; + } + } + + if (corrigiuEstadoOrfao) + { + Variaveis.MostrarLog( + "[TRJ] Estado de visita nao contiguo corrigido sem avancar a trajetoria." + ); } } @@ -837,10 +1094,41 @@ namespace AgroBase.Models for (int i = idxInicial; i <= idxFinal; i++) { - _TrajetoriaFixa[i].AtualizarPropriedades(); + _TrajetoriaFixa[i].AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); } } + private bool AtualizarProgressoSequencial() + { + DefinirPontoAtual(); + DefinirProximoPonto(); + + if (ProximoPonto == null || ReferenceEquals(ProximoPonto, PontoAtual)) + return false; + + bool visitadoAntes = ProximoPonto.Visitado; + + ProximoPonto.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: true + ); + + if (!visitadoAntes && ProximoPonto.Visitado) + { + DefinirPontoAtual(); + AtualizarCorredorAtual(); + DefinirProximoPonto(); + return true; + } + + return false; + } + private void MarcarVisitadoAte(int idx) { if (!_TrajetoriaFixaDefinida) @@ -859,13 +1147,17 @@ namespace AgroBase.Models if (!_TrajetoriaFixaDefinida) return -1; - for (int i = _TrajetoriaFixa.Count - 1; i >= 0; i--) + int ultimo = -1; + + for (int i = 0; i < _TrajetoriaFixa.Count; i++) { - if (_TrajetoriaFixa[i].Visitado) - return i; + if (!_TrajetoriaFixa[i].Visitado) + break; + + ultimo = i; } - return -1; + return ultimo; } private void ConfirmarEntradaAutonomiaCorredor() @@ -940,6 +1232,14 @@ namespace AgroBase.Models private void AtualizarDadosTrajetoria() + { + if (_gpsAtualCiclo == null || _gpsAnteriorCiclo == null) + throw new InvalidOperationException("Ciclo de trajetoria iniciado sem par GNSS coerente."); + + AtualizarDadosTrajetoriaCore(); + } + + private void AtualizarDadosTrajetoriaCore() { if (!_TrajetoriaFixaDefinida) return; @@ -960,6 +1260,12 @@ namespace AgroBase.Models AtualizarIndicesJanela(); AtualizarTrajetoriaJanela(); AtualizarPropriedadesPontos(idxInicialJanela, idxFinalJanela); + CorredorAtual?.AtualizarDados(); + + bool avancouPontoSequencial = AtualizarProgressoSequencial(); + + if (avancouPontoSequencial) + continue; VerificaFimCorredorSegmentacao(); @@ -1010,6 +1316,7 @@ namespace AgroBase.Models AtualizarIndicesJanela(); AtualizarTrajetoriaJanela(); AtualizarPropriedadesPontos(idxInicialJanela, idxFinalJanela); + CorredorAtual?.AtualizarDados(); MarcarPontosIntermediariosNaoVisitados(); @@ -1030,13 +1337,12 @@ namespace AgroBase.Models AtualizarStatusAtual(); ConfirmarEntradaAutonomiaCorredor(); AtualizarManobrandoEntreRuas(); - AtualizarPercentualTrajetoria(); - AtualizarTempoEstimado(); - AtualizarErroCombinado(); - AtualizarTrajetoriaDinamica(); + AtualizarTrajetoriaDinamicaCore(); AtualizarDistanciaRestante(); + AtualizarPercentualTrajetoria(); + AtualizarTempoEstimado(); AtualizarTrajetoriaConcluida(); AtualizarDistanciaErroMapaPlantacao(); @@ -1045,13 +1351,23 @@ namespace AgroBase.Models private int VerificaEquipamentoDentroCorredor(GPSModel posicaoAtual) { + if (posicaoAtual == null || Corredores == null || Corredores.Count == 0) + return -1; + Dictionary melhoresPontosCorredores = new Dictionary(); foreach (var corredor in Corredores) { + if (corredor == null || corredor.Count < 2) + { + melhoresPontosCorredores.Add(melhoresPontosCorredores.Count, (false, double.MaxValue)); + continue; + } + (GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(posicaoAtual, corredor); if (pontoMaisProximo == null) { - return -1; + melhoresPontosCorredores.Add(melhoresPontosCorredores.Count, (false, double.MaxValue)); + continue; } double distanciaMargem = LarguraCorredorPadrao * 0.8; @@ -1064,17 +1380,39 @@ namespace AgroBase.Models private (bool, int) VerificaEquipamentoDentroCorredorMontado(GPSModel posicaoAtual) { - if (Corredores == null) + if ( + posicaoAtual == null || + Corredores == null || + _Corredores == null || + Corredores.Count == 0 || + Corredores.Count != _Corredores.Count + ) return (false, -1); List melhoresPontosCorredores = new List(); - foreach (var corredor in Corredores) + for (int i = 0; i < Corredores.Count; i++) { + var corredor = Corredores[i]; + + if (corredor == null || corredor.Count < 2 || _Corredores[i]?.Pontos == null) + continue; + (GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(posicaoAtual, corredor); if (pontoMaisProximo == null) continue; - var melhorPonto = _Corredores[Corredores.IndexOf(corredor)].Pontos.FirstOrDefault(x => x.Posicao.Latitude == pontoMaisProximo.Latitude && x.Posicao.Longitude == pontoMaisProximo.Longitude); - melhorPonto.AtualizarPropriedades(); + var melhorPonto = _Corredores[i].Pontos + .Where(x => x?.Posicao != null) + .OrderBy(x => GPSUtils.DistanciaEntrePontos(x.Posicao, pontoMaisProximo)) + .FirstOrDefault(); + + if (melhorPonto == null) + continue; + + melhorPonto.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); melhoresPontosCorredores.Add(melhorPonto); } var ponto = melhoresPontosCorredores.Where(x => x.NaMargem).OrderBy(x => x.DistanciaAtual).FirstOrDefault(); @@ -1114,6 +1452,18 @@ namespace AgroBase.Models if (noCoredor && idxPontoMaisProximo >= 0 && idxPontoMaisProximo < _TrajetoriaFixa.Count) { int idxCorredorDentro = VerificaEquipamentoDentroCorredor(_posicaoAtual); + + if ( + idxCorredorDentro < 0 || + _TrajetoriaFixa[idxPontoMaisProximo].idxCorredor != idxCorredorDentro + ) + { + msg = "Inicio no meio da rua rejeitado: geometria e trajetoria discordam sobre o corredor atual."; + Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Falha, 0, msg); + Variaveis.MostrarLog("[TRJ] " + msg); + throw new InvalidOperationException(msg); + } + for (int i = 0; i <= idxPontoMaisProximo; i++) { _TrajetoriaFixa[i].Visitado = true; @@ -1140,73 +1490,86 @@ namespace AgroBase.Models private void VerificaFimCorredorSegmentacao() { var op = Variaveis.OperacaoEmAndamento; - // Limiar de "margem" para considerar proximidade do fim - //double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 10.0; - double limiarDistanciaMargem = op.Parametros?.Controle?.AnteciparManobraCorredorM ?? 0; - if (limiarDistanciaMargem <= 0) return; + double limiteFim = op?.Parametros?.Controle?.AnteciparManobraCorredorM ?? 0; - if (CorredorAtual == null) return; - if (CorredorAtual.DadosVisuais.JaAntecipado) return; + if ( + limiteFim <= 0 || + CorredorAtual == null || + CorredorAtual.DadosVisuais?.JaAntecipado == true || + GPSPosicaoAtual == null + ) + { + return; + } - bool modoSimulador = false && op.Simulando; + var moduloVisual = op?.Sensoriamento?.OperadorSaude?.ModulosSaude? + .FirstOrDefault(x => x.modulo == T_Code.Snr); - double distanciaCorredor = CorredorAtual.DistanciaTotal; - double distanciaRestanteCorredor = CorredorAtual.DistanciaRestante; - double distanciaUltimoPontoCorredor = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, CorredorAtual.Pontos.LastOrDefault().Posicao); - - var opVisual = op.Sensoriamento?.OperadorVisual ?? new VisualWorkerModel(); - StatusModulo opVisualStatus = op.Sensoriamento.OperadorSaude.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado; - var dadosSegmentacao = opVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel(); - - // Se não tem visual operante, não está no modoSimulador ou o corredor eh curto demais, não faz nada - if (!(opVisualStatus == StatusModulo.Operante || modoSimulador || distanciaCorredor <= limiarDistanciaMargem)) + if (moduloVisual?.status != StatusModulo.Operante) return; - // Garante estrutura de dados visuais do corredor - var dv = CorredorAtual.DadosVisuais ?? (CorredorAtual.DadosVisuais = new DadosVisuaisCorredor()); + var segmentacao = op?.Sensoriamento?.OperadorVisual?.Analises?.segmentacao; - StatusCarroMapa statusAtual = dadosSegmentacao.status_corredor; - StatusCarroMapa statusAnterior = dadosSegmentacao.status_corredor_anterior; - var agora = DateTime.Now; + if (segmentacao == null) + return; + + double agoraUnix = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0; + double idadeSegmentacao = agoraUnix - segmentacao.timestamp; + + /* Timestamp futuro tambem e invalido. */ + if (idadeSegmentacao < 0 || idadeSegmentacao > 2.0) + return; + + var dv = CorredorAtual.DadosVisuais ?? + (CorredorAtual.DadosVisuais = new DadosVisuaisCorredor()); + + double distanciaRestante = Math.Max(0, CorredorAtual.DistanciaRestante); + GPSModel ultimoGps = CorredorAtual.Pontos?.LastOrDefault()?.Posicao; + + if (ultimoGps == null) + return; + + double distanciaUltimo = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, ultimoGps); + DateTime agoraUtc = DateTime.UtcNow; - // 1) Atualiza tempo e distância por status (usando o status anterior) if (dv.UltimoUpdate > DateTime.MinValue) { - double dt = (agora - dv.UltimoUpdate).TotalSeconds; - if (dt < 0) dt = 0; + double dt = Math.Max(0, Math.Min(1.0, (agoraUtc - dv.UltimoUpdate.ToUniversalTime()).TotalSeconds)); + double deltaRestante = dv.DistanciaRestanteAnterior >= 0 + ? Math.Max(0, dv.DistanciaRestanteAnterior - distanciaRestante) + : 0; - // Aproximação da distância percorrida nesse intervalo pelo mapa - double deltaDist = 0; - if (dv.DistanciaRestanteAnterior >= 0) + double deslocamentoGnss = GPSUtils.DistanciaEntrePontos( + GPSPosicaoAnterior, + GPSPosicaoAtual + ); + + bool moveu = + !double.IsNaN(deslocamentoGnss) && + !double.IsInfinity(deslocamentoGnss) && + deslocamentoGnss >= 0.02; + + if (moveu) { - deltaDist = dv.DistanciaRestanteAnterior - distanciaRestanteCorredor; - if (deltaDist < 0) deltaDist = 0; // proteção contra ruído - } - - bool emMovimento = statusAnterior != StatusCarroMapa.Parado; - - if (emMovimento) - { - switch (statusAnterior) + switch (segmentacao.status_corredor_anterior) { case StatusCarroMapa.CaminhandoRua: case StatusCarroMapa.EntrandoRua: case StatusCarroMapa.SaindoRua: dv.TempoDentroRuaTotal += dt; - dv.DistanciaDentroRuaTotal += deltaDist; + dv.DistanciaDentroRuaTotal += deltaRestante; dv.TempoForaRuaContinuo = 0; dv.DistanciaForaRuaContinuo = 0; break; case StatusCarroMapa.Direcionando: dv.TempoForaRuaTotal += dt; - dv.DistanciaForaRuaTotal += deltaDist; + dv.DistanciaForaRuaTotal += deltaRestante; dv.TempoForaRuaContinuo += dt; - dv.DistanciaForaRuaContinuo += deltaDist; + dv.DistanciaForaRuaContinuo += Math.Max(deltaRestante, deslocamentoGnss); break; default: - // Outros estados quebram a continuidade "fora do corredor" dv.TempoForaRuaContinuo = 0; dv.DistanciaForaRuaContinuo = 0; break; @@ -1214,142 +1577,392 @@ namespace AgroBase.Models } else { - // Parado: não acumula movimento, mas quebra janela contínua dv.TempoForaRuaContinuo = 0; dv.DistanciaForaRuaContinuo = 0; } } - dv.UltimoUpdate = agora; - dv.DistanciaRestanteAnterior = distanciaRestanteCorredor; + dv.UltimoUpdate = agoraUtc; + dv.DistanciaRestanteAnterior = distanciaRestante; - // 2) Confirmar se este corredor realmente existiu (teve cana) - if (!dv.CorredorConfirmado) + if ( + !dv.CorredorConfirmado && + (dv.DistanciaDentroRuaTotal >= 5.0 || dv.TempoDentroRuaTotal >= 10.0) + ) { - const double MIN_DIST_CORREDOR_CONFIRMADO = 5.0; // metros dentro da rua - const double MIN_TEMPO_CORREDOR_CONFIRMADO = 10.0; // segundos dentro da rua - if (dv.DistanciaDentroRuaTotal >= MIN_DIST_CORREDOR_CONFIRMADO || dv.TempoDentroRuaTotal >= MIN_TEMPO_CORREDOR_CONFIRMADO) - { - dv.CorredorConfirmado = true; - } + dv.CorredorConfirmado = true; } - // 3) Critérios para considerar que chegou ao fim do corredor antes do mapa acabar + bool pertoDoFim = + distanciaRestante <= limiteFim && + distanciaUltimo <= limiteFim; - double tsAgora = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0; - bool dadoFresco = (tsAgora - dadosSegmentacao.timestamp) < 2.0; + bool visualDeclarouFim = + segmentacao.status_corredor == StatusCarroMapa.Direcionando; - bool pertoDoFim = distanciaRestanteCorredor < limiarDistanciaMargem && distanciaUltimoPontoCorredor < limiarDistanciaMargem; + bool confirmacaoContinua = + dv.TempoForaRuaContinuo >= 1.0 || + dv.DistanciaForaRuaContinuo >= 1.0; - bool foraDoCorredorAgora = (statusAtual == StatusCarroMapa.Direcionando); + if (!(dv.CorredorConfirmado && pertoDoFim && visualDeclarouFim && confirmacaoContinua)) + return; - // Limiar de "quanto tempo/distância já estou fora do corredor" para evitar buracos pontuais - const double MIN_TEMPO_FORA_CORREDOR = 1.0; // segundos - const double MIN_DIST_FORA_CORREDOR = 1.0; // metros + bool aplicado = CorredorAtual.Ultimo + ? ConcluirUltimoCorredorPorAntecipacaoVisual() + : TentarAplicarAntecipacaoVisual(op); - bool foraContinuoSuficiente = dv.TempoForaRuaContinuo >= MIN_TEMPO_FORA_CORREDOR || dv.DistanciaForaRuaContinuo >= MIN_DIST_FORA_CORREDOR; + if (!aplicado) + return; - bool podeUsarSegmentacao = modoSimulador || (dadoFresco && dv.CorredorConfirmado); + dv.JaAntecipado = true; - CorredorAtual.DadosVisuais.JaAntecipado = podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente; + Variaveis.MostrarLog( + $"[TRJ] Fim visual antecipado aplicado no corredor {CorredorAtual?.Idx + 1}." + ); - //Console.WriteLine($"podeUsarSegmentacao: {podeUsarSegmentacao}, pertoDoFim: {pertoDoFim}, foraDoCorredorAgora: {foraDoCorredorAgora}, foraContinuoSuficiente: {foraContinuoSuficiente}"); + op?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Operante, + 100, + $"Fim visual antecipado aplicado no corredor {CorredorAtual?.Idx + 1}." + ); + } - if (CorredorAtual.DadosVisuais.JaAntecipado) + private bool ConcluirUltimoCorredorPorAntecipacaoVisual() + { + if (CorredorAtual?.Pontos == null) + return false; + + var estadosVisita = CorredorAtual.Pontos + .Select(x => new { Ponto = x, x.Visitado }) + .ToList(); + + try { - List pontosRemover = CorredorAtual.Pontos.Where(x => !x.Visitado).ToList(); - pontosRemover.ForEach(x => x.Visitado = true); + foreach (var ponto in CorredorAtual.Pontos.Where(x => !x.Visitado)) + ponto.Visitado = true; - if (!CorredorAtual.Ultimo) - { - foreach (var ponto in pontosRemover) - { - CorredorAtual.Pontos.Remove(ponto); - _TrajetoriaFixa.Remove(ponto); - } + MarcarPontosIntermediariosNaoVisitados(); + AtualizarEstruturaAposMutacaoTrajetoria(); + return true; + } + catch + { + foreach (var estado in estadosVisita) + estado.Ponto.Visitado = estado.Visitado; - var pls = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 1]; - if (CorredorAtual.Pontos.Count >= 2) - { - var pbs = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 2]; - pls.AlterarTipo(TipoPontoRua.LigacaoSaida); - pbs.AlterarTipo(TipoPontoRua.BordaSaida); - _TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pls))?.AlterarTipo(pls.Tipo); - _TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pbs))?.AlterarTipo(pbs.Tipo); - } + try { AtualizarEstruturaAposMutacaoTrajetoria(); } + catch { } - // acha o ponto mais próximo do corredor seguinte - var pc = _Corredores[CorredorAtual.Idx + 1]; - (GPSModel pp, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(GPSPosicaoAtual, pc.Pontos.Select(x => x.Posicao).ToList()); - var _pp = pc.Pontos.FirstOrDefault(x => Math.Abs(x.Posicao.Latitude - pp.Latitude) < 1e-8 && Math.Abs(x.Posicao.Longitude - pp.Longitude) < 1e-8); - - double anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda ? -AnguloAberturaCurva : AnguloAberturaCurva; - double anguloDesvio = PontoAtual.Orientacao + anguloControle; - double fd = FuncoesMatematicas.Map(op.Controle.PercentualVelocidadeSP, 10, 100, 0.7, 2.5); - - var pontoDesvio = GPSUtils.GerarPontoDeslocado(pls.Posicao, anguloDesvio, DistanciaProjecaoRua * fd); - - - double _FatorLarguraCorredor = CorredorAtual.Largura / LarguraCorredorPadrao; - var proximoPontoProximoCorredor = pc.Pontos[_pp.idxPontoCorredor].Posicao; - List CurvaConexao = CriarCurvaEntrePontos(pontoDesvio, proximoPontoProximoCorredor, DistanciaProjecaoRua * 1.5, pls.Orientacao, 6, false); - CurvaConexao.Insert(0, pontoDesvio); - - - IncluirDesvioNaTrajetoria(CurvaConexao, pls.idxPonto, pls.LarguraCorredor * 0.8); - - - - // Garante que os primeiros pontos do corredor atual estão como visitados - if (_pp != null) - { - int idxProx = _pp.idxPontoCorredor; - - // garante que não vai remover tudo (pra sobrar pelo menos 2 pontos) - int qtdRemoverProx = Math.Max(0, Math.Min(idxProx, pc.Pontos.Count - 2)); - - var pontosRemoverProx = pc.Pontos.Take(qtdRemoverProx).ToList(); - - foreach (var p in pontosRemoverProx) - { - pc.Pontos.Remove(p); - _TrajetoriaFixa.Remove(p); - } - - if (pc.Pontos.Count >= 2) - { - var ple = pc.Pontos[0]; - var pbe = pc.Pontos[1]; - ple.AlterarTipo(TipoPontoRua.LigacaoEntrada); - pbe.AlterarTipo(TipoPontoRua.BordaEntrada); - _TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, ple))?.AlterarTipo(ple.Tipo); - _TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pbe))?.AlterarTipo(pbe.Tipo); - } - } - - ReindexarTrajetoriaFixa(); - - op.Sensoriamento.AtualizarDados(); - HealthWorkerService.AtualizarDadosOperacao(true); - } + throw; } } + private bool TentarAplicarAntecipacaoVisual(OperacaoModel op) + { + if ( + CorredorAtual == null || + _Corredores == null || + CorredorAtual.Idx + 1 >= _Corredores.Count || + _TrajetoriaFixa == null + ) + { + return false; + } + var atual = CorredorAtual; + var proximo = _Corredores[atual.Idx + 1]; + var pontosAtuais = atual.Pontos?.OrderBy(x => x.idxPontoCorredor).ToList(); + var pontosProximos = proximo.Pontos?.OrderBy(x => x.idxPontoCorredor).ToList(); + + if (pontosAtuais == null || pontosAtuais.Count < 2 || pontosProximos == null || pontosProximos.Count < 2) + return false; + + int idxUltimoVisitadoAtual = pontosAtuais.FindLastIndex(x => x.Visitado); + + if (idxUltimoVisitadoAtual < 1) + return false; + + var mantidosAtual = pontosAtuais.Take(idxUltimoVisitadoAtual + 1).ToList(); + var saida = mantidosAtual[mantidosAtual.Count - 1]; + var anteriorSaida = mantidosAtual[mantidosAtual.Count - 2]; + + /* + * A entrada so pode ser escolhida no inicio logico do proximo + * corredor. Procurar na rua inteira permitiria entrar pelo lado + * oposto e reproduzir exatamente o comportamento observado na BP. + */ + int janelaEntrada = Math.Min(12, pontosProximos.Count); + int idxEntrada = -1; + double melhorDistancia = double.MaxValue; + + for (int i = 0; i < janelaEntrada; i++) + { + double distancia = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, pontosProximos[i].Posicao); + + if (distancia < melhorDistancia) + { + melhorDistancia = distancia; + idxEntrada = i; + } + } + + if (idxEntrada < 0 || idxEntrada > pontosProximos.Count - 2) + return false; + + var mantidosProximo = pontosProximos.Skip(idxEntrada).ToList(); + var entrada = mantidosProximo[0]; + + double headingSaida = GPSUtils.CalcularOrientacao(anteriorSaida.Posicao, saida.Posicao); + double fatorVelocidade = FuncoesMatematicas.Map( + op?.Controle?.PercentualVelocidadeSP ?? 0, + 10, + 100, + 0.7, + 2.0 + ); + + fatorVelocidade = Math.Max(0.7, Math.Min(2.0, fatorVelocidade)); + double distanciaAbertura = Math.Max(0.70, Math.Min(6.0, DistanciaProjecaoRua * fatorVelocidade)); + + GPSModel candidatoPositivo = GPSUtils.GerarPontoDeslocado( + saida.Posicao, + headingSaida + AnguloAberturaCurva, + distanciaAbertura + ); + + GPSModel candidatoNegativo = GPSUtils.GerarPontoDeslocado( + saida.Posicao, + headingSaida - AnguloAberturaCurva, + distanciaAbertura + ); + + GPSModel pontoAbertura = + GPSUtils.DistanciaEntrePontos(candidatoPositivo, entrada.Posicao) >= + GPSUtils.DistanciaEntrePontos(candidatoNegativo, entrada.Posicao) + ? candidatoPositivo + : candidatoNegativo; + + var curvaGps = CriarCurvaEntrePontos( + pontoAbertura, + entrada.Posicao, + Math.Max(1.0, DistanciaProjecaoRua * 1.5), + headingSaida, + 8, + false + ); + + curvaGps.Insert(0, pontoAbertura); + + if (curvaGps.Count < 2 || curvaGps.Any(x => !PontoGpsValido(x))) + return false; + + /* + * O ponto onde a percepcao encerrou a rua vira a borda real. + * A ligacao de saida deve ser o ultimo ponto da curva ainda + * pertencente ao corredor atual; isso preserva a ordem + * Rua -> BordaSaida -> Desvio -> LigacaoSaida. + */ + int idxGlobalSaida = _TrajetoriaFixa.IndexOf(saida); + + if (idxGlobalSaida < 0) + return false; + + var novaTrajetoria = _TrajetoriaFixa.Take(idxGlobalSaida + 1).ToList(); + GPSModel pontoOrientacaoAnterior = saida.Posicao; + + for (int i = 0; i < curvaGps.Count; i++) + { + GPSModel gps = curvaGps[i]; + TipoPontoRua tipoCurva = i == curvaGps.Count - 1 + ? TipoPontoRua.LigacaoSaida + : TipoPontoRua.Desvio; + + novaTrajetoria.Add( + new PontoTrajetoriaModel(tipoCurva) + { + idxCorredor = atual.Idx, + Posicao = gps, + Direcao = DirecaoCarroRua.Manobra, + LarguraCorredor = Math.Max(0.50, atual.Largura * 0.8), + Orientacao = GPSUtils.CalcularOrientacao(pontoOrientacaoAnterior, gps), + Visitado = false + } + ); + + pontoOrientacaoAnterior = gps; + } + + novaTrajetoria.AddRange(mantidosProximo); + + int idxDepoisProximo = _TrajetoriaFixa.FindLastIndex(x => x.idxCorredor == proximo.Idx); + + if (idxDepoisProximo >= 0 && idxDepoisProximo + 1 < _TrajetoriaFixa.Count) + novaTrajetoria.AddRange(_TrajetoriaFixa.Skip(idxDepoisProximo + 1)); + + var trajetoriaAnterior = _TrajetoriaFixa; + var tiposAnteriores = new[] + { + new { Ponto = saida, Tipo = saida.Tipo }, + new { Ponto = entrada, Tipo = entrada.Tipo }, + new { Ponto = mantidosProximo[1], Tipo = mantidosProximo[1].Tipo } + }; + + try + { + saida.AlterarTipo(TipoPontoRua.BordaSaida); + entrada.AlterarTipo(TipoPontoRua.LigacaoEntrada); + mantidosProximo[1].AlterarTipo(TipoPontoRua.BordaEntrada); + + _TrajetoriaFixa = novaTrajetoria; + AtualizarEstruturaAposMutacaoTrajetoria(); + } + catch + { + _TrajetoriaFixa = trajetoriaAnterior; + + foreach (var estado in tiposAnteriores) + estado.Ponto.AlterarTipo(estado.Tipo); + + try { AtualizarEstruturaAposMutacaoTrajetoria(); } + catch { } + + throw; + } + + return true; + } + + private void AtualizarEstruturaAposMutacaoTrajetoria() + { + if (_TrajetoriaFixa == null) + return; + + for (int i = 0; i < _TrajetoriaFixa.Count; i++) + { + var ponto = _TrajetoriaFixa[i]; + ponto.idxPonto = i; + + if (i > 0) + ponto.Orientacao = GPSUtils.CalcularOrientacao(_TrajetoriaFixa[i - 1].Posicao, ponto.Posicao); + } + + ValidarTrajetoriaGerada(_TrajetoriaFixa); + + foreach (var corredor in _Corredores ?? new List()) + { + corredor.Pontos = _TrajetoriaFixa + .Where(x => x.Tipo != TipoPontoRua.PosicaoRobo && x.idxCorredor == corredor.Idx) + .ToList(); + + for (int i = 0; i < corredor.Pontos.Count; i++) + corredor.Pontos[i].idxPontoCorredor = i; + + corredor.QtdPontos = corredor.Pontos.Count; + corredor.DistanciaTotal = GPSUtils.DistanciaDoTrecho(corredor.Pontos.Select(x => x.Posicao).ToList()); + corredor.Ultimo = corredor.Idx == _Corredores.Count - 1; + corredor.AtualizarDados(); + } + + DistanciaTotal = GPSUtils.DistanciaDoTrecho( + _TrajetoriaFixa + .Where(x => x.Tipo != TipoPontoRua.PosicaoRobo) + .Select(x => x.Posicao) + .ToList() + ); + + DefinirPontoAtual(); + AtualizarCorredorAtual(); + DefinirProximoPonto(); + AtualizarTrajetoriaDinamicaCore(); + AtualizarDistanciaRestante(); + AtualizarPercentualTrajetoria(); + } public void AtualizarDadosAutonomiaCorredor(int? idxCorredor = null, bool? bat_liberada = null, bool? herb_liberado = null) { - if (idxCorredor == null) idxCorredor = CorredorAtual?.Idx ?? 0; - if (bat_liberada == null) bat_liberada = false; - if (herb_liberado == null) herb_liberado = false; - AutonomiaCorredor.AtualizarDados((int)idxCorredor, (bool)bat_liberada, (bool)herb_liberado); + lock (_syncTrajetoria) + { + if (idxCorredor == null) idxCorredor = CorredorAtual?.Idx ?? 0; + if (bat_liberada == null) bat_liberada = false; + if (herb_liberado == null) herb_liberado = false; + + if (AutonomiaCorredor == null) + AutonomiaCorredor = new AutonomiaCorredorModel(); + + AutonomiaCorredor.AtualizarDados( + (int)idxCorredor, + (bool)bat_liberada, + (bool)herb_liberado + ); + } } public void LoopAtualizaDados() + { + if (System.Threading.Interlocked.CompareExchange(ref _loopEmExecucao, 1, 0) != 0) + return; + + bool lockObtido = false; + + try + { + var parGps = ObterParGpsCoerente(); + System.Threading.Monitor.TryEnter(_syncTrajetoria, ref lockObtido); + + /* + * Geracao, retorno ou desvio estao alterando a rota. Nao + * enfileiramos um ciclo GNSS antigo; a proxima amostra tenta + * novamente com um snapshot novo. + */ + if (!lockObtido) + return; + + AplicarSnapshotGpsCiclo(parGps); + + try + { + LoopAtualizaDadosCore(); + } + catch (Exception ex) + { + Variaveis.MostrarLog("[TRJ] Falha no ciclo da trajetoria: " + ex.Message); + Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Falha, + 0, + "Falha no ciclo da trajetoria: " + ex.Message + ); + + /* + * Falha fechada: sem um estado de trajetoria coerente, + * a operacao automatica deixa de estar liberada. + */ + RedisService.AtualizarCampos( + CtxKey.DadosOperacao, + ("liberado", false), + ("motivo_nao_liberado", "Falha interna na trajetoria: " + ex.Message) + ); + } + finally + { + LimparSnapshotGpsCiclo(); + } + } + finally + { + if (lockObtido) + System.Threading.Monitor.Exit(_syncTrajetoria); + + System.Threading.Interlocked.Exchange(ref _loopEmExecucao, 0); + } + } + + private void LoopAtualizaDadosCore() { var op = Variaveis.OperacaoEmAndamento; - if (!(op.Parametros?.ControleAutomatico ?? false)) + if (!(op?.Parametros?.ControleAutomatico ?? false)) return; //AtualizarPropriedadesPontosTrajetoria(); @@ -1661,7 +2274,11 @@ namespace AgroBase.Models foreach (var ponto in pontosCorredorAtual .Skip(Math.Max(0, pontosCorredorAtual.Count - 8))) { - ponto.AtualizarPropriedades(); + ponto.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); } CorredorAtual.AtualizarDados(); @@ -1672,6 +2289,42 @@ namespace AgroBase.Models int pontosRestantesCorredorAtual = pontosCorredorAtual.Count(x => !x.Visitado); + var proximoCorredor = + _Corredores[idxProximoCorredor]; + + var pontosProximoCorredor = + proximoCorredor.Pontos? + .OrderBy(x => x.idxPontoCorredor) + .ToList(); + + if (pontosProximoCorredor == null || + pontosProximoCorredor.Count == 0) + { + ResetarCandidatoTransicaoGeometrica(); + return false; + } + + /* + * Recuperação forte de transição: + * + * 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. + */ + if (TentarRecuperarTransicaoPorEntradaConfirmada( + idxCorredorAtual, + idxProximoCorredor, + pontosProximoCorredor, + pontosRestantesCorredorAtual + )) + { + return true; + } + /* * Regra soberana: * @@ -1691,13 +2344,8 @@ namespace AgroBase.Models return false; } - var proximoCorredor = - _Corredores[idxProximoCorredor]; - var primeiroPontoProximoCorredor = - proximoCorredor.Pontos? - .OrderBy(x => x.idxPontoCorredor) - .FirstOrDefault(); + pontosProximoCorredor.FirstOrDefault(); if (primeiroPontoProximoCorredor == null) { @@ -1712,7 +2360,11 @@ namespace AgroBase.Models * * LigacaoEntrada -> BordaEntrada -> ponto interno. */ - primeiroPontoProximoCorredor.AtualizarPropriedades(); + primeiroPontoProximoCorredor.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); double distanciaEntrada = primeiroPontoProximoCorredor.DistanciaAtual; @@ -1734,6 +2386,8 @@ namespace AgroBase.Models */ primeiroPontoProximoCorredor.Visitado = true; + ResetarCandidatoTransicaoGeometrica(); + DefinirPontoAtual(); AtualizarCorredorAtual(); DefinirProximoPonto(); @@ -1755,6 +2409,230 @@ namespace AgroBase.Models return true; } + private bool TentarRecuperarTransicaoPorEntradaConfirmada( + int idxCorredorAtual, + int idxProximoCorredor, + List pontosProximoCorredor, + int pontosRestantesCorredorAtual + ) + { + if (GPSPosicaoAtual == null || + PontoAtual == null || + pontosProximoCorredor == null || + pontosProximoCorredor.Count < 2) + { + ResetarCandidatoTransicaoGeometrica(); + return false; + } + + /* + * 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. + */ + int idxCorredorGeometrico = + VerificaEquipamentoDentroCorredor(GPSPosicaoAtual); + + if (idxCorredorGeometrico != idxProximoCorredor) + { + ResetarCandidatoTransicaoGeometrica(); + return false; + } + + int limite = Math.Min( + pontosProximoCorredor.Count - 1, + MaxPontosEntradaRecuperacaoTransicao + ); + + int idxUltimoRecuperavel = -1; + int quantidadePontosNovoCorredor = 0; + bool entradaFisicaConfirmada = false; + double maiorErroOrientacaoGraus = 0.0; + + for (int i = 0; i < limite; i++) + { + var ponto = pontosProximoCorredor[i]; + var proximo = pontosProximoCorredor[i + 1]; + + if (ponto.idxCorredor != idxProximoCorredor || + proximo.idxCorredor != idxProximoCorredor) + { + break; + } + + ponto.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); + proximo.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); + + var progresso = + CalcularProgressoNoSegmento( + ponto.Posicao, + proximo.Posicao, + GPSPosicaoAtual + ); + + double erroOrientacaoGraus; + + if (!PodeRecuperarPontoDeixadoParaTras( + ponto, + proximo.Posicao, + progresso, + out erroOrientacaoGraus + )) + { + /* + * O prefixo é contínuo: se este ponto ainda não ficou + * para trás, nenhum ponto posterior pode ser confirmado. + */ + break; + } + + idxUltimoRecuperavel = ponto.idxPonto; + quantidadePontosNovoCorredor++; + maiorErroOrientacaoGraus = Math.Max( + maiorErroOrientacaoGraus, + erroOrientacaoGraus + ); + + /* + * 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) + { + entradaFisicaConfirmada = true; + } + } + + if (!entradaFisicaConfirmada || + idxUltimoRecuperavel <= PontoAtual.idxPonto) + { + ResetarCandidatoTransicaoGeometrica(); + return false; + } + + if (_idxCorredorCandidatoTransicao != idxProximoCorredor) + { + _idxCorredorCandidatoTransicao = idxProximoCorredor; + _confirmacoesCandidatoTransicao = 0; + _amostraGpsCandidatoTransicao = null; + } + + if (RegistrarNovaAmostraGpsCandidatoTransicao()) + { + _confirmacoesCandidatoTransicao++; + } + + if (_confirmacoesCandidatoTransicao < + ConfirmacoesMinimasEntradaProximoCorredor) + { + return false; + } + + 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. + */ + MarcarVisitadoAte(idxUltimoRecuperavel); + ResetarCandidatoTransicaoGeometrica(); + + DefinirPontoAtual(); + AtualizarCorredorAtual(); + DefinirProximoPonto(); + + Variaveis + .OperacaoEmAndamento + .Sensoriamento? + .InserirLog( + 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" + ); + + return true; + } + + private bool RegistrarNovaAmostraGpsCandidatoTransicao() + { + var gps = GPSPosicaoAtual; + + if (gps == null) + return false; + + double timestampPos = + gps.TimestampPos?.valor ?? + double.NaN; + + if (ValorFinito(timestampPos)) + { + if (ValorFinito(_timestampPosCandidatoTransicao) && + Math.Abs( + timestampPos - + _timestampPosCandidatoTransicao + ) <= ToleranciaCoordenada) + { + return false; + } + + _timestampPosCandidatoTransicao = timestampPos; + _momentoCandidatoTransicao = gps.Momento; + _amostraGpsCandidatoTransicao = gps; + return true; + } + + if (gps.Momento != DateTime.MinValue) + { + if (gps.Momento == _momentoCandidatoTransicao) + return false; + + _momentoCandidatoTransicao = gps.Momento; + _amostraGpsCandidatoTransicao = gps; + return true; + } + + /* + * Compatibilidade defensiva para simuladores que não publicam + * TimestampPos nem Momento. Dentro do mesmo ciclo o snapshot é + * a mesma instância; no ciclo seguinte GetSnapshot() devolve um + * novo clone. + */ + if (ReferenceEquals(_amostraGpsCandidatoTransicao, gps)) + return false; + + _amostraGpsCandidatoTransicao = gps; + return true; + } + + private void ResetarCandidatoTransicaoGeometrica() + { + _idxCorredorCandidatoTransicao = -1; + _confirmacoesCandidatoTransicao = 0; + _amostraGpsCandidatoTransicao = null; + _timestampPosCandidatoTransicao = double.NaN; + _momentoCandidatoTransicao = DateTime.MinValue; + } + private bool RecuperarPontosDeixadosParaTras() { if (!_TrajetoriaFixaDefinida || @@ -1776,6 +2654,9 @@ namespace AgroBase.Models CorredorAtual?.Idx ?? PontoAtual.idxCorredor; + if (idxAtual >= _TrajetoriaFixa.Count - 1) + return false; + int idxPrimeiroNaoVisitado = _TrajetoriaFixa.FindIndex( idxAtual + 1, @@ -1787,6 +2668,8 @@ namespace AgroBase.Models bool recuperou = false; int quantidadeRecuperada = 0; + int quantidadeEstruturalRecuperada = 0; + double maiorErroOrientacaoGraus = 0.0; int idxLimite = Math.Min( _TrajetoriaFixa.Count - 1, @@ -1806,43 +2689,97 @@ namespace AgroBase.Models if (ponto.idxCorredor != idxCorredorAtual) break; + ponto.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); + + GPSModel fimReferencia = null; + /* - * Borda e ligação são confirmadas pelos fluxos - * específicos de entrada, saída e transição. + * Preferimos o segmento que sai do ponto candidato + * em direção ao próximo ponto do MESMO corredor. + * Assim a recuperação respeita o sentido real da rota. */ - if (EhPontoEstrutural(ponto)) + if (i + 1 < _TrajetoriaFixa.Count) + { + var proximo = _TrajetoriaFixa[i + 1]; + + if (proximo.idxCorredor == idxCorredorAtual) + { + proximo.AtualizarPropriedades( + _gpsAtualCiclo, + _gpsAnteriorCiclo, + permitirMarcarVisitado: false + ); + + fimReferencia = proximo.Posicao; + } + } + + /* + * O último ponto do corredor não possui um próximo ponto + * local. Nesse caso prolongamos o último segmento em 1 m, + * sem usar o primeiro ponto do corredor seguinte. Isso + * permite recuperar uma LigacaoSaida ultrapassada sem + * autorizar um salto entre corredores. + */ + if (fimReferencia == null && i > 0) + { + var anterior = _TrajetoriaFixa[i - 1]; + + if (anterior.idxCorredor == idxCorredorAtual) + { + double orientacaoUltimoSegmento = + GPSUtils.CalcularOrientacao( + anterior.Posicao, + ponto.Posicao + ); + + if (ValorFinito(orientacaoUltimoSegmento)) + { + fimReferencia = + GPSUtils.GerarPontoDeslocado( + ponto.Posicao, + orientacaoUltimoSegmento, + 1.0 + ); + } + } + } + + if (fimReferencia == null) break; - if (i + 1 >= _TrajetoriaFixa.Count) - break; - - var proximo = _TrajetoriaFixa[i + 1]; - - if (proximo.idxCorredor != idxCorredorAtual) - break; - - ponto.AtualizarPropriedades(); - proximo.AtualizarPropriedades(); - var progresso = CalcularProgressoNoSegmento( ponto.Posicao, - proximo.Posicao, + fimReferencia, GPSPosicaoAtual ); /* - * avançoLongitudinal: + * Para considerar um ponto ultrapassado exigimos: * - * < 0 -> rover ainda está antes do ponto - * = 0 -> rover está na perpendicular do ponto - * > 0 -> ponto ficou atrás do rover + * - mesmo corredor (validado acima); + * - rover longitudinalmente à frente do ponto; + * - rover próximo à linha local da trajetória; + * - heading do rover compatível com o sentido da rota. + * + * Bordas e ligações usam limites mais rígidos. Portanto, + * elas podem ser recuperadas dentro do próprio corredor, + * mas não apenas por proximidade e nunca pelo segmento do + * corredor seguinte. */ + double erroOrientacaoGraus; bool pontoFicouParaTras = - progresso.avancoLongitudinalM >= - -ToleranciaLongitudinalAntesPontoM && - progresso.distanciaLateralM <= - ObterToleranciaLateralRecuperacao(ponto); + PodeRecuperarPontoDeixadoParaTras( + ponto, + fimReferencia, + progresso, + out erroOrientacaoGraus + ); if (!pontoFicouParaTras) { @@ -1858,6 +2795,13 @@ namespace AgroBase.Models recuperou = true; quantidadeRecuperada++; + maiorErroOrientacaoGraus = Math.Max( + maiorErroOrientacaoGraus, + erroOrientacaoGraus + ); + + if (EhPontoEstrutural(ponto)) + quantidadeEstruturalRecuperada++; } if (!recuperou) @@ -1876,12 +2820,90 @@ namespace AgroBase.Models 100, "[TRJ] Pontos deixados para trás recuperados. " + $"qtd={quantidadeRecuperada}, " + + $"estruturais={quantidadeEstruturalRecuperada}, " + + $"erro_heading_max={maiorErroOrientacaoGraus:F1}graus, " + $"novo_atual={PontoAtual?.idxPonto}, " + $"proximo={ProximoPonto?.idxPonto}" ); return true; } + private bool PodeRecuperarPontoDeixadoParaTras( + PontoTrajetoriaModel ponto, + GPSModel fimReferencia, + (double avancoLongitudinalM, double distanciaLateralM, double parametroSegmento) progresso, + out double erroOrientacaoGraus + ) + { + erroOrientacaoGraus = double.PositiveInfinity; + + if (ponto?.Posicao == null || + fimReferencia == null || + GPSPosicaoAtual == null || + !ValorFinito(progresso.avancoLongitudinalM) || + !ValorFinito(progresso.distanciaLateralM) || + !ValorFinito(progresso.parametroSegmento)) + { + return false; + } + + bool estrutural = EhPontoEstrutural(ponto); + + /* + * Ponto comum mantém 10 cm de tolerância para ruído do GNSS. + * Ponto estrutural precisa ter sido efetivamente alcançado: + * não aceitamos avanço longitudinal negativo em borda/ligação. + */ + double avancoMinimo = + estrutural + ? 0.0 + : -ToleranciaLongitudinalAntesPontoM; + + if (progresso.avancoLongitudinalM < avancoMinimo || + progresso.parametroSegmento < + (estrutural ? 0.0 : -0.10) || + progresso.distanciaLateralM > + ObterToleranciaLateralRecuperacao(ponto)) + { + return false; + } + + double orientacaoTrajetoria = + GPSUtils.NormalizarAngulo( + GPSUtils.CalcularOrientacao( + ponto.Posicao, + fimReferencia + ) + ); + + double orientacaoRobo = + GPSUtils.NormalizarAngulo( + GPSPosicaoAtual.AnguloCarroDefinido + ); + + if (!ValorFinito(orientacaoTrajetoria) || + !ValorFinito(orientacaoRobo)) + { + return false; + } + + erroOrientacaoGraus = Math.Abs( + GPSUtils.CalcularDiferencaAngulo( + orientacaoTrajetoria, + orientacaoRobo + ) + ); + + if (!ValorFinito(erroOrientacaoGraus)) + return false; + + double erroMaximo = + estrutural + ? ErroOrientacaoMaximoRecuperacaoEstruturalGraus + : ErroOrientacaoMaximoRecuperacaoGraus; + + return erroOrientacaoGraus <= erroMaximo; + } private static bool EhPontoEstrutural(PontoTrajetoriaModel ponto) { if (ponto == null) @@ -1990,6 +3012,78 @@ namespace AgroBase.Models ); } + private static List InterpolarRotaPorDistancia( + List rotaOriginal, + int novaQuantidadeDePontos + ) + { + if (rotaOriginal == null) + throw new ArgumentNullException(nameof(rotaOriginal)); + + var rota = rotaOriginal + .Where(PontoGpsValido) + .Select(ClonarPontoGeometrico) + .ToList(); + + if (rota.Count < 2) + throw new InvalidOperationException("A rota precisa de pelo menos dois pontos validos para interpolacao."); + + novaQuantidadeDePontos = Math.Max(2, novaQuantidadeDePontos); + + var acumuladas = new double[rota.Count]; + double distanciaTotal = 0; + + for (int i = 1; i < rota.Count; i++) + { + double segmento = GPSUtils.DistanciaEntrePontos(rota[i - 1], rota[i]); + + if (double.IsNaN(segmento) || double.IsInfinity(segmento) || segmento < 0) + throw new InvalidOperationException($"Segmento invalido durante interpolacao: {i - 1}->{i}."); + + distanciaTotal += segmento; + acumuladas[i] = distanciaTotal; + } + + if (distanciaTotal <= 0.001) + throw new InvalidOperationException("A rota possui comprimento insuficiente para interpolacao."); + + var resultado = new List(novaQuantidadeDePontos); + int idxSegmento = 0; + + for (int i = 0; i < novaQuantidadeDePontos; i++) + { + double alvo = distanciaTotal * i / (novaQuantidadeDePontos - 1.0); + + while ( + idxSegmento < rota.Count - 2 && + acumuladas[idxSegmento + 1] < alvo + ) + { + idxSegmento++; + } + + double inicio = acumuladas[idxSegmento]; + double fim = acumuladas[idxSegmento + 1]; + double comprimento = fim - inicio; + double proporcao = comprimento <= 1e-9 ? 0 : (alvo - inicio) / comprimento; + + proporcao = Math.Max(0, Math.Min(1, proporcao)); + + resultado.Add( + GPSUtils.InterpolarPonto( + rota[idxSegmento], + rota[idxSegmento + 1], + proporcao + ) + ); + } + + resultado[0] = ClonarPontoGeometrico(rota[0]); + resultado[resultado.Count - 1] = ClonarPontoGeometrico(rota[rota.Count - 1]); + + return resultado; + } + private (List>, List) ProjetarCorredores() { @@ -2040,29 +3134,21 @@ namespace AgroBase.Models throw new InvalidOperationException($"Extensão do corredor inválida: {DistanciaExtensaoCorredorM}."); } - /* - * Calcula o deslocamento lateral do GPS. - * - * Mantive a fórmula atual para não alterar a calibração existente. - * O tratamento do sinal do offset pode ser revisado separadamente. - */ + double distanciaProjecao = DistanciaProjecaoRua; - double larguraTotal = VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita; - - if (larguraTotal <= toleranciaNumero || NumeroInvalido(larguraTotal)) + if (NumeroInvalido(distanciaProjecao) || distanciaProjecao < 0.20 || distanciaProjecao > 20.0) { - throw new InvalidOperationException($"Largura total do equipamento inválida: {larguraTotal}."); + throw new InvalidOperationException($"Distancia de manobra invalida: {distanciaProjecao:F2} m."); } - double diferencaLateral = Math.Abs(VariaveisEquipamento.LarguraEsquerda - VariaveisEquipamento.LarguraDireita); - - double deslocamentoLateralGPSpercentual = (diferencaLateral / larguraTotal) / 2.0; - - double deslocamentoLateralGPSmetros = LarguraCorredorPadrao * deslocamentoLateralGPSpercentual; - - if (NumeroInvalido(deslocamentoLateralGPSmetros)) + if (NumeroInvalido(AnguloAberturaCurva) || AnguloAberturaCurva < 0 || AnguloAberturaCurva > 80.0) { - throw new InvalidOperationException("O cálculo do deslocamento lateral do GPS resultou em um valor inválido."); + throw new InvalidOperationException($"Angulo de abertura de curva invalido: {AnguloAberturaCurva:F1} graus."); + } + + if (NumeroInvalido(LarguraCorredorPadrao) || LarguraCorredorPadrao < 0.30 || LarguraCorredorPadrao > 10.0) + { + throw new InvalidOperationException($"Largura padrao de corredor invalida: {LarguraCorredorPadrao:F2} m."); } /* @@ -2074,7 +3160,10 @@ namespace AgroBase.Models if (Variaveis.OperacaoEmAndamento?.Mapa?.TipoMapa == TipoMapaOperacao.Corredores) { - ruasParaProjetar = ProjetarRuasApartirDosCorredores(RuasPlantacao); + throw new NotSupportedException( + "O tipo de mapa Corredores ainda nao possui geracao validada para campo. " + + "Use LinhasPlantio ate que esse fluxo tenha mapas e ensaios proprios." + ); } else { @@ -2104,7 +3193,12 @@ namespace AgroBase.Models throw new InvalidOperationException($"A rua de índice {i} possui menos de dois pontos válidos."); } - ruasNormalizadas.Add(new List(ruaOriginal)); + if (ruaOriginal.Any(x => !PontoGpsValido(x))) + { + throw new InvalidOperationException($"A rua de indice {i} possui coordenadas invalidas."); + } + + ruasNormalizadas.Add(ruaOriginal.Select(ClonarPontoGeometrico).ToList()); } /* @@ -2123,7 +3217,7 @@ namespace AgroBase.Models DistanciaSegura(ruaAnterior[0], ruaAtual[0], $"início das ruas {i - 1} e {i}") + DistanciaSegura(ruaAnterior[ruaAnterior.Count - 1], ruaAtual[ruaAtual.Count - 1], $"final das ruas {i - 1} e {i}"); - double distanciaSentidoInvertido = + double distanciaSentidoInvertido = DistanciaSegura(ruaAnterior[0], ruaAtual[ruaAtual.Count - 1], $"início da rua {i - 1} e final da rua {i}") + DistanciaSegura(ruaAnterior[ruaAnterior.Count - 1], ruaAtual[0], $"final da rua {i - 1} e início da rua {i}"); @@ -2131,6 +3225,30 @@ namespace AgroBase.Models { ruaAtual.Reverse(); } + + double anguloAnterior = GPSUtils.CalcularOrientacao( + ruaAnterior[0], + ruaAnterior[ruaAnterior.Count - 1] + ); + + double anguloAtual = GPSUtils.CalcularOrientacao( + ruaAtual[0], + ruaAtual[ruaAtual.Count - 1] + ); + + double divergencia = Math.Abs( + GPSUtils.CalcularDiferencaAngulo(anguloAtual, anguloAnterior) + ); + + divergencia = Math.Min(divergencia, Math.Abs(180.0 - divergencia)); + + if (divergencia > 45.0) + { + throw new InvalidOperationException( + $"As ruas {i} e {i + 1} nao sao paralelas o suficiente para formar um corredor seguro " + + $"(divergencia={divergencia:F1} graus)." + ); + } } /* @@ -2180,8 +3298,8 @@ namespace AgroBase.Models * não simplesmente pelos índices existentes. */ - rua1 = GPSUtils.InterpolarRota(rua1, quantidadePontos); - rua2 = GPSUtils.InterpolarRota(rua2, quantidadePontos); + rua1 = InterpolarRotaPorDistancia(rua1, quantidadePontos); + rua2 = InterpolarRotaPorDistancia(rua2, quantidadePontos); if (rua1 == null || rua2 == null || rua1.Count < 2 || rua2.Count < 2 || rua1.Count != rua2.Count) { @@ -2203,8 +3321,17 @@ namespace AgroBase.Models var ponto1Rua2 = rua2[j]; var ponto2Rua2 = rua2[j + 1]; - var pontoMedioInicial = GPSUtils.PontoMedioComOffset(ponto1Rua1, ponto1Rua2, deslocamentoLateralGPSmetros); - var pontoMedioFinal = GPSUtils.PontoMedioComOffset(ponto2Rua1, ponto2Rua2, deslocamentoLateralGPSmetros); + /* + * Latitude/Longitude recebidas do GPSService ja representam + * o centro do veiculo. A rota geometrica, portanto, fica no + * centro real entre as duas linhas do mapa. + * + * A compensacao antiga baseada em LarguraEsquerda/Direita + * deslocava o ponto na direcao longitudinal e perdia o sinal + * com Math.Abs; ela nao era uma correcao valida de lever arm. + */ + var pontoMedioInicial = GPSUtils.PontoMedio(ponto1Rua1, ponto1Rua2); + var pontoMedioFinal = GPSUtils.PontoMedio(ponto2Rua1, ponto2Rua2); double distancia = DistanciaSegura(pontoMedioInicial, pontoMedioFinal, $"segmento {j} do corredor {i}"); @@ -2302,6 +3429,30 @@ namespace AgroBase.Models throw new InvalidOperationException($"A largura calculada para o corredor {i} é inválida: {larguraCorredor}."); } + double larguraMaximaSegura = Math.Max(6.0, LarguraCorredorPadrao * 4.0); + double larguraRover = Math.Max( + 0, + (VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 100.0 + ); + double larguraMinimaSegura = Math.Max(0.50, larguraRover + 0.15); + + if (larguraCorredor < larguraMinimaSegura) + { + throw new InvalidOperationException( + $"O corredor {i + 1} possui largura media de {larguraCorredor:F2} m, " + + $"abaixo do minimo seguro de {larguraMinimaSegura:F2} m para o rover." + ); + } + + if (larguraCorredor > larguraMaximaSegura) + { + throw new InvalidOperationException( + $"O corredor {i + 1} possui largura media de {larguraCorredor:F2} m, " + + $"acima do limite seguro de {larguraMaximaSegura:F2} m. " + + "Verifique IDs ausentes ou ordem das ruas selecionadas." + ); + } + corredores.Add(corredor); larguras.Add(larguraCorredor); } @@ -2414,8 +3565,15 @@ namespace AgroBase.Models public static double CalcularLarguraMediaDoCorredor(List rua1, List rua2) { // 1. Interpolar as duas ruas para garantir pontos uniformemente distribuídos - List rua1Interpolada = GPSUtils.InterpolarRota(rua1, Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua1))); - List rua2Interpolada = GPSUtils.InterpolarRota(rua2, Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua2))); + List rua1Interpolada = InterpolarRotaPorDistancia( + rua1, + Math.Max(2, Convert.ToInt32(Math.Ceiling(GPSUtils.DistanciaDoTrecho(rua1))) + 1) + ); + + List rua2Interpolada = InterpolarRotaPorDistancia( + rua2, + Math.Max(2, Convert.ToInt32(Math.Ceiling(GPSUtils.DistanciaDoTrecho(rua2))) + 1) + ); double somaDistancias = 0; int totalPontos = 0; @@ -2437,7 +3595,46 @@ namespace AgroBase.Models public void ProjetarTrajetoriaFixa(GPSModel posicaoRobo = null) { - if (Variaveis.OperacaoEmAndamento.Sensoriamento?.Operacao?.Modo != ModoOperacao.MapaGPS) + var parGps = ObterParGpsCoerente(); + + lock (_syncTrajetoria) + { + var trajetoriaAnterior = _TrajetoriaFixa; + var corredoresGeometricosAnteriores = Corredores; + var corredoresAnteriores = _Corredores; + var corredorAtualAnterior = CorredorAtual; + double distanciaTotalAnterior = DistanciaTotal; + double distanciaPercorridaAnterior = DistanciaPercorrida; + bool retornandoAnterior = RetornandoBase; + var autonomiaAnterior = AutonomiaCorredor?.Clone(); + + try + { + AplicarSnapshotGpsCiclo(parGps); + ProjetarTrajetoriaFixaCore(posicaoRobo); + } + catch + { + _TrajetoriaFixa = trajetoriaAnterior; + Corredores = corredoresGeometricosAnteriores; + _Corredores = corredoresAnteriores; + CorredorAtual = corredorAtualAnterior; + DistanciaTotal = distanciaTotalAnterior; + DistanciaPercorrida = distanciaPercorridaAnterior; + RetornandoBase = retornandoAnterior; + AutonomiaCorredor = autonomiaAnterior ?? new AutonomiaCorredorModel(); + throw; + } + finally + { + LimparSnapshotGpsCiclo(); + } + } + } + + private void ProjetarTrajetoriaFixaCore(GPSModel posicaoRobo) + { + if (Variaveis.OperacaoEmAndamento?.Sensoriamento?.Operacao?.Modo != ModoOperacao.MapaGPS) return; List _trajetoriaFixa = new List(); @@ -2455,20 +3652,31 @@ namespace AgroBase.Models throw; } - + Corredores = corredores; double larguraCorredorMenor = LarguraCorredorPadrao * 0.9; - DirecaoCarroRua direcaoAtual = ExtremoMaisProximoMapa == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta; + GPSModel posicaoInicial = posicaoRobo ?? GPSPosicaoAtual; + + if (!PontoGpsValido(posicaoInicial)) + throw new InvalidOperationException("Posicao atual do rover invalida para iniciar a trajetoria."); + + double distanciaInicioPrimeiraRua = GPSUtils.DistanciaEntrePontos(posicaoInicial, RuasPlantacao[0][0]); + double distanciaFimPrimeiraRua = GPSUtils.DistanciaEntrePontos(posicaoInicial, RuasPlantacao[0][RuasPlantacao[0].Count - 1]); + + DirecaoCarroRua direcaoAtual = + distanciaInicioPrimeiraRua <= distanciaFimPrimeiraRua + ? DirecaoCarroRua.Ida + : DirecaoCarroRua.Volta; PontoTrajetoriaModel PontoRobo = new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo) { idxCorredor = 0, idxPonto = 0, idxPontoCorredor = 0, - Posicao = posicaoRobo ?? GPSPosicaoAtual, + Posicao = posicaoInicial, Orientacao = 0, - Direcao = direcaoAtual, + Direcao = direcaoAtual, Visitado = true, LarguraCorredor = 1.0 }; @@ -2479,10 +3687,22 @@ namespace AgroBase.Models var corredor = Corredores[idxCorredorDentro]; double orientacaoCorredor = GPSUtils.CalcularOrientacao(corredor[0], corredor[1]); - double orientacaoRobo = PontoRobo.Posicao.OrientacaoReal; + double orientacaoRobo = PontoRobo.Posicao.AnguloCarroDefinido; + + if (double.IsNaN(orientacaoRobo) || double.IsInfinity(orientacaoRobo)) + throw new InvalidOperationException("Orientacao atual invalida para definir o sentido inicial da rota."); // Diferença angular circular entre 0° e 180° double diferencaAngular = Math.Abs(((orientacaoRobo - orientacaoCorredor + 540.0) % 360.0) - 180.0); + double desalinhamentoEixo = Math.Min(diferencaAngular, Math.Abs(180.0 - diferencaAngular)); + + if (desalinhamentoEixo > 60.0) + { + throw new InvalidOperationException( + $"Rover transversal ao corredor {idxCorredorDentro + 1}; " + + $"nao e seguro inferir o sentido inicial (desalinhamento={desalinhamentoEixo:F1} graus)." + ); + } // Menos de 90°: robô olha no sentido corredor[0] -> corredor[^1] // Mais de 90°: robô olha no sentido corredor[^1] -> corredor[0] @@ -2493,7 +3713,7 @@ namespace AgroBase.Models double distanciaDestino = GPSUtils.DistanciaEntrePontos(PontoRobo.Posicao, pontoDestino); - if (!double.IsNaN(distanciaDestino) && !double.IsInfinity(distanciaDestino) && distanciaDestino > 5.0) + if (!double.IsNaN(distanciaDestino) && !double.IsInfinity(distanciaDestino)) { /* * A lista do corredor sempre está no sentido: @@ -2514,41 +3734,24 @@ namespace AgroBase.Models } PontoRobo.Direcao = direcaoAtual; + + Variaveis.MostrarLog( + $"[TRJ] Sentido inicial definido pelo corredor {idxCorredorDentro + 1}: " + + $"direcao_base={direcaoAtual}, heading={orientacaoRobo:F1}, " + + $"eixo={orientacaoCorredor:F1}, destino={distanciaDestino:F1}m." + ); } } _trajetoriaFixa.Add(PontoRobo); - int idxEsq = 0; - int idxDir = 0; - - for (int idx = 0; idx < Corredores.Count(); idx++) + for (int idx = 0; idx < Corredores.Count(); idx++) { double _FatorLarguraCorredor = CorredoresLarguras[idx] / LarguraCorredorPadrao; bool _FatorLarguraElevado = _FatorLarguraCorredor > 1.2; - if (_trajetoriaFixa.Count() > 1 && idxEsq == 0 && idxDir == 0) - { - var pontos = _trajetoriaFixa.Where(x => x.Tipo == TipoPontoRua.Rua).ToList(); - if (pontos.Count >= 2) - { - var p0 = pontos[0]; - var p1 = pontos[1]; - - (List ruaEsq, List ruaDir) = DeterminarRuasLaterais(p0.Posicao, p1.Posicao, RuasPlantacao[0], RuasPlantacao[1]); - - idxEsq = RuasPlantacao.IndexOf(ruaEsq); - idxDir = RuasPlantacao.IndexOf(ruaDir); - } - else - { - // ⚠️ Caso de erro: não há dois pontos de rua, mesmo com mais de 1 ponto na trajetória. - Console.WriteLine("Não há ao menos dois pontos do tipo Rua."); - } - } - bool ultimoCorredor = idx == Corredores.Count() - 1; - List CorredorAtual = Corredores[idx]; + List CorredorAtual = new List(Corredores[idx]); //double dP0 = GPSUtils.DistanciaEntrePontos(_trajetoriaFixa.Last().Posicao, CorredorAtual[0]); //double dP1 = GPSUtils.DistanciaEntrePontos(_trajetoriaFixa.Last().Posicao, CorredorAtual[CorredorAtual.Count() - 1]); @@ -2558,6 +3761,9 @@ namespace AgroBase.Models CorredorAtual.Reverse(); } + /* Guarda a geometria exatamente no sentido de percurso. */ + Corredores[idx] = CorredorAtual; + double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).FirstOrDefault(), CorredorAtual.FirstOrDefault()); GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), DistanciaProjecaoRua, anguloProjetar1); @@ -2575,75 +3781,10 @@ namespace AgroBase.Models LarguraCorredor = larguraCorredorMenor }; - bool PontoInicialAdicionado = false; - // Criar curva suave para ligacao dos corredores - if (idx > 0) + _trajetoriaFixa.Add(PontoInicial); + for (int idxPonto = 0; idxPonto < CorredorAtual.Count; idxPonto++) { - // Verifica se a distância entre o ultimo ponto da rua anterior e o primeiro ponto da nova rua é curto o bastante para gerar a curva de manobra - double distancia = GPSUtils.DistanciaEntrePontos(ultimoPontoTrajetoria.Posicao, PontoInicial.Posicao); - bool distanciaMinima = distancia < DistanciaManobraEntreRuas; - - if (distanciaMinima && false) - { - int pontosAdicionr = Convert.ToInt32(CorredoresLarguras[idx] / (DistanciaEntrePontosCurva * _FatorLarguraCorredor)); - var _penultimoPonto = _trajetoriaFixa[_trajetoriaFixa.Count() - 2].Posicao; - double anguloProjecao = GPSUtils.CalcularOrientacao(Corredores[idx - 1].FirstOrDefault(), Corredores[idx - 1].LastOrDefault()); - GPSModel _ultimoPonto = GPSUtils.ProjetarPontoDeslocado(Corredores[idx - 1].LastOrDefault(), DistanciaProjecaoRua, anguloProjecao); - List CurvaConexao = CriarCurvaEntrePontos(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, DistanciaProjecaoRua * 2.0, anguloProjecao, pontosAdicionr, false); - - for (int i = 0; i < CurvaConexao.Count; i++) - { - var ponto = CurvaConexao[i]; - bool pontoCorredorAnterior = i < (pontosAdicionr / 2); - int idxCorredorPontoCurva = pontoCorredorAnterior ? idx - 1 : idx; - bool pontoLigacao = CurvaConexao.IndexOf(ponto) == CurvaConexao.Count() - 1; - - bool meiaCurva = false; - - if ((meiaCurva && pontoCorredorAnterior) || !meiaCurva) - { - _trajetoriaFixa.Add(new PontoTrajetoriaModel(pontoLigacao ? TipoPontoRua.LigacaoEntrada : TipoPontoRua.CruvaEntreCorredores) - { - idxCorredor = idxCorredorPontoCurva, - idxPonto = _trajetoriaFixa.Count(), - idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx), - Posicao = ponto, - Direcao = pontoLigacao ? direcaoAtual : DirecaoCarroRua.Manobra, - Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.LastOrDefault().Posicao, ponto), - Visitado = false, - LarguraCorredor = larguraCorredorMenor * 0.9 - }); - } - - } - PontoInicialAdicionado = true; - } - } - - if (!PontoInicialAdicionado) - { - _trajetoriaFixa.Add(PontoInicial); - if (false) - { - GPSModel SegundoPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), (VariaveisEquipamento.DistanciaEntreBicosCm / 100.0 / 2.0), anguloProjetar1); - PontoTrajetoriaModel PontoSecundario = new PontoTrajetoriaModel(TipoPontoRua.LigacaoEntrada) - { - idxCorredor = idx, - idxPonto = _trajetoriaFixa.Count(), - idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx), - Posicao = SegundoPonto, - Direcao = direcaoAtual, - Orientacao = GPSUtils.CalcularOrientacao(ultimoPontoTrajetoria.Posicao, SegundoPonto), - Visitado = false, - LarguraCorredor = larguraCorredorMenor - }; - _trajetoriaFixa.Add(PontoSecundario); - } - } - - foreach (GPSModel pontoAtual in CorredorAtual) - { - int idxPonto = CorredorAtual.IndexOf(pontoAtual); + GPSModel pontoAtual = CorredorAtual[idxPonto]; bool bordaEntrada = idxPonto == 0; bool bordaSaida = idxPonto == CorredorAtual.Count() - 1; double distanciaCorredor = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.idxCorredor == idx).Select(x => x.Posicao).ToList()); @@ -2703,34 +3844,43 @@ namespace AgroBase.Models direcaoAtual = direcaoAtual == Enums.DirecaoCarroRua.Ida ? Enums.DirecaoCarroRua.Volta : Enums.DirecaoCarroRua.Ida; } + ValidarTrajetoriaGerada(_trajetoriaFixa); + _TrajetoriaFixa = new List(_trajetoriaFixa); - if (_trajetoriaFixa.Count() > 1 && idxEsq == 0 && idxDir == 0) - { - (List ruaEsq, List ruaDir) = DeterminarRuasLaterais(_trajetoriaFixa[1].Posicao, _trajetoriaFixa[2].Posicao, RuasPlantacao[0], RuasPlantacao[1]); - idxEsq = RuasPlantacao.IndexOf(ruaEsq); - idxDir = RuasPlantacao.IndexOf(ruaDir); - } - int incEsq = 0; - int incDir = 0; _Corredores = _TrajetoriaFixa .Where(x => x.Tipo != TipoPontoRua.PosicaoRobo) .GroupBy(x => x.idxCorredor) .Select((x, i) => { - double distanciaTotal = GPSUtils.DistanciaDoTrecho(x.Select(y => y.Posicao).ToList()); + var pontosCorredor = x.ToList(); + double distanciaTotal = GPSUtils.DistanciaDoTrecho(pontosCorredor.Select(y => y.Posicao).ToList()); - (incDir, incEsq) = AtualizaIndiceRuaLateralCorredor(i, idxDir, incDir, incEsq); + if (i + 1 >= RuasPlantacao.Count || pontosCorredor.Count < 2) + throw new InvalidOperationException($"Nao foi possivel associar as ruas laterais do corredor {i + 1}."); + + (List ruaEsq, List ruaDir) = DeterminarRuasLaterais( + pontosCorredor[0].Posicao, + pontosCorredor[1].Posicao, + RuasPlantacao[i], + RuasPlantacao[i + 1] + ); + + int idxEsq = RuasPlantacao.IndexOf(ruaEsq); + int idxDir = RuasPlantacao.IndexOf(ruaDir); + + if (idxEsq < 0 || idxDir < 0 || idxEsq == idxDir) + throw new InvalidOperationException($"Ruas laterais inconsistentes no corredor {i + 1}."); var obj = new CorredorTrajetoriaModel() { Idx = x.Key, Largura = CorredoresLarguras[x.Key], DistanciaTotal = distanciaTotal, - Pontos = x.ToList(), - QtdPontos = x.Count(), - idxRuaDireita = idxDir + incDir, - idxRuaEsquerda = idxEsq + incEsq, + Pontos = pontosCorredor, + QtdPontos = pontosCorredor.Count, + idxRuaDireita = idxDir, + idxRuaEsquerda = idxEsq, FatorLarguraCorredor = CorredoresLarguras[x.Key] / LarguraCorredorPadrao }; @@ -2740,6 +3890,8 @@ namespace AgroBase.Models _Corredores.ForEach(x => x.Ultimo = x.Idx == _Corredores.Count() - 1); + ValidarCorredoresMontados(_Corredores); + DistanciaTotal = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo).Select(x => x.Posicao).ToList()); DistanciaPercorrida = 0.0; @@ -2753,11 +3905,167 @@ namespace AgroBase.Models AtualizarDadosTrajetoria(); - LoopAtualizaDados(); + LoopAtualizaDadosCore(); RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false)); } + private static List> ClonarRuasMapa(List> ruasMapa) + { + var resultado = new List>(); + + if (ruasMapa == null) + return resultado; + + for (int i = 0; i < ruasMapa.Count; i++) + { + var rua = ruasMapa[i]; + + if (rua == null) + throw new InvalidOperationException($"Rua {i + 1} nula no mapa selecionado."); + + var copia = new List(rua.Count); + + for (int j = 0; j < rua.Count; j++) + { + if (!PontoGpsValido(rua[j])) + { + throw new InvalidOperationException( + $"Coordenada invalida na rua {i + 1}, ponto {j + 1}." + ); + } + + copia.Add(ClonarPontoGeometrico(rua[j])); + } + + resultado.Add(copia); + } + + return resultado; + } + + private static GPSModel ClonarPontoGeometrico(GPSModel ponto) + { + if (ponto == null) + return null; + + return new GPSModel + { + Latitude = ponto.Latitude, + Longitude = ponto.Longitude, + Altitude = ponto.Altitude, + OrientacaoReal = ponto.OrientacaoReal, + AnguloCarroDefinido = ponto.AnguloCarroDefinido, + Momento = ponto.Momento + }; + } + + private static bool PontoGpsValido(GPSModel ponto) + { + return + ponto != null && + !double.IsNaN(ponto.Latitude) && + !double.IsInfinity(ponto.Latitude) && + !double.IsNaN(ponto.Longitude) && + !double.IsInfinity(ponto.Longitude) && + ponto.Latitude >= -90 && + ponto.Latitude <= 90 && + ponto.Longitude >= -180 && + ponto.Longitude <= 180; + } + + private void ValidarTrajetoriaGerada(List pontos) + { + if (pontos == null || pontos.Count < 3) + throw new InvalidOperationException("A trajetoria gerada possui menos de tres pontos."); + + if (pontos[0].Tipo != TipoPontoRua.PosicaoRobo || !pontos[0].Visitado) + throw new InvalidOperationException("A trajetoria nao inicia com a posicao visitada do rover."); + + int ultimoCorredor = -1; + + for (int i = 0; i < pontos.Count; i++) + { + var ponto = pontos[i]; + + if (ponto == null || !PontoGpsValido(ponto.Posicao)) + throw new InvalidOperationException($"Ponto invalido na trajetoria: indice {i}."); + + if (ponto.idxPonto != i) + throw new InvalidOperationException($"Indice global inconsistente na trajetoria: {ponto.idxPonto}/{i}."); + + if (i > 0) + { + double salto = GPSUtils.DistanciaEntrePontos(pontos[i - 1].Posicao, ponto.Posicao); + + if (double.IsNaN(salto) || double.IsInfinity(salto) || salto > DistanciaMaximaSaltoMapaM) + { + throw new InvalidOperationException( + $"Salto geometrico inseguro entre os pontos {i - 1} e {i}: {salto:F2} m." + ); + } + + if (ponto.idxCorredor < ultimoCorredor || ponto.idxCorredor > ultimoCorredor + 1) + { + throw new InvalidOperationException( + $"Sequencia de corredores invalida no ponto {i}: {ultimoCorredor}->{ponto.idxCorredor}." + ); + } + } + + ultimoCorredor = Math.Max(ultimoCorredor, ponto.idxCorredor); + } + } + + private void ValidarCorredoresMontados(List corredores) + { + if (corredores == null || corredores.Count == 0) + throw new InvalidOperationException("Nenhum corredor operacional foi montado."); + + for (int i = 0; i < corredores.Count; i++) + { + var corredor = corredores[i]; + + if (corredor == null || corredor.Idx != i || corredor.Pontos == null || corredor.Pontos.Count < 4) + throw new InvalidOperationException($"Estrutura invalida no corredor {i + 1}."); + + if ( + corredor.idxRuaEsquerda < 0 || + corredor.idxRuaEsquerda >= RuasPlantacao.Count || + corredor.idxRuaDireita < 0 || + corredor.idxRuaDireita >= RuasPlantacao.Count + ) + { + throw new InvalidOperationException( + $"Ruas laterais invalidas no corredor {i + 1}: " + + $"esq={corredor.idxRuaEsquerda}, dir={corredor.idxRuaDireita}." + ); + } + + bool usaParAdjacenteEsperado = + (corredor.idxRuaEsquerda == i && corredor.idxRuaDireita == i + 1) || + (corredor.idxRuaDireita == i && corredor.idxRuaEsquerda == i + 1); + + if (!usaParAdjacenteEsperado) + { + throw new InvalidOperationException( + $"Associacao lateral inconsistente no corredor {i + 1}: " + + $"esperado={i}/{i + 1}, esq={corredor.idxRuaEsquerda}, dir={corredor.idxRuaDireita}." + ); + } + + if ( + corredor.Pontos.First().Tipo != TipoPontoRua.LigacaoEntrada || + corredor.Pontos.Last().Tipo != TipoPontoRua.LigacaoSaida || + !corredor.Pontos.Any(x => x.Tipo == TipoPontoRua.BordaEntrada) || + !corredor.Pontos.Any(x => x.Tipo == TipoPontoRua.BordaSaida) + ) + { + throw new InvalidOperationException($"Marcadores estruturais incompletos no corredor {i + 1}."); + } + } + } + private List CriarCurvaEntrePontos(GPSModel _p1, GPSModel _p2, double distanciaCurva, double anguloCurva, int qtdPontos, bool manterPontos) { GPSModel P0 = CalcularPontoControle(anguloCurva, _p1, _p2, distanciaCurva); @@ -2767,25 +4075,6 @@ namespace AgroBase.Models return curva; } - private (int, int) AtualizaIndiceRuaLateralCorredor(int i, int idxDir, int incDir, int incEsq) - { - bool par = (i % 2 == 0); - if (i > 0) - { - if (idxDir == 0) - { - incDir += !par ? 2 : 0; - incEsq += !par ? 0 : 2; - } - else - { - incDir += par ? 2 : 0; - incEsq += par ? 0 : 2; - } - } - return (incDir, incEsq); - } - public static GPSModel CalcularPontoControle(double anguloCurva, GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia) { double angulo = GPSUtils.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua); @@ -2831,34 +4120,131 @@ namespace AgroBase.Models public void AjustarTrajetoriaParaDesvio(Obstaculo obstaculo) { + var parGps = ObterParGpsCoerente(); + + lock (_syncTrajetoria) + { + AplicarSnapshotGpsCiclo(parGps); + + try + { + AjustarTrajetoriaParaDesvioCore(obstaculo); + } + finally + { + LimparSnapshotGpsCiclo(); + } + } + } + + private void AjustarTrajetoriaParaDesvioCore(Obstaculo obstaculo) + { + if (ReferenceEquals(obstaculo, null) || !_TrajetoriaFixaDefinida || PontoAtual == null) + return; + double distanciaObstaculo = ((double)obstaculo.DistanciaMedia_mm / 1000); double larguraObstaculo = ((double)obstaculo.Largura_mm / 1000); + double anguloDesvio = Convert.ToDouble(obstaculo.AnguloParaDesvio); + + if ( + double.IsNaN(distanciaObstaculo) || + double.IsInfinity(distanciaObstaculo) || + distanciaObstaculo < 0.20 || + distanciaObstaculo > 20.0 || + double.IsNaN(larguraObstaculo) || + double.IsInfinity(larguraObstaculo) || + larguraObstaculo < 0 || + larguraObstaculo > 10.0 || + double.IsNaN(anguloDesvio) || + double.IsInfinity(anguloDesvio) || + Math.Abs(anguloDesvio) < 1.0 || + Math.Abs(anguloDesvio) > 80.0 + ) + { + throw new InvalidOperationException("Obstaculo com dimensoes invalidas para desvio."); + } + double anguloCaminho = AnguloCaminho; - double anguloDesvioInicial = anguloCaminho + obstaculo.AnguloParaDesvio; - double anguloDesvioFinal = anguloCaminho - obstaculo.AnguloParaDesvio; + double anguloDesvioInicial = anguloCaminho + anguloDesvio; + double anguloDesvioFinal = anguloCaminho - anguloDesvio; GPSModel ponto1 = GPSUtils.GerarPontoDeslocado(GPSPosicaoAtual, anguloDesvioInicial, distanciaObstaculo); GPSModel ponto2 = GPSUtils.GerarPontoDeslocado(ponto1, anguloCaminho, larguraObstaculo); GPSModel ponto3 = GPSUtils.GerarPontoDeslocado(ponto2, anguloDesvioFinal, distanciaObstaculo); - List lista = new List() + List pontosDesvio = new List() { - new PontoTrajetoriaModel(TipoPontoRua.Desvio) - { - Posicao = ponto1, - }, - new PontoTrajetoriaModel(TipoPontoRua.Desvio) - { - Posicao = ponto2, - }, - new PontoTrajetoriaModel(TipoPontoRua.Desvio) - { - Posicao = ponto3, - } + ponto1, + ponto2, + ponto3 }; - var idxRemover = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, ponto3, distanciaObstaculo); - _TrajetoriaDinamica.RemoveRange(1, idxRemover); - _TrajetoriaDinamica.InsertRange(1, lista); + if (pontosDesvio.Any(x => !PontoGpsValido(x))) + throw new InvalidOperationException("Desvio gerou coordenada invalida."); + + int idxInicioBusca = Math.Max(PontoAtual.idxPonto + 1, 1); + int idxFimBusca = Math.Min(_TrajetoriaFixa.Count - 1, idxInicioBusca + 40); + int idxReentrada = -1; + double menorDistancia = double.MaxValue; + + for (int i = idxInicioBusca; i <= idxFimBusca; i++) + { + var candidato = _TrajetoriaFixa[i]; + + if (candidato.idxCorredor != PontoAtual.idxCorredor) + break; + + if (EhPontoEstrutural(candidato)) + break; + + double distancia = GPSUtils.DistanciaEntrePontos(ponto3, candidato.Posicao); + + if (distancia < menorDistancia) + { + menorDistancia = distancia; + idxReentrada = i; + } + } + + if (idxReentrada <= PontoAtual.idxPonto || menorDistancia > Math.Max(3.0, distanciaObstaculo)) + throw new InvalidOperationException("Nao foi encontrado ponto seguro de reentrada apos o obstaculo."); + + var novaTrajetoria = _TrajetoriaFixa.Take(PontoAtual.idxPonto + 1).ToList(); + GPSModel anterior = novaTrajetoria[novaTrajetoria.Count - 1].Posicao; + + foreach (GPSModel gps in pontosDesvio) + { + novaTrajetoria.Add( + new PontoTrajetoriaModel(TipoPontoRua.Desvio) + { + idxCorredor = PontoAtual.idxCorredor, + Posicao = gps, + Direcao = DirecaoCarroRua.Manobra, + LarguraCorredor = Math.Max(0.50, PontoAtual.LarguraCorredor * 0.8), + Orientacao = GPSUtils.CalcularOrientacao(anterior, gps), + Visitado = false + } + ); + + anterior = gps; + } + + novaTrajetoria.AddRange(_TrajetoriaFixa.Skip(idxReentrada)); + var trajetoriaAnterior = _TrajetoriaFixa; + + try + { + _TrajetoriaFixa = novaTrajetoria; + AtualizarEstruturaAposMutacaoTrajetoria(); + } + catch + { + _TrajetoriaFixa = trajetoriaAnterior; + + try { AtualizarEstruturaAposMutacaoTrajetoria(); } + catch { } + + throw; + } } private int EncontrarIndiceFinalDesvio(List trajetoria, GPSModel ultimoWaypointDesvio, double distanciaSeguranca) @@ -2886,7 +4272,12 @@ namespace AgroBase.Models { // Determinar qual está à esquerda e qual está à direita var r1 = new List(rua1); - if (ExtremoMaisProximoMapa == 1) + + if ( + r1.Count >= 2 && + GPSUtils.DistanciaEntrePontos(robo, r1[r1.Count - 1]) < + GPSUtils.DistanciaEntrePontos(robo, r1[0]) + ) { r1.Reverse(); } @@ -2917,9 +4308,76 @@ namespace AgroBase.Models } + private static bool ValorFinito(double valor) + { + return + !double.IsNaN(valor) && + !double.IsInfinity(valor); + } + // Retorno a base automatico public void IniciarRetornoBase(GPSModel baseGps, List waypointsManual = null) + { + var parGps = ObterParGpsCoerente(); + + lock (_syncTrajetoria) + { + var trajetoriaAnterior = _TrajetoriaFixa; + var corredoresAnteriores = _Corredores; + var corredoresGeometricosAnteriores = Corredores; + var corredorAtualAnterior = CorredorAtual; + bool retornandoAnterior = RetornandoBase; + double distanciaTotalAnterior = DistanciaTotal; + double distanciaPercorridaAnterior = DistanciaPercorrida; + var autonomiaAnterior = AutonomiaCorredor?.Clone(); + var op = Variaveis.OperacaoEmAndamento; + var parametrosAnteriores = op?.Parametros; + bool simulandoAnterior = op?.Simulando ?? false; + bool? operacaoIniciadaAnterior = op?.Sensoriamento?.Operacao?.OperacaoIniciada; + + try + { + AplicarSnapshotGpsCiclo(parGps); + IniciarRetornoBaseCore(baseGps, waypointsManual); + } + catch (Exception ex) + { + _TrajetoriaFixa = trajetoriaAnterior; + _Corredores = corredoresAnteriores; + Corredores = corredoresGeometricosAnteriores; + CorredorAtual = corredorAtualAnterior; + RetornandoBase = retornandoAnterior; + DistanciaTotal = distanciaTotalAnterior; + DistanciaPercorrida = distanciaPercorridaAnterior; + AutonomiaCorredor = autonomiaAnterior ?? new AutonomiaCorredorModel(); + + if (op != null) + { + op.Parametros = parametrosAnteriores; + op.Simulando = simulandoAnterior; + + if (operacaoIniciadaAnterior.HasValue && op.Sensoriamento?.Operacao != null) + op.Sensoriamento.Operacao.OperacaoIniciada = operacaoIniciadaAnterior.Value; + } + + Variaveis.MostrarLog("[RETORNO] Trajetoria rejeitada: " + ex.Message); + Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Falha, + 0, + "Trajetoria de retorno rejeitada: " + ex.Message + ); + throw; + } + finally + { + LimparSnapshotGpsCiclo(); + } + } + } + + private void IniciarRetornoBaseCore(GPSModel baseGps, List waypointsManual) { bool valido = false; List pontosRetorno = null; @@ -2929,8 +4387,7 @@ namespace AgroBase.Models // ========================= if (waypointsManual != null && waypointsManual.Count > 0) { - pontosRetorno = new List(); - pontosRetorno.AddRange(waypointsManual); + pontosRetorno = ValidarEPrepararWaypointsRetorno(waypointsManual, "manual"); valido = true; } else @@ -2938,22 +4395,80 @@ namespace AgroBase.Models // ========================= // MODO AUTOMÁTICO // ========================= - if (baseGps != null) + if (PontoGpsValido(baseGps)) (valido, pontosRetorno) = GerarTrajetoriaRetornoAutomatico(baseGps); } if (!valido || pontosRetorno == null || pontosRetorno.Count < 1) - Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Falha, 0, "Trajetória de retorno inválida."); + { + throw new InvalidOperationException("Trajetoria de retorno invalida ou rota segura indisponivel."); + } else { + pontosRetorno = ValidarEPrepararWaypointsRetorno(pontosRetorno, "calculado"); AplicarTrajetoriaFixaRetorno(pontosRetorno, "RetornoBase", waypointsManual?.Any() ?? false); AtualizarDadosOperacaoParaRetorno(); } } + private List ValidarEPrepararWaypointsRetorno(List pontos, string origem) + { + if (pontos == null || pontos.Count == 0) + throw new InvalidOperationException($"Retorno {origem} sem waypoints."); + + var validos = new List(); + + foreach (var ponto in pontos) + { + if (!PontoGpsValido(ponto)) + throw new InvalidOperationException($"Retorno {origem} possui coordenada invalida."); + + if ( + validos.Count == 0 || + GPSUtils.DistanciaEntrePontos(validos[validos.Count - 1], ponto) >= 0.05 + ) + { + validos.Add(ClonarPontoGeometrico(ponto)); + } + } + + if (validos.Count == 0) + throw new InvalidOperationException($"Retorno {origem} ficou vazio apos validacao."); + + var densificados = new List(); + GPSModel anterior = GPSPosicaoAtual; + + foreach (var ponto in validos) + { + double salto = GPSUtils.DistanciaEntrePontos(anterior, ponto); + + if (double.IsNaN(salto) || double.IsInfinity(salto)) + throw new InvalidOperationException($"Retorno {origem} possui distancia invalida."); + + densificados.AddRange( + GerarTrechoReto( + anterior, + ponto, + Math.Max(0.20, DistanciaEntrePontos) + ) + ); + + anterior = ponto; + } + + if (densificados.Count > 100000) + throw new InvalidOperationException($"Retorno {origem} excedeu o limite de pontos."); + + return densificados; + } + private void AplicarTrajetoriaFixaRetorno(List pontos, string motivo, bool manual) { var traj = new List(); + GPSModel posicaoAtual = GPSPosicaoAtual; + + if (!PontoGpsValido(posicaoAtual)) + throw new InvalidOperationException("Posicao atual invalida para aplicar o retorno."); // Ponto atual do robô (padrão do sistema) traj.Add(new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo) @@ -2961,33 +4476,38 @@ namespace AgroBase.Models idxCorredor = 0, idxPonto = 0, idxPontoCorredor = 0, - Posicao = GPSPosicaoAtual, + Posicao = posicaoAtual, Visitado = true, LarguraCorredor = 1.0 }); var pontos_corrigidos = RemoverPontosMuitoProximos(pontos, DistanciaEntrePontos, 0, DistanciaEntrePontosCurva); double distanciaPararAntecipado = 3.5; - int idx = 1; - int qtdPontosRemoverFinal = manual ? 0 : (int)(distanciaPararAntecipado / DistanciaEntrePontos); + + if (!manual) + pontos_corrigidos = CortarFinalDaRota(pontos_corrigidos, distanciaPararAntecipado); + + GPSModel anterior = posicaoAtual; + foreach (var p in pontos_corrigidos) { - if (idx < (pontos_corrigidos.Count - qtdPontosRemoverFinal) || idx == pontos_corrigidos.Count - 1) + traj.Add(new PontoTrajetoriaModel(TipoPontoRua.Rua) { - traj.Add(new PontoTrajetoriaModel(TipoPontoRua.Rua) - { - idxCorredor = 0, - idxPonto = traj.Count, - idxPontoCorredor = traj.Count, - Posicao = p, - Visitado = false, - LarguraCorredor = 1.5 - }); - } - idx++; + idxCorredor = 0, + idxPonto = traj.Count, + idxPontoCorredor = traj.Count - 1, + Posicao = p, + Orientacao = GPSUtils.CalcularOrientacao(anterior, p), + Direcao = DirecaoCarroRua.Manobra, + Visitado = false, + LarguraCorredor = 1.5 + }); + + anterior = p; } - traj[traj.Count - 1].LarguraCorredor = distanciaPararAntecipado; + if (traj.Count < 2) + throw new InvalidOperationException("Trajetoria de retorno ficou sem pontos navegaveis."); DistanciaTotal = GPSUtils.DistanciaDoTrecho(traj.Select(x => x.Posicao).ToList()); DistanciaPercorrida = 0.0; @@ -2996,28 +4516,100 @@ namespace AgroBase.Models _TrajetoriaFixa = traj; + var corredorRetorno = new CorredorTrajetoriaModel + { + Idx = 0, + Largura = 1.5, + DistanciaTotal = DistanciaTotal, + Pontos = traj.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo).ToList(), + QtdPontos = traj.Count - 1, + idxRuaDireita = 0, + idxRuaEsquerda = 0, + FatorLarguraCorredor = 1.0, + Ultimo = true + }; + + _Corredores = new List { corredorRetorno }; + Corredores = new List> + { + corredorRetorno.Pontos.Select(x => x.Posicao).ToList() + }; + CorredorAtual = corredorRetorno; + corredorRetorno.AtualizarDados(); + + AtualizarTrajetoriaDinamicaCore(); + AtualizarDistanciaRestante(); + Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, $"[RETORNO] Trajetória aplicada ({motivo}) - Pontos: {traj.Count}"); } + private List CortarFinalDaRota(List pontos, double distanciaAntesDoFimM) + { + if (pontos == null || pontos.Count < 2 || distanciaAntesDoFimM <= 0) + return pontos ?? new List(); + + double comprimento = GPSUtils.DistanciaDoTrecho(pontos); + + if (comprimento <= distanciaAntesDoFimM + 0.50) + return new List(pontos); + + double acumuladaDoFim = 0; + + for (int i = pontos.Count - 1; i > 0; i--) + { + GPSModel fim = pontos[i]; + GPSModel inicio = pontos[i - 1]; + double segmento = GPSUtils.DistanciaEntrePontos(inicio, fim); + + if (acumuladaDoFim + segmento >= distanciaAntesDoFimM) + { + double restanteNoSegmento = distanciaAntesDoFimM - acumuladaDoFim; + double tDoFimParaInicio = segmento <= 1e-9 ? 0 : restanteNoSegmento / segmento; + GPSModel pontoParada = GPSUtils.InterpolarPonto(fim, inicio, tDoFimParaInicio); + + var resultado = pontos.Take(i).Select(ClonarPontoGeometrico).ToList(); + resultado.Add(pontoParada); + return resultado; + } + + acumuladaDoFim += segmento; + } + + return new List(pontos); + } + private void AtualizarDadosOperacaoParaRetorno() { var op = Variaveis.OperacaoEmAndamento; - AutonomiaCorredor.Parar(); + if (op == null) + throw new InvalidOperationException("Operacao indisponivel ao iniciar retorno."); - AtualizarDadosTrajetoria(); - - LoopAtualizaDados(); + if (op.Sensoriamento?.Operacao == null) + throw new InvalidOperationException("Estado operacional indisponivel ao iniciar retorno."); var new_op = OperacaoModel.CarregarParametrosOperacaoPadrao(ModoOperacao.RetornoBase, CarregarModulos: false); + + if (new_op?.Parametros == null) + throw new InvalidOperationException("Parametros padrao de retorno indisponiveis."); + op.Parametros = new_op.Parametros; - + if (!op.Sensoriamento.Operacao.OperacaoIniciada) { op.Sensoriamento.Operacao.OperacaoIniciada = true; op.Simulando = !GPSService.Iniciado; } + /* + * No retorno o herbicida deixa de ser exigido, mas a bateria + * continua sendo intertravamento ate a base. + */ + AutonomiaCorredor.Iniciar(); + AtualizarDadosAutonomiaCorredor(idxCorredor: 0); + + LoopAtualizaDadosCore(); + RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false)); RedisService.Publish(CmdKey.ManagerWorkerRx, JsonConvert.SerializeObject(new { cmd = ManagerWorkerCommandType.IniciarMPC })); @@ -3033,7 +4625,13 @@ namespace AgroBase.Models if (distancia < 5) return (false, pontos); - VerificaInicioOperacaoMeioRua(false); + if (!VerificacaoInicialMeioRuaConcluida) + { + throw new InvalidOperationException( + "Retorno automatico recusado: a trajetoria ainda nao confirmou a posicao inicial do rover." + ); + } + AtualizarDadosTrajetoria(); // ========================= @@ -3073,7 +4671,13 @@ namespace AgroBase.Models // CASO 3 – Existe mapa entre a base e o rover, mas nao existe corredores com bordas proximos a ambos // (V1 simples para validar tudo) // ========================= - trecho.AddRange(GerarTrajetoriaPerimetral(posicaoAtual, baseGps, margem_m: 2.0)); + trecho.AddRange( + GerarTrajetoriaPerimetral( + posicaoAtual, + baseGps, + margem_m: Math.Max(3.0, DistanciaProjecaoRua) + ) + ); break; } pontos.AddRange(trecho); @@ -3185,17 +4789,34 @@ namespace AgroBase.Models { var lista = new List(); - if (a == null || b == null) return lista; + if (!PontoGpsValido(a) || !PontoGpsValido(b)) + throw new InvalidOperationException("Trecho reto recebeu coordenada invalida."); + + if ( + double.IsNaN(espacamento_m) || + double.IsInfinity(espacamento_m) || + espacamento_m < 0.05 + ) + { + throw new InvalidOperationException("Espacamento invalido ao gerar trecho reto."); + } double dist = GPSUtils.DistanciaEntrePontos(a, b); + + if (double.IsNaN(dist) || double.IsInfinity(dist)) + throw new InvalidOperationException("Distancia invalida ao gerar trecho reto."); + if (dist < espacamento_m) { - lista.Add(b); + lista.Add(ClonarPontoGeometrico(b)); return lista; } int passos = (int)Math.Ceiling(dist / espacamento_m); + if (passos > 100000) + throw new InvalidOperationException("Trecho reto excedeu o limite seguro de pontos."); + for (int i = 1; i <= passos; i++) { double t = (double)i / passos; @@ -3208,7 +4829,7 @@ namespace AgroBase.Models #endregion #region CASO 2 - + private enum TipoCondicaoRoboRua { Indefinido, @@ -3258,13 +4879,12 @@ namespace AgroBase.Models else if (dRoboFim <= raioRobo && dBaseInicio <= raioBase) candidatos.Add((i, TipoCondicaoRoboRua.EntraPeloFim)); - // Caso 2: robô sai pelo inicio, base entra pelo início - else if (dRoboInicio <= raioRobo && dBaseInicio <= raioBase) - candidatos.Add((i, TipoCondicaoRoboRua.SaiPeloInicio)); - - // Caso 2: robô sai pelo fim, base entra pelo fim - else if (dRoboFim <= raioRobo && dBaseFim <= raioBase) - candidatos.Add((i, TipoCondicaoRoboRua.SaiPeloFim)); + /* + * Mesma extremidade nao e aceita automaticamente. + * Esse caso fazia o rover atravessar o corredor inteiro e + * depois retornar para o mesmo lado. O perimetro e a opcao + * conservadora quando nao ha travessia util ponta-a-ponta. + */ } return candidatos; @@ -3278,9 +4898,9 @@ namespace AgroBase.Models foreach (var c in candidatos) { var corredor = Corredores[c.idxCorredor]; - var ponto = - c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio ? corredor.FirstOrDefault() : - c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloFim || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloFim ? corredor.LastOrDefault() : + var ponto = + c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio ? corredor.FirstOrDefault() : + c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloFim || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloFim ? corredor.LastOrDefault() : corredor.FirstOrDefault(); if (ponto == null) continue; @@ -3312,7 +4932,7 @@ namespace AgroBase.Models var pontos = new List(); var corredor = Corredores[idxCorredor]; - bool sentidoCorreto = condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio; + bool sentidoCorreto = condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio; GPSModel entrada = sentidoCorreto ? corredor.FirstOrDefault() : corredor.LastOrDefault(); GPSModel saida = sentidoCorreto ? corredor.LastOrDefault() : corredor.FirstOrDefault(); @@ -3345,14 +4965,61 @@ namespace AgroBase.Models // Se mapa não tiver pontos suficientes, não dá pra perímetro if (ptsMapa.Count < 3) return new List(); + double refLatMapa = ptsMapa.Average(p => p.Latitude); + double refLonMapa = ptsMapa.Average(p => p.Longitude); + var envelopeMapa = ConvexHull( + ptsMapa.Select(p => LatLonToXY(p, refLatMapa, refLonMapa)).ToList() + ); + + if ( + envelopeMapa.Count >= 3 && + PontoDentroPoligono( + LatLonToXY(posRobo, refLatMapa, refLonMapa), + envelopeMapa + ) + ) + { + Variaveis.MostrarLog( + "[RETORNO] Rota perimetral recusada: rover ainda esta dentro do envelope da cultura." + ); + return new List(); + } + // 2) Constrói o perímetro (convex hull) e aplica offset pra fora (margem) var perimetro = ConstruirPerimetroConvexoComOffset(ptsMapa, margem_m); if (perimetro.Count < 3) return new List(); + if ( + perimetro.Any(p => + !PontoGpsValido(p) || + PontoDentroPoligono( + LatLonToXY(p, refLatMapa, refLonMapa), + envelopeMapa + ) + ) + ) + { + Variaveis.MostrarLog( + "[RETORNO] Rota perimetral recusada: offset nao ficou integralmente fora da cultura." + ); + return new List(); + } + // 3) Encontra os pontos de conexão (robô e base) no perímetro var (idxRobo, pRoboPerim) = PontoMaisProximoNaPolilinha(perimetro, posRobo); var (idxBase, pBasePerim) = PontoMaisProximoNaPolilinha(perimetro, posBase); + if ( + LinhaCruzaEnvelopeConvexo(posRobo, pRoboPerim, ptsMapa) || + LinhaCruzaEnvelopeConvexo(pBasePerim, posBase, ptsMapa) + ) + { + Variaveis.MostrarLog( + "[RETORNO] Rota perimetral recusada: uma conexao reta cruzaria a cultura." + ); + return new List(); + } + // 4) Caminho pelo perímetro: escolhe sentido menor var arco1 = ExtrairArcoPerimetro(perimetro, idxRobo, idxBase, sentidoHorario: true); var arco2 = ExtrairArcoPerimetro(perimetro, idxRobo, idxBase, sentidoHorario: false); @@ -3387,38 +5054,145 @@ namespace AgroBase.Models private List ConstruirPerimetroConvexoComOffset(List pts, double margem_m) { - // Usa uma projeção local simples (equiretangular) pra converter lat/lon em XY(m) + if (pts == null || pts.Count < 3 || margem_m < 0) + return new List(); + var refLat = pts.Average(p => p.Latitude); var refLon = pts.Average(p => p.Longitude); - var xy = pts.Select(p => (p, xy: LatLonToXY(p, refLat, refLon))).ToList(); - var hullXY = ConvexHull(xy.Select(t => t.xy).ToList()); + var hullXY = ConvexHull(pts.Select(p => LatLonToXY(p, refLat, refLon)).ToList()); if (hullXY.Count < 3) return new List(); - // Centróide no plano XY - double cx = hullXY.Average(v => v.x); - double cy = hullXY.Average(v => v.y); - - // Offset pra fora: empurra cada vértice pra longe do centróide - var perimetro = new List(); - foreach (var v in hullXY) + /* Garante orientacao anti-horaria. */ + double areaDobrada = 0; + for (int i = 0; i < hullXY.Count; i++) { - double vx = v.x - cx; - double vy = v.y - cy; - double norm = Math.Sqrt(vx * vx + vy * vy); - if (norm < 1e-6) continue; - - double ox = v.x + (vx / norm) * margem_m; - double oy = v.y + (vy / norm) * margem_m; - perimetro.Add(XYToLatLon(ox, oy, refLat, refLon)); + var a = hullXY[i]; + var b = hullXY[(i + 1) % hullXY.Count]; + areaDobrada += a.x * b.y - b.x * a.y; } - // Fecha o loop (opcional). Eu gosto de deixar sem repetir o primeiro, - // e tratar a polilinha como circular nos métodos de arco. + if (areaDobrada < 0) + hullXY.Reverse(); + + var verticesOffset = new List<(double x, double y)>(); + + for (int i = 0; i < hullXY.Count; i++) + { + var anterior = hullXY[(i - 1 + hullXY.Count) % hullXY.Count]; + var atual = hullXY[i]; + var proximo = hullXY[(i + 1) % hullXY.Count]; + + var linhaAnterior = CriarLinhaOffset(anterior, atual, margem_m); + var linhaProxima = CriarLinhaOffset(atual, proximo, margem_m); + + (double x, double y) intersecao; + + if (!TentarIntersecaoLinhas(linhaAnterior, linhaProxima, out intersecao)) + { + double nx = linhaAnterior.nx + linhaProxima.nx; + double ny = linhaAnterior.ny + linhaProxima.ny; + double norma = Math.Sqrt(nx * nx + ny * ny); + + if (norma <= 1e-9) + { + nx = linhaProxima.nx; + ny = linhaProxima.ny; + norma = Math.Sqrt(nx * nx + ny * ny); + } + + intersecao = ( + atual.x + nx / norma * margem_m, + atual.y + ny / norma * margem_m + ); + } + + double distanciaMiter = Math.Sqrt( + Math.Pow(intersecao.x - atual.x, 2) + + Math.Pow(intersecao.y - atual.y, 2) + ); + + /* Evita vertices explosivos em cantos muito agudos. */ + if (distanciaMiter > Math.Max(1.0, margem_m * 4.0)) + { + double nx = linhaAnterior.nx + linhaProxima.nx; + double ny = linhaAnterior.ny + linhaProxima.ny; + double norma = Math.Sqrt(nx * nx + ny * ny); + + if (norma <= 1e-9) + { + nx = linhaProxima.nx; + ny = linhaProxima.ny; + norma = Math.Sqrt(nx * nx + ny * ny); + } + + intersecao = ( + atual.x + nx / norma * margem_m, + atual.y + ny / norma * margem_m + ); + } + + verticesOffset.Add(intersecao); + } + + var perimetro = new List(); + + foreach (var vertice in verticesOffset) + perimetro.Add(XYToLatLon(vertice.x, vertice.y, refLat, refLon)); + return perimetro; } + private static (double px, double py, double dx, double dy, double nx, double ny) CriarLinhaOffset( + (double x, double y) a, + (double x, double y) b, + double margem + ) + { + double dx = b.x - a.x; + double dy = b.y - a.y; + double norma = Math.Sqrt(dx * dx + dy * dy); + + if (norma <= 1e-9) + return (a.x, a.y, 1, 0, 0, -1); + + /* Para poligono CCW, a normal externa fica a direita da aresta. */ + double nx = dy / norma; + double ny = -dx / norma; + + return ( + a.x + nx * margem, + a.y + ny * margem, + dx, + dy, + nx, + ny + ); + } + + private static bool TentarIntersecaoLinhas( + (double px, double py, double dx, double dy, double nx, double ny) a, + (double px, double py, double dx, double dy, double nx, double ny) b, + out (double x, double y) intersecao + ) + { + double determinante = a.dx * b.dy - a.dy * b.dx; + + if (Math.Abs(determinante) <= 1e-9) + { + intersecao = (0, 0); + return false; + } + + double qpx = b.px - a.px; + double qpy = b.py - a.py; + double t = (qpx * b.dy - qpy * b.dx) / determinante; + + intersecao = (a.px + t * a.dx, a.py + t * a.dy); + return true; + } + private (double x, double y) LatLonToXY(GPSModel p, double refLat, double refLon) { const double R = 6378137.0; @@ -3651,47 +5425,75 @@ namespace AgroBase.Models public void AtualizarDados() { Dentro = false; - // Itera diretamente sem criar uma lista temporária - foreach (var ponto in Pontos) + + if (Pontos == null || Pontos.Count == 0) { - // Precisa estar na margem do ponto e, ser um ponto de rua, que significa que já está dentro, ou então de borda, e que esteja se afastando do centro dele + DistanciaRestante = 0; + Progresso = 0; + Concluido = true; + return; + } + + int idxReferencia = Pontos.FindLastIndex(x => x.Visitado); + idxReferencia = Math.Max(0, idxReferencia); + int idxInicio = Math.Max(0, idxReferencia - 2); + int idxFim = Math.Min(Pontos.Count - 1, idxReferencia + 3); + + /* + * Somente a vizinhanca do progresso atual pode definir Dentro. + * Pontos visitados antigos conservam propriedades diagnosticas, + * mas nao podem manter o rover eternamente dentro da rua. + */ + for (int i = idxInicio; i <= idxFim; i++) + { + var ponto = Pontos[i]; + if ( - ponto.Visitado && - ponto.NaMargem && - (ponto.Tipo == TipoPontoRua.Rua || + ponto.Visitado && + ponto.NaMargem && + (ponto.Tipo == TipoPontoRua.Rua || ( - (ponto.Tipo == TipoPontoRua.BordaEntrada && !ponto.Aproximando) || + (ponto.Tipo == TipoPontoRua.BordaEntrada && !ponto.Aproximando) || (ponto.Tipo == TipoPontoRua.BordaSaida && ponto.Aproximando) ) ) ) { Dentro = true; - break; // Encontramos um ponto válido, podemos sair + break; } } - //DistanciaRestante = DistanciaTotal - DistanciaPercorridaCorredor; - DistanciaRestante = GPSUtils.DistanciaDoTrecho(Pontos.Where(x => !x.Visitado).Select(x => x.Posicao).ToList()); + var naoVisitados = Pontos.Where(x => !x.Visitado).ToList(); + DistanciaRestante = GPSUtils.DistanciaDoTrecho(naoVisitados.Select(x => x.Posicao).ToList()); + + if (naoVisitados.Count > 0) + { + double distanciaEntrada = naoVisitados[0].DistanciaAtual; + + if (!double.IsNaN(distanciaEntrada) && !double.IsInfinity(distanciaEntrada) && distanciaEntrada > 0) + DistanciaRestante += distanciaEntrada; + } + Progresso = (DistanciaTotal > 0) ? FuncoesMatematicas.Clamp((DistanciaPercorridaCorredor / DistanciaTotal) * 100.0, 0, 100) : 0; - Concluido = !Pontos.Any(x => !x.Visitado); + Concluido = naoVisitados.Count == 0; } public CorredorTrajetoriaModel Clone(bool clonarPontos = true) { var obj = new CorredorTrajetoriaModel() { - Idx = Idx, + Idx = Idx, Largura = Largura, DistanciaTotal = DistanciaTotal, DistanciaPercorridaCorredor = DistanciaPercorridaCorredor, DistanciaPercorridaTotal = DistanciaPercorridaTotal, QtdPontos = QtdPontos, Dentro = Dentro, - Concluido = Concluido, + Concluido = Concluido, idxRuaEsquerda = idxRuaEsquerda, idxRuaDireita = idxRuaDireita, - DistanciaRestante = DistanciaRestante, + DistanciaRestante = DistanciaRestante, Progresso = Progresso, Pontos = new List(), Ultimo = Ultimo, @@ -3783,60 +5585,91 @@ namespace AgroBase.Models public void AtualizarPropriedades() { - AtualizarDistanciaAtual(); - AtualizarDistanciaAnterior(); - AtualizarOrientacaoAtual(); - AtualizarOrientacaoAnterior(); + AtualizarPropriedades( + GPSService.GetSnapshot(), + GPSService.GetPreviousSnapshot(), + permitirMarcarVisitado: true + ); + } + + public void AtualizarPropriedades( + GPSModel posicaoAtual, + GPSModel posicaoAnterior, + bool permitirMarcarVisitado = true + ) + { + AtualizarDistanciaAtual(posicaoAtual); + AtualizarDistanciaAnterior(posicaoAnterior); + AtualizarOrientacaoAtual(posicaoAtual); + AtualizarOrientacaoAnterior(posicaoAnterior); AtualizarNaMargem(); AtualizarAproximando(); AtualizarDistanciaTrajeto(); - AtualizarVisitado(); + + if (permitirMarcarVisitado) + AtualizarVisitado(); } private void AtualizarVisitado() { if (Visitado) return; // Se já foi visitado, não faz nada - double distanciaEntrePontos = - Tipo == TipoPontoRua.CruvaEntreCorredores ? TrajetoriaMapaOperacaoModel.DistanciaEntrePontosCurva : - TrajetoriaMapaOperacaoModel.DistanciaEntrePontos; + double espacamento = + Tipo == TipoPontoRua.CruvaEntreCorredores || Tipo == TipoPontoRua.Desvio + ? TrajetoriaMapaOperacaoModel.DistanciaEntrePontosCurva + : TrajetoriaMapaOperacaoModel.DistanciaEntrePontos; - // Define o limite de distância para considerar o ponto como visitado - double fatorAjuste = Tipo == TipoPontoRua.CruvaEntreCorredores ? 3.0 : 2.0; // Curvas podem ter menos tolerância - double limiteDistancia = Math.Max(0.1, Math.Ceiling(TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras / distanciaEntrePontos) * fatorAjuste * distanciaEntrePontos); + espacamento = Math.Max(0.10, espacamento); - // Se o ponto está dentro da margem e dentro do limite de distância, marca como visitado - if (NaMargem && DistanciaTrajeto <= limiteDistancia) + double limiteDistancia = + espacamento * 0.75 + + Math.Max(0, TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras); + + limiteDistancia = Math.Max(0.25, Math.Min(DistanciaMargem, limiteDistancia)); + + if (PontoBorda || PontoLigacao) + limiteDistancia = Math.Min(limiteDistancia, 0.80); + + bool cruzouJanelaDeAceitacao = + DistanciaAtual <= limiteDistancia && + ( + Aproximando || + DistanciaAnterior <= limiteDistancia || + DistanciaAtual <= 0.25 + ); + + if (NaMargem && cruzouJanelaDeAceitacao) { Visitado = true; } } - private void AtualizarOrientacaoAtual() + private void AtualizarOrientacaoAtual(GPSModel posicaoAtual) { - OrientacaoAtual = GPSUtils.CalcularOrientacao(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, Posicao); + OrientacaoAtual = GPSUtils.CalcularOrientacao(posicaoAtual, Posicao); } - private void AtualizarOrientacaoAnterior() + private void AtualizarOrientacaoAnterior(GPSModel posicaoAnterior) { - OrientacaoAnterior = GPSUtils.CalcularOrientacao(GPSService.PenultimaLeitura, Posicao); + OrientacaoAnterior = GPSUtils.CalcularOrientacao(posicaoAnterior, Posicao); } private void AtualizarDistanciaTrajeto() { var op = Variaveis.OperacaoEmAndamento; + var trajetoria = op?.Trajetoria; - if (!op.Trajetoria._TrajetoriaFixaDefinida) + if (trajetoria == null || !trajetoria._TrajetoriaFixaDefinida) return; - var pontoAtual = op.Trajetoria.PontoAtual; + var pontoAtual = trajetoria.PontoAtual; if (pontoAtual == null) { DistanciaTrajeto = DistanciaAtual; return; } - var primeiroNaoVisitado = op.Trajetoria._TrajetoriaFixa.FirstOrDefault(x => !x.Visitado); + var primeiroNaoVisitado = trajetoria._TrajetoriaFixa.FirstOrDefault(x => !x.Visitado); if (primeiroNaoVisitado == null) { DistanciaTrajeto = 0; @@ -3859,7 +5692,7 @@ namespace AgroBase.Models int idxFim = Math.Max(idxPontoAtual, idxPonto); // ✅ Pega os pontos corretos, independentemente de direção - List trecho = op.Trajetoria._TrajetoriaFixa + List trecho = trajetoria._TrajetoriaFixa .Skip(idxInicio) .Take(qtdPontos) .ToList() @@ -3880,27 +5713,33 @@ namespace AgroBase.Models DistanciaTrajeto = distancia + distanciaP0; } - private void AtualizarDistanciaAtual() + private void AtualizarDistanciaAtual(GPSModel posicaoAtual) { - DistanciaAtual = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, Posicao); + DistanciaAtual = GPSUtils.DistanciaEntrePontos(posicaoAtual, Posicao); } - private void AtualizarDistanciaAnterior() + private void AtualizarDistanciaAnterior(GPSModel posicaoAnterior) { - DistanciaAnterior = GPSUtils.DistanciaEntrePontos(GPSService.PenultimaLeitura, Posicao); + DistanciaAnterior = GPSUtils.DistanciaEntrePontos(posicaoAnterior, Posicao); } private void AtualizarAproximando() { - // ✅ Agora verificamos tudo de uma vez sem variáveis intermediárias - Aproximando = Math.Abs(OrientacaoAtual - OrientacaoAnterior) <= 10.0 - && DistanciaAtual < DistanciaAnterior - && Math.Abs(OrientacaoAtual - OrientacaoAnterior) < 90.0; + double diferencaAngular = Math.Abs( + GPSUtils.CalcularDiferencaAngulo( + OrientacaoAtual, + OrientacaoAnterior + ) + ); + + Aproximando = + diferencaAngular <= 45.0 && + DistanciaAtual <= DistanciaAnterior + 0.02; } private void AtualizarNaMargem() { - DistanciaMargem = LarguraCorredor * 0.8; + DistanciaMargem = Math.Max(0.25, LarguraCorredor * 0.8); NaMargem = DistanciaAtual < DistanciaMargem; } @@ -3910,7 +5749,7 @@ namespace AgroBase.Models return new PontoTrajetoriaModel(Tipo) { Aproximando = Aproximando, - Direcao = Direcao, + Direcao = Direcao, DistanciaAnterior = DistanciaAnterior, DistanciaAtual = DistanciaAtual, DistanciaTrajeto = DistanciaTrajeto, @@ -3918,8 +5757,8 @@ namespace AgroBase.Models idxPonto = idxPonto, idxPontoCorredor = idxPontoCorredor, LarguraCorredor = LarguraCorredor, - NaMargem = NaMargem, - Orientacao = Orientacao, + NaMargem = NaMargem, + Orientacao = Orientacao, OrientacaoAnterior = OrientacaoAnterior, OrientacaoAtual = OrientacaoAtual, PontoBorda = PontoBorda, @@ -3943,6 +5782,9 @@ namespace AgroBase.Models public class AutonomiaCorredorModel { + [JsonIgnore] + private readonly object _syncAutonomia = new object(); + [Flags] private enum MotivoTrava { @@ -3976,6 +5818,7 @@ namespace AgroBase.Models public bool BateriaSuficiente { get; set; } public bool HerbicidaSuficiente { get; set; } + public bool HerbicidaEstimado { get; set; } public bool BateriaOverrideAplicado { get; set; } public bool HerbicidaOverrideAplicado { get; set; } @@ -4170,6 +6013,12 @@ namespace AgroBase.Models // ============================================================ public void Iniciar() + { + lock (_syncAutonomia) + IniciarCore(); + } + + private void IniciarCore() { Reiniciar(); @@ -4182,6 +6031,12 @@ namespace AgroBase.Models } public void Parar() + { + lock (_syncAutonomia) + PararCore(); + } + + private void PararCore() { Iniciado = false; Liberado = true; @@ -4193,6 +6048,12 @@ namespace AgroBase.Models // ============================================================ public void AtualizarDados(int idxCorredor, bool bateria_liberada, bool reservatorio_liberado) + { + lock (_syncAutonomia) + AtualizarDadosCore(idxCorredor, bateria_liberada, reservatorio_liberado); + } + + private void AtualizarDadosCore(int idxCorredor, bool bateria_liberada, bool reservatorio_liberado) { bool pulsoLiberacaoBateria = bateria_liberada && !_ultimoSinalLiberacaoBateria; @@ -4252,7 +6113,24 @@ namespace AgroBase.Models return; } - double distanciaCorredor = op.Trajetoria._Corredores[idxCorredor]?.DistanciaTotal ?? 0; + var corredorAvaliado = op.Trajetoria._Corredores[idxCorredor]; + + double distanciaCorredor = corredorAvaliado?.DistanciaTotal ?? 0; + + /* + * Ao iniciar no meio de uma rua, exige somente o trecho que + * ainda precisa ser concluido. Antes da entrada normal, mantem + * a distancia total do corredor. + */ + if ( + corredorAvaliado != null && + corredorAvaliado.Dentro && + corredorAvaliado.DistanciaRestante > 0 && + corredorAvaliado.DistanciaRestante < distanciaCorredor + ) + { + distanciaCorredor = corredorAvaliado.DistanciaRestante; + } if (!ValorFinito(distanciaCorredor) || distanciaCorredor <= 0) { @@ -4724,6 +6602,7 @@ namespace AgroBase.Models FonteAutonomiaHerbicida = fonte; DadosHerbicidaValidos = true; + HerbicidaEstimado = false; _ultimoHerbicidaValidoUtc = DateTime.UtcNow; _ultimaDistanciaSeguraHerbicida_m = distancia; @@ -4750,7 +6629,10 @@ namespace AgroBase.Models return; } - double segundosSemDado = (DateTime.UtcNow - _ultimoHerbicidaValidoUtc.Value).TotalSeconds; + double segundosSemDado = Math.Max( + 0, + (DateTime.UtcNow - _ultimoHerbicidaValidoUtc.Value).TotalSeconds + ); if (segundosSemDado > ToleranciaFalhaHerbicida_s) { @@ -4949,6 +6831,12 @@ namespace AgroBase.Models * Retorna false se o corredor não estiver liberado. */ public bool ConfirmarEntradaCorredor(int idxCorredor) + { + lock (_syncAutonomia) + return ConfirmarEntradaCorredorCore(idxCorredor); + } + + private bool ConfirmarEntradaCorredorCore(int idxCorredor) { if ( idxCorredor != @@ -4980,10 +6868,13 @@ namespace AgroBase.Models public bool CorredorEstaConfirmado(int idxCorredor) { - return - _corredoresConfirmados.Contains( - idxCorredor - ); + lock (_syncAutonomia) + { + return + _corredoresConfirmados.Contains( + idxCorredor + ); + } } // ============================================================ @@ -5121,6 +7012,9 @@ namespace AgroBase.Models HerbicidaSuficiente = HerbicidaSuficiente, + HerbicidaEstimado = + HerbicidaEstimado, + BateriaOverrideAplicado = BateriaOverrideAplicado, @@ -5215,6 +7109,9 @@ namespace AgroBase.Models HerbicidaSuficiente = snapshot.HerbicidaSuficiente; + HerbicidaEstimado = + snapshot.HerbicidaEstimado; + BateriaOverrideAplicado = snapshot.BateriaOverrideAplicado; @@ -5328,6 +7225,7 @@ namespace AgroBase.Models BateriaSuficiente = false; HerbicidaSuficiente = false; + HerbicidaEstimado = false; } // ============================================================ @@ -5335,6 +7233,12 @@ namespace AgroBase.Models // ============================================================ public void Reiniciar() + { + lock (_syncAutonomia) + ReiniciarCore(); + } + + private void ReiniciarCore() { Iniciado = false; @@ -5360,6 +7264,8 @@ namespace AgroBase.Models BateriaSuficiente = false; HerbicidaSuficiente = false; + HerbicidaEstimado = false; + PulverizacaoExigida = false; BateriaOverrideAplicado = false; @@ -5391,11 +7297,23 @@ namespace AgroBase.Models _ultimoSinalLiberacaoReservatorio = false; _ultimoIndiceAvaliado = -1; + + _ultimoHerbicidaValidoUtc = null; + _ultimaDistanciaSeguraHerbicida_m = 0; + _ultimoPercentualReservatorio = 0; + _ultimoLPorMetroHerbicida = 0; + _ultimaFonteAutonomiaHerbicida = ""; } public AutonomiaCorredorModel Clone() { - return + lock (_syncAutonomia) + return CloneCore(); + } + + private AutonomiaCorredorModel CloneCore() + { + var clone = new AutonomiaCorredorModel( tempoParaTravar_s: TempoParaTravar_s, @@ -5455,6 +7373,9 @@ namespace AgroBase.Models HerbicidaSuficiente = HerbicidaSuficiente, + HerbicidaEstimado = + HerbicidaEstimado, + PulverizacaoExigida = PulverizacaoExigida, @@ -5497,6 +7418,75 @@ namespace AgroBase.Models new List() ), }; + + clone.ToleranciaFalhaHerbicida_s = ToleranciaFalhaHerbicida_s; + clone.VelocidadeMaximaEstimativa_mps = VelocidadeMaximaEstimativa_mps; + clone._ultimoSinalLiberacaoBateria = _ultimoSinalLiberacaoBateria; + clone._ultimoSinalLiberacaoReservatorio = _ultimoSinalLiberacaoReservatorio; + clone._ultimoIndiceAvaliado = _ultimoIndiceAvaliado; + clone._ultimoHerbicidaValidoUtc = _ultimoHerbicidaValidoUtc; + clone._ultimaDistanciaSeguraHerbicida_m = _ultimaDistanciaSeguraHerbicida_m; + clone._ultimoPercentualReservatorio = _ultimoPercentualReservatorio; + clone._ultimoLPorMetroHerbicida = _ultimoLPorMetroHerbicida; + clone._ultimaFonteAutonomiaHerbicida = _ultimaFonteAutonomiaHerbicida; + + foreach (var item in _historicoCorredores) + clone._historicoCorredores[item.Key] = ClonarSnapshotCorredor(item.Value); + + foreach (int item in _corredoresConfirmados) + clone._corredoresConfirmados.Add(item); + + foreach (var item in _travasPorCorredor) + clone._travasPorCorredor[item.Key] = item.Value; + + foreach (var item in _inicioCriticoUtc) + clone._inicioCriticoUtc[item.Key] = item.Value; + + foreach (var item in _overridesPorCorredor) + clone._overridesPorCorredor[item.Key] = item.Value; + + foreach (var item in _ultimaAssinaturaLog) + clone._ultimaAssinaturaLog[item.Key] = item.Value; + + return clone; + } + + private static SnapshotCorredor ClonarSnapshotCorredor(SnapshotCorredor origem) + { + if (origem == null) + return null; + + return new SnapshotCorredor + { + Indice = origem.Indice, + Liberado = origem.Liberado, + Status = origem.Status, + Motivo = origem.Motivo, + Motivos = new List(origem.Motivos ?? new List()), + Avisos = new List(origem.Avisos ?? new List()), + DistanciaCorredor_m = origem.DistanciaCorredor_m, + DistanciaRequeridaBateria_m = origem.DistanciaRequeridaBateria_m, + DistanciaRequeridaHerbicida_m = origem.DistanciaRequeridaHerbicida_m, + DistanciaSeguraBateria_m = origem.DistanciaSeguraBateria_m, + DistanciaSeguraHerbicida_m = origem.DistanciaSeguraHerbicida_m, + FolgaBateria_m = origem.FolgaBateria_m, + FolgaHerbicida_m = origem.FolgaHerbicida_m, + DadosBateriaValidos = origem.DadosBateriaValidos, + DadosHerbicidaValidos = origem.DadosHerbicidaValidos, + BateriaSuficiente = origem.BateriaSuficiente, + HerbicidaSuficiente = origem.HerbicidaSuficiente, + HerbicidaEstimado = origem.HerbicidaEstimado, + BateriaOverrideAplicado = origem.BateriaOverrideAplicado, + HerbicidaOverrideAplicado = origem.HerbicidaOverrideAplicado, + FonteBateria = origem.FonteBateria, + FonteAutonomiaBateria = origem.FonteAutonomiaBateria, + FonteAutonomiaHerbicida = origem.FonteAutonomiaHerbicida, + PercentualBateria = origem.PercentualBateria, + PercentualReservatorio = origem.PercentualReservatorio, + PulverizacaoExigida = origem.PulverizacaoExigida, + ConfirmadoParaEntrada = origem.ConfirmadoParaEntrada, + TimestampUtc = origem.TimestampUtc + }; } // ============================================================ diff --git a/AgroBase/AgroBase/Services/GPSService.cs b/AgroBase/AgroBase/Services/GPSService.cs index ff2ca07e7..cb50a9e72 100644 --- a/AgroBase/AgroBase/Services/GPSService.cs +++ b/AgroBase/AgroBase/Services/GPSService.cs @@ -2071,6 +2071,17 @@ namespace AgroBase.Services return PenultimaLeitura.Clone(); } + public static (GPSModel atual, GPSModel anterior) GetSnapshotPair() + { + lock (_stateLock) + { + return ( + UltimaLeitura.Clone(), + PenultimaLeitura.Clone() + ); + } + } + private static bool PossuiHeadingValido(GPSModel leitura) { if (leitura == null) diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index 1b6c5d4f9..1a09aaf85 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -1,4 +1,4 @@ -using AgroBase.Models; +using AgroBase.Models; using AgroBase.Models.Modules; using AgroBase.Models.Operadores; using Newtonsoft.Json; @@ -344,7 +344,29 @@ namespace AgroBase.Services.Operadores lat = p.Posicao.Latitude, lon = p.Posicao.Longitude, tipo = (int)p.Tipo, - distanciaMargem = p.LarguraCorredor * 0.8 + distanciaMargem = Math.Max( + 0.25, + p.LarguraCorredor * 0.8 + ), + + /* + * Identidade global preservada até o MPC. + * Não renumerar após recortes/conversões. + */ + idxPonto = p.idxPonto, + idxCorredor = p.idxCorredor, + + /* + * O MPC usa um limiar de heading mais + * conservador para bordas e ligações. + */ + estrutural = + p.PontoBorda || + p.PontoLigacao || + p.Tipo == TipoPontoRua.BordaEntrada || + p.Tipo == TipoPontoRua.BordaSaida || + p.Tipo == TipoPontoRua.LigacaoEntrada || + p.Tipo == TipoPontoRua.LigacaoSaida }) .ToList(), angulo_max_graus = pControle.DirAnguloMaximo, 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 1e477deb7..84e27b0ae 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 @@ -216,6 +216,20 @@ def _assinatura_mapa(mapa, p_ref): round(_safe_float( ponto.get("distanciaMargem", 0.7) ), 3), + _safe_int( + ponto.get("idxPonto", ponto.get("IdxPonto", -1)), + -1, + ), + _safe_int( + ponto.get("idxCorredor", ponto.get("IdxCorredor", -1)), + -1, + ), + bool( + ponto.get( + "estrutural", + ponto.get("PontoEstrutural", False), + ) + ), )) referencia = ( @@ -273,6 +287,81 @@ class ControladorMPC: self.velocidade_max = _safe_float(parametros_mpc.get("velocidade_max", 2.0), 2.0, min_value=max(self.velocidade_min, 1e-6)) self.horizonte = _safe_float(parametros_mpc.get("horizonte", 2.0), 2.0, min_value=0.1) + # O progresso continua sendo autoridade exclusiva do C#. A direcao, + # porem, nao deve perseguir o waypoint discreto mais proximo: com + # pontos a cada ~0,8 m isso inverte o erro a cada passagem e produz + # zigue-zague. Estes parametros criam um alvo continuo adiante da + # projecao do rover sobre a polilinha, sem marcar nenhum ponto. + self._lookahead_reta_base_m = _safe_float( + parametros_mpc.get("lookahead_reta_base_m", 1.80), + 1.80, + min_value=0.50, + max_value=8.0, + ) + self._lookahead_reta_velocidade_s = _safe_float( + parametros_mpc.get("lookahead_reta_velocidade_s", 0.70), + 0.70, + min_value=0.0, + max_value=4.0, + ) + self._lookahead_reta_min_m = _safe_float( + parametros_mpc.get("lookahead_reta_min_m", 1.60), + 1.60, + min_value=0.30, + max_value=8.0, + ) + self._lookahead_reta_max_m = _safe_float( + parametros_mpc.get("lookahead_reta_max_m", 3.00), + 3.00, + min_value=self._lookahead_reta_min_m, + max_value=12.0, + ) + self._lookahead_manobra_base_m = _safe_float( + parametros_mpc.get("lookahead_manobra_base_m", 0.80), + 0.80, + min_value=0.25, + max_value=4.0, + ) + self._lookahead_manobra_velocidade_s = _safe_float( + parametros_mpc.get("lookahead_manobra_velocidade_s", 0.35), + 0.35, + min_value=0.0, + max_value=2.0, + ) + self._lookahead_manobra_max_m = _safe_float( + parametros_mpc.get("lookahead_manobra_max_m", 1.40), + 1.40, + min_value=self._lookahead_manobra_base_m, + max_value=6.0, + ) + self._janela_projecao_alvo_m = _safe_float( + parametros_mpc.get("janela_projecao_alvo_m", 5.0), + 5.0, + min_value=self._lookahead_reta_max_m, + max_value=20.0, + ) + + # Amortecimento aplicado somente durante CaminhandoRua. Nas curvas o + # MPC preserva toda a autoridade angular necessaria para a manobra. + self._filtro_angulo_reta_tau_s = _safe_float( + parametros_mpc.get("filtro_angulo_reta_tau_s", 0.25), + 0.25, + min_value=0.0, + max_value=2.0, + ) + self._taxa_angulo_reta_graus_s = _safe_float( + parametros_mpc.get("taxa_angulo_reta_graus_s", 18.0), + 18.0, + min_value=1.0, + max_value=120.0, + ) + self._deadband_angulo_reta_graus = _safe_float( + parametros_mpc.get("deadband_angulo_reta_graus", 0.60), + 0.60, + min_value=0.0, + max_value=5.0, + ) + self._beam_traj_min = _safe_int(parametros_mpc.get("beam_traj_min", 3), 3, min_value=1, max_value=20) self._beam_topN_max = _safe_int(parametros_mpc.get("beam_topN_max", 5), 5, min_value=1, max_value=50) self._beam_topN_min = _safe_int(parametros_mpc.get("beam_topN_min", 2), 2, min_value=1, max_value=self._beam_topN_max) @@ -287,6 +376,22 @@ class ControladorMPC: self.visitados_execucao = np.zeros(len(self.pontos_info), dtype=self._np_bool) if self.pontos_info else np.zeros(0, dtype=self._np_bool) + # Índices globais são opcionais para manter compatibilidade. Quando + # presentes, eliminam a ambiguidade entre o índice da trajetória C# + # e o índice local de uma lista dinâmica enviada ao MPC. + self._idx_global_pontos = np.array([ + _safe_int( + p.get("idxPonto", p.get("IdxPonto", i)), + i, + ) + for i, p in enumerate(self.pontos_info) + ], dtype=np.int64) + self._idx_global_para_local = { + int(idx_global): int(i) + for i, idx_global in enumerate(self._idx_global_pontos) + } + self._ultima_divergencia_visitados = False + try: if self.pontos_info: P = np.array([p.get("xy", (0.0, 0.0)) for p in self.pontos_info], dtype=np.float32) @@ -356,7 +461,69 @@ class ControladorMPC: self._s_nodes = s_nodes self._mg_nodes = mg_nodes - def cross_track_error_point(self, x, y): + def _projetar_na_trajetoria_local(self, x, y, idx_referencia=None): + """Projeta a pose apenas na vizinhanca do progresso corrente. + + A busca global e ambigua em mapas agricolas: duas passadas paralelas + podem estar a apenas 1,5 m. Ancorar a busca no proximo indice da rota + impede o erro lateral de trocar silenciosamente para outra rua. + """ + if self._seg_p0 is None or self._seg_p0.shape[0] == 0: + return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7 + + total_segmentos = int(self._seg_p0.shape[0]) + + if idx_referencia is None: + i0 = 0 + i1 = total_segmentos - 1 + else: + idx = max(0, min(_safe_int(idx_referencia, 0), len(self.pontos_info) - 1)) + s_base = float(self._s_nodes[idx]) + s_min = max(0.0, s_base - 1.25) + s_max = min( + float(self._s_nodes[-1]), + s_base + float(self._janela_projecao_alvo_m), + ) + + i0 = max(0, int(np.searchsorted(self._s_nodes, s_min, side="right")) - 1) + i1 = min( + total_segmentos - 1, + int(np.searchsorted(self._s_nodes, s_max, side="left")), + ) + + # Nunca exclui o segmento que termina no proximo ponto + # autoritativo, mesmo diante de pequenos segmentos degenerados. + i0 = min(i0, max(0, idx - 1)) + i1 = max(i1, min(total_segmentos - 1, idx)) + + P = np.array([x, y], dtype=float) + p0 = self._seg_p0[i0:i1 + 1] + vetores = self._seg_v[i0:i1 + 1] + comprimentos = self._seg_L[i0:i1 + 1] + l2 = np.maximum(comprimentos ** 2, 1e-12) + + w = P - p0 + t = np.clip(np.sum(w * vetores, axis=1) / l2, 0.0, 1.0) + projecoes = p0 + t[:, None] * vetores + d2 = np.sum((projecoes - P) ** 2, axis=1) + indice_relativo = int(np.argmin(d2)) + i = i0 + indice_relativo + + proj_i = projecoes[indice_relativo] + t_i = float(t[indice_relativo]) + e_lat = float(np.dot(P - proj_i, self._seg_n_hat[i])) + s_proj = float(self._s_nodes[i] + t_i * self._seg_L[i]) + theta_path = float( + np.arctan2(self._seg_t_hat[i, 0], self._seg_t_hat[i, 1]) + ) + + mg0 = self._mg_nodes[i] + mg1 = self._mg_nodes[i + 1] if (i + 1) < self._mg_nodes.size else mg0 + margem = float((1.0 - t_i) * mg0 + t_i * mg1) + + return e_lat, s_proj, i, proj_i, theta_path, margem + + def cross_track_error_point(self, x, y, idx_referencia=None): try: """ Retorna: @@ -367,33 +534,124 @@ class ControladorMPC: theta_path -> orientação do caminho (rad) naquele segmento margem_interp -> margem interpolada (m) naquele s """ - if self._seg_p0 is None or self._seg_p0.shape[0] == 0: - return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7 - - P = np.array([x, y], float) - w = P - self._seg_p0 # (M,2) - L2 = (self._seg_L ** 2) # (M,) - t = np.clip(np.sum(w * self._seg_v, axis=1) / L2, 0.0, 1.0) # (M,) - proj = self._seg_p0 + t[:, None] * self._seg_v # (M,2) - d2 = np.sum((proj - P)**2, axis=1) # (M,) - i = int(np.argmin(d2)) - - # projeção escolhida - proj_i = proj[i] - e_lat = float(np.dot(P - proj_i, self._seg_n_hat[i])) # sinal pela normal esquerda - s_proj = float(self._s_nodes[i] + t[i] * self._seg_L[i]) - theta_path = float(np.arctan2(self._seg_t_hat[i,0], self._seg_t_hat[i,1])) # 0=Norte se (x=E,y=N) - - # margem interpolada entre nós i e i+1 - mg0 = self._mg_nodes[i] - mg1 = self._mg_nodes[i+1] if (i+1) < self._mg_nodes.size else mg0 - margem = float((1.0 - t[i]) * mg0 + t[i] * mg1) - - return e_lat, s_proj, i, proj_i, theta_path, margem + return self._projetar_na_trajetoria_local( + x, + y, + idx_referencia=idx_referencia, + ) except Exception as e: _log(f"Erro ao calcular erro lateral: {e}") return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7 + def _xy_na_abscissa(self, s_alvo): + """Interpola um ponto continuo na polilinha pela distancia acumulada.""" + if self._cl_xy is None or self._cl_xy.shape[0] == 0: + return (0.0, 0.0), 0 + + if self._cl_xy.shape[0] == 1 or self._seg_p0 is None: + return tuple(self._cl_xy[0]), 0 + + s = float(np.clip(s_alvo, 0.0, float(self._s_nodes[-1]))) + i = int(np.searchsorted(self._s_nodes, s, side="right") - 1) + i = max(0, min(i, self._seg_p0.shape[0] - 1)) + + comprimento = float(self._seg_L[i]) + t = 0.0 if comprimento <= 1e-9 else (s - float(self._s_nodes[i])) / comprimento + t = float(np.clip(t, 0.0, 1.0)) + xy = self._seg_p0[i] + t * self._seg_v[i] + return (float(xy[0]), float(xy[1])), i + + def _distancia_lookahead(self, status_carro, velocidade): + v = max(0.0, _safe_float(velocidade, 0.0)) + + if _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]): + desejado = self._lookahead_reta_base_m + self._lookahead_reta_velocidade_s * v + return float(np.clip( + desejado, + self._lookahead_reta_min_m, + self._lookahead_reta_max_m, + )) + + desejado = self._lookahead_manobra_base_m + self._lookahead_manobra_velocidade_s * v + return float(np.clip( + desejado, + self._lookahead_manobra_base_m, + self._lookahead_manobra_max_m, + )) + + def _construir_alvo_direcional(self, x, y, idx_base, status_carro, velocidade): + """Cria alvo pure-pursuit continuo sem alterar o progresso visitado.""" + n = len(self.pontos_info) + if n <= 0: + return {"xy": (float(x), float(y)), "_idx_base": 0} + + idx = max(0, min(_safe_int(idx_base, 0), n - 1)) + _, s_proj, _, _, _, margem = self._projetar_na_trajetoria_local( + x, + y, + idx_referencia=idx, + ) + + lookahead = self._distancia_lookahead(status_carro, velocidade) + s_alvo = min(float(self._s_nodes[-1]), float(s_proj) + lookahead) + xy_alvo, i_seg = self._xy_na_abscissa(s_alvo) + + idx_meta = min(n - 1, i_seg + 1) + alvo = dict(self.pontos_info[idx_meta]) + alvo["xy"] = xy_alvo + alvo["distanciaMargem"] = margem + alvo["_idx_base"] = idx + alvo["_s_projecao"] = float(s_proj) + alvo["_s_alvo"] = float(s_alvo) + alvo["_lookahead_m"] = float(max(0.0, s_alvo - s_proj)) + return alvo + + def _estabilizar_angulo_saida( + self, + angulo_desejado_rad, + status_carro, + comando_anterior, + dt_controle, + ): + """Filtra/rate-limita esterco somente na passada reta.""" + desejado = float(np.clip( + angulo_desejado_rad, + -np.radians(self.angulo_max_graus), + np.radians(self.angulo_max_graus), + )) + + if not _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]): + return desejado + + anterior = np.radians(_safe_float( + _as_dict(comando_anterior).get("angulo", 0.0), + 0.0, + min_value=-self.angulo_max_graus, + max_value=self.angulo_max_graus, + )) + dt = _safe_float(dt_controle, 0.5, min_value=0.05, max_value=1.5) + + tau = float(self._filtro_angulo_reta_tau_s) + alpha = 1.0 if tau <= 1e-6 else dt / (tau + dt) + filtrado = anterior + alpha * (desejado - anterior) + + max_delta = np.radians(self._taxa_angulo_reta_graus_s) * dt + filtrado = anterior + float(np.clip( + filtrado - anterior, + -max_delta, + max_delta, + )) + + dead = np.radians(self._deadband_angulo_reta_graus) + if abs(desejado) <= dead and abs(anterior) <= 2.0 * dead: + filtrado = 0.0 + + return float(np.clip( + filtrado, + -np.radians(self.angulo_max_graus), + np.radians(self.angulo_max_graus), + )) + def _get_costmap_direcional(self, contexto): """ Lê o contrato VisualWorker.CostMapDirecional e devolve uma estrutura interna @@ -775,27 +1033,162 @@ class ControladorMPC: _log(f"Erro ao consultar proximo ponto nao visitado: {e}") return 0 - def _corrigir_pontos_visitados(self, x, y, pontos_visitados, idx_atual=-1, limite_max_avanco=5.0, limite_max_pontos: int = 10, velocidade: float = -1, dt: float = -1, look_ahead: bool = True): - if (idx_atual > -1): - for idx, ponto in enumerate(self.pontos_info): - if (idx < idx_atual): - pontos_visitados[idx] = True - else: - break - return idx_atual - - for idx, ponto in enumerate(self.pontos_info): - if pontos_visitados[idx]: - continue - pos = ponto["xy"] - margem = ponto.get("distanciaMargem", 0.7) - dist = np.linalg.norm([x - pos[0], y - pos[1]]) - if dist < margem: - for i in range(idx + 1): - pontos_visitados[i] = True - return idx - if idx > 0 and not pontos_visitados[idx - 1] and idx >= limite_max_avanco: + def _resolver_indice_local_autoritativo(self, idx_atual: int) -> int: + """Converte o índice publicado pelo C# para o índice local do mapa.""" + n = len(self.pontos_info) + if n <= 0: + return 0 + + idx_bruto = _safe_int(idx_atual, 0) + + if idx_bruto in self._idx_global_para_local: + return self._idx_global_para_local[idx_bruto] + + # Compatibilidade com o contrato antigo, no qual o C# já publicava + # o índice na mesma base do array entregue ao MPC. + return max(0, min(idx_bruto, n)) + + def _sincronizar_visitados_com_csharp(self, pontos_visitados, idx_atual: int) -> int: + """Aplica exatamente o prefixo autoritativo publicado pelo C#. + + O MPC pode avançar pontos em cópias usadas nas simulações, mas esse + avanço é especulativo e nunca pode sobreviver no estado da execução + real se o C# ainda não confirmou a mesma transição. + """ + n = len(self.pontos_info) + if n <= 0: + return 0 + + idx_local = self._resolver_indice_local_autoritativo(idx_atual) + idx_local = max(0, min(idx_local, n)) + + havia_avanco_especulativo = bool( + np.any(pontos_visitados[idx_local:]) + ) if idx_local < n else False + + pontos_visitados[:] = False + if idx_local > 0: + pontos_visitados[:idx_local] = True + + if havia_avanco_especulativo and not self._ultima_divergencia_visitados: + _log( + f"[MPC] Progresso especulativo descartado; " + f"C# autoritativo no idx_local={idx_local}." + ) + + self._ultima_divergencia_visitados = havia_avanco_especulativo + + # Quando o C# sinaliza conclusão (idx == n), o chamador já ordena a + # parada. Retornamos n-1 apenas para manter acessos de debug seguros. + return min(idx_local, n - 1) + + @staticmethod + def _progresso_segmento_xy(inicio, fim, robo): + a = np.asarray(inicio, dtype=float) + b = np.asarray(fim, dtype=float) + p = np.asarray(robo, dtype=float) + v = b - a + w = p - a + l2 = float(np.dot(v, v)) + + if not math.isfinite(l2) or l2 < 1e-8: + return float("-inf"), float("inf"), 0.0 + + comprimento = math.sqrt(l2) + produto = float(np.dot(w, v)) + avanco = produto / comprimento + lateral = abs(float(v[0] * w[1] - v[1] * w[0])) / comprimento + parametro = produto / l2 + return avanco, lateral, parametro + + def _corrigir_pontos_visitados( + self, + x, + y, + pontos_visitados, + idx_atual=-1, + limite_max_avanco=5.0, + limite_max_pontos: int = 10, + velocidade: float = -1, + dt: float = -1, + look_ahead: bool = True, + theta=None, + ): + if idx_atual > -1: + return self._sincronizar_visitados_com_csharp( + pontos_visitados, + idx_atual, + ) + + n = len(self.pontos_info) + if n <= 0: + return 0 + + idx_primeiro = self._proximo_nao_visitado(pontos_visitados) + idx_primeiro = max(0, min(idx_primeiro, n - 1)) + idx_limite = min( + n - 2, + idx_primeiro + max(1, int(limite_max_pontos)) - 1, + ) + + # Sem heading confiável, a simulação mantém a regra antiga de alvo + # sequencial e não tenta recuperar pontos pulados. + theta_valido = theta is not None and math.isfinite(float(theta)) + + if not theta_valido or idx_primeiro > idx_limite: + return idx_primeiro + + robo = (float(x), float(y)) + theta_robo = float(theta) + + for idx in range(idx_primeiro, idx_limite + 1): + ponto = self.pontos_info[idx] + proximo = self.pontos_info[idx + 1] + inicio = ponto.get("xy", (0.0, 0.0)) + fim = proximo.get("xy", inicio) + + avanco, lateral, parametro = self._progresso_segmento_xy( + inicio, + fim, + robo, + ) + + margem_ponto = max( + 0.30, + _safe_float(ponto.get("distanciaMargem", 0.7), 0.7), + ) + tolerancia_lateral = min(1.50, margem_ponto + 0.35) + estrutural = bool( + ponto.get( + "estrutural", + ponto.get("PontoEstrutural", False), + ) + ) + avanco_minimo = 0.0 if estrutural else -0.10 + + vetor = np.asarray(fim, dtype=float) - np.asarray(inicio, dtype=float) + theta_trecho = math.atan2(float(vetor[0]), float(vetor[1])) + erro_heading = abs( + (theta_trecho - theta_robo + math.pi) % (2.0 * math.pi) + - math.pi + ) + erro_maximo = math.radians(35.0 if estrutural else 45.0) + + ponto_ficou_para_tras = ( + math.isfinite(avanco) + and math.isfinite(lateral) + and math.isfinite(parametro) + and avanco >= avanco_minimo + and parametro >= (0.0 if estrutural else -0.10) + and lateral <= tolerancia_lateral + and erro_heading <= erro_maximo + ) + + if not ponto_ficou_para_tras: break + + pontos_visitados[idx] = True + return self._proximo_nao_visitado(pontos_visitados) def _calcular_pesos_movimento(self, contexto, erro_ori_graus, erro_lat_m): @@ -859,14 +1252,17 @@ class ControladorMPC: TipoMovimentoDirecional.MovimentoDiagonal: 0.0 } + erro_ori_abs = abs(float(erro_ori_graus)) + erro_lat_abs = abs(float(erro_lat_m)) + if status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]: custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.0 custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 2.0 else: - custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 if erro_ori_graus >= 10.0 else 1.0 - custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.2 if erro_ori_graus >= 10.0 else 0.0 - custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.0 if erro_ori_graus <= 2.0 and erro_lat_m <= 0.4 else 2.0 + custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 if erro_ori_abs >= 10.0 else 1.0 + custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.2 if erro_ori_abs >= 10.0 else 0.0 + custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.0 if erro_ori_abs <= 2.0 and erro_lat_abs <= 0.4 else 2.0 return (peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, custo_movimento) except Exception as e: @@ -938,7 +1334,12 @@ class ControladorMPC: if time_left_ms() <= 0.0: return x, y, theta, visitados x, y, theta = aplicar_passo_dt(x, y, theta, u, dt_eff) - idx_alvo = self._corrigir_pontos_visitados(x, y, visitados) + idx_alvo = self._corrigir_pontos_visitados( + x, + y, + visitados, + theta=theta, + ) t_rem -= seg @@ -1120,7 +1521,20 @@ class ControladorMPC: time_left_ms=lambda: (deadline - now()) * 1000.0, calc_omega_fn=self.gps_handler.calcular_omega ) - idx_alvo_correcao = self._corrigir_pontos_visitados(x, y, self.visitados_execucao, idx_proximo_ponto_real) + idx_alvo_correcao = self._corrigir_pontos_visitados( + x, + y, + self.visitados_execucao, + idx_proximo_ponto_real, + theta=theta, + ) + ponto_alvo_real = self._construir_alvo_direcional( + x, + y, + idx_alvo_correcao, + status_carro, + velocidade, + ) # -------------------- Planejamento: horizonte ESPACIAL fixo -------------------- S_ALVO = float(self.horizonte) @@ -1222,11 +1636,32 @@ class ControladorMPC: } #look_ahead = passo > 2 and status_carro not in [StatusCarroMapa.Manobrando.value, StatusCarroMapa.Direcionando.value] - idx_alvo = self._corrigir_pontos_visitados(x_atual, y_atual, visitados, velocidade=v_sim, dt=dt_pred) - #if passo == 0: - # print(f"idx_alvo: {idx_alvo}") - #if idx_alvo < len(self.pontos_info) - 1: idx_alvo += 1 - ponto_alvo = self.pontos_info[idx_alvo] + if passo == 0: + # O primeiro comando nasce obrigatoriamente do + # próximo ponto confirmado pelo C#. O MPC não + # pode usar a pose real para promover um ponto + # especulativamente e continuar andando enquanto + # a trajetória autoritativa permanece parada. + idx_alvo = idx_alvo_correcao + else: + idx_alvo = self._corrigir_pontos_visitados( + x_atual, + y_atual, + visitados, + velocidade=v_sim, + dt=dt_pred, + theta=theta_atual, + ) + # A autoridade de visitados continua em idx_alvo. O + # ponto usado para dirigir e continuo e fica adiante + # da projecao local do candidato sobre a rota. + ponto_alvo = self._construir_alvo_direcional( + x_atual, + y_atual, + idx_alvo, + status_carro, + v_sim, + ) pares_candidatos, custos_candidatos = self._gerar_angulos_candidatos_receding( ponto_alvo, @@ -1235,7 +1670,8 @@ class ControladorMPC: contexto, deadline, 15.0, - dados_costmap=dados_costmap_ciclo + dados_costmap=dados_costmap_ciclo, + tipo_preferido=cmd_anterior_local.get("tipo"), ) if ms_left() <= 0 or exp_budget <= 0: @@ -1371,8 +1807,20 @@ class ControladorMPC: tipo_final = TipoMovimentoDirecional.RodasDianteiras simulacao_latlon = [] - ponto_alvo_primario = self.pontos_info[idx_alvo_correcao]["xy"] - erro_lateral, _, _, _, theta_path, _ = self.cross_track_error_point(x, y) + if not parada_necessaria: + angulo_final = self._estabilizar_angulo_saida( + angulo_final, + status_carro, + comando_anterior, + self.tempo_execucao_local, + ) + + ponto_alvo_primario = ponto_alvo_real["xy"] + erro_lateral, _, _, _, theta_path, _ = self.cross_track_error_point( + x, + y, + idx_referencia=idx_alvo_correcao, + ) _, erro_orientacao = self._calcula_erro_orientacao(x, y, theta, ponto_alvo_primario, theta_path, 0.5) _cmd = { @@ -1385,6 +1833,9 @@ class ControladorMPC: "simulacao": simulacao_latlon, "erro_lateral": erro_lateral, "erro_orientacao": erro_orientacao, + "idx_alvo_autoritativo": int(idx_alvo_correcao), + "alvo_direcional_xy": ponto_alvo_primario, + "lookahead_direcional_m": float(ponto_alvo_real.get("_lookahead_m", 0.0)), "debug_custo": debug_custo, "candidatos_testados": K, "motivos": [] @@ -1400,13 +1851,14 @@ class ControladorMPC: ): deadline_dbg = time.perf_counter() + 0.03 pares_dbg, _ = self._gerar_angulos_candidatos_receding( - ponto_alvo=self.pontos_info[idx_alvo_correcao], + ponto_alvo=ponto_alvo_real, ponto_atual=(x, y, theta), ponto_ref=(x, y, theta), contexto=contexto, deadline=deadline_dbg, margem_ms=8.0, - dados_costmap=dados_costmap_dbg + dados_costmap=dados_costmap_dbg, + tipo_preferido=comando_anterior.get("tipo"), ) tipos_dbg = [] @@ -1460,7 +1912,17 @@ class ControladorMPC: "motivos": _safe_motivos(motivos), } - def _gerar_angulos_candidatos_receding(self, ponto_alvo, ponto_atual, ponto_ref, contexto, deadline, margem_ms, dados_costmap=None): + def _gerar_angulos_candidatos_receding( + self, + ponto_alvo, + ponto_atual, + ponto_ref, + contexto, + deadline, + margem_ms, + dados_costmap=None, + tipo_preferido=None, + ): """ Gera candidatos de (tipo, ângulo) respeitando o deadline do ciclo: 1) Ângulo ideal @@ -1506,7 +1968,11 @@ class ControladorMPC: angulo_ideal_deg = float(np.degrees(angulo_ideal_rad)) #print(f"Orient: {np.degrees(orient)}, delta theta: {np.degrees(delta_theta)}, angulo ideal: {angulo_ideal_deg}") - e_lat, _, _, _, theta_path, _ = self.cross_track_error_point(ponto_atual[0], ponto_atual[1]) + e_lat, _, _, _, theta_path, _ = self.cross_track_error_point( + ponto_atual[0], + ponto_atual[1], + idx_referencia=ponto_alvo.get("_idx_base"), + ) (_, e_ori) = self._calcula_erro_orientacao(ponto_atual[0], ponto_atual[1], ponto_atual[2], ponto_alvo["xy"], theta_path, 0.5) # 2) Tipos válidos conforme contexto @@ -1516,10 +1982,23 @@ class ControladorMPC: tipos_validos = [TipoMovimentoDirecional.MovimentoArco] elif abs(e_ori) > 15.0: tipos_validos = [TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.MovimentoArco] - elif abs(e_ori) <= 3.0 and abs(e_lat) <= 0.2: - tipos_validos = [TipoMovimentoDirecional.MovimentoDiagonal] else: - tipos_validos = [TipoMovimentoDirecional.RodasDianteiras] + # Histerese de modo na passada. Antes, 0,20 m/3 graus era + # uma fronteira seca: qualquer ruido alternava Diagonal e + # RodasDianteiras. Entrar exige uma faixa estreita; depois de + # entrar, a faixa de permanencia e um pouco maior. + tipo_preferido_enum = _movimento_from_value(tipo_preferido) + manter_diagonal = ( + tipo_preferido_enum == TipoMovimentoDirecional.MovimentoDiagonal + and abs(e_ori) <= 4.0 + and abs(e_lat) <= 0.30 + ) + entrar_diagonal = abs(e_ori) <= 1.5 and abs(e_lat) <= 0.12 + + if manter_diagonal or entrar_diagonal: + tipos_validos = [TipoMovimentoDirecional.MovimentoDiagonal] + else: + tipos_validos = [TipoMovimentoDirecional.RodasDianteiras] # 3) Flags de matriz if dados_costmap is None: @@ -2379,9 +2858,18 @@ class ControladorMPC: # alvo mais próximo após avançar (mantido) idx_alvo_sim = self._corrigir_pontos_visitados( x_sim, y_sim, pontos_visitados, - velocidade=v_planejado, dt=sub_dt + velocidade=v_planejado, + dt=sub_dt, + theta=theta_sim, ) - ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"] + alvo_direcional_sim = self._construir_alvo_direcional( + x_sim, + y_sim, + idx_alvo_sim, + contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value), + v_planejado, + ) + ponto_alvo_sim = alvo_direcional_sim["xy"] # -------------------------- # ERROS / CUSTOS @@ -2391,7 +2879,11 @@ class ControladorMPC: # --- PATCH A: usa theta do CAMINHO (segmento projetado) como referência # e usa também margem local pra normalização do custo lateral - e_lat, _, _, _, theta_path, margem = self.cross_track_error_point(x_sim, y_sim) + e_lat, _, _, _, theta_path, margem = self.cross_track_error_point( + x_sim, + y_sim, + idx_referencia=idx_alvo_sim, + ) # erro de orientação (o_norm, erro_ori_sim) = self._calcula_erro_orientacao(x_sim, y_sim, theta_sim, ponto_alvo_sim, theta_path, 0.5) @@ -2403,7 +2895,9 @@ class ControladorMPC: # ângulo relativo ao alvo e suavidade (parcialmente mantido) vetor_alvo = np.array(ponto_alvo_sim) - np.array([x_sim, y_sim]) vetor_alvo_norm = vetor_alvo / (np.linalg.norm(vetor_alvo) + 1e-9) - vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)]) + # Mesmo referencial de _nova_posicao: x=Leste usa sin(theta) + # e y=Norte usa cos(theta). + vetor_movel = np.array([np.sin(theta_sim), np.cos(theta_sim)]) cos_delta = float(np.dot(vetor_movel, vetor_alvo_norm)) delta_angulo = abs(angulo_testado - prev_ang) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py index 52355f47b..779d12510 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py @@ -1511,22 +1511,56 @@ class ContextoGlobalRedis: handler = GPSHandler(float(p_ref[0]), float(p_ref[1])) - for p in points_iter(pontos): + indices_globais = set() + identidade_ambigua = False + + for indice_local, p in enumerate(points_iter(pontos)): try: lat = cls._float(p.get("lat", 0.0), 0.0) lon = cls._float(p.get("lon", 0.0), 0.0) + idx_ponto = cls._int( + p.get("idxPonto", p.get("IdxPonto", indice_local)), + indice_local, + ) + idx_corredor = cls._int( + p.get("idxCorredor", p.get("IdxCorredor", -1)), + -1, + ) + + if idx_ponto in indices_globais: + identidade_ambigua = True + raise ValueError( + f"idxPonto global duplicado: {idx_ponto}" + ) + x, y = handler.converter_latlon_para_xz(lat, lon) pontos_info.append({ "xy": (x, y), "tipo": cls._int(p.get("tipo", 3), 3), "distanciaMargem": cls._float(p.get("distanciaMargem", 0.7), 0.7), + "idxPonto": idx_ponto, + "idxCorredor": idx_corredor, + "estrutural": cls._bool( + p.get( + "estrutural", + p.get("PontoEstrutural", False), + ) + ), }) + indices_globais.add(idx_ponto) + except Exception as e: mostrar_log(f"Erro ao converter ponto MPC: {p} | {e}") + if identidade_ambigua: + mostrar_log( + "Mapa MPC rejeitado: identidade global dos pontos é ambígua." + ) + return [] + except Exception as e: mostrar_log(f"Erro ao atualizar pontos do mapa: {e}") cls.atualizar_ctx_dict(CtxKey.DadosOperacao, pontos_mapa=pontos_info) diff --git a/Firmware/Modulos/I2CService.h b/Firmware/Modulos/I2CService.h index 5f67bff6d..6c0f4eb7a 100644 --- a/Firmware/Modulos/I2CService.h +++ b/Firmware/Modulos/I2CService.h @@ -3213,7 +3213,7 @@ int I2CService::_pinoTcaEn = -1; int I2CService::_pinoMuxRst = -1; int I2CService::_pinoAdsPwrEn = -1; -unsigned long I2CService::LimiteTempoI2C = 2000; +unsigned long I2CService::LimiteTempoI2C = 4000; bool I2CService::I2CIniciado = false; bool I2CService::MuxIniciado = false; diff --git a/Firmware/Modulos/SensorTemperaturaNtcModel.h b/Firmware/Modulos/SensorTemperaturaNtcModel.h index 724844724..d8b87ab53 100644 --- a/Firmware/Modulos/SensorTemperaturaNtcModel.h +++ b/Firmware/Modulos/SensorTemperaturaNtcModel.h @@ -514,7 +514,7 @@ class SensorTemperaturaNtc : public ComponenteCAN { * 4 NTC * 4 conversões * 2 Hz = 32 conversões/s. */ static constexpr int - QuantidadeAmostras = 4; + QuantidadeAmostras = 3; /* * Filtro final leve.