From 9fcd2420b9da4a9e060416453fd96ffa288d638f Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Thu, 17 Sep 2026 23:06:32 -0300 Subject: [PATCH] trajetoria atualizada --- .../Models/TrajetoriaMapaOperacaoModel.cs | 920 +++++++++++++++++- 1 file changed, 910 insertions(+), 10 deletions(-) diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index a539806c2..6749963d7 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -53,7 +53,32 @@ namespace AgroBase.Models #region PARAMETROS - public double AnguloAberturaCurva { get; set; } = 25; // Angulo usado para deslocar o ponto de curva + public double AnguloAberturaCurva { get; set; } = 25; // Legado: abertura da ligação anterior ao Dubins + // V7 - fator explícito para abrir a curva Dubins da cabeceira. + // 1.00 = raio seguro mínimo calculado pela geometria do rover. + // >1.00 = curva mais aberta, com maior reserva de direção para o MPC. + // 1.10 abre moderadamente (~10%) sem inflar demais a cabeceira. + public double FatorAberturaCurvaDubins { get; set; } = 1.10; + + // V9 - avanço reto antes do início da curva Dubins. + // 0.00 = comportamento anterior (Dubins começa exatamente na BordaSaida). + // 0.80 = projeta a pose inicial 80 cm para fora do corredor e cria + // um pequeno trecho reto antes da curva, deixando a manobra mais + // alongada (formato de gota) sem alterar o raio mínimo da Dubins. + public double AvancoAntesCurvaDubinsM { get; set; } = 0.00; + + // V10 - faixa de desaceleracao antes da BordaSaida real. + // + // A BordaSaida continua sendo um UNICO ponto estrutural. Em vez de + // duplicar o tipo BordaSaida (o que mudaria a ancora usada pelo Dubins + // e pelas recuperacoes estruturais), os pontos Rua anteriores recebem + // a flag SaidaAntecipada. O Status passa para SaindoRua ao entrar nessa + // faixa, permitindo que o controle de velocidade desacelere antes do U. + // + // 0.00 = desabilitado / comportamento anterior. + // 2.50 = normalmente cobre ~3 pontos de rua com espacamento de 0.8 m. + public double DistanciaAntecipacaoSaidaM { get; set; } = 2.50; + public double DistanciaProjecaoRua => (VariaveisEquipamento.DistanciaEntreEixosCm / 100.0 / 2.0) + (op?.Parametros?.Controle?.DistanciaManobra ?? 3.0); // Distancia para projetar o primeiro ponto para fora do corredor @@ -76,6 +101,16 @@ namespace AgroBase.Models private double ErroOrientacaoMaximoRecuperacaoGraus { get; set; } = 45.0; private double ErroOrientacaoMaximoRecuperacaoEstruturalGraus { get; set; } = 35.0; private int MaxPontosRecuperadosPorCiclo { get; set; } = 8; + + // RetornoBase V5.1: fallback de recuperação do prefixo quando um + // waypoint discreto é perdido durante uma aquisição/curva. A busca é + // somente para FRENTE na própria rota de retorno e exige proximidade + // geométrica + heading compatível. + private int JanelaRecuperacaoRetornoPontos { get; set; } = 40; + private double ToleranciaLateralRecuperacaoRetornoM { get; set; } = 2.0; + private double ErroOrientacaoMaximoRecuperacaoRetornoGraus { get; set; } = 65.0; + private double ToleranciaParametroRecuperacaoRetorno { get; set; } = 0.25; + private int MaxPontosEntradaRecuperacaoTransicao { get; set; } = 12; private int ConfirmacoesMinimasEntradaProximoCorredor { get; set; } = 2; @@ -674,27 +709,79 @@ namespace AgroBase.Models StatusAtual = StatusCarroMapa.Parado; if (ProximoPonto != null) { + // RetornoBase é um estado operacional próprio. Não deixar a + // largura sintética da rota reclassificar o rover como + // CaminhandoRua/Manobrando, pois o MPC possui uma política + // específica de aquisição e reacoplamento para o retorno. + if (RetornandoBase) + { + StatusAtual = StatusCarroMapa.RetornandoBase; + return; + } + if (CorredorAtual != null) { bool operacaoEmAndamento = op?.Sensoriamento?.Operacao?.StatusOperacaoAtual == StatusOperacao.EmAndamento; - if ((RetornandoBase && CorredorAtual.Dentro) || (!RetornandoBase && operacaoEmAndamento)) + if (operacaoEmAndamento) { PontoAtual.AtualizarPropriedades( _gpsAtualCiclo, _gpsAnteriorCiclo, permitirMarcarVisitado: false ); - if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas) + /* + * V6 - uma manobra estrutural NÃO pode virar + * Direcionando só porque a distância para o próximo + * waypoint ultrapassou 3 m. + * + * Esse era o gatilho do ciclo observado em campo: + * Manobrando -> Direcionando -> Manobrando, fazendo o + * MPC alternar entre pure-pursuit estrutural e tracking + * de centerline no meio do mesmo U-turn. + */ + bool emSaidaAntecipada = + PontoAtual?.SaidaAntecipada == true || + ( + PontoAtual?.Tipo == TipoPontoRua.Rua && + ProximoPonto?.SaidaAntecipada == true + ); + + bool emGeometriaManobra = + PontoAtual.PontoLigacao || + ProximoPonto.PontoBorda || + PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores || + PontoAtual.Tipo == TipoPontoRua.Desvio || + ProximoPonto.Tipo == TipoPontoRua.CruvaEntreCorredores || + ProximoPonto.Tipo == TipoPontoRua.Desvio; + + /* + * V10 - desaceleracao antecipada antes da cabeceira. + * + * Enquanto ainda estamos nos pontos Rua marcados como + * SaidaAntecipada, SaindoRua tem prioridade sobre o gatilho + * estrutural de Manobrando. Assim o Movimentacao reduz a + * velocidade antes de chegar na BordaSaida real. + * + * A BordaSaida real NAO recebe a flag. Portanto, assim que + * ela vira PontoAtual e o proximo ponto pertence ao Dubins, + * a regra volta naturalmente para Manobrando e o seguidor + * Pure Pursuit assume a ferradura. + */ + if (emSaidaAntecipada) { - StatusAtual = StatusCarroMapa.Direcionando; + StatusAtual = StatusCarroMapa.SaindoRua; } - else if (!CorredorAtual.Dentro && (PontoAtual.PontoBorda || PontoAtual.PontoLigacao || PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores || PontoAtual.Tipo == TipoPontoRua.Desvio || ProximoPonto.Tipo == TipoPontoRua.LigacaoEntrada)) + else if (emGeometriaManobra) { StatusAtual = StatusCarroMapa.Manobrando; } + else if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas) + { + StatusAtual = StatusCarroMapa.Direcionando; + } else if ( !CorredorAtual.Dentro && PontoAtual.Tipo == TipoPontoRua.Rua && @@ -1322,8 +1409,14 @@ namespace AgroBase.Models MarcarPontosIntermediariosNaoVisitados(); - //bool recuperouAvanco = RecuperarAvancoPorPontoFuturo(); + // Primeiro usa a recuperação estrita comum. No RetornoBase, + // se ela travar em um waypoint antigo, tenta o fallback + // geométrico sobre um segmento futuro da própria rota. bool recuperouAvanco = RecuperarPontosDeixadosParaTras(); + if (!recuperouAvanco && RetornandoBase) + { + recuperouAvanco = RecuperarAvancoRetornoBase(); + } DefinirPontoAtual(); AtualizarCorredorAtual(); @@ -2676,6 +2769,146 @@ namespace AgroBase.Models _momentoCandidatoTransicao = DateTime.MinValue; } + private bool RecuperarAvancoRetornoBase() + { + if (!RetornandoBase || + !_TrajetoriaFixaDefinida || + GPSPosicaoAtual == null || + PontoAtual == null || + _TrajetoriaFixa.Count < 3) + { + return false; + } + + int idxAtual = Math.Max( + 0, + Math.Min(PontoAtual.idxPonto, _TrajetoriaFixa.Count - 1) + ); + + int idxInicio = Math.Min(idxAtual + 1, _TrajetoriaFixa.Count - 2); + int idxFim = Math.Min( + _TrajetoriaFixa.Count - 2, + idxInicio + Math.Max(1, JanelaRecuperacaoRetornoPontos) - 1 + ); + + if (idxInicio > idxFim) + return false; + + int melhorIdx = -1; + double melhorScore = double.MaxValue; + double melhorLateral = double.MaxValue; + double melhorErroHeading = double.MaxValue; + + double orientacaoRobo = + GPSUtils.NormalizarAngulo( + GPSPosicaoAtual.AnguloCarroDefinido + ); + + if (!ValorFinito(orientacaoRobo)) + return false; + + for (int i = idxInicio; i <= idxFim; i++) + { + var inicio = _TrajetoriaFixa[i]; + var fim = _TrajetoriaFixa[i + 1]; + + if (inicio?.Posicao == null || fim?.Posicao == null) + continue; + + var progresso = CalcularProgressoNoSegmento( + inicio.Posicao, + fim.Posicao, + GPSPosicaoAtual + ); + + if (!ValorFinito(progresso.avancoLongitudinalM) || + !ValorFinito(progresso.distanciaLateralM) || + !ValorFinito(progresso.parametroSegmento)) + { + continue; + } + + double tolParametro = + Math.Max(0.05, ToleranciaParametroRecuperacaoRetorno); + + if (progresso.distanciaLateralM > + ToleranciaLateralRecuperacaoRetornoM || + progresso.parametroSegmento < -tolParametro || + progresso.parametroSegmento > 1.0 + tolParametro || + progresso.avancoLongitudinalM < -0.25) + { + continue; + } + + double orientacaoTrecho = + GPSUtils.NormalizarAngulo( + GPSUtils.CalcularOrientacao( + inicio.Posicao, + fim.Posicao + ) + ); + + if (!ValorFinito(orientacaoTrecho)) + continue; + + double erroHeading = Math.Abs( + GPSUtils.CalcularDiferencaAngulo( + orientacaoTrecho, + orientacaoRobo + ) + ); + + if (!ValorFinito(erroHeading) || + erroHeading > ErroOrientacaoMaximoRecuperacaoRetornoGraus) + { + continue; + } + + // Prefere o segmento geometricamente melhor, mas adiciona uma + // pequena penalidade ao salto de índice. Assim um cruzamento + // distante da própria rota não ganha por acaso. + int avancoIndices = Math.Max(0, i - idxAtual); + double score = + progresso.distanciaLateralM + + erroHeading * 0.015 + + avancoIndices * 0.02; + + if (score < melhorScore) + { + melhorScore = score; + melhorIdx = i; + melhorLateral = progresso.distanciaLateralM; + melhorErroHeading = erroHeading; + } + } + + if (melhorIdx <= idxAtual) + return false; + + int idxAntes = idxAtual; + + // O rover está comprovadamente sobre um segmento futuro da rota de + // retorno. Confirmar o prefixo até o início desse segmento evita o + // deadlock em um waypoint perdido, sem permitir avanço arbitrário. + MarcarVisitadoAte(melhorIdx); + + DefinirPontoAtual(); + AtualizarCorredorAtual(); + DefinirProximoPonto(); + + op?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Alerta, + 100, + "[RETORNO] Prefixo recuperado geometricamente. " + + $"idx={idxAntes}->{melhorIdx}, " + + $"lateral={melhorLateral:F2}m, " + + $"erro_heading={melhorErroHeading:F1}graus" + ); + + return true; + } + private bool RecuperarPontosDeixadosParaTras() { if (!_TrajetoriaFixaDefinida || @@ -3888,6 +4121,20 @@ namespace AgroBase.Models direcaoAtual = direcaoAtual == Enums.DirecaoCarroRua.Ida ? Enums.DirecaoCarroRua.Volta : Enums.DirecaoCarroRua.Ida; } + /* + * V7 - substitui as quinas LigacaoSaida -> LigacaoEntrada por uma + * curva Dubins tangencial à rua que termina e à rua que começa. + * A manobra deixa de depender da orientação global do mapa: usa + * somente as tangentes locais reais de cada corredor. + */ + AplicarCurvasTangenciaisEntreCorredores(_trajetoriaFixa); + ReindexarTrajetoriaAposCurvas(_trajetoriaFixa); + + // V10 - marca, em metros de trajeto, os pontos Rua imediatamente + // anteriores a cada BordaSaida. O ponto estrutural real permanece + // unico e intocado para nao alterar a geometria Dubins. + AplicarFaixaAntecipacaoSaida(_trajetoriaFixa); + ValidarTrajetoriaGerada(_trajetoriaFixa); _TrajetoriaFixa = new List(_trajetoriaFixa); @@ -3903,9 +4150,29 @@ namespace AgroBase.Models if (i + 1 >= RuasPlantacao.Count || pontosCorredor.Count < 2) throw new InvalidOperationException($"Nao foi possivel associar as ruas laterais do corredor {i + 1}."); + // Usa a tangente da passada física para decidir + // esquerda/direita. Os pontos iniciais do corredor agora + // podem pertencer à curva de aproximação. + var bordaEntradaRef = pontosCorredor.FirstOrDefault( + p => p.Tipo == TipoPontoRua.BordaEntrada + ); + int idxBordaEntradaRef = bordaEntradaRef == null + ? -1 + : pontosCorredor.IndexOf(bordaEntradaRef); + var pontoRuaRef = idxBordaEntradaRef >= 0 + ? pontosCorredor + .Skip(idxBordaEntradaRef + 1) + .FirstOrDefault(p => + p.Tipo == TipoPontoRua.Rua || + p.Tipo == TipoPontoRua.BordaSaida) + : null; + + GPSModel refA = bordaEntradaRef?.Posicao ?? pontosCorredor[0].Posicao; + GPSModel refB = pontoRuaRef?.Posicao ?? pontosCorredor[Math.Min(1, pontosCorredor.Count - 1)].Posicao; + (List ruaEsq, List ruaDir) = DeterminarRuasLaterais( - pontosCorredor[0].Posicao, - pontosCorredor[1].Posicao, + refA, + refB, RuasPlantacao[i], RuasPlantacao[i + 1] ); @@ -4018,6 +4285,627 @@ namespace AgroBase.Models ponto.Longitude <= 180; } + /* ================================================================ + * V9 - MANOBRA TANGENCIAL ENTRE CORREDORES + * ================================================================ + * + * O gerador antigo criava dois pontos projetados e deixava uma quina + * implícita entre LigacaoSaida e LigacaoEntrada. Em ruas curvas ou + * inclinadas essa quina podia passar de 120/140 graus. O MPC então + * recebia uma referência descontínua e, combinado com a troca de + * Status, alternava Arco +30 / -30 sem convergir. + * + * Aqui a conexão usa Dubins com poses inicial/final tangentes às ruas: + * P0 = BordaSaida da rua atual + * tangente inicial = heading LOCAL dos últimos metros da rua atual + * P3 = BordaEntrada da próxima rua + * tangente final = heading LOCAL dos primeiros metros da próxima rua + * + * Portanto a geometria é invariante à orientação global do talhão e + * continua válida para ruas curvas: só interessa a tangente local. + */ + private void AplicarFaixaAntecipacaoSaida(List trajetoria) + { + if (trajetoria == null || trajetoria.Count == 0) + return; + + // Limpa flags para que uma eventual regeneracao da trajetoria nao + // preserve uma faixa antiga com outro parametro. + foreach (var ponto in trajetoria) + ponto.SaidaAntecipada = false; + + double distanciaAlvo = Math.Max(0.0, Math.Min(10.0, DistanciaAntecipacaoSaidaM)); + + if (distanciaAlvo <= 1e-6) + return; + + int totalMarcados = 0; + + for (int idxBorda = 0; idxBorda < trajetoria.Count; idxBorda++) + { + var borda = trajetoria[idxBorda]; + + if (borda == null || borda.Tipo != TipoPontoRua.BordaSaida) + continue; + + int corredor = borda.idxCorredor; + + // A antecipacao existe para preparar uma MANOBRA entre ruas. + // No ultimo corredor nao ha proxima ferradura. + if (Corredores == null || corredor < 0 || corredor >= Corredores.Count - 1) + continue; + + double acumulada = 0.0; + double coberturaMarcada = 0.0; + int marcadosCorredor = 0; + GPSModel referenciaAdiante = borda.Posicao; + + for (int i = idxBorda - 1; i >= 0; i--) + { + var ponto = trajetoria[i]; + + if (ponto == null || ponto.idxCorredor != corredor) + break; + + // A faixa de antecipacao pertence apenas ao trecho operacional + // da rua. Nao invade BordaEntrada, Ligacoes, Desvio ou Dubins. + if (ponto.Tipo != TipoPontoRua.Rua) + break; + + double ds = GPSUtils.DistanciaEntrePontos( + ponto.Posicao, + referenciaAdiante + ); + + if (double.IsNaN(ds) || double.IsInfinity(ds) || ds < 0) + break; + + acumulada += ds; + + // Nao marca um ponto que ja esteja alem da faixa pedida. + if (acumulada > distanciaAlvo + 1e-6) + break; + + ponto.SaidaAntecipada = true; + coberturaMarcada = acumulada; + marcadosCorredor++; + totalMarcados++; + referenciaAdiante = ponto.Posicao; + } + + Variaveis.MostrarLog( + $"[TRJ/V10] Saida antecipada corredor={corredor + 1} | " + + $"alvo={distanciaAlvo:F2}m, pontos={marcadosCorredor}, " + + $"cobertura={coberturaMarcada:F2}m." + ); + } + + Variaveis.MostrarLog( + $"[TRJ/V10] Faixas de saida antecipada prontas | " + + $"distancia={distanciaAlvo:F2}m, pontos_total={totalMarcados}." + ); + } + + private void AplicarCurvasTangenciaisEntreCorredores(List trajetoria) + { + if (trajetoria == null || trajetoria.Count < 6 || Corredores == null || Corredores.Count < 2) + return; + + // Processa de trás para frente para que inserções não invalidem os + // índices dos pares que ainda serão tratados. + for (int idxCorredor = Corredores.Count - 2; idxCorredor >= 0; idxCorredor--) + { + int idxBordaSaida = trajetoria.FindIndex(x => + x.idxCorredor == idxCorredor && + x.Tipo == TipoPontoRua.BordaSaida); + + int idxLigacaoSaida = trajetoria.FindIndex(x => + x.idxCorredor == idxCorredor && + x.Tipo == TipoPontoRua.LigacaoSaida); + + int idxLigacaoEntrada = trajetoria.FindIndex(x => + x.idxCorredor == idxCorredor + 1 && + x.Tipo == TipoPontoRua.LigacaoEntrada); + + int idxBordaEntrada = trajetoria.FindIndex(x => + x.idxCorredor == idxCorredor + 1 && + x.Tipo == TipoPontoRua.BordaEntrada); + + if ( + idxBordaSaida < 1 || + idxLigacaoSaida <= idxBordaSaida || + idxLigacaoEntrada <= idxLigacaoSaida || + idxBordaEntrada <= idxLigacaoEntrada || + idxBordaEntrada >= trajetoria.Count - 1 + ) + { + continue; + } + + var bordaSaida = trajetoria[idxBordaSaida]; + var ligacaoSaidaAntiga = trajetoria[idxLigacaoSaida]; + var ligacaoEntradaAntiga = trajetoria[idxLigacaoEntrada]; + var bordaEntrada = trajetoria[idxBordaEntrada]; + + double headingSaida = CalcularHeadingLocalSaida( + trajetoria, + idxBordaSaida, + 1.50 + ); + + double headingEntrada = CalcularHeadingLocalEntrada( + trajetoria, + idxBordaEntrada, + 1.50 + ); + + if ( + double.IsNaN(headingSaida) || double.IsInfinity(headingSaida) || + double.IsNaN(headingEntrada) || double.IsInfinity(headingEntrada) + ) + { + continue; + } + + double distanciaEntreBordas = GPSUtils.DistanciaEntrePontos( + bordaSaida.Posicao, + bordaEntrada.Posicao + ); + + if (double.IsNaN(distanciaEntreBordas) || double.IsInfinity(distanciaEntreBordas)) + continue; + + /* + * Curva cinematicamente viável por Dubins. + * + * O raio é derivado da geometria do rover usando uma margem + * conservadora sobre o Arco de 4WS. Assim a rota gerada já + * respeita uma curvatura que o chassi consegue executar, em vez + * de apenas produzir uma spline visualmente suave. + */ + double entreEixosM = Math.Max(0.30, VariaveisEquipamento.DistanciaEntreEixosCm / 100.0); + const double anguloProjetoGraus = 28.0; + double raioMinimoSeguro = Math.Max( + 0.75, + (entreEixosM / (2.0 * Math.Tan(anguloProjetoGraus * Math.PI / 180.0))) * 1.05 + ); + + // V7 - o raio mínimo é cinematicamente possível, mas usar a + // manobra sempre colada nesse limite deixa pouca autoridade + // residual para o MPC corrigir erro de pose/heading durante o U. + // O fator de abertura cria essa folga de controle. + double fatorAberturaDubins = Math.Max(1.0, Math.Min(1.35, FatorAberturaCurvaDubins)); + double raioCurvaDubins = raioMinimoSeguro * fatorAberturaDubins; + + // V9 - em cabeceiras muito pareadas, iniciar a Dubins exatamente + // na BordaSaida produz uma ferradura muito compacta (quase um C). + // Para dar espaço físico ao rover e reproduzir a geometria das + // cabeceiras que já funcionam bem, avançamos primeiro no heading + // de saída e só então iniciamos a curva Dubins. + // + // IMPORTANTE: o trecho de avanço também vira parte da trajetória + // de manobra, com o mesmo espaçamento dos pontos da curva. Assim + // não existe salto geométrico entre BordaSaida e início da Dubins. + double passoCurva = Math.Max(0.15, DistanciaEntrePontosCurva); + double avancoAntesDubins = Math.Max(0.0, Math.Min(2.50, AvancoAntesCurvaDubinsM)); + + GPSModel inicioDubins = avancoAntesDubins > 1e-3 + ? GPSUtils.ProjetarPontoDeslocado( + bordaSaida.Posicao, + avancoAntesDubins, + headingSaida + ) + : ClonarPontoGeometrico(bordaSaida.Posicao); + + string tipoDubins; + List curvaDubins = GerarCurvaDubins( + inicioDubins, + headingSaida, + bordaEntrada.Posicao, + headingEntrada, + raioCurvaDubins, + passoCurva, + out tipoDubins + ); + + if (curvaDubins == null || curvaDubins.Count < 2 || curvaDubins.Any(x => !PontoGpsValido(x))) + continue; + + // Monta a manobra completa: + // BordaSaida -> avanço reto -> Dubins -> BordaEntrada. + var curva = new List + { + ClonarPontoGeometrico(bordaSaida.Posicao) + }; + + int pontosAvanco = 0; + if (avancoAntesDubins > 1e-3) + { + // Pontos intermediários do trecho reto. O início exato da + // Dubins é adicionado depois para eliminar erro acumulado. + for (double d = passoCurva; d < avancoAntesDubins - 1e-6; d += passoCurva) + { + curva.Add( + GPSUtils.ProjetarPontoDeslocado( + bordaSaida.Posicao, + d, + headingSaida + ) + ); + pontosAvanco++; + } + + curva.Add(ClonarPontoGeometrico(inicioDubins)); + pontosAvanco++; + } + + // curvaDubins[0] == inicioDubins; não duplicamos esse ponto. + for (int k = 1; k < curvaDubins.Count; k++) + curva.Add(curvaDubins[k]); + + if (curva.Count < 7 || curva.Any(x => !PontoGpsValido(x))) + continue; + + int ultimo = curva.Count - 1; + int idxCurvaLigacaoSaida = Math.Max(1, (int)Math.Round(ultimo * 0.45)); + int idxCurvaLigacaoEntrada = Math.Min(ultimo - 1, (int)Math.Round(ultimo * 0.55)); + + if (idxCurvaLigacaoEntrada <= idxCurvaLigacaoSaida) + idxCurvaLigacaoEntrada = Math.Min(ultimo - 1, idxCurvaLigacaoSaida + 1); + + if (idxCurvaLigacaoEntrada <= idxCurvaLigacaoSaida) + continue; + + var novos = new List(); + + // Metade inicial ainda pertence ao corredor que está saindo. + for (int k = 1; k < idxCurvaLigacaoSaida; k++) + { + novos.Add( + CriarPontoCurvaManobra( + TipoPontoRua.CruvaEntreCorredores, + idxCorredor, + curva[k], + ligacaoSaidaAntiga.Direcao, + Math.Max(0.50, ligacaoSaidaAntiga.LarguraCorredor) + ) + ); + } + + novos.Add( + CriarPontoCurvaManobra( + TipoPontoRua.LigacaoSaida, + idxCorredor, + curva[idxCurvaLigacaoSaida], + ligacaoSaidaAntiga.Direcao, + Math.Max(0.50, ligacaoSaidaAntiga.LarguraCorredor) + ) + ); + + // A pequena ponte central cruza a fronteira de corredor. + novos.Add( + CriarPontoCurvaManobra( + TipoPontoRua.LigacaoEntrada, + idxCorredor + 1, + curva[idxCurvaLigacaoEntrada], + ligacaoEntradaAntiga.Direcao, + Math.Max(0.50, ligacaoEntradaAntiga.LarguraCorredor) + ) + ); + + // Metade final já pertence ao corredor que está entrando. + for (int k = idxCurvaLigacaoEntrada + 1; k < ultimo; k++) + { + novos.Add( + CriarPontoCurvaManobra( + TipoPontoRua.CruvaEntreCorredores, + idxCorredor + 1, + curva[k], + ligacaoEntradaAntiga.Direcao, + Math.Max(0.50, ligacaoEntradaAntiga.LarguraCorredor) + ) + ); + } + + // Remove a antiga ligação em duas retas e insere a curva. + int inicioRemocao = idxBordaSaida + 1; + int quantidadeRemover = idxBordaEntrada - idxBordaSaida - 1; + + trajetoria.RemoveRange(inicioRemocao, quantidadeRemover); + trajetoria.InsertRange(inicioRemocao, novos); + + Variaveis.MostrarLog( + $"[TRJ/V7] Curva tangencial {idxCorredor + 1}->{idxCorredor + 2} | " + + $"heading_saida={headingSaida:F1}°, heading_entrada={headingEntrada:F1}°, " + + $"bordas={distanciaEntreBordas:F2}m, raio_base={raioMinimoSeguro:F2}m, " + + $"abertura={fatorAberturaDubins:F2}x, raio={raioCurvaDubins:F2}m, " + + $"avanco_pre_dubins={avancoAntesDubins:F2}m, pontos_avanco={pontosAvanco}, " + + $"dubins={tipoDubins}, pontos_dubins={curvaDubins.Count}, pontos_total={curva.Count}." + ); + } + } + + private static PontoTrajetoriaModel CriarPontoCurvaManobra( + TipoPontoRua tipo, + int idxCorredor, + GPSModel posicao, + DirecaoCarroRua direcao, + double larguraCorredor + ) + { + return new PontoTrajetoriaModel(tipo) + { + idxCorredor = idxCorredor, + Posicao = ClonarPontoGeometrico(posicao), + Direcao = direcao, + Visitado = false, + LarguraCorredor = larguraCorredor + }; + } + + private static double CalcularHeadingLocalSaida( + List trajetoria, + int idxBordaSaida, + double distanciaReferenciaM + ) + { + var borda = trajetoria[idxBordaSaida]; + int idxReferencia = idxBordaSaida - 1; + double acumulada = 0.0; + + for (int i = idxBordaSaida - 1; i >= 0; i--) + { + if (trajetoria[i].idxCorredor != borda.idxCorredor) + break; + + idxReferencia = i; + acumulada += GPSUtils.DistanciaEntrePontos( + trajetoria[i].Posicao, + trajetoria[i + 1].Posicao + ); + + if (acumulada >= distanciaReferenciaM) + break; + } + + return GPSUtils.CalcularOrientacao( + trajetoria[idxReferencia].Posicao, + borda.Posicao + ); + } + + private static double CalcularHeadingLocalEntrada( + List trajetoria, + int idxBordaEntrada, + double distanciaReferenciaM + ) + { + var borda = trajetoria[idxBordaEntrada]; + int idxReferencia = idxBordaEntrada + 1; + double acumulada = 0.0; + + for (int i = idxBordaEntrada + 1; i < trajetoria.Count; i++) + { + if (trajetoria[i].idxCorredor != borda.idxCorredor) + break; + + idxReferencia = i; + acumulada += GPSUtils.DistanciaEntrePontos( + trajetoria[i - 1].Posicao, + trajetoria[i].Posicao + ); + + if (acumulada >= distanciaReferenciaM) + break; + } + + return GPSUtils.CalcularOrientacao( + borda.Posicao, + trajetoria[idxReferencia].Posicao + ); + } + + private List GerarCurvaDubins( + GPSModel inicio, + double headingInicioGraus, + GPSModel fim, + double headingFimGraus, + double raioM, + double espacamentoM, + out string tipoEscolhido + ) + { + tipoEscolhido = ""; + double refLat = inicio.Latitude; + double refLon = inicio.Longitude; + var fimXY = LatLonToXY(fim, refLat, refLon); + + double theta0 = Mod2Pi(Math.PI / 2.0 - headingInicioGraus * Math.PI / 180.0); + double theta1 = Mod2Pi(Math.PI / 2.0 - headingFimGraus * Math.PI / 180.0); + double dx = fimXY.x; + double dy = fimXY.y; + double D = Math.Sqrt(dx * dx + dy * dy); + double d = D / raioM; + double theta = Mod2Pi(Math.Atan2(dy, dx)); + double alpha = Mod2Pi(theta0 - theta); + double beta = Mod2Pi(theta1 - theta); + + var candidatos = new List<(string tipo, double t, double p, double q, double custo)>(); + AdicionarDubins(candidatos, "LSL", DubinsLSL(alpha, beta, d), raioM); + AdicionarDubins(candidatos, "RSR", DubinsRSR(alpha, beta, d), raioM); + AdicionarDubins(candidatos, "LSR", DubinsLSR(alpha, beta, d), raioM); + AdicionarDubins(candidatos, "RSL", DubinsRSL(alpha, beta, d), raioM); + AdicionarDubins(candidatos, "RLR", DubinsRLR(alpha, beta, d), raioM); + AdicionarDubins(candidatos, "LRL", DubinsLRL(alpha, beta, d), raioM); + + if (candidatos.Count == 0) + return new List(); + + var melhor = candidatos.OrderBy(a => a.custo).First(); + tipoEscolhido = melhor.tipo; + double[] parametros = { melhor.t, melhor.p, melhor.q }; + + var resultado = new List { ClonarPontoGeometrico(inicio) }; + double x = 0.0; + double y = 0.0; + double th = theta0; + double passo = Math.Max(0.10, espacamentoM); + + for (int seg = 0; seg < 3; seg++) + { + char tipo = melhor.tipo[seg]; + double restante = parametros[seg] * raioM; + + while (restante > 1e-6) + { + double ds = Math.Min(passo, restante); + restante -= ds; + + if (tipo == 'S') + { + x += Math.Cos(th) * ds; + y += Math.Sin(th) * ds; + } + else + { + double k = tipo == 'L' ? 1.0 / raioM : -1.0 / raioM; + double dth = k * ds; + x += (Math.Sin(th + dth) - Math.Sin(th)) / k; + y += -(Math.Cos(th + dth) - Math.Cos(th)) / k; + th = Mod2Pi(th + dth); + } + + resultado.Add(XYToLatLon(x, y, refLat, refLon)); + } + } + + // Elimina erro numérico acumulado e garante exatamente a pose final. + if (resultado.Count > 0) + resultado[resultado.Count - 1] = ClonarPontoGeometrico(fim); + + return resultado; + } + + private static void AdicionarDubins( + List<(string tipo, double t, double p, double q, double custo)> lista, + string tipo, + double[] valores, + double raioM + ) + { + if (valores == null || valores.Length != 3) + return; + + double custo = (valores[0] + valores[1] + valores[2]) * raioM; + if (double.IsNaN(custo) || double.IsInfinity(custo)) + return; + + lista.Add((tipo, valores[0], valores[1], valores[2], custo)); + } + + private static double Mod2Pi(double a) + { + double doisPi = 2.0 * Math.PI; + a %= doisPi; + if (a < 0) a += doisPi; + return a; + } + + private static double[] DubinsLSL(double a, double b, double d) + { + double sa = Math.Sin(a), sb = Math.Sin(b), ca = Math.Cos(a), cb = Math.Cos(b); + double p2 = 2 + d * d - 2 * Math.Cos(a - b) + 2 * d * (sa - sb); + if (p2 < 0) return null; + double tmp = Math.Atan2(cb - ca, d + sa - sb); + return new[] { Mod2Pi(-a + tmp), Math.Sqrt(p2), Mod2Pi(b - tmp) }; + } + + private static double[] DubinsRSR(double a, double b, double d) + { + double sa = Math.Sin(a), sb = Math.Sin(b), ca = Math.Cos(a), cb = Math.Cos(b); + double p2 = 2 + d * d - 2 * Math.Cos(a - b) + 2 * d * (-sa + sb); + if (p2 < 0) return null; + double tmp = Math.Atan2(ca - cb, d - sa + sb); + return new[] { Mod2Pi(a - tmp), Math.Sqrt(p2), Mod2Pi(-b + tmp) }; + } + + private static double[] DubinsLSR(double a, double b, double d) + { + double sa = Math.Sin(a), sb = Math.Sin(b), ca = Math.Cos(a), cb = Math.Cos(b); + double p2 = -2 + d * d + 2 * Math.Cos(a - b) + 2 * d * (sa + sb); + if (p2 < 0) return null; + double p = Math.Sqrt(p2); + double tmp = Math.Atan2(-ca - cb, d + sa + sb) - Math.Atan2(-2.0, p); + return new[] { Mod2Pi(-a + tmp), p, Mod2Pi(-b + tmp) }; + } + + private static double[] DubinsRSL(double a, double b, double d) + { + double sa = Math.Sin(a), sb = Math.Sin(b), ca = Math.Cos(a), cb = Math.Cos(b); + double p2 = d * d - 2 + 2 * Math.Cos(a - b) - 2 * d * (sa + sb); + if (p2 < 0) return null; + double p = Math.Sqrt(p2); + double tmp = Math.Atan2(ca + cb, d - sa - sb) - Math.Atan2(2.0, p); + return new[] { Mod2Pi(a - tmp), p, Mod2Pi(b - tmp) }; + } + + private static double[] DubinsRLR(double a, double b, double d) + { + double sa = Math.Sin(a), sb = Math.Sin(b), ca = Math.Cos(a), cb = Math.Cos(b); + double tmp = (6 - d * d + 2 * Math.Cos(a - b) + 2 * d * (sa - sb)) / 8.0; + if (Math.Abs(tmp) > 1.0) return null; + double p = Mod2Pi(2 * Math.PI - Math.Acos(tmp)); + double t = Mod2Pi(a - Math.Atan2(ca - cb, d - sa + sb) + p / 2.0); + double q = Mod2Pi(a - b - t + p); + return new[] { t, p, q }; + } + + private static double[] DubinsLRL(double a, double b, double d) + { + double sa = Math.Sin(a), sb = Math.Sin(b), ca = Math.Cos(a), cb = Math.Cos(b); + double tmp = (6 - d * d + 2 * Math.Cos(a - b) + 2 * d * (-sa + sb)) / 8.0; + if (Math.Abs(tmp) > 1.0) return null; + double p = Mod2Pi(2 * Math.PI - Math.Acos(tmp)); + double t = Mod2Pi(-a - Math.Atan2(ca - cb, d + sa - sb) + p / 2.0); + double q = Mod2Pi(b - a - t + p); + return new[] { t, p, q }; + } + + private static void ReindexarTrajetoriaAposCurvas(List trajetoria) + { + if (trajetoria == null) + return; + + var indicesCorredor = new Dictionary(); + + for (int i = 0; i < trajetoria.Count; i++) + { + var ponto = trajetoria[i]; + ponto.idxPonto = i; + + if (ponto.Tipo == TipoPontoRua.PosicaoRobo) + { + ponto.idxPontoCorredor = 0; + } + else + { + if (!indicesCorredor.ContainsKey(ponto.idxCorredor)) + indicesCorredor[ponto.idxCorredor] = 0; + + ponto.idxPontoCorredor = indicesCorredor[ponto.idxCorredor]; + indicesCorredor[ponto.idxCorredor]++; + } + + if (i > 0) + { + ponto.Orientacao = GPSUtils.CalcularOrientacao( + trajetoria[i - 1].Posicao, + ponto.Posicao + ); + } + } + } + private void ValidarTrajetoriaGerada(List pontos) { if (pontos == null || pontos.Count < 3) @@ -4779,7 +5667,7 @@ namespace AgroBase.Models * reiniciada ou encerrada. */ if (!ReferenceEquals( - op, + Variaveis.OperacaoEmAndamento, op)) { Variaveis.MostrarLog( @@ -5875,6 +6763,11 @@ namespace AgroBase.Models public double DistanciaMargem { get; private set; } public bool Visitado { get; set; } = false; + // V10 - ponto Rua pertencente à faixa de desaceleração antes da + // BordaSaida estrutural real. Não altera Tipo/PontoBorda. + [JsonProperty] + public bool SaidaAntecipada { get; set; } = false; + // ✅ Agora, propriedades podem ser atualizadas com `set; private` [JsonProperty] public double OrientacaoAtual { get; private set; } @@ -5967,7 +6860,14 @@ namespace AgroBase.Models */ limiteDistancia = Math.Min(limiteDistancia, 0.80); - if (NaMargem) // && DistanciaAtual <= limiteDistancia + // V8 - marco estrutural precisa ser realmente alcançado. + // Antes, somente NaMargem era exigido; com corredor de 1,5 m + // isso podia confirmar Borda/Ligacao a ~1,2 m de distância e + // antecipar a troca de estado/corredor no meio da cabeceira. + // ``limiteDistancia`` já considera espaçamento e deslocamento + // máximo entre leituras, mantendo tolerância para GNSS sem + // permitir salto geométrico da transição. + if (NaMargem && DistanciaAtual <= limiteDistancia) { Visitado = true; }