2641 lines
116 KiB
C#
2641 lines
116 KiB
C#
using AgroBase.Models.Operadores;
|
||
using AgroBase.Services;
|
||
using AgroBase.Services.Operadores;
|
||
using Newtonsoft.Json;
|
||
using Newtonsoft.Json.Linq;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using static AgroBase.Models.Enums;
|
||
|
||
namespace AgroBase.Models
|
||
{
|
||
public class TrajetoriaMapaOperacaoModel
|
||
{
|
||
public TrajetoriaMapaOperacaoModel(List<List<GPSModel>> RuasMapa)
|
||
{
|
||
RuasPlantacao = new List<List<GPSModel>>(RuasMapa);
|
||
AutonomiaCorredor = new AutonomiaCorredorModel();
|
||
}
|
||
|
||
#region PARAMETROS
|
||
|
||
public double DistanciaProjecaoRua { get; set; } = 1.5; // 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
|
||
private bool EspacamentoPrimeirosPontosProjecao { get; set; } = false; // Projetar os primeiros pontos com distancia menor entre eles
|
||
private double DistanciaPrimeirosPontosProjecao { get; set; } = 3.0; // Distancia máxima para projetar os primeiros pontos com distancia menor
|
||
public static double LarguraCorredorPadrao { get; set; } = 1.5; // Largura de um corredor padrão
|
||
private int JanelaAtualizacaoDePontos { get; set; } = 20; // Define o tamanho da janela de pontos a atualizar
|
||
private bool VerificacaoInicialMeioRuaConcluida { get; set; } = false; // Realiza a verificacao inicial para saber se o robo deve comecar a operacao do meio da rua
|
||
|
||
#endregion
|
||
|
||
public DateTime UltimaAtualizacaoDados { get; set; } = DateTime.MinValue;
|
||
public bool RetornandoBase { get; set; }
|
||
public AutonomiaCorredorModel AutonomiaCorredor { get; set; }
|
||
|
||
[JsonProperty]
|
||
public double TempoEntreLeituras { get; private set; }
|
||
private void AtualizarTempoEntreLeituras()
|
||
{
|
||
DateTime leitura_gps = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps?.Momento ?? DateTime.MinValue;
|
||
double t_min = (1.0 / GPSService.TaxaAmostragemHz);
|
||
double dt = (leitura_gps - UltimaAtualizacaoDados).TotalSeconds;
|
||
TempoEntreLeituras = Math.Min(t_min, dt);
|
||
UltimaAtualizacaoDados = leitura_gps;
|
||
}
|
||
|
||
public List<List<GPSModel>> RuasPlantacao { get; set; }
|
||
public List<List<GPSModel>> Corredores { get; set; }
|
||
public GPSModel PrimeiroPontoRuaMapa
|
||
{
|
||
get
|
||
{
|
||
// 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)
|
||
{
|
||
return RuasPlantacao[CorredorAtual.Idx][0]; // Acessa diretamente o primeiro ponto da rua
|
||
}
|
||
else if (CorredorAtual == null && RuasPlantacao.Count > 0)
|
||
{
|
||
return RuasPlantacao[0][0];
|
||
}
|
||
|
||
return new GPSModel(); // Pode ser null se preferir
|
||
}
|
||
}
|
||
public GPSModel UltimoPontoRuaMapa
|
||
{
|
||
get
|
||
{
|
||
// 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)
|
||
{
|
||
return RuasPlantacao[CorredorAtual.Idx][RuasPlantacao[CorredorAtual.Idx].Count - 1]; // Acessa diretamente o ultimo ponto da rua
|
||
}
|
||
else if (CorredorAtual == null && RuasPlantacao.Count > 0)
|
||
{
|
||
return RuasPlantacao[0][RuasPlantacao[0].Count - 1];
|
||
}
|
||
|
||
return new GPSModel(); // Pode ser null se preferir
|
||
}
|
||
}
|
||
public int ExtremoMaisProximoMapa
|
||
{
|
||
get
|
||
{
|
||
double distPP = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps, PrimeiroPontoRuaMapa);
|
||
double distUP = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps, UltimoPontoRuaMapa);
|
||
|
||
// 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
|
||
// 1 para mais próximo do último ponto da rua (SAINDO)
|
||
return (distPP < distUP) ? 0 : 1;
|
||
}
|
||
}
|
||
|
||
public List<PontoTrajetoriaModel> _TrajetoriaFixa { get; set; }
|
||
public bool _TrajetoriaFixaDefinida
|
||
{
|
||
get
|
||
{
|
||
return _TrajetoriaFixa != null && _TrajetoriaFixa.Count > 0;
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public List<PontoTrajetoriaModel> _TrajetoriaJanela { get; private set; }
|
||
private void AtualizarTrajetoriaJanela()
|
||
{
|
||
_TrajetoriaJanela = new List<PontoTrajetoriaModel>();
|
||
|
||
if (!_TrajetoriaFixaDefinida) return;
|
||
|
||
for (int i = idxInicialJanela; i <= idxFinalJanela; i++)
|
||
{
|
||
_TrajetoriaJanela.Add(_TrajetoriaFixa[i]);
|
||
}
|
||
}
|
||
public List<CorredorTrajetoriaModel> _Corredores { get; set; }
|
||
public List<GPSModel> TrajetoriaFixa => _TrajetoriaFixa?.ConvertAll(x => x.Posicao);
|
||
public List<PontoTrajetoriaModel> _TrajetoriaDinamica { get; private set; }
|
||
public List<GPSModel> TrajetoriaDinamica => _TrajetoriaDinamica.ConvertAll(x => x.Posicao);
|
||
public void AtualizarTrajetoriaDinamica()
|
||
{
|
||
// Cria a lista com a última leitura do robô
|
||
var trajetoria = new List<PontoTrajetoriaModel>()
|
||
{
|
||
new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
|
||
{
|
||
Posicao = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps,
|
||
LarguraCorredor = 1.0,
|
||
idxCorredor = CorredorAtual?.Idx ?? 0,
|
||
Visitado = true
|
||
}
|
||
};
|
||
|
||
// Se a trajetória fixa estiver vazia, retorna apenas a leitura atual
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
_TrajetoriaDinamica = trajetoria;
|
||
return;
|
||
}
|
||
|
||
// Adiciona os pontos não visitados, já ordenados por idxPonto
|
||
foreach (var ponto in _TrajetoriaFixa)
|
||
{
|
||
if (!ponto.Visitado)
|
||
{
|
||
trajetoria.Add(ponto);
|
||
}
|
||
}
|
||
|
||
_TrajetoriaDinamica = trajetoria;
|
||
}
|
||
|
||
[JsonProperty]
|
||
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;
|
||
}
|
||
|
||
private int idxInicialJanela = 0;
|
||
private int idxFinalJanela = 0;
|
||
private void AtualizarIndicesJanela(int qtdPontos = -1)
|
||
{
|
||
if (!_TrajetoriaFixaDefinida) return;
|
||
|
||
if (qtdPontos == -1) qtdPontos = JanelaAtualizacaoDePontos;
|
||
|
||
idxInicialJanela = Math.Max(0, (PontoAtual?.idxPonto ?? 0) - qtdPontos / 2);
|
||
idxFinalJanela = Math.Min(_TrajetoriaFixa.Count - 1, (PontoAtual?.idxPonto ?? 0) + qtdPontos / 2);
|
||
}
|
||
|
||
[JsonProperty]
|
||
public CorredorTrajetoriaModel CorredorAtual { get; private set; }
|
||
private bool _CorredorAtualDefinido
|
||
{
|
||
get
|
||
{
|
||
return (CorredorAtual?.Pontos?.Count ?? 0) > 0;
|
||
}
|
||
}
|
||
private void AtualizarCorredorAtual()
|
||
{
|
||
if (_Corredores == null || _Corredores.Count == 0)
|
||
{
|
||
CorredorAtual = null;
|
||
return;
|
||
}
|
||
|
||
int idxNovoCorredor = PontoAtual?.idxCorredor ?? 0;
|
||
|
||
if ((CorredorAtual?.Idx ?? 0) != idxNovoCorredor)
|
||
{
|
||
CorredorAtual?.AtualizarDados();
|
||
|
||
AtualizarDadosAutonomiaCorredor(idxCorredor: idxNovoCorredor);
|
||
}
|
||
|
||
if (AutonomiaCorredor.Liberado)
|
||
{
|
||
CorredorAtual = _Corredores[idxNovoCorredor];
|
||
CorredorAtual.AtualizarDados();
|
||
}
|
||
}
|
||
public DirecaoCarroRua DirecaoCaminho
|
||
{
|
||
get
|
||
{
|
||
return PontoAtual?.Direcao ?? Enums.DirecaoCarroRua.Parado;
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public double AnguloMedioCorredor { get; private set; }
|
||
private void AtualizarAnguloMedioCorredor()
|
||
{
|
||
AnguloMedioCorredor = GPSUtils.CalcularOrientacao(PontoAtual.Posicao, ProximoPonto.Posicao);
|
||
}
|
||
[JsonProperty]
|
||
public double AnguloCaminho { get; private set; }
|
||
private void AtualizarAnguloCaminho()
|
||
{
|
||
PontoTrajetoriaModel pontoComparar = PontoAtual.Aproximando ? PontoAtual : ProximoPonto != null ? ProximoPonto : PontoAtual;
|
||
AnguloCaminho = GPSUtils.CalcularOrientacao(Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps, pontoComparar.Posicao);
|
||
}
|
||
[JsonProperty]
|
||
public double DistanciaEsquerda { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaDireita { get; private set; }
|
||
private void AtualizarDistanciasLaterais()
|
||
{
|
||
DistanciaEsquerda = CalcularDistanciaLateral(true, Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps, CorredorAtual?.idxRuaEsquerda ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
|
||
DistanciaDireita = CalcularDistanciaLateral(false, Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps, CorredorAtual?.idxRuaDireita ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
|
||
}
|
||
|
||
public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, DirecaoCarroRua direcaoAtual)
|
||
{
|
||
if (posicaoAtual == null) return 0;
|
||
|
||
// Verifica se o corredor atual existe
|
||
if (!_CorredorAtualDefinido) return 0;
|
||
|
||
List<GPSModel> rua = new List<GPSModel>(RuasPlantacao[idxRua]);
|
||
|
||
double d0 = double.MaxValue;
|
||
int idx = -1;
|
||
for (int i = 0; i < rua.Count; i++)
|
||
{
|
||
double dist = GPSUtils.DistanciaEntrePontos(posicaoAtual, rua[i]);
|
||
if (dist < d0)
|
||
{
|
||
d0 = dist;
|
||
idx = i;
|
||
}
|
||
else
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
|
||
GPSModel p0 = rua[idx - (idx > 0 ? 1 : 0)];
|
||
GPSModel p1 = rua[idx + (idx < (rua.Count - 1) ? 1 : 0)];
|
||
int idxPa = rua.IndexOf(p0);
|
||
int idxPb = rua.IndexOf(p1);
|
||
double anguloAprojetar = GPSUtils.CalcularOrientacao(rua[idxPb], rua[idxPa]);
|
||
double anguloBprojetar = GPSUtils.CalcularOrientacao(rua[idxPa], rua[idxPb]);
|
||
|
||
GPSModel pontoA = GPSUtils.GerarPontoDeslocado(rua[idxPa], anguloAprojetar, DistanciaManobraEntreRuas);
|
||
GPSModel pontoB = GPSUtils.GerarPontoDeslocado(rua[idxPb], anguloBprojetar, DistanciaManobraEntreRuas);
|
||
|
||
List<GPSModel> novaRua = new List<GPSModel>();
|
||
novaRua.Add(pontoA);
|
||
for (int i = idxPa; i <= idxPb; i++)
|
||
{
|
||
novaRua.Add(rua[i]);
|
||
}
|
||
novaRua.Add(pontoB);
|
||
|
||
rua = novaRua;
|
||
|
||
int qtdPontosRua = Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua));
|
||
rua = GPSUtils.InterpolarRota(rua, 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 =
|
||
(pontoB.Longitude - pontoA.Longitude) * (posicaoAtual.Latitude - pontoA.Latitude) -
|
||
(pontoB.Latitude - pontoA.Latitude) * (posicaoAtual.Longitude - pontoA.Longitude);
|
||
|
||
// Ajusta a distância com base no sentido de deslocamento do robô
|
||
if ((direcaoAtual == DirecaoCarroRua.Volta && ladoEsquerdo) || (direcaoAtual == DirecaoCarroRua.Ida && !ladoEsquerdo))
|
||
{
|
||
distancia *= crossProduct > 0 ? 1 : -1;
|
||
}
|
||
else
|
||
{
|
||
distancia *= crossProduct > 0 ? -1 : 1;
|
||
}
|
||
|
||
// Ajusta a largura do robô no cálculo final
|
||
double larguraAjuste = ladoEsquerdo ? VariaveisEquipamento.LarguraEsquerda : VariaveisEquipamento.LarguraDireita;
|
||
distancia -= (larguraAjuste / 100.0);
|
||
|
||
return distancia;
|
||
}
|
||
[JsonProperty]
|
||
public PontoTrajetoriaModel PontoAtual { get; private set; }
|
||
private void DefinirPontoAtual()
|
||
{
|
||
// ✅ Evita processamento desnecessário se a lista for nula ou vazia
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
PontoAtual = null;
|
||
return;
|
||
}
|
||
|
||
// Busca eficiente: percorre do começo ao fim para achar o primeiro não visitado
|
||
for (int i = idxFinalJanela; i >= idxInicialJanela; i--)
|
||
{
|
||
if (_TrajetoriaFixa[i].Visitado)
|
||
{
|
||
PontoAtual = _TrajetoriaFixa[i];
|
||
return;
|
||
}
|
||
}
|
||
|
||
PontoAtual = null;
|
||
}
|
||
[JsonProperty]
|
||
public PontoTrajetoriaModel ProximoPonto { get; private set; }
|
||
private void DefinirProximoPonto()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
ProximoPonto = null;
|
||
return;
|
||
}
|
||
|
||
if (PontoAtual?.idxPonto == _TrajetoriaFixa.Count - 1)
|
||
{
|
||
ProximoPonto = PontoAtual;
|
||
}
|
||
else
|
||
{
|
||
int idx = _TrajetoriaFixa.IndexOf(PontoAtual) + 1;
|
||
ProximoPonto = _TrajetoriaFixa[idx];
|
||
}
|
||
|
||
/*// Busca eficiente: percorre do começo ao fim para achar o primeiro não visitado
|
||
for (int i = idxInicialJanela; i <= idxFinalJanela; i++)
|
||
{
|
||
if (!_TrajetoriaFixa[i].Visitado)
|
||
{
|
||
ProximoPonto = _TrajetoriaFixa[i];
|
||
return;
|
||
}
|
||
}
|
||
|
||
ProximoPonto = PontoAtual; // Se não houver pontos não visitados, retorna o último ponto visitado
|
||
*/
|
||
}
|
||
[JsonProperty]
|
||
public PontoTrajetoriaModel PontoMaisProximo { get; private set; }
|
||
private void DefinirPontoMaisProximo()
|
||
{
|
||
// ✅ Evita processamento desnecessário se a lista for nula ou vazia
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
PontoMaisProximo = null;
|
||
return;
|
||
}
|
||
|
||
double menorDistancia = double.MaxValue;
|
||
PontoTrajetoriaModel melhorPonto = null;
|
||
|
||
// ✅ Busca apenas na janela otimizada
|
||
for (int i = idxInicialJanela; i <= idxFinalJanela && i < _TrajetoriaFixa.Count; i++)
|
||
{
|
||
if (Math.Abs(_TrajetoriaFixa[i].DistanciaTrajeto) < menorDistancia)
|
||
{
|
||
melhorPonto = _TrajetoriaFixa[i];
|
||
menorDistancia = Math.Abs(melhorPonto.DistanciaTrajeto);
|
||
}
|
||
}
|
||
|
||
// ✅ Só atualiza se encontrou um ponto
|
||
if (melhorPonto != null)
|
||
{
|
||
PontoMaisProximo = melhorPonto;
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public bool NaMargemDoCorredor { get; private set; }
|
||
private void AtualizarNaMargemDoCorredor()
|
||
{
|
||
NaMargemDoCorredor = false;
|
||
foreach (var ponto in _TrajetoriaJanela)
|
||
{
|
||
if (ponto.NaMargem && ponto.PontoBorda)
|
||
{
|
||
NaMargemDoCorredor = true; // Paramos a busca assim que encontramos um ponto válido
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public bool ManobrandoEntreRuas { get; private set; }
|
||
private void AtualizarManobrandoEntreRuas()
|
||
{
|
||
//bool proximoAentrada = (ProximoPonto.PontoBorda || ProximoPonto.PontoLigacao) && ProximoPonto.DistanciaAtual < DistanciaManobraEntreRuas;
|
||
//ManobrandoEntreRuas = (proximoAentrada && StatusAtual == StatusCarroMapa.Direcionando) || NaMargemDoCorredor;
|
||
|
||
ManobrandoEntreRuas = StatusAtual == StatusCarroMapa.Manobrando;
|
||
}
|
||
[JsonProperty]
|
||
public StatusCarroMapa StatusAtual { get; private set; }
|
||
private void AtualizarStatusAtual()
|
||
{
|
||
StatusAtual = StatusCarroMapa.Parado;
|
||
if (ProximoPonto != null)
|
||
{
|
||
if (CorredorAtual != null)
|
||
{
|
||
if ((RetornandoBase && CorredorAtual.Dentro) || (!RetornandoBase && Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.EmAndamento))
|
||
{
|
||
if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas)
|
||
{
|
||
StatusAtual = StatusCarroMapa.Direcionando;
|
||
}
|
||
else if (!CorredorAtual.Dentro && (PontoAtual.PontoBorda || PontoAtual.PontoLigacao || ProximoPonto.Tipo == TipoPontoRua.LigacaoEntrada || PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores))
|
||
{
|
||
StatusAtual = StatusCarroMapa.Manobrando;
|
||
}
|
||
else if (!CorredorAtual.Dentro && PontoAtual.Tipo == TipoPontoRua.Rua && ProximoPonto.Tipo == TipoPontoRua.Rua && ProximoPonto.OrientacaoAtual > 7.0)
|
||
{
|
||
StatusAtual = StatusCarroMapa.Manobrando;
|
||
}
|
||
else if (CorredorAtual.Dentro && !NaMargemDoCorredor)
|
||
{
|
||
StatusAtual = StatusCarroMapa.CaminhandoRua;
|
||
}
|
||
else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaEntrada))
|
||
{
|
||
StatusAtual = StatusCarroMapa.EntrandoRua;
|
||
}
|
||
else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaSaida))
|
||
{
|
||
StatusAtual = StatusCarroMapa.SaindoRua;
|
||
}
|
||
else if (!CorredorAtual.Dentro)
|
||
{
|
||
StatusAtual = StatusCarroMapa.Direcionando;
|
||
}
|
||
}
|
||
else if (RetornandoBase)
|
||
{
|
||
StatusAtual = StatusCarroMapa.RetornandoBase;
|
||
}
|
||
}
|
||
else if (RetornandoBase)
|
||
{
|
||
StatusAtual = StatusCarroMapa.RetornandoBase;
|
||
}
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public double DistanciaTotal { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaPercorrida { get; private set; }
|
||
public void AtualizarDistanciaPercorrida(double distancia)
|
||
{
|
||
DistanciaPercorrida += distancia;
|
||
CorredorAtual?.AtualizarDistancias(distancia);
|
||
}
|
||
[JsonProperty]
|
||
public double DistanciaRestante { get; private set; }
|
||
private void AtualizarDistanciaRestante()
|
||
{
|
||
var _trajetoria = (_TrajetoriaDinamica ?? new List<PontoTrajetoriaModel>()).Select(x => x.Posicao).ToList();
|
||
DistanciaRestante = GPSUtils.DistanciaDoTrecho(_trajetoria);
|
||
}
|
||
[JsonProperty]
|
||
public double PercentualTrajetoria { get; private set; }
|
||
public void AtualizarPercentualTrajetoria()
|
||
{
|
||
PercentualTrajetoria = DistanciaTotal > 0 ? (DistanciaPercorrida / DistanciaTotal) * 100.0 : 0;
|
||
}
|
||
[JsonProperty]
|
||
public string TempoEstimadoOperacao { get; private set; }
|
||
[JsonProperty]
|
||
public string TempoEstimadoRestante { get; private set; }
|
||
public void AtualizarTempoEstimado(double? velocidadeSemErvasMs = null, double? velocidadeComErvasMs = null)
|
||
{
|
||
if (!velocidadeSemErvasMs.HasValue)
|
||
{
|
||
velocidadeSemErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeSErvasPercent) * 0.8;
|
||
}
|
||
if (!velocidadeComErvasMs.HasValue)
|
||
{
|
||
velocidadeComErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent);
|
||
}
|
||
|
||
// Supondo que metade do percurso tem ervas e a outra metade não tem
|
||
double distanciaComErvas = DistanciaTotal * VariaveisOperacao.PercentualComErvas;
|
||
double distanciaSemErvas = DistanciaTotal * VariaveisOperacao.PercentualSemErvas;
|
||
|
||
// Tempo em segundos para cada parte do percurso
|
||
double tempoSemErvas = velocidadeSemErvasMs.Value > 0 ? distanciaSemErvas / velocidadeSemErvasMs.Value : 0;
|
||
double tempoComErvas = velocidadeComErvasMs.Value > 0 ? distanciaComErvas / velocidadeComErvasMs.Value : 0;
|
||
|
||
// Tempo total em segundos
|
||
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");
|
||
|
||
// Distâncias (em metros)
|
||
double distanciaRestante = DistanciaRestante;
|
||
|
||
// Calcular a distância restante sem ervas e com ervas
|
||
double distanciaSemErvasRestante = distanciaRestante * VariaveisOperacao.PercentualSemErvas;
|
||
double distanciaComErvasRestante = distanciaRestante * VariaveisOperacao.PercentualComErvas;
|
||
|
||
// Calcular o tempo restante para as áreas sem ervas e com ervas
|
||
double tempoRestanteSemErvas = distanciaSemErvasRestante / velocidadeSemErvasMs.Value;
|
||
double tempoRestanteComErvas = distanciaComErvasRestante / velocidadeComErvasMs.Value;
|
||
|
||
// 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 = totalRestante.ToString("HH:mm:ss");
|
||
}
|
||
[JsonProperty]
|
||
public double DistanciaErroMapaPlantacao { get; private set; } = -1;
|
||
public GPSModel PrimeiroPontoDentroMapa = null;
|
||
public GPSModel PrimeiroPontoDentroCana = null;
|
||
public void AtualizarDistanciaErroMapaPlantacao()
|
||
{
|
||
if (DistanciaErroMapaPlantacao > -1) return;
|
||
if (CorredorAtual == null) return;
|
||
if (CorredorAtual.Idx > 0) return;
|
||
if (!CorredorAtual.Dentro) return;
|
||
|
||
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps.Clone();
|
||
|
||
if (PrimeiroPontoDentroMapa == null) PrimeiroPontoDentroMapa = posicaoAtual;
|
||
|
||
var OpVisual = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura?.OperadorVisual ?? new VisualWorkerModel();
|
||
StatusModulo OpVisualStatus = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado;
|
||
|
||
if (OpVisualStatus != StatusModulo.Operante) return;
|
||
|
||
var DadosSegmentacao = OpVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel();
|
||
bool CanaNoRadar = new List<StatusCarroMapa>() { StatusCarroMapa.EntrandoRua, StatusCarroMapa.CaminhandoRua, StatusCarroMapa.SaindoRua }.Contains(DadosSegmentacao.status_corredor);
|
||
if (!CanaNoRadar) return;
|
||
|
||
PrimeiroPontoDentroCana = posicaoAtual;
|
||
|
||
DistanciaErroMapaPlantacao = GPSUtils.DistanciaEntrePontos(PrimeiroPontoDentroMapa, PrimeiroPontoDentroCana);
|
||
}
|
||
|
||
[JsonProperty]
|
||
public double ErroOrientacaoAngular { get; private set; }
|
||
[JsonProperty]
|
||
public double ErroOrientacaoAngularCombinado { get; private set; }
|
||
[JsonProperty]
|
||
public double ErroOrientacaoAngularCaminho { get; private set; }
|
||
[JsonProperty]
|
||
public double ErroOrientacaoAngularProximoPonto { get; private set; }
|
||
[JsonProperty]
|
||
public double ErroLateralAngular { get; private set; }
|
||
[JsonProperty]
|
||
public bool TrajeotiraConcluida { get; private set; }
|
||
private void AtualizarTrajetoriaConcluida()
|
||
{
|
||
TrajeotiraConcluida = _TrajetoriaDinamica.All(x => x.Visitado);
|
||
}
|
||
|
||
private void AtualizarErroCombinado()
|
||
{
|
||
var _Sensoriamento = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura;
|
||
|
||
//double anguloCamera = _Sensoriamento.OperadorVisual.Leitura.obj?.segmentacao?.angulo_rua ?? 0;
|
||
double anguloCorredor = AnguloMedioCorredor;
|
||
double anguloProximoPonto = AnguloCaminho;
|
||
double anguloCarro = _Sensoriamento.Gps?.AnguloCarroDefinido ?? 0;
|
||
|
||
// Erro 1: Direção para onde o robô deve ir (centro do corredor)
|
||
double erroIrParaCentro = GPSUtils.CalcularDiferencaAngulo(anguloCarro, anguloProximoPonto);
|
||
|
||
// Erro 2: Quanto o robô está desalinhado da orientação da rua
|
||
double desalinhamentoComRua = GPSUtils.CalcularDiferencaAngulo(anguloCarro, anguloCorredor);
|
||
|
||
// Erro 3 (secundário): Diferença entre o ângulo do corredor e o ponto (curva futura)
|
||
double curvaDaRua = GPSUtils.CalcularDiferencaAngulo(anguloProximoPonto, anguloCorredor);
|
||
|
||
// Peso do desalinhamento atual (prioriza alinhar o robô ao corredor)
|
||
double pesoAlinhamento = Math.Min(Math.Abs(desalinhamentoComRua) / Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo, 1.0); // Normaliza até 30°
|
||
|
||
// Peso do centro (prioriza ir pro ponto central do corredor)
|
||
double pesoCentro = 1.0 - pesoAlinhamento;
|
||
|
||
// Peso extra para curvas mais fechadas
|
||
double boostCurva = 1.0 + (Math.Abs(curvaDaRua) / 45.0); // aumenta peso em curvas
|
||
|
||
// Calcule o erro lateral baseado nas distâncias entre as ruas do mapa atraves do GPS
|
||
double erroLateral = !(CorredorAtual?.Dentro ?? false) ? 0 : (DistanciaEsquerda - DistanciaDireita);
|
||
double distanciaProximoPonto = ProximoPonto.DistanciaAtual;
|
||
double erroLateralGraus = Math.Atan2(erroLateral, distanciaProximoPonto) * (180.0 / Math.PI);
|
||
|
||
// Calcule o erro do ultrassom
|
||
double erroUltrassom = UltrasonicA05Helper.CalcularErroUltrassons();
|
||
|
||
// Combina os erros com pesos dinâmicos
|
||
double erroCombinado = (erroIrParaCentro * pesoCentro) + (desalinhamentoComRua * pesoAlinhamento);
|
||
|
||
|
||
ErroOrientacaoAngularProximoPonto = erroIrParaCentro;
|
||
ErroOrientacaoAngularCaminho = desalinhamentoComRua;
|
||
ErroOrientacaoAngular = curvaDaRua;
|
||
ErroOrientacaoAngularCombinado = erroCombinado * boostCurva;
|
||
ErroLateralAngular = erroLateralGraus;
|
||
}
|
||
|
||
private void MarcarPontosIntermediariosNaoVisitados()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida) return;
|
||
|
||
for (int i = PontoAtual.idxPonto; i >= Math.Max(0, PontoAtual.idxPonto - JanelaAtualizacaoDePontos); i--)
|
||
{
|
||
var ponto = _TrajetoriaFixa[i];
|
||
if (!ponto.Visitado)
|
||
{
|
||
ponto.Visitado = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
private void AtualizarPropriedadesPontosTrajetoria()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida) return;
|
||
|
||
// Atualiza apenas os pontos dentro do intervalo definido
|
||
foreach (var ponto in _TrajetoriaJanela)
|
||
{
|
||
ponto.AtualizarPropriedades();
|
||
}
|
||
|
||
AtualizarTrajetoriaConcluida();
|
||
}
|
||
|
||
private void AtualizarDadosTrajetoria()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida) return;
|
||
|
||
AtualizarTempoEntreLeituras();
|
||
AtualizarDistanciaMaximaEntreLeituras();
|
||
|
||
Inicio:
|
||
AtualizarIndicesJanela();
|
||
AtualizarTrajetoriaJanela();
|
||
|
||
VerificaFimCorredorSegmentacao();
|
||
|
||
DefinirPontoAtual();
|
||
AtualizarCorredorAtual();
|
||
|
||
//if (AutonomiaCorredor.Liberado)
|
||
DefinirProximoPonto();
|
||
|
||
if (PontoAtual == null) goto Inicio;
|
||
if (VerificaInicioOperacaoMeioRua()) goto Inicio;
|
||
|
||
MarcarPontosIntermediariosNaoVisitados();
|
||
DefinirPontoMaisProximo();
|
||
AtualizarAnguloMedioCorredor();
|
||
AtualizarAnguloCaminho();
|
||
AtualizarDistanciasLaterais();
|
||
AtualizarNaMargemDoCorredor();
|
||
AtualizarManobrandoEntreRuas();
|
||
AtualizarStatusAtual();
|
||
AtualizarDistanciaRestante();
|
||
AtualizarPercentualTrajetoria();
|
||
AtualizarTempoEstimado();
|
||
|
||
AtualizarErroCombinado();
|
||
|
||
AtualizarTrajetoriaDinamica();
|
||
|
||
AtualizarDistanciaErroMapaPlantacao();
|
||
}
|
||
|
||
|
||
private int VerificaEquipamentoDentroCorredor(GPSModel posicaoAtual)
|
||
{
|
||
Dictionary<int, (bool, double)> melhoresPontosCorredores = new Dictionary<int, (bool, double)>();
|
||
foreach (var corredor in Corredores)
|
||
{
|
||
(GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(posicaoAtual, corredor);
|
||
if (pontoMaisProximo == null)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
double distanciaMargem = LarguraCorredorPadrao * 0.8;
|
||
bool naMargem = distanciaAtual < distanciaMargem;
|
||
|
||
melhoresPontosCorredores.Add(melhoresPontosCorredores.Count, (naMargem, distanciaAtual));
|
||
}
|
||
return melhoresPontosCorredores.Any(x => x.Value.Item1) ? melhoresPontosCorredores.Where(x => x.Value.Item1).OrderBy(x => x.Value.Item2).FirstOrDefault().Key : -1;
|
||
}
|
||
|
||
private (bool, int) VerificaEquipamentoDentroCorredor()
|
||
{
|
||
_TrajetoriaJanela = _TrajetoriaFixa;
|
||
AtualizarPropriedadesPontosTrajetoria();
|
||
AtualizarTrajetoriaJanela();
|
||
List<PontoTrajetoriaModel> melhoresPontosCorredores = new List<PontoTrajetoriaModel>();
|
||
foreach (var corredor in Corredores)
|
||
{
|
||
(GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(_TrajetoriaFixa[0].Posicao, corredor);
|
||
if (pontoMaisProximo == null)
|
||
{
|
||
return (false, -1);
|
||
}
|
||
|
||
var melhorPonto = _TrajetoriaFixa.FirstOrDefault(x => x.Posicao.Latitude == pontoMaisProximo.Latitude && x.Posicao.Longitude == pontoMaisProximo.Longitude);
|
||
melhoresPontosCorredores.Add(melhorPonto);
|
||
}
|
||
bool pontoNaMargem = melhoresPontosCorredores.Any(x => x.NaMargem);
|
||
int idxPonto = !pontoNaMargem ? 0 : melhoresPontosCorredores.Where(x => x.NaMargem).OrderBy(x => x.DistanciaAtual).FirstOrDefault().idxPonto;
|
||
return (pontoNaMargem, idxPonto);
|
||
}
|
||
|
||
private bool VerificaInicioOperacaoMeioRua()
|
||
{
|
||
if (!VerificacaoInicialMeioRuaConcluida && new List<StatusOperacao>() { StatusOperacao.Aguardando, StatusOperacao.EmAndamento }.Contains(Variaveis.OperacaoEmAndamento.StatusAtual))
|
||
{
|
||
VerificacaoInicialMeioRuaConcluida = true;
|
||
(bool noCoredor, var idxPontoMaisProximo) = VerificaEquipamentoDentroCorredor();
|
||
if (noCoredor)
|
||
{
|
||
for (int i = 0; i < idxPontoMaisProximo; i++)
|
||
{
|
||
_TrajetoriaFixa[i].Visitado = true;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
|
||
private void VerificaFimCorredorSegmentacao()
|
||
{
|
||
if (CorredorAtual == null) return;
|
||
|
||
bool abrirCurva = true;
|
||
double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 5.0;
|
||
double distanciaCorredor = CorredorAtual.DistanciaTotal;
|
||
double distanciaRestanteCorredor = CorredorAtual.DistanciaRestante;
|
||
double distanciaUltimoPontoCorredor = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps, CorredorAtual.Pontos.LastOrDefault().Posicao);
|
||
var OpVisual = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura?.OperadorVisual ?? new VisualWorkerModel();
|
||
StatusModulo OpVisualStatus = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado;
|
||
var DadosSegmentacao = OpVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel();
|
||
|
||
if (OpVisualStatus == StatusModulo.Operante || Variaveis.OperacaoEmAndamento.Simulando) //OpVisual.Iniciado &&
|
||
{
|
||
// Contagem de tempo por status em segundos
|
||
if (CorredorAtual.TempoPorStatus == null) CorredorAtual.TempoPorStatus = new Dictionary<StatusCarroMapa, double>();
|
||
StatusCarroMapa statusCarro = DadosSegmentacao.status_corredor;
|
||
StatusCarroMapa statusCarroAnt = DadosSegmentacao.status_corredor_anterior;
|
||
if (!CorredorAtual.TempoPorStatus.TryGetValue(statusCarro, out var status))
|
||
{
|
||
CorredorAtual.TempoPorStatus.Add(statusCarro, 0);
|
||
}
|
||
if (CorredorAtual.UltimoTempoStatus > DateTime.MinValue)
|
||
{
|
||
CorredorAtual.TempoPorStatus[statusCarro] += (DateTime.Now - CorredorAtual.UltimoTempoStatus).TotalSeconds;
|
||
}
|
||
CorredorAtual.UltimoTempoStatus = DateTime.Now;
|
||
double TempoTotalContagem = CorredorAtual.TempoPorStatus.Sum(x => x.Value);
|
||
double TempoMovimento = CorredorAtual.TempoPorStatus.Where(x => x.Key != StatusCarroMapa.Parado).Sum(x => x.Value);
|
||
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.CaminhandoRua, out double TempoCaminhandoRua);
|
||
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.EntrandoRua, out double TempoEntrandoRua);
|
||
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.SaindoRua, out double TempoSaindoRua);
|
||
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.Direcionando, out double TempoDirecionando);
|
||
double TempoDentroRua = TempoEntrandoRua + TempoCaminhandoRua + TempoSaindoRua;
|
||
double PercentualTempoComCana = TempoMovimento == 0 ? 0 : TempoDentroRua / TempoMovimento;
|
||
double PercentualTempoSemCana = TempoMovimento == 0 ? 0 : TempoDirecionando / TempoMovimento;
|
||
double ts_agora = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0;
|
||
|
||
if (
|
||
(
|
||
(Variaveis.OperacaoEmAndamento.Simulando ||
|
||
(ts_agora - DadosSegmentacao.timestamp < 2.0 && PercentualTempoComCana > 0.4)) // Esteve a pelo menos 40% do tempo em movimento com presenca de cana e o dado esta atualizado
|
||
)
|
||
&& distanciaCorredor >= limiarDistanciaMargem
|
||
&& distanciaRestanteCorredor < limiarDistanciaMargem // Esta a pelo menos x metros do final do corredor
|
||
&& distanciaUltimoPontoCorredor < limiarDistanciaMargem // Esta a pelo menos x metros do ultimo ponto do corredor
|
||
&& statusCarro == StatusCarroMapa.Direcionando // Carro esta fora do corredor
|
||
//&& statusCarroAnt != statusCarro
|
||
)
|
||
{
|
||
double anguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo;
|
||
if (abrirCurva && !CorredorAtual.Ultimo)
|
||
{
|
||
anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda ? -Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo : Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo;
|
||
}
|
||
|
||
int pontosMarcar = 0;
|
||
foreach (var Ponto in CorredorAtual.Pontos.Where(x => !x.Visitado))
|
||
{
|
||
Ponto.Visitado = true;
|
||
pontosMarcar++;
|
||
}
|
||
if (!CorredorAtual.Ultimo)
|
||
{
|
||
_TrajetoriaFixa[CorredorAtual.Pontos[CorredorAtual.QtdPontos - 1].idxPonto + 1].Visitado = true; // Marca ponto de ligacao de entrada do proximo corredor como visitado
|
||
AtualizarIndicesJanela((pontosMarcar + 2) * 2);
|
||
AtualizarTrajetoriaJanela();
|
||
DefinirPontoAtual();
|
||
AtualizarCorredorAtual();
|
||
for (int i = 0; i < pontosMarcar; i++)
|
||
{
|
||
CorredorAtual.Pontos[i].Visitado = true;
|
||
}
|
||
AtualizarIndicesJanela();
|
||
AtualizarTrajetoriaJanela();
|
||
|
||
DefinirPontoAtual();
|
||
DefinirProximoPonto();
|
||
|
||
if (abrirCurva)
|
||
{
|
||
Console.WriteLine($"Angulo de controle definido de {Variaveis.OperacaoEmAndamento.Controle.Angulo} para {anguloControle}");
|
||
Variaveis.OperacaoEmAndamento.Controle.Angulo = anguloControle;
|
||
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent;
|
||
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.MovimentoArco;
|
||
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).DelayEnvioComandoExtra = 1500;
|
||
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DelayEnvioComandoExtra = 2000;
|
||
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(true, new List<T_Code>() { T_Code.Mov, T_Code.Dir });
|
||
}
|
||
}
|
||
}
|
||
|
||
RedisService.AtualizarCampos(
|
||
CtxKey.DadosVisualWorker,
|
||
("segmentacao.status_corredor_anterior", (int)statusCarro)
|
||
);
|
||
if (VisualWorkerService.DadosLeitura.Analises.segmentacao != null)
|
||
VisualWorkerService.DadosLeitura.Analises.segmentacao.status_corredor_anterior = statusCarro;
|
||
}
|
||
}
|
||
|
||
|
||
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);
|
||
}
|
||
|
||
|
||
public void LoopAtualizaDados()
|
||
{
|
||
if (Variaveis.OperacaoEmAndamento.Modo != ModoOperacao.MapaGPS)
|
||
return;
|
||
|
||
AtualizarPropriedadesPontosTrajetoria();
|
||
|
||
AtualizarDadosTrajetoria();
|
||
|
||
//Variaveis.OperacaoEmAndamento.AtualizaInformacoesControleOperacao();
|
||
|
||
//RedisService.Publish(CmdKey.ManagerWorkerRx, JsonConvert.SerializeObject(new { cmd = ManagerWorkerCommandType.AtualizarDadosControle }));
|
||
|
||
AtualizarDadosControleRedis();
|
||
}
|
||
|
||
private void AtualizarDadosControleRedis()
|
||
{
|
||
if (!Variaveis.OperacaoEmAndamento.Iniciado)
|
||
return;
|
||
|
||
int min_ticks_sem_resposta = 500;
|
||
int max_ticks_sem_resposta = 2000;
|
||
ManagerWorkerMessageResponseComandoModel novoComando = null;
|
||
try
|
||
{
|
||
string controleStr = RedisService.Get(CtxKey.DadosControle);
|
||
var dictParams = JObject.Parse(controleStr);
|
||
dictParams.TryGetValue("angulo_sp", out var _angulo);
|
||
dictParams.TryGetValue("velocidade_sp", out var _velocidade);
|
||
dictParams.TryGetValue("em_freio", out var _em_freio);
|
||
dictParams.TryGetValue("tipo_movimento_direcional", out var _movimento);
|
||
dictParams.TryGetValue("simulacao", out var _simulacao);
|
||
dictParams.TryGetValue("heartbeat", out var _heartbeat);
|
||
dictParams.TryGetValue("latencia", out var _latencia);
|
||
dictParams.TryGetValue("erro_lateral", out var _erro_lateral);
|
||
dictParams.TryGetValue("debug_custo", out var _debug_custo);
|
||
novoComando = new ManagerWorkerMessageResponseComandoModel()
|
||
{
|
||
angulo = Convert.ToDouble(_angulo),
|
||
percentual_velocidade = Convert.ToDouble(_velocidade),
|
||
em_freio = Convert.ToBoolean(_em_freio),
|
||
tipo_movimento = (TipoMovimentoDirecional)Convert.ToInt32(_movimento),
|
||
simulacao = JsonConvert.DeserializeObject<List<double[]>>((_simulacao ?? "").ToString()),
|
||
heartbeat = Convert.ToInt32(_heartbeat),
|
||
latencia = Convert.ToDouble(_latencia.ToString().Replace(".", ",")),
|
||
erro_lateral = Convert.ToDouble(_erro_lateral.ToString().Replace(".", ",")),
|
||
debug_custo = JsonConvert.DeserializeObject<Dictionary<string, ManagerWorkerMessageResponseDebugCustoModel>>(_debug_custo.ToString()),
|
||
};
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
//MostrarLog($"Erro ao receber novo comando: {ex.Message}");
|
||
}
|
||
|
||
DateTime agora = DateTime.Now;
|
||
|
||
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
|
||
bool comandoMudou = novoComando != null && (_Controle.Angulo != novoComando.angulo || _Controle.PercentualVelocidadeSP != novoComando.percentual_velocidade || _Controle.TipoMovimento != novoComando.tipo_movimento || _Controle.EmFreio != novoComando.em_freio);
|
||
bool mangerDestravou = novoComando != null && (_Controle.ticks_sem_resposta.AddMilliseconds(min_ticks_sem_resposta) < agora && novoComando.heartbeat != _Controle.heartbeat);
|
||
if (novoComando != null && (comandoMudou || mangerDestravou))
|
||
{
|
||
_Controle.Angulo = novoComando.angulo;
|
||
_Controle.TipoMovimento = novoComando.tipo_movimento;
|
||
_Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade;
|
||
_Controle.EmFreio = novoComando.em_freio;
|
||
_Controle.SimulacaoMPC = novoComando.simulacao?.Select(x => new MPCSimulacaoModel() { latitude = x[0], longitude = x[1], orientacao = x[2] }).ToList() ?? new List<MPCSimulacaoModel>();
|
||
_Controle.Latencia = novoComando.latencia;
|
||
_Controle.ErroLateral = novoComando.erro_lateral;
|
||
_Controle.DebugCustoMpc = novoComando.debug_custo;
|
||
Variaveis.OperacaoEmAndamento.AtualizaInformacoesControleOperacao();
|
||
}
|
||
|
||
if (novoComando != null && novoComando.heartbeat == _Controle.heartbeat)
|
||
{
|
||
if (_Controle.ticks_sem_resposta == DateTime.MinValue)
|
||
{
|
||
_Controle.ticks_sem_resposta = agora;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
_Controle.ticks_sem_resposta = DateTime.MinValue;
|
||
}
|
||
if (novoComando != null)
|
||
_Controle.heartbeat = novoComando.heartbeat;
|
||
if (_Controle.ticks_sem_resposta != DateTime.MinValue)
|
||
{
|
||
if (_Controle.ticks_sem_resposta.AddMilliseconds(max_ticks_sem_resposta) < agora)
|
||
{
|
||
Console.WriteLine("Muito tempo sem receber novo comando do Manager Worker! Parando Movimento!");
|
||
RedisService.AtualizarCampos(
|
||
CtxKey.DadosControle,
|
||
("velocidade_sp", 0)
|
||
);
|
||
}
|
||
else if (_Controle.ticks_sem_resposta.AddMilliseconds(min_ticks_sem_resposta) < agora)
|
||
{
|
||
Console.WriteLine("Muito tempo sem receber novo comando do Manager Worker! Reduzindo Velocidade!");
|
||
RedisService.AtualizarCampos(
|
||
CtxKey.DadosControle,
|
||
("velocidade_sp", Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent)
|
||
);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
private (List<List<GPSModel>>, List<double>) ProjetarCorredores()
|
||
{
|
||
var corredores = new List<List<GPSModel>>();
|
||
var larguras = new List<double>();
|
||
|
||
// Calcula o deslocamento lateral do GPS
|
||
double larguraTotal = VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita; // Largura total do robô
|
||
double deslocamentoLateralGPSpercentual = ((Math.Abs(VariaveisEquipamento.LarguraEsquerda - VariaveisEquipamento.LarguraDireita) / larguraTotal) / 2.0);
|
||
double deslocamentoLateralGPSmetros = (LarguraCorredorPadrao * deslocamentoLateralGPSpercentual);
|
||
|
||
if (Variaveis.OperacaoEmAndamento.Mapa.TipoMapa == TipoMapaOperacao.Corredores)
|
||
{
|
||
var ruasProjetadas = ProjetarRuasApartirDosCorredores(RuasPlantacao);
|
||
|
||
// Atualiza RuasPlantacao com as ruas projetadas
|
||
RuasPlantacao = ruasProjetadas;
|
||
}
|
||
|
||
for (int i = 0; i < RuasPlantacao.Count - 1; i++)
|
||
{
|
||
var rua1 = RuasPlantacao[i];
|
||
var rua2 = RuasPlantacao[i + 1];
|
||
|
||
// Garantir que as ruas tenham o mesmo número de pontos
|
||
if (rua1.Count != rua2.Count)
|
||
{
|
||
if (rua1.Count > rua2.Count)
|
||
{
|
||
rua2 = GPSUtils.InterpolarRota(rua2, rua1.Count);
|
||
}
|
||
else
|
||
{
|
||
rua1 = GPSUtils.InterpolarRota(rua1, rua2.Count);
|
||
}
|
||
}
|
||
|
||
var corredor = new List<GPSModel>();
|
||
|
||
double primeiraDistancia = 0;
|
||
|
||
// Calcular pontos médios entre as duas ruas e interpolar a cada 25 cm
|
||
for (int j = 0; j < rua1.Count - 1; j++)
|
||
{
|
||
var ponto1Rua1 = rua1[j];
|
||
var ponto2Rua1 = rua1[j + 1];
|
||
var ponto1Rua2 = rua2[j];
|
||
var ponto2Rua2 = rua2[j + 1];
|
||
|
||
//var pontoMedioInicial = GPSUtils.PontoMedio(ponto1Rua1, ponto1Rua2);
|
||
//var pontoMedioFinal = GPSUtils.PontoMedio(ponto2Rua1, ponto2Rua2);
|
||
|
||
var pontoMedioInicial = GPSUtils.PontoMedioComOffset(ponto1Rua1, ponto1Rua2, deslocamentoLateralGPSmetros);
|
||
var pontoMedioFinal = GPSUtils.PontoMedioComOffset(ponto2Rua1, ponto2Rua2, deslocamentoLateralGPSmetros);
|
||
|
||
var distancia = GPSUtils.DistanciaEntrePontos(pontoMedioInicial, pontoMedioFinal);
|
||
if (EspacamentoPrimeirosPontosProjecao && j == 0)
|
||
{
|
||
primeiraDistancia = distancia;
|
||
}
|
||
double distInterpolar = (EspacamentoPrimeirosPontosProjecao && j == 0) ? DistanciaEntrePontosCurva : DistanciaEntrePontos;
|
||
|
||
int numInterpolacoes = (int)(distancia / distInterpolar);
|
||
|
||
for (int k = 0; k <= numInterpolacoes; k++)
|
||
{
|
||
double t = numInterpolacoes == 0 ? 0 : k / (double)numInterpolacoes;
|
||
var pontoInterpolado = GPSUtils.InterpolarPonto(pontoMedioInicial, pontoMedioFinal, t);
|
||
corredor.Add(pontoInterpolado);
|
||
}
|
||
}
|
||
|
||
double distanciaCorredor = GPSUtils.DistanciaDoTrecho(corredor);
|
||
double distanciaRua1 = GPSUtils.DistanciaDoTrecho(rua1);
|
||
double distanciaRua2 = GPSUtils.DistanciaDoTrecho(rua2);
|
||
|
||
if ((distanciaCorredor + DistanciaEntrePontos) < distanciaRua1)
|
||
{
|
||
double dif = Math.Abs(distanciaRua1 - distanciaCorredor);
|
||
double anguloCorredor = GPSUtils.CalcularOrientacao(corredor[corredor.Count - 1], corredor[corredor.Count - 2]);
|
||
GPSModel novoPontoCorredor = GPSUtils.ProjetarPontoDeslocado(corredor[0], dif, anguloCorredor);
|
||
corredor.Insert(0, novoPontoCorredor);
|
||
}
|
||
else if ((distanciaCorredor + DistanciaEntrePontos) < distanciaRua2)
|
||
{
|
||
double dif = Math.Abs(distanciaRua2 - distanciaCorredor);
|
||
double anguloCorredor = GPSUtils.CalcularOrientacao(corredor[corredor.Count - 1], corredor[corredor.Count - 2]);
|
||
GPSModel novoPontoCorredor = GPSUtils.ProjetarPontoDeslocado(corredor[0], dif, anguloCorredor);
|
||
corredor.Insert(0, novoPontoCorredor);
|
||
}
|
||
|
||
// Remover pontos que estão a menos de 1m de distância
|
||
corredor = RemoverPontosMuitoProximos(corredor, DistanciaEntrePontos, primeiraDistancia, DistanciaEntrePontosCurva);
|
||
|
||
// Calcular largura média entre as duas ruas
|
||
double larguraCorredor = CalcularLarguraMediaDoCorredor(rua1, rua2);
|
||
|
||
corredores.Add(corredor);
|
||
larguras.Add(larguraCorredor);
|
||
}
|
||
|
||
|
||
return (corredores, larguras);
|
||
}
|
||
|
||
private List<List<GPSModel>> ProjetarRuasApartirDosCorredores(List<List<GPSModel>> corredores)
|
||
{
|
||
var ruasProjetadas = new List<List<GPSModel>>();
|
||
double larguraCorredor = LarguraCorredorPadrao;
|
||
|
||
if (corredores.Count > 1)
|
||
{
|
||
larguraCorredor = CalcularLarguraMediaDoCorredor(corredores[0], corredores[1]);
|
||
}
|
||
|
||
for (int i = 0; i < corredores.Count; i++)
|
||
{
|
||
var corredor = corredores[i];
|
||
double offset = larguraCorredor / 2.0;
|
||
|
||
// Calcular ruas apenas para o primeiro corredor
|
||
if (i == 0)
|
||
{
|
||
var ruaEsquerda = new List<GPSModel>();
|
||
|
||
for (int j = 0; j < corredor.Count - 1; j++)
|
||
{
|
||
var pontoAtual = corredor[j];
|
||
var pontoProximo = corredor[j + 1];
|
||
|
||
double angulo = GPSUtils.CalcularOrientacao(pontoAtual, pontoProximo);
|
||
var pontoEsquerda = GPSUtils.ProjetarPontoDeslocado(pontoAtual, offset, angulo + 90);
|
||
ruaEsquerda.Add(pontoEsquerda);
|
||
}
|
||
|
||
// Adiciona último ponto da rua esquerda
|
||
var ultimo = corredor[corredor.Count - 1];
|
||
var anterior = corredor[corredor.Count - 2];
|
||
double anguloFinal = GPSUtils.CalcularOrientacao(anterior, ultimo);
|
||
ruaEsquerda.Add(GPSUtils.ProjetarPontoDeslocado(ultimo, offset, anguloFinal + 90));
|
||
|
||
ruasProjetadas.Add(ruaEsquerda);
|
||
}
|
||
|
||
// Sempre criar a rua direita (inclusive para o último corredor)
|
||
var ruaDireita = new List<GPSModel>();
|
||
|
||
for (int j = 0; j < corredor.Count - 1; j++)
|
||
{
|
||
var pontoAtual = corredor[j];
|
||
var pontoProximo = corredor[j + 1];
|
||
|
||
double angulo = GPSUtils.CalcularOrientacao(pontoAtual, pontoProximo);
|
||
var pontoDireita = GPSUtils.ProjetarPontoDeslocado(pontoAtual, offset, angulo - 90);
|
||
ruaDireita.Add(pontoDireita);
|
||
}
|
||
|
||
var ultimoPonto = corredor[corredor.Count - 1];
|
||
var pontoAnterior = corredor[corredor.Count - 2];
|
||
double anguloUltimo = GPSUtils.CalcularOrientacao(pontoAnterior, ultimoPonto);
|
||
ruaDireita.Add(GPSUtils.ProjetarPontoDeslocado(ultimoPonto, offset, anguloUltimo - 90));
|
||
|
||
ruasProjetadas.Add(ruaDireita);
|
||
}
|
||
|
||
return ruasProjetadas;
|
||
}
|
||
|
||
// Método auxiliar para remover pontos muito próximos de forma balanceada
|
||
private List<GPSModel> RemoverPontosMuitoProximos(List<GPSModel> pontos, double distanciaMinima, double apartirDe, double distanciaMenor)
|
||
{
|
||
if (pontos == null || pontos.Count == 0)
|
||
return pontos;
|
||
|
||
pontos = pontos.Where(x => !double.IsNaN(x.Latitude) && !double.IsNaN(x.Longitude)).ToList();
|
||
|
||
var resultado = new List<GPSModel> { pontos[0] }; // Sempre incluir o primeiro ponto
|
||
double distanciaAcumulada = 0.0; // Distância total percorrida
|
||
|
||
for (int i = 1; i < pontos.Count - 1; i++) // Evita verificar o último ponto diretamente
|
||
{
|
||
double distancia = GPSUtils.DistanciaEntrePontos(resultado.Last(), pontos[i]);
|
||
distanciaAcumulada += distancia;
|
||
|
||
// Nos primeiros 2 metros, mantemos um espaçamento mínimo de 25 cm, depois 1 metro
|
||
double distanciaRequerida = distanciaAcumulada <= apartirDe ? distanciaMenor : distanciaMinima;
|
||
|
||
if (distancia >= distanciaRequerida)
|
||
{
|
||
resultado.Add(pontos[i]); // Mantém o ponto se a distância for suficiente
|
||
}
|
||
}
|
||
|
||
// Sempre adiciona o último ponto da lista para garantir fechamento da trajetória
|
||
if (!resultado.Contains(pontos.Last()))
|
||
{
|
||
resultado.Add(pontos.Last());
|
||
}
|
||
|
||
return resultado;
|
||
}
|
||
|
||
public static double CalcularLarguraMediaDoCorredor(List<GPSModel> rua1, List<GPSModel> rua2)
|
||
{
|
||
// 1. Interpolar as duas ruas para garantir pontos uniformemente distribuídos
|
||
List<GPSModel> rua1Interpolada = GPSUtils.InterpolarRota(rua1, Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua1)));
|
||
List<GPSModel> rua2Interpolada = GPSUtils.InterpolarRota(rua2, Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua2)));
|
||
|
||
double somaDistancias = 0;
|
||
int totalPontos = 0;
|
||
|
||
// 2. Para cada ponto da rua1 interpolada, encontrar o mais próximo na rua2 interpolada
|
||
foreach (var ponto1 in rua1Interpolada)
|
||
{
|
||
var pontoMaisProximo = rua2Interpolada.OrderBy(ponto2 => GPSUtils.DistanciaEntrePontos(ponto1, ponto2)).First();
|
||
|
||
double distancia = GPSUtils.DistanciaEntrePontos(ponto1, pontoMaisProximo);
|
||
|
||
somaDistancias += distancia;
|
||
totalPontos++;
|
||
}
|
||
|
||
// 3. Retorna a largura média do corredor
|
||
return totalPontos > 0 ? somaDistancias / totalPontos : 0;
|
||
}
|
||
|
||
public void ProjetarTrajetoriaFixa(GPSModel posicaoRobo = null)
|
||
{
|
||
List<PontoTrajetoriaModel> _trajetoriaFixa = new List<PontoTrajetoriaModel>();
|
||
(List<List<GPSModel>> corredores, List<double> CorredoresLarguras) = ProjetarCorredores();
|
||
Corredores = corredores;
|
||
|
||
double larguraCorredorMenor = LarguraCorredorPadrao * 0.9;
|
||
DirecaoCarroRua direcaoAtual = ExtremoMaisProximoMapa == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
||
|
||
PontoTrajetoriaModel PontoRobo = new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
|
||
{
|
||
idxCorredor = 0,
|
||
idxPonto = 0,
|
||
idxPontoCorredor = 0,
|
||
Posicao = posicaoRobo ?? Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps,
|
||
Orientacao = 0,
|
||
Direcao = direcaoAtual,
|
||
Visitado = true,
|
||
LarguraCorredor = 1.0
|
||
};
|
||
|
||
int idxCorredorDentro = VerificaEquipamentoDentroCorredor(PontoRobo.Posicao);
|
||
if (idxCorredorDentro > -1)
|
||
{
|
||
var corredor = Corredores[idxCorredorDentro];
|
||
double orientacaoCorredor = GPSUtils.CalcularOrientacao(corredor[0], corredor[1]);
|
||
double difOri = Math.Abs(orientacaoCorredor - PontoRobo.Posicao?.OrientacaoReal ?? 0);
|
||
bool alinhado = difOri < 180;
|
||
if (idxCorredorDentro % 2 == 0)
|
||
{
|
||
direcaoAtual = alinhado ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
||
}
|
||
else
|
||
{
|
||
direcaoAtual = alinhado ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
|
||
}
|
||
PontoRobo.Direcao = direcaoAtual;
|
||
}
|
||
|
||
_trajetoriaFixa.Add(PontoRobo);
|
||
|
||
int idxEsq = 0;
|
||
int idxDir = 0;
|
||
|
||
double anguloAcrescentar = direcaoAtual == DirecaoCarroRua.Ida ? 45 : -45;
|
||
|
||
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<GPSModel> ruaEsq, List<GPSModel> 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<GPSModel> CorredorAtual = Corredores[idx];
|
||
|
||
//double dP0 = GPSUtils.DistanciaEntrePontos(_trajetoriaFixa.Last().Posicao, CorredorAtual[0]);
|
||
//double dP1 = GPSUtils.DistanciaEntrePontos(_trajetoriaFixa.Last().Posicao, CorredorAtual[CorredorAtual.Count() - 1]);
|
||
//if (dP1 < dP0)
|
||
if (direcaoAtual == DirecaoCarroRua.Volta)
|
||
{
|
||
CorredorAtual.Reverse();
|
||
}
|
||
|
||
double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).First(), CorredorAtual.First());
|
||
GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.First(), DistanciaProjecaoRua, anguloProjetar1);
|
||
|
||
var ultimoPontoTrajetoria = _trajetoriaFixa.LastOrDefault();
|
||
|
||
PontoTrajetoriaModel PontoInicial = new PontoTrajetoriaModel(TipoPontoRua.LigacaoEntrada)
|
||
{
|
||
idxCorredor = idx,
|
||
idxPonto = _trajetoriaFixa.Count(),
|
||
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
||
Posicao = PrimeiroPonto,
|
||
Direcao = direcaoAtual,
|
||
Orientacao = GPSUtils.CalcularOrientacao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto),
|
||
Visitado = false,
|
||
LarguraCorredor = larguraCorredorMenor
|
||
};
|
||
|
||
bool PontoInicialAdicionado = false;
|
||
// Criar curva suave para ligacao dos corredores
|
||
if (idx > 0)
|
||
{
|
||
// 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].First(), Corredores[idx - 1].Last());
|
||
GPSModel _ultimoPonto = GPSUtils.ProjetarPontoDeslocado(Corredores[idx - 1].Last(), DistanciaProjecaoRua, anguloProjecao);
|
||
|
||
GPSModel P0 = CalcularPontoControle(_penultimoPonto, _ultimoPonto, PrimeiroPonto, DistanciaProjecaoRua * 2.0);
|
||
List<GPSModel> CurvaConexao = GerarCurvaConexao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, P0, pontosAdicionr);
|
||
|
||
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.Last().Posicao, ponto),
|
||
Visitado = false,
|
||
LarguraCorredor = larguraCorredorMenor * 0.9
|
||
});
|
||
}
|
||
|
||
}
|
||
PontoInicialAdicionado = true;
|
||
}
|
||
}
|
||
|
||
if (!PontoInicialAdicionado)
|
||
{
|
||
_trajetoriaFixa.Add(PontoInicial);
|
||
}
|
||
|
||
foreach (GPSModel pontoAtual in CorredorAtual)
|
||
{
|
||
int idxPonto = CorredorAtual.IndexOf(pontoAtual);
|
||
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());
|
||
|
||
PontoTrajetoriaModel PontoTrajetoria = new PontoTrajetoriaModel(bordaEntrada ? TipoPontoRua.BordaEntrada : bordaSaida ? TipoPontoRua.BordaSaida : TipoPontoRua.Rua)
|
||
{
|
||
idxCorredor = idx,
|
||
idxPonto = _trajetoriaFixa.Count(),
|
||
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
||
Posicao = pontoAtual,
|
||
Direcao = direcaoAtual,
|
||
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, pontoAtual),
|
||
Visitado = false,
|
||
LarguraCorredor = bordaEntrada || bordaSaida ? larguraCorredorMenor : CorredoresLarguras[idx]
|
||
};
|
||
|
||
_trajetoriaFixa.Add(PontoTrajetoria);
|
||
}
|
||
|
||
double anguloProjetar2 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(CorredorAtual.Count() - 2).First(), CorredorAtual.Last());
|
||
// Realiza um acréscimo no angulo à projetar, permitindo que o equipamento faça uma curva mais aberta entre corredores
|
||
if (!ultimoCorredor && !_FatorLarguraElevado)
|
||
{
|
||
anguloProjetar2 += anguloAcrescentar;
|
||
anguloAcrescentar *= -1;
|
||
}
|
||
double percentualAcrescimoPontoAberturaCurva = 0.8;
|
||
GPSModel UltimoPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.Last(), !ultimoCorredor ? (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva) : (DistanciaProjecaoRua * 2.0), anguloProjetar2);
|
||
|
||
PontoTrajetoriaModel PontoFinal = new PontoTrajetoriaModel(TipoPontoRua.LigacaoSaida)
|
||
{
|
||
idxCorredor = idx,
|
||
idxPonto = _trajetoriaFixa.Count(),
|
||
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
||
Posicao = UltimoPonto,
|
||
Direcao = direcaoAtual,
|
||
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, UltimoPonto),
|
||
Visitado = false,
|
||
LarguraCorredor = ultimoCorredor ? larguraCorredorMenor : (larguraCorredorMenor * 0.8)
|
||
};
|
||
_trajetoriaFixa.Add(PontoFinal);
|
||
|
||
direcaoAtual = direcaoAtual == Enums.DirecaoCarroRua.Ida ? Enums.DirecaoCarroRua.Volta : Enums.DirecaoCarroRua.Ida;
|
||
}
|
||
|
||
_TrajetoriaFixa = new List<PontoTrajetoriaModel>(_trajetoriaFixa);
|
||
|
||
if (_trajetoriaFixa.Count() > 1 && idxEsq == 0 && idxDir == 0)
|
||
{
|
||
(List<GPSModel> ruaEsq, List<GPSModel> 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());
|
||
|
||
(incDir, incEsq) = AtualizaIndiceRuaLateralCorredor(i, idxDir, incDir, incEsq);
|
||
|
||
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,
|
||
FatorLarguraCorredor = CorredoresLarguras[x.Key] / LarguraCorredorPadrao
|
||
};
|
||
|
||
return obj;
|
||
})
|
||
.ToList();
|
||
|
||
_Corredores.ForEach(x => x.Ultimo = x.Idx == _Corredores.Count() - 1);
|
||
|
||
DistanciaTotal = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo).Select(x => x.Posicao).ToList());
|
||
|
||
RetornandoBase = false;
|
||
|
||
AutonomiaCorredor.Iniciar();
|
||
|
||
AtualizarDadosTrajetoria();
|
||
|
||
LoopAtualizaDados();
|
||
|
||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||
}
|
||
|
||
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(GPSModel penultimoPontoRuaAtual, GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
|
||
{
|
||
double anguloCurva = GPSUtils.CalcularOrientacao(penultimoPontoRuaAtual, ultimoPontoRuaAtual);
|
||
double angulo = GPSUtils.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
|
||
|
||
// Calcular a diferença de ângulo corretamente no espaço de 360 graus
|
||
double diferencaAngulo = ((angulo - anguloCurva + 540) % 360) - 180;
|
||
|
||
bool ParaDentro = diferencaAngulo > 0; // Agora o cálculo é confiável
|
||
|
||
double lat = (ultimoPontoRuaAtual.Latitude + primeiroPontoProximaRua.Latitude) / 2;
|
||
double lon = (ultimoPontoRuaAtual.Longitude + primeiroPontoProximaRua.Longitude) / 2;
|
||
GPSModel pontoMedio = new GPSModel()
|
||
{
|
||
Latitude = lat,
|
||
Longitude = lon,
|
||
};
|
||
|
||
// Ajuste o ângulo com base na direção da curva
|
||
double ajusteAngulo = ParaDentro ? -90 : 90;
|
||
|
||
// Projetar um ponto a uma certa distância na direção ajustada
|
||
return GPSUtils.ProjetarPontoDeslocado(pontoMedio, distancia, angulo + ajusteAngulo);
|
||
}
|
||
|
||
public static List<GPSModel> GerarCurvaConexao(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, GPSModel pontoControle, int numeroPontos)
|
||
{
|
||
List<GPSModel> pontosCurva = new List<GPSModel>();
|
||
for (int i = 2; i <= numeroPontos; i++)
|
||
{
|
||
double t = i / (double)numeroPontos;
|
||
pontosCurva.Add(CalcularBezierQuadratica(ultimoPontoRuaAtual, pontoControle, primeiroPontoProximaRua, t));
|
||
}
|
||
return pontosCurva;
|
||
}
|
||
|
||
public static GPSModel CalcularBezierQuadratica(GPSModel p0, GPSModel p1, GPSModel p2, double t)
|
||
{
|
||
double x = Math.Pow(1 - t, 2) * p0.Latitude + 2 * (1 - t) * t * p1.Latitude + Math.Pow(t, 2) * p2.Latitude;
|
||
double y = Math.Pow(1 - t, 2) * p0.Longitude + 2 * (1 - t) * t * p1.Longitude + Math.Pow(t, 2) * p2.Longitude;
|
||
return new GPSModel { Latitude = x, Longitude = y };
|
||
}
|
||
|
||
|
||
public void AjustarTrajetoriaParaDesvio(Obstaculo obstaculo)
|
||
{
|
||
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps;
|
||
|
||
double distanciaObstaculo = ((double)obstaculo.DistanciaMedia_mm / 1000);
|
||
double larguraObstaculo = ((double)obstaculo.Largura_mm / 1000);
|
||
double anguloCaminho = AnguloCaminho;
|
||
double anguloDesvioInicial = anguloCaminho + obstaculo.AnguloParaDesvio;
|
||
double anguloDesvioFinal = anguloCaminho - obstaculo.AnguloParaDesvio;
|
||
|
||
GPSModel ponto1 = GPSUtils.GerarPontoDeslocado(posicaoAtual, anguloDesvioInicial, distanciaObstaculo);
|
||
GPSModel ponto2 = GPSUtils.GerarPontoDeslocado(ponto1, anguloCaminho, larguraObstaculo);
|
||
GPSModel ponto3 = GPSUtils.GerarPontoDeslocado(ponto2, anguloDesvioFinal, distanciaObstaculo);
|
||
List<PontoTrajetoriaModel> lista = new List<PontoTrajetoriaModel>()
|
||
{
|
||
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
||
{
|
||
Posicao = ponto1,
|
||
},
|
||
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
||
{
|
||
Posicao = ponto2,
|
||
},
|
||
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
||
{
|
||
Posicao = ponto3,
|
||
}
|
||
};
|
||
|
||
var idxRemover = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, ponto3, distanciaObstaculo);
|
||
_TrajetoriaDinamica.RemoveRange(1, idxRemover);
|
||
_TrajetoriaDinamica.InsertRange(1, lista);
|
||
}
|
||
|
||
private int EncontrarIndiceFinalDesvio(List<GPSModel> trajetoria, GPSModel ultimoWaypointDesvio, double distanciaSeguranca)
|
||
{
|
||
// Encontrar o índice do waypoint que está imediatamente após o retorno do desvio
|
||
// Este é um método simplificado. A lógica exata pode variar dependendo da precisão necessária.
|
||
double distanciaAnterior = 0;
|
||
for (int i = 0; i < trajetoria.Count; i++)
|
||
{
|
||
double distancia = GPSUtils.DistanciaEntrePontos(trajetoria[i], ultimoWaypointDesvio);
|
||
bool aumentando = distancia > distanciaAnterior;
|
||
distanciaAnterior = distancia;
|
||
if (distancia < distanciaSeguranca)
|
||
{
|
||
if (aumentando)
|
||
{
|
||
return i;
|
||
}
|
||
}
|
||
}
|
||
return 0; // Se não encontrar, assuma o último waypoint
|
||
}
|
||
|
||
private (List<GPSModel> ruaEsquerda, List<GPSModel> ruaDireita) DeterminarRuasLaterais(GPSModel robo, GPSModel proximoPonto, List<GPSModel> rua1, List<GPSModel> rua2)
|
||
{
|
||
// Determinar qual está à esquerda e qual está à direita
|
||
var r1 = new List<GPSModel>(rua1);
|
||
if (ExtremoMaisProximoMapa == 1)
|
||
{
|
||
r1.Reverse();
|
||
}
|
||
r1 = r1.Take(3).ToList();
|
||
bool rua1EstaEsquerda = EstaEsquerda(robo, proximoPonto, Centroide(r1));
|
||
|
||
return rua1EstaEsquerda ? (rua1, rua2) : (rua2, rua1);
|
||
}
|
||
|
||
// Retorna o ponto central da rua (média das coordenadas)
|
||
private GPSModel Centroide(List<GPSModel> rua)
|
||
{
|
||
double latMedia = rua.Average(p => p.Latitude);
|
||
double lonMedia = rua.Average(p => p.Longitude);
|
||
return new GPSModel { Latitude = latMedia, Longitude = lonMedia };
|
||
}
|
||
|
||
// Determina se um ponto está à esquerda ou à direita do movimento do robô
|
||
private bool EstaEsquerda(GPSModel robo, GPSModel proximoPonto, GPSModel ponto)
|
||
{
|
||
double dxRobo = proximoPonto.Longitude - robo.Longitude;
|
||
double dyRobo = proximoPonto.Latitude - robo.Latitude;
|
||
double dxPonto = ponto.Longitude - robo.Longitude;
|
||
double dyPonto = ponto.Latitude - robo.Latitude;
|
||
|
||
double crossProduct = (dxRobo * dyPonto) - (dyRobo * dxPonto);
|
||
return crossProduct > 0; // Se for positivo, está à esquerda; se for negativo, está à direita.
|
||
}
|
||
|
||
|
||
|
||
// Retorno a base automatico
|
||
public void IniciarRetornoBase(GPSModel baseGps, List<GPSModel> waypointsManual = null)
|
||
{
|
||
if (baseGps == null)
|
||
throw new ArgumentNullException(nameof(baseGps));
|
||
|
||
bool valido = false;
|
||
List<GPSModel> pontosRetorno;
|
||
|
||
// =========================
|
||
// MODO MANUAL (operador)
|
||
// =========================
|
||
if (waypointsManual != null && waypointsManual.Count > 0)
|
||
{
|
||
pontosRetorno = new List<GPSModel>();
|
||
pontosRetorno.AddRange(waypointsManual);
|
||
valido = true;
|
||
}
|
||
else
|
||
{
|
||
// =========================
|
||
// MODO AUTOMÁTICO
|
||
// =========================
|
||
(valido, pontosRetorno) = GerarTrajetoriaRetornoAutomatico(baseGps);
|
||
}
|
||
|
||
if (!valido || pontosRetorno == null || pontosRetorno.Count < 1)
|
||
Variaveis.OperacaoEmAndamento.Parametros?.DadosLeitura?.InserirLog(T_Code.Trj, StatusModulo.Falha, 0, "Trajetória de retorno inválida.");
|
||
else
|
||
AplicarTrajetoriaFixaRetorno(pontosRetorno, "RetornoBase");
|
||
}
|
||
|
||
private void AplicarTrajetoriaFixaRetorno(List<GPSModel> pontos, string motivo)
|
||
{
|
||
var traj = new List<PontoTrajetoriaModel>();
|
||
|
||
// Ponto atual do robô (padrão do sistema)
|
||
traj.Add(new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
|
||
{
|
||
idxCorredor = 0,
|
||
idxPonto = 0,
|
||
idxPontoCorredor = 0,
|
||
Posicao = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps,
|
||
Visitado = true,
|
||
LarguraCorredor = 1.0
|
||
});
|
||
|
||
var pontos_corrigidos = RemoverPontosMuitoProximos(pontos, DistanciaEntrePontos, 0, DistanciaEntrePontosCurva);
|
||
|
||
int idx = 1;
|
||
|
||
foreach (var p in pontos_corrigidos)
|
||
{
|
||
traj.Add(new PontoTrajetoriaModel(TipoPontoRua.Rua)
|
||
{
|
||
idxCorredor = 0,
|
||
idxPonto = idx,
|
||
idxPontoCorredor = idx,
|
||
Posicao = p,
|
||
Visitado = false,
|
||
LarguraCorredor = 1.5
|
||
});
|
||
|
||
idx++;
|
||
}
|
||
|
||
//traj[traj.Count - 1].LarguraCorredor = 3.0; // Considerar que chegou na base a 2,5 m
|
||
|
||
DistanciaTotal = GPSUtils.DistanciaDoTrecho(traj.Select(x => x.Posicao).ToList());
|
||
DistanciaPercorrida = 0.0;
|
||
|
||
RetornandoBase = true;
|
||
|
||
_TrajetoriaFixa = traj;
|
||
|
||
Variaveis.OperacaoEmAndamento.Parametros?.DadosLeitura?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, $"[RETORNO] Trajetória aplicada ({motivo}) - Pontos: {traj.Count}");
|
||
|
||
AutonomiaCorredor.Parar();
|
||
|
||
AtualizarDadosTrajetoria();
|
||
|
||
LoopAtualizaDados();
|
||
|
||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||
}
|
||
|
||
private (bool valido, List<GPSModel> pontos) GerarTrajetoriaRetornoAutomatico(GPSModel baseGps)
|
||
{
|
||
var pontos = new List<GPSModel>();
|
||
|
||
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps;
|
||
|
||
// =========================
|
||
// REGRA 0 – Está dentro de corredor?
|
||
// =========================
|
||
if (CorredorAtual != null && CorredorAtual.Dentro && !CorredorAtual.Concluido)
|
||
{
|
||
var pontosRestantesCorredor = CorredorAtual.Pontos.Where(x => x.idxPonto >= ProximoPonto.idxPonto).Select(x => x.Posicao).ToList();
|
||
if (pontosRestantesCorredor.Count > 0)
|
||
{
|
||
pontos.AddRange(pontosRestantesCorredor);
|
||
posicaoAtual = pontos[pontos.Count - 1];
|
||
}
|
||
}
|
||
|
||
int caso = EscolherCasoRetorno(posicaoAtual, baseGps);
|
||
|
||
List<GPSModel> trecho = new List<GPSModel>();
|
||
switch (caso)
|
||
{
|
||
case 1:
|
||
// =========================
|
||
// CASO 1 – Linha reta até a base
|
||
// (V1 simples para validar tudo)
|
||
// =========================
|
||
trecho.AddRange(GerarTrechoReto(posicaoAtual, baseGps, DistanciaEntrePontos));
|
||
break;
|
||
case 2:
|
||
// =========================
|
||
// CASO 2 – Existe mapa entre a base e o rover, e tambem existe corredores com bordas proximos a ambos
|
||
// (V1 simples para validar tudo)
|
||
// =========================
|
||
trecho.AddRange(GerarTrajetoriaViaCorredor(posicaoAtual, baseGps));
|
||
break;
|
||
case 3:
|
||
// =========================
|
||
// 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));
|
||
break;
|
||
}
|
||
pontos.AddRange(trecho);
|
||
|
||
return (trecho.Any(), pontos);
|
||
}
|
||
|
||
private int EscolherCasoRetorno(GPSModel posRobo, GPSModel posBase)
|
||
{
|
||
// Coleta pontos do mapa (use RuasPlantacao, que é o "mapa real")
|
||
var ptsMapa = new List<GPSModel>();
|
||
foreach (var r in RuasPlantacao)
|
||
ptsMapa.AddRange(r);
|
||
|
||
// Se não tem mapa suficiente, não tem envelope pra cruzar => caso 1
|
||
if (ptsMapa.Count < 3)
|
||
return 1;
|
||
|
||
// 1) Dá pra ir reto sem cruzar envelope?
|
||
bool cruzaEnvelope = LinhaCruzaEnvelopeConvexo(posRobo, posBase, ptsMapa);
|
||
|
||
if (!cruzaEnvelope)
|
||
return 1;
|
||
|
||
// 2) Se cruza, tenta usar corredor
|
||
var cand = EncontrarCorredoresProximos(posRobo, posBase, raioRobo: 15.0, raioBase: 15.0);
|
||
if (cand != null && cand.Count > 0)
|
||
return 2;
|
||
|
||
// 3) Se cruza e não tem corredor, vai perimetral
|
||
return 3;
|
||
}
|
||
|
||
private bool LinhaCruzaEnvelopeConvexo(GPSModel a, GPSModel b, List<GPSModel> ptsMapa)
|
||
{
|
||
var refLat = ptsMapa.Average(p => p.Latitude);
|
||
var refLon = ptsMapa.Average(p => p.Longitude);
|
||
|
||
var aXY = LatLonToXY(a, refLat, refLon);
|
||
var bXY = LatLonToXY(b, refLat, refLon);
|
||
|
||
var hull = ConvexHull(ptsMapa.Select(p => LatLonToXY(p, refLat, refLon)).ToList());
|
||
if (hull.Count < 3) return false;
|
||
|
||
// Se A ou B está dentro do envelope, consideramos que "cruza" (está no mapa)
|
||
if (PontoDentroPoligono(aXY, hull) || PontoDentroPoligono(bXY, hull))
|
||
return true;
|
||
|
||
// Interseção do segmento com arestas do hull
|
||
for (int i = 0; i < hull.Count; i++)
|
||
{
|
||
var p1 = hull[i];
|
||
var p2 = hull[(i + 1) % hull.Count];
|
||
|
||
if (SegmentosIntersectam(aXY, bXY, p1, p2))
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private bool PontoDentroPoligono((double x, double y) p, List<(double x, double y)> poly)
|
||
{
|
||
bool inside = false;
|
||
for (int i = 0, j = poly.Count - 1; i < poly.Count; j = i++)
|
||
{
|
||
var pi = poly[i];
|
||
var pj = poly[j];
|
||
|
||
bool intersect = ((pi.y > p.y) != (pj.y > p.y)) &&
|
||
(p.x < (pj.x - pi.x) * (p.y - pi.y) / (pj.y - pi.y + 1e-12) + pi.x);
|
||
|
||
if (intersect) inside = !inside;
|
||
}
|
||
return inside;
|
||
}
|
||
|
||
private bool SegmentosIntersectam((double x, double y) a, (double x, double y) b, (double x, double y) c, (double x, double y) d)
|
||
{
|
||
double o1 = Orient(a, b, c);
|
||
double o2 = Orient(a, b, d);
|
||
double o3 = Orient(c, d, a);
|
||
double o4 = Orient(c, d, b);
|
||
|
||
if (o1 * o2 < 0 && o3 * o4 < 0) return true;
|
||
|
||
if (Math.Abs(o1) < 1e-12 && OnSegment(a, b, c)) return true;
|
||
if (Math.Abs(o2) < 1e-12 && OnSegment(a, b, d)) return true;
|
||
if (Math.Abs(o3) < 1e-12 && OnSegment(c, d, a)) return true;
|
||
if (Math.Abs(o4) < 1e-12 && OnSegment(c, d, b)) return true;
|
||
|
||
return false;
|
||
}
|
||
|
||
private double Orient((double x, double y) a, (double x, double y) b, (double x, double y) c)
|
||
{
|
||
return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
|
||
}
|
||
|
||
private bool OnSegment((double x, double y) a, (double x, double y) b, (double x, double y) p)
|
||
{
|
||
return p.x >= Math.Min(a.x, b.x) - 1e-9 && p.x <= Math.Max(a.x, b.x) + 1e-9 &&
|
||
p.y >= Math.Min(a.y, b.y) - 1e-9 && p.y <= Math.Max(a.y, b.y) + 1e-9;
|
||
}
|
||
|
||
#region CASO 1
|
||
|
||
private List<GPSModel> GerarTrechoReto(GPSModel a, GPSModel b, double espacamento_m)
|
||
{
|
||
var lista = new List<GPSModel>();
|
||
|
||
double dist = GPSUtils.DistanciaEntrePontos(a, b);
|
||
if (dist < espacamento_m)
|
||
{
|
||
lista.Add(b);
|
||
return lista;
|
||
}
|
||
|
||
int passos = (int)Math.Ceiling(dist / espacamento_m);
|
||
|
||
for (int i = 1; i <= passos; i++)
|
||
{
|
||
double t = (double)i / passos;
|
||
lista.Add(GPSUtils.InterpolarPonto(a, b, t));
|
||
}
|
||
|
||
return lista;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region CASO 2
|
||
|
||
private enum TipoCondicaoRoboRua
|
||
{
|
||
Indefinido,
|
||
EntraPeloInicio,
|
||
EntraPeloFim,
|
||
SaiPeloInicio,
|
||
SaiPeloFim
|
||
}
|
||
|
||
private List<(int idxCorredor, TipoCondicaoRoboRua condicaoCarro)> EncontrarCorredoresProximos(GPSModel posRobo, GPSModel posBase, double raioRobo = 15.0, double raioBase = 15.0)
|
||
{
|
||
var candidatos = new List<(int idxCorredor, TipoCondicaoRoboRua condicaoCarro)>();
|
||
|
||
double angRoboBase = GPSUtils.CalcularOrientacao(posRobo, posBase);
|
||
double limiar = 45.0;
|
||
|
||
for (int i = 0; i < Corredores.Count; i++)
|
||
{
|
||
var corredor = Corredores[i];
|
||
if (corredor.Count < 2) continue;
|
||
|
||
double angCorredor = GPSUtils.CalcularOrientacao(corredor[0], corredor[corredor.Count - 1]);
|
||
double angCorredorI = (angCorredor + 180.0) % 360.0;
|
||
|
||
double difAng = GPSUtils.CalcularDiferencaAngulo(angRoboBase, angCorredor);
|
||
double difAngI = GPSUtils.CalcularDiferencaAngulo(angRoboBase, angCorredorI);
|
||
bool angCorreto = Math.Abs(difAng) <= limiar;
|
||
bool angCorretoI = Math.Abs(difAngI) <= limiar;
|
||
if (!angCorreto && !angCorretoI) continue;
|
||
|
||
var pInicio = corredor.First();
|
||
var pFim = corredor.Last();
|
||
|
||
double dBaseInicio = GPSUtils.DistanciaEntrePontos(posBase, pInicio);
|
||
double dBaseFim = GPSUtils.DistanciaEntrePontos(posBase, pFim);
|
||
|
||
double dRoboInicio = GPSUtils.DistanciaEntrePontos(posRobo, pInicio);
|
||
double dRoboFim = GPSUtils.DistanciaEntrePontos(posRobo, pFim);
|
||
|
||
// Caso 1: robô entra pelo início, base sai pelo fim
|
||
if (dRoboInicio <= raioRobo && dBaseFim <= raioBase)
|
||
candidatos.Add((i, TipoCondicaoRoboRua.EntraPeloInicio));
|
||
|
||
// Caso 2: robô entra pelo fim, base sai pelo início
|
||
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));
|
||
}
|
||
|
||
return candidatos;
|
||
}
|
||
|
||
private (int idxCorredor, TipoCondicaoRoboRua condicaoCarro) SelecionarMelhorCorredor(List<(int idxCorredor, TipoCondicaoRoboRua condicaoCarro)> candidatos, GPSModel posRobo)
|
||
{
|
||
double melhorScore = double.MaxValue;
|
||
(int idxCorredor, TipoCondicaoRoboRua condicaoCarro) melhor = (-1, TipoCondicaoRoboRua.Indefinido);
|
||
|
||
foreach (var c in candidatos)
|
||
{
|
||
var corredor = Corredores[c.idxCorredor];
|
||
var ponto =
|
||
c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio ? corredor.First() :
|
||
c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloFim || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloFim ? corredor.Last() :
|
||
corredor.First();
|
||
|
||
double comprimento = 0;
|
||
for (int i = 1; i < corredor.Count; i++)
|
||
comprimento += GPSUtils.DistanciaEntrePontos(corredor[i - 1], corredor[i]);
|
||
|
||
double score = comprimento + GPSUtils.DistanciaEntrePontos(posRobo, ponto);
|
||
|
||
if (score < melhorScore)
|
||
{
|
||
melhorScore = score;
|
||
melhor = c;
|
||
}
|
||
}
|
||
|
||
return melhor;
|
||
}
|
||
|
||
private List<GPSModel> GerarTrajetoriaViaCorredor(GPSModel posRobo, GPSModel posBase)
|
||
{
|
||
var candidatos = EncontrarCorredoresProximos(posRobo, posBase);
|
||
if (candidatos.Count == 0) return new List<GPSModel>();
|
||
|
||
(int idxCorredor, TipoCondicaoRoboRua condicaoCarro) = SelecionarMelhorCorredor(candidatos, posRobo);
|
||
if (idxCorredor < 0) return new List<GPSModel>();
|
||
|
||
var pontos = new List<GPSModel>();
|
||
var corredor = Corredores[idxCorredor];
|
||
|
||
bool sentidoCorreto = condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio;
|
||
GPSModel entrada = sentidoCorreto ? corredor.First() : corredor.Last();
|
||
GPSModel saida = sentidoCorreto ? corredor.Last() : corredor.First();
|
||
|
||
// 1️⃣ robô → entrada do corredor
|
||
pontos.AddRange(GerarTrechoReto(posRobo, entrada, DistanciaEntrePontos));
|
||
|
||
// 2️⃣ polilinha do corredor (no sentido correto)
|
||
if (sentidoCorreto)
|
||
pontos.AddRange(corredor);
|
||
else
|
||
pontos.AddRange(corredor.AsEnumerable().Reverse());
|
||
|
||
// 3️⃣ saída do corredor → base
|
||
pontos.AddRange(GerarTrechoReto(saida, posBase, DistanciaEntrePontos));
|
||
|
||
return pontos;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region CASO 3
|
||
|
||
private List<GPSModel> GerarTrajetoriaPerimetral(GPSModel posRobo, GPSModel posBase, double margem_m = 12.0)
|
||
{
|
||
// 1) Coleta todos os pontos do mapa (corredores)
|
||
var ptsMapa = new List<GPSModel>();
|
||
foreach (var c in RuasPlantacao)
|
||
ptsMapa.AddRange(c);
|
||
|
||
// Se mapa não tiver pontos suficientes, não dá pra perímetro
|
||
if (ptsMapa.Count < 3) return new List<GPSModel>();
|
||
|
||
// 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<GPSModel>();
|
||
|
||
// 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);
|
||
|
||
// 4) Caminho pelo perímetro: escolhe sentido menor
|
||
var arco1 = ExtrairArcoPerimetro(perimetro, idxRobo, idxBase, sentidoHorario: true);
|
||
var arco2 = ExtrairArcoPerimetro(perimetro, idxRobo, idxBase, sentidoHorario: false);
|
||
|
||
double dist1 = DistanciaPolilinha(arco1);
|
||
double dist2 = DistanciaPolilinha(arco2);
|
||
|
||
var arco = dist1 <= dist2 ? arco1 : arco2;
|
||
|
||
// 5) Monta a trajetória final
|
||
var outPts = new List<GPSModel>();
|
||
|
||
// robô -> ponto de entrada no perímetro
|
||
outPts.AddRange(GerarTrechoReto(posRobo, pRoboPerim, DistanciaEntrePontos));
|
||
|
||
// entra no perímetro (garante que comece exatamente do ponto projetado)
|
||
if (outPts.Count == 0 || GPSUtils.DistanciaEntrePontos(outPts[outPts.Count - 1], pRoboPerim) > 0.2)
|
||
outPts.Add(pRoboPerim);
|
||
|
||
// arco do perímetro
|
||
outPts.AddRange(arco);
|
||
|
||
// garante que termina exatamente no ponto projetado da base
|
||
if (outPts.Count == 0 || GPSUtils.DistanciaEntrePontos(outPts[outPts.Count - 1], pBasePerim) > 0.2)
|
||
outPts.Add(pBasePerim);
|
||
|
||
// perímetro -> base
|
||
outPts.AddRange(GerarTrechoReto(pBasePerim, posBase, DistanciaEntrePontos));
|
||
|
||
return outPts;
|
||
}
|
||
|
||
private List<GPSModel> ConstruirPerimetroConvexoComOffset(List<GPSModel> pts, double margem_m)
|
||
{
|
||
// Usa uma projeção local simples (equiretangular) pra converter lat/lon em XY(m)
|
||
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());
|
||
|
||
if (hullXY.Count < 3) return new List<GPSModel>();
|
||
|
||
// 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<GPSModel>();
|
||
foreach (var v in hullXY)
|
||
{
|
||
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));
|
||
}
|
||
|
||
// Fecha o loop (opcional). Eu gosto de deixar sem repetir o primeiro,
|
||
// e tratar a polilinha como circular nos métodos de arco.
|
||
return perimetro;
|
||
}
|
||
|
||
private (double x, double y) LatLonToXY(GPSModel p, double refLat, double refLon)
|
||
{
|
||
const double R = 6378137.0;
|
||
double lat = p.Latitude * Math.PI / 180.0;
|
||
double lon = p.Longitude * Math.PI / 180.0;
|
||
double lat0 = refLat * Math.PI / 180.0;
|
||
double lon0 = refLon * Math.PI / 180.0;
|
||
|
||
double x = (lon - lon0) * Math.Cos(lat0) * R;
|
||
double y = (lat - lat0) * R;
|
||
return (x, y);
|
||
}
|
||
|
||
private GPSModel XYToLatLon(double x, double y, double refLat, double refLon)
|
||
{
|
||
const double R = 6378137.0;
|
||
double lat0 = refLat * Math.PI / 180.0;
|
||
double lon0 = refLon * Math.PI / 180.0;
|
||
|
||
double lat = (y / R) + lat0;
|
||
double lon = (x / (R * Math.Cos(lat0))) + lon0;
|
||
|
||
return new GPSModel
|
||
{
|
||
Latitude = lat * 180.0 / Math.PI,
|
||
Longitude = lon * 180.0 / Math.PI,
|
||
Momento = DateTime.UtcNow
|
||
};
|
||
}
|
||
|
||
private List<(double x, double y)> ConvexHull(List<(double x, double y)> pts)
|
||
{
|
||
// Remove duplicados grosseiros
|
||
pts = pts.Distinct().ToList();
|
||
if (pts.Count < 3) return pts;
|
||
|
||
// pivot: menor y, depois menor x
|
||
var pivot = pts.OrderBy(p => p.y).ThenBy(p => p.x).First();
|
||
|
||
// ordena por ângulo polar com pivot
|
||
var sorted = pts
|
||
.Where(p => p != pivot)
|
||
.OrderBy(p => Math.Atan2(p.y - pivot.y, p.x - pivot.x))
|
||
.ThenBy(p => Dist2(pivot, p))
|
||
.ToList();
|
||
|
||
var stack = new List<(double x, double y)>();
|
||
stack.Add(pivot);
|
||
stack.Add(sorted[0]);
|
||
|
||
for (int i = 1; i < sorted.Count; i++)
|
||
{
|
||
var p = sorted[i];
|
||
while (stack.Count >= 2 && Cross(stack[stack.Count - 2], stack[stack.Count - 1], p) <= 0)
|
||
stack.RemoveAt(stack.Count - 1);
|
||
|
||
stack.Add(p);
|
||
}
|
||
|
||
return stack;
|
||
}
|
||
|
||
private double Dist2((double x, double y) a, (double x, double y) b)
|
||
{
|
||
double dx = a.x - b.x, dy = a.y - b.y;
|
||
return dx * dx + dy * dy;
|
||
}
|
||
|
||
private double Cross((double x, double y) a, (double x, double y) b, (double x, double y) c)
|
||
{
|
||
// (b-a) x (c-a)
|
||
return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
|
||
}
|
||
|
||
private (int idx, GPSModel ponto) PontoMaisProximoNaPolilinha(List<GPSModel> poly, GPSModel alvo)
|
||
{
|
||
int best = 0;
|
||
double bestD = double.MaxValue;
|
||
|
||
for (int i = 0; i < poly.Count; i++)
|
||
{
|
||
double d = GPSUtils.DistanciaEntrePontos(alvo, poly[i]);
|
||
if (d < bestD)
|
||
{
|
||
bestD = d;
|
||
best = i;
|
||
}
|
||
}
|
||
|
||
return (best, poly[best]);
|
||
}
|
||
|
||
private List<GPSModel> ExtrairArcoPerimetro(List<GPSModel> perim, int idxIni, int idxFim, bool sentidoHorario)
|
||
{
|
||
var arco = new List<GPSModel>();
|
||
int n = perim.Count;
|
||
|
||
int i = idxIni;
|
||
while (true)
|
||
{
|
||
arco.Add(perim[i]);
|
||
if (i == idxFim) break;
|
||
|
||
i = sentidoHorario ? (i + 1) % n : (i - 1 + n) % n;
|
||
|
||
// segurança (evita loop infinito em caso de bug)
|
||
if (arco.Count > n + 2) break;
|
||
}
|
||
|
||
return arco;
|
||
}
|
||
|
||
private double DistanciaPolilinha(List<GPSModel> pts)
|
||
{
|
||
double sum = 0;
|
||
for (int i = 1; i < pts.Count; i++)
|
||
sum += GPSUtils.DistanciaEntrePontos(pts[i - 1], pts[i]);
|
||
return sum;
|
||
}
|
||
|
||
#endregion
|
||
|
||
|
||
}
|
||
|
||
public class CorredorTrajetoriaModel
|
||
{
|
||
public int Idx { get; set; }
|
||
public double Largura { get; set; }
|
||
public double DistanciaTotal { get; set; }
|
||
[JsonProperty]
|
||
public double DistanciaPercorridaCorredor { get; private set; } = 0;
|
||
[JsonProperty]
|
||
public double DistanciaPercorridaTotal { get; private set; } = 0;
|
||
public void AtualizarDistancias(double distancia)
|
||
{
|
||
DistanciaTotal += distancia;
|
||
if (Dentro)
|
||
{
|
||
DistanciaPercorridaCorredor += distancia;
|
||
}
|
||
}
|
||
public List<PontoTrajetoriaModel> Pontos { get; set; }
|
||
public int QtdPontos { get; set; }
|
||
[JsonProperty]
|
||
public bool Dentro { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaRestante { get; private set; }
|
||
[JsonProperty]
|
||
public double Progresso { get; private set; }
|
||
[JsonProperty]
|
||
public bool Concluido { get; private set; }
|
||
public bool Ultimo { get; set; }
|
||
public int idxRuaEsquerda { get; set; }
|
||
public int idxRuaDireita { get; set; }
|
||
public double FatorLarguraCorredor { get; set; }
|
||
public DateTime UltimoTempoStatus { get; set; } = DateTime.MinValue;
|
||
public Dictionary<StatusCarroMapa, double> TempoPorStatus { get; set; }
|
||
|
||
public void AtualizarDados()
|
||
{
|
||
Dentro = false;
|
||
// Itera diretamente sem criar uma lista temporária
|
||
foreach (var ponto in Pontos)
|
||
{
|
||
// 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
|
||
if (
|
||
ponto.Visitado &&
|
||
ponto.NaMargem &&
|
||
(ponto.Tipo == TipoPontoRua.Rua ||
|
||
(
|
||
(ponto.Tipo == TipoPontoRua.BordaEntrada && !ponto.Aproximando) ||
|
||
(ponto.Tipo == TipoPontoRua.BordaSaida && ponto.Aproximando)
|
||
)
|
||
)
|
||
)
|
||
{
|
||
Dentro = true;
|
||
break; // Encontramos um ponto válido, podemos sair
|
||
}
|
||
}
|
||
|
||
//DistanciaRestante = DistanciaTotal - DistanciaPercorridaCorredor;
|
||
DistanciaRestante = GPSUtils.DistanciaDoTrecho(Pontos.Where(x => !x.Visitado).Select(x => x.Posicao).ToList());
|
||
Progresso = (DistanciaTotal > 0) ? FuncoesMatematicas.Clamp((DistanciaPercorridaCorredor / DistanciaTotal) * 100.0, 0, 100) : 0;
|
||
Concluido = !Pontos.Any(x => !x.Visitado);
|
||
}
|
||
|
||
public CorredorTrajetoriaModel Clone(bool clonarPontos = true)
|
||
{
|
||
var obj = new CorredorTrajetoriaModel()
|
||
{
|
||
Idx = Idx,
|
||
Largura = Largura,
|
||
DistanciaTotal = DistanciaTotal,
|
||
DistanciaPercorridaCorredor = DistanciaPercorridaCorredor,
|
||
DistanciaPercorridaTotal = DistanciaPercorridaTotal,
|
||
QtdPontos = QtdPontos,
|
||
Dentro = Dentro,
|
||
Concluido = Concluido,
|
||
idxRuaEsquerda = idxRuaEsquerda,
|
||
idxRuaDireita = idxRuaDireita,
|
||
DistanciaRestante = DistanciaRestante,
|
||
Progresso = Progresso,
|
||
Pontos = new List<PontoTrajetoriaModel>(),
|
||
Ultimo = Ultimo,
|
||
FatorLarguraCorredor = FatorLarguraCorredor,
|
||
UltimoTempoStatus = UltimoTempoStatus,
|
||
TempoPorStatus = new Dictionary<StatusCarroMapa, double>(TempoPorStatus ?? new Dictionary<StatusCarroMapa, double>())
|
||
};
|
||
if (clonarPontos)
|
||
{
|
||
obj.Pontos = new List<PontoTrajetoriaModel>(Pontos);
|
||
}
|
||
|
||
return obj;
|
||
}
|
||
}
|
||
|
||
public class PontoTrajetoriaModel
|
||
{
|
||
public int idxCorredor { get; set; }
|
||
public int idxPonto { get; set; }
|
||
public int idxPontoCorredor { get; set; }
|
||
public GPSModel Posicao { get; set; }
|
||
public double Orientacao { get; set; }
|
||
public DirecaoCarroRua Direcao { get; set; }
|
||
public TipoPontoRua Tipo { get; }
|
||
public double LarguraCorredor { get; set; }
|
||
public double DistanciaMargem { get; private set; }
|
||
public bool Visitado { get; set; } = false;
|
||
|
||
// ✅ Agora, propriedades podem ser atualizadas com `set; private`
|
||
[JsonProperty]
|
||
public double OrientacaoAtual { get; private set; }
|
||
[JsonProperty]
|
||
public double OrientacaoAnterior { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaTrajeto { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaAtual { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaAnterior { get; private set; }
|
||
[JsonProperty]
|
||
public bool Aproximando { get; private set; }
|
||
[JsonProperty]
|
||
public bool NaMargem { get; private set; }
|
||
[JsonProperty]
|
||
public bool PontoBorda { get; private set; }
|
||
[JsonProperty]
|
||
public bool PontoLigacao { get; private set; }
|
||
|
||
// ✅ Construtor: Calcula propriedades fixas uma única vez
|
||
public PontoTrajetoriaModel(TipoPontoRua tipo)
|
||
{
|
||
Tipo = tipo;
|
||
|
||
PontoBorda = Tipo == Enums.TipoPontoRua.BordaEntrada || Tipo == Enums.TipoPontoRua.BordaSaida;
|
||
PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida;
|
||
}
|
||
|
||
public void AtualizarPropriedades()
|
||
{
|
||
AtualizarDistanciaAtual();
|
||
AtualizarDistanciaAnterior();
|
||
AtualizarOrientacaoAtual();
|
||
AtualizarOrientacaoAnterior();
|
||
AtualizarNaMargem();
|
||
AtualizarAproximando();
|
||
AtualizarDistanciaTrajeto();
|
||
AtualizarVisitado();
|
||
}
|
||
|
||
private void AtualizarVisitado()
|
||
{
|
||
if (Visitado) return; // Se já foi visitado, não faz nada
|
||
|
||
double distanciaEntrePontos =
|
||
Tipo == TipoPontoRua.CruvaEntreCorredores ? 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.Ceiling(TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras / distanciaEntrePontos) * fatorAjuste * distanciaEntrePontos;
|
||
|
||
// Se o ponto está dentro da margem e dentro do limite de distância, marca como visitado
|
||
if (NaMargem && DistanciaTrajeto <= limiteDistancia)
|
||
{
|
||
Visitado = true;
|
||
}
|
||
}
|
||
|
||
private void AtualizarOrientacaoAtual()
|
||
{
|
||
OrientacaoAtual = GPSUtils.CalcularOrientacao(Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps, Posicao);
|
||
}
|
||
|
||
private void AtualizarOrientacaoAnterior()
|
||
{
|
||
OrientacaoAnterior = GPSUtils.CalcularOrientacao(GPSService.PenultimaLeitura, Posicao);
|
||
}
|
||
|
||
private void AtualizarDistanciaTrajeto()
|
||
{
|
||
if (!Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixaDefinida)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var pontoAtual = Variaveis.OperacaoEmAndamento.Trajetoria.PontoAtual;
|
||
int idxPontoAtual = pontoAtual.idxPonto + 1;
|
||
int qtdPontos = Math.Abs(idxPonto - idxPontoAtual) + 1; // Sempre positivo
|
||
|
||
if (qtdPontos == 1)
|
||
{
|
||
//DistanciaTrajeto = pontoAtual.Aproximando ? pontoAtual.DistanciaAtual : -pontoAtual.DistanciaAtual;
|
||
//DistanciaTrajeto = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaDinamica.First().Posicao, Posicao);
|
||
DistanciaTrajeto = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixa.Where(x => !x.Visitado).First().Posicao, Posicao);
|
||
return;
|
||
}
|
||
|
||
// ✅ Definimos corretamente o intervalo da trajetória
|
||
int idxInicio = Math.Min(idxPontoAtual, idxPonto);
|
||
int idxFim = Math.Max(idxPontoAtual, idxPonto);
|
||
|
||
// ✅ Pega os pontos corretos, independentemente de direção
|
||
List<GPSModel> trecho = Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixa
|
||
.Skip(idxInicio)
|
||
.Take(qtdPontos)
|
||
.ToList()
|
||
.ConvertAll(x => x.Posicao);
|
||
|
||
double distancia = GPSUtils.DistanciaDoTrecho(trecho);
|
||
|
||
// ✅ Agora decidimos se devemos somar ou subtrair a distância do ponto atual
|
||
double distanciaP0 = pontoAtual.Aproximando ? pontoAtual.DistanciaAtual : -pontoAtual.DistanciaAtual;
|
||
|
||
// ✅ Se o ponto está atrás, invertemos a distância final
|
||
if (idxPonto < idxPontoAtual)
|
||
{
|
||
distanciaP0 *= -1;
|
||
distancia *= -1;
|
||
}
|
||
|
||
DistanciaTrajeto = distancia + distanciaP0;
|
||
}
|
||
|
||
private void AtualizarDistanciaAtual()
|
||
{
|
||
DistanciaAtual = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps, Posicao);
|
||
}
|
||
|
||
private void AtualizarDistanciaAnterior()
|
||
{
|
||
DistanciaAnterior = GPSUtils.DistanciaEntrePontos(GPSService.PenultimaLeitura, 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;
|
||
}
|
||
|
||
private void AtualizarNaMargem()
|
||
{
|
||
DistanciaMargem = LarguraCorredor * 0.8;
|
||
NaMargem = DistanciaAtual < DistanciaMargem;
|
||
}
|
||
|
||
|
||
public PontoTrajetoriaModel Clone()
|
||
{
|
||
return new PontoTrajetoriaModel(Tipo)
|
||
{
|
||
Aproximando = Aproximando,
|
||
Direcao = Direcao,
|
||
DistanciaAnterior = DistanciaAnterior,
|
||
DistanciaAtual = DistanciaAtual,
|
||
DistanciaTrajeto = DistanciaTrajeto,
|
||
idxCorredor = idxCorredor,
|
||
idxPonto = idxPonto,
|
||
idxPontoCorredor = idxPontoCorredor,
|
||
LarguraCorredor = LarguraCorredor,
|
||
NaMargem = NaMargem,
|
||
Orientacao = Orientacao,
|
||
OrientacaoAnterior = OrientacaoAnterior,
|
||
OrientacaoAtual = OrientacaoAtual,
|
||
PontoBorda = PontoBorda,
|
||
PontoLigacao = PontoLigacao,
|
||
Posicao = Posicao?.Clone() ?? new GPSModel(),
|
||
Visitado = Visitado
|
||
};
|
||
}
|
||
}
|
||
|
||
|
||
public enum AutonomiaCorredorStatus
|
||
{
|
||
Desconhecido = 0,
|
||
OkCompleto, // Bateria + herbicida suficientes pra entrar e pulverizar
|
||
OkSomenteTransito, // Bateria ok, herbicida insuficiente pra pulverizar tudo
|
||
OkSupervisionado, // Liberado manualmente
|
||
Critico // Nem a bateria garante atravessar com segurança
|
||
}
|
||
|
||
public class AutonomiaCorredorModel
|
||
{
|
||
[JsonProperty]
|
||
public bool Iniciado { get; private set; }
|
||
[JsonProperty]
|
||
public AutonomiaCorredorStatus Status { get; private set; }
|
||
private bool _liberado =>
|
||
Status == AutonomiaCorredorStatus.OkCompleto ||
|
||
Status == AutonomiaCorredorStatus.OkSomenteTransito ||
|
||
Status == AutonomiaCorredorStatus.OkSupervisionado;
|
||
[JsonProperty]
|
||
public bool Liberado { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaCorredor_m { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaSeguraBateria_m { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaSeguraHerbicida_m { get; private set; }
|
||
private Dictionary<int, AutonomiaCorredorModel> CorredoresLiberados { get; set; } = new Dictionary<int, AutonomiaCorredorModel>();
|
||
|
||
// Flags de conveniência
|
||
public bool BateriaSuficiente => DistanciaSeguraBateria_m >= DistanciaCorredor_m;
|
||
public bool HerbicidaSuficiente => DistanciaSeguraHerbicida_m >= DistanciaCorredor_m;
|
||
|
||
// Texto pra UI/log
|
||
[JsonProperty]
|
||
public string Motivo { get; private set; } = "";
|
||
|
||
public void Iniciar()
|
||
{
|
||
Iniciado = true;
|
||
}
|
||
|
||
public void Parar()
|
||
{
|
||
Iniciado = false;
|
||
}
|
||
|
||
public void AtualizarDados(int idxCorredor, bool bateria_liberada, bool reservatorio_liberado)
|
||
{
|
||
if (!Iniciado)
|
||
{
|
||
Liberado = true;
|
||
Motivo = "Não iniciado";
|
||
Status = AutonomiaCorredorStatus.Desconhecido;
|
||
return;
|
||
}
|
||
|
||
if (Variaveis.OperacaoEmAndamento.Trajetoria?._Corredores?.Count <= idxCorredor)
|
||
return;
|
||
|
||
if (CorredoresLiberados.ContainsKey(idxCorredor) && CorredoresLiberados[idxCorredor].Liberado)
|
||
return;
|
||
|
||
double distanciaCorredor = Variaveis.OperacaoEmAndamento.Trajetoria._Corredores[idxCorredor]?.DistanciaTotal ?? 0;
|
||
|
||
var _Bateria = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.ContadorCarga;
|
||
var _Reservatorio = Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.ContadorCarga;
|
||
|
||
DistanciaCorredor_m = distanciaCorredor;
|
||
DistanciaSeguraBateria_m = _Bateria?.DistanciaEstimadaRestante_m ?? 0;
|
||
DistanciaSeguraHerbicida_m = _Reservatorio?.DistanciaEstimadaRestantePulverizando_m ?? 0;
|
||
|
||
bool bateriaOk = BateriaSuficiente && (_Bateria?.Iniciado ?? false);
|
||
bool herbOk = HerbicidaSuficiente && (_Reservatorio?.Iniciado ?? false);
|
||
bool exigirPulverizacaoTotal = Variaveis.OperacaoEmAndamento.Parametros?.ModulosMandatorios?.Any(x => x.Dispositivo == T_Code.Atu && x.Mandatorio && x.Utilizar) ?? false;
|
||
|
||
List<AutonomiaCorredorStatus> status_mods = new List<AutonomiaCorredorStatus>();
|
||
string _motivo = "";
|
||
string _motivo_bat = "Bateria " + (bateriaOk ? "suficiente" : "insuficiente") + $". Distância segura: {DistanciaSeguraBateria_m:F1} m, corredor: {distanciaCorredor:F1} m";
|
||
string _motivo_herb = "Herbicida " + (herbOk ? "suficiente" : "insuficiente") + $" para pulverizar o corredor. Distância segura pulverizando: {DistanciaSeguraHerbicida_m:F1} m, corredor: {distanciaCorredor:F1} m";
|
||
|
||
if (!bateriaOk)
|
||
{
|
||
status_mods.Add(!bateria_liberada ? AutonomiaCorredorStatus.Critico : AutonomiaCorredorStatus.OkSupervisionado);
|
||
if (bateria_liberada) _motivo_bat = $"Liberação humana: {_motivo_bat}";
|
||
_motivo += _motivo_bat;
|
||
}
|
||
|
||
if (!herbOk)
|
||
{
|
||
if (exigirPulverizacaoTotal)
|
||
status_mods.Add(!reservatorio_liberado ? AutonomiaCorredorStatus.Critico : AutonomiaCorredorStatus.OkSupervisionado);
|
||
else
|
||
status_mods.Add(AutonomiaCorredorStatus.OkSomenteTransito);
|
||
if (reservatorio_liberado) _motivo_herb = $"Liberação humana: {_motivo_herb}";
|
||
_motivo += (string.IsNullOrEmpty(_motivo) ? "" : "; ") + _motivo_herb;
|
||
}
|
||
|
||
Status =
|
||
status_mods.Contains(AutonomiaCorredorStatus.Critico) ? AutonomiaCorredorStatus.Critico :
|
||
status_mods.Contains(AutonomiaCorredorStatus.OkSupervisionado) ? AutonomiaCorredorStatus.OkSupervisionado :
|
||
status_mods.Contains(AutonomiaCorredorStatus.OkSomenteTransito) ? AutonomiaCorredorStatus.OkSomenteTransito :
|
||
AutonomiaCorredorStatus.OkCompleto;
|
||
|
||
var res = new AutonomiaCorredorModel()
|
||
{
|
||
Liberado = _liberado,
|
||
Motivo = $"Corredor {idxCorredor + 1}: " + (string.IsNullOrEmpty(_motivo) ? "Liberado" : $"{_motivo}"),
|
||
Status = Status,
|
||
DistanciaCorredor_m = DistanciaCorredor_m,
|
||
DistanciaSeguraBateria_m = DistanciaSeguraBateria_m,
|
||
DistanciaSeguraHerbicida_m = DistanciaSeguraHerbicida_m,
|
||
};
|
||
|
||
bool addLog = false;
|
||
|
||
if (CorredoresLiberados.ContainsKey(idxCorredor))
|
||
{
|
||
if (CorredoresLiberados[idxCorredor].Liberado == false && _liberado)
|
||
{
|
||
CorredoresLiberados[idxCorredor] = res;
|
||
addLog = true;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
CorredoresLiberados.Add(idxCorredor, res);
|
||
addLog = true;
|
||
}
|
||
|
||
if (addLog)
|
||
{
|
||
Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.InserirLog(T_Code.Trj, _liberado ? StatusModulo.Operante : StatusModulo.Alerta, _liberado ? 100 : 0, res.Motivo);
|
||
}
|
||
|
||
Liberado = CorredoresLiberados[idxCorredor].Liberado;
|
||
Motivo = CorredoresLiberados[idxCorredor].Motivo;
|
||
}
|
||
|
||
|
||
public AutonomiaCorredorModel Clone()
|
||
{
|
||
return new AutonomiaCorredorModel()
|
||
{
|
||
Liberado = Liberado,
|
||
Motivo = Motivo,
|
||
Status = Status,
|
||
DistanciaCorredor_m = DistanciaCorredor_m,
|
||
DistanciaSeguraBateria_m = DistanciaSeguraBateria_m,
|
||
DistanciaSeguraHerbicida_m = DistanciaSeguraHerbicida_m,
|
||
};
|
||
}
|
||
}
|
||
|
||
}
|