997 lines
42 KiB
C#
997 lines
42 KiB
C#
using AgroBase.Properties;
|
|
using AgroBase.Services;
|
|
using CefSharp.DevTools.Page;
|
|
using CefSharp.WinForms;
|
|
using Emgu.CV.Features2D;
|
|
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Security.Cryptography;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models
|
|
{
|
|
public class MapasModel
|
|
{
|
|
public ChromiumWebBrowser chromiumWebBrowser { get; set; }
|
|
public Panel pnlMapa { get; set; }
|
|
private PictureBox picLoading { get; set; }
|
|
public Label lblMapa { get; set; }
|
|
public MapasService mapaService { get; set; } = new MapasService();
|
|
public List<List<GPSModel>> Trajetoria { get; set; } = new List<List<GPSModel>>();
|
|
public List<GPSModel> TrajetoriaDinamica { get; set; } = new List<GPSModel>();
|
|
public List<string> RuasPercorrer { get; set; } = new List<string>();
|
|
public int IdxRuaAtual { get; set; } = 0;
|
|
public int IdxUltimoPontoTrajetoria { get; set;} = 0;
|
|
public int IdxUltimoPontoTrajetoriaDinamica { get; set;} = 0;
|
|
public double DistanciaTotal
|
|
{
|
|
get
|
|
{
|
|
double distancia = 0.0;
|
|
|
|
foreach (var trecho in Trajetoria)
|
|
{
|
|
distancia += GPSService.DistanciaDoTrecho(trecho);
|
|
}
|
|
|
|
return distancia;
|
|
}
|
|
}
|
|
public DirecaoCarroRua DirecaoCaminho
|
|
{
|
|
get
|
|
{
|
|
DirecaoCarroRua direcaoCaminho = DirecaoCarroRua.Parado;
|
|
if (TrajetoriaDinamica.Any())
|
|
{
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(TrajetoriaDinamica, TrajetoriaTipo.Dinamica);
|
|
int idxPontoMaisProximo = TrajetoriaDinamica.IndexOf(PontoMaisProximo);
|
|
direcaoCaminho = TrajetoriaDinamica[idxPontoMaisProximo].DirecaoTrecho;
|
|
}
|
|
else
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ExtremoMaisProximo == 0)
|
|
{
|
|
direcaoCaminho = DirecaoCarroRua.Ida;
|
|
}
|
|
else
|
|
{
|
|
direcaoCaminho = DirecaoCarroRua.Volta;
|
|
}
|
|
}
|
|
return direcaoCaminho;
|
|
}
|
|
}
|
|
public bool DentroDaRua
|
|
{
|
|
get
|
|
{
|
|
if (mapaService.DadosMapa != null && RuasPercorrer.Count() > IdxRuaAtual)
|
|
{
|
|
try
|
|
{
|
|
string IDruaAtual = RuasPercorrer[IdxRuaAtual];
|
|
var Rua = mapaService.DadosMapa.features.Where(x => x.geometry.id == IDruaAtual).FirstOrDefault();
|
|
if (Rua != null)
|
|
{
|
|
bool dentro = false;
|
|
List<GPSModel> Trecho = new List<GPSModel>();
|
|
List<GPSModel> PontosRua = new List<GPSModel>();
|
|
Trajetoria[IdxRuaAtual].ToList().ForEach(x => PontosRua.Add(x));
|
|
if (DirecaoCaminho == DirecaoCarroRua.Volta)
|
|
{
|
|
PontosRua.Reverse();
|
|
}
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(PontosRua, TrajetoriaTipo.Rua);
|
|
int idx = PontosRua.IndexOf(PontoMaisProximo);
|
|
foreach (var Ponto in PontosRua)
|
|
{
|
|
// Adiciona os pontos que o robô já passou por eles no trecho, e quando chegar no ponto mais próximo, substitui ele pela posição atual
|
|
int idxPonto = PontosRua.IndexOf(Ponto);
|
|
if (idxPonto < idx)
|
|
{
|
|
Trecho.Add(Ponto);
|
|
}
|
|
else
|
|
{
|
|
if (idx > 0)
|
|
{
|
|
Trecho.Add(GPSService.UltimaLeitura);
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
double distTrecho = GPSService.DistanciaDoTrecho(Trecho);
|
|
dentro = (distTrecho > 0 && distTrecho < Rua.properties.Length) || (idx == 0 && !aproximando);
|
|
|
|
return dentro;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
public GPSModel PrimeiroPontoRua
|
|
{
|
|
get
|
|
{
|
|
if (Trajetoria.Any(x => x.Any()) && Trajetoria.Count() > IdxRuaAtual)
|
|
{
|
|
var primeiroPontoRua = Trajetoria.Skip(IdxRuaAtual).First().First();
|
|
return primeiroPontoRua;
|
|
}
|
|
else
|
|
{
|
|
return new GPSModel();
|
|
}
|
|
|
|
}
|
|
}
|
|
public GPSModel UltimoPontoRua
|
|
{
|
|
get
|
|
{
|
|
if (Trajetoria.Any(x => x.Any()) && Trajetoria.Count() > IdxRuaAtual)
|
|
{
|
|
var ultimoPontoRua = Trajetoria.Skip(IdxRuaAtual).First().Last();
|
|
return ultimoPontoRua;
|
|
}
|
|
else
|
|
{
|
|
return new GPSModel();
|
|
}
|
|
}
|
|
}
|
|
public int ExtremoMaisProximo
|
|
{
|
|
get
|
|
{
|
|
double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRua);
|
|
double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRua);
|
|
|
|
// 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 double AnguloCaminho
|
|
{
|
|
get
|
|
{
|
|
try
|
|
{
|
|
if (TrajetoriaDinamica.Any())
|
|
{
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(TrajetoriaDinamica, TrajetoriaTipo.Dinamica);
|
|
int idx = TrajetoriaDinamica.IndexOf(PontoMaisProximo);
|
|
|
|
if ((idx + 1) >= TrajetoriaDinamica.Count())
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
double angulo = GPSService.CalcularOrientacao(TrajetoriaDinamica[idx], TrajetoriaDinamica[idx + 1]);
|
|
if (angulo == 0)
|
|
{
|
|
angulo = GPSService.CalcularOrientacao(TrajetoriaDinamica[idx + 1], TrajetoriaDinamica[idx + 2]);
|
|
}
|
|
|
|
return (!(angulo >= 0 || angulo <= 0)) ? 0 : angulo;
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
public double DistanciaEsquerda
|
|
{
|
|
get
|
|
{
|
|
if (IdxRuaAtual >= Trajetoria.Count())
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
double distancia = 0;
|
|
|
|
List<GPSModel> RuaEsquerda = new List<GPSModel>();
|
|
if (Trajetoria.Any())
|
|
{
|
|
if (DirecaoCaminho == DirecaoCarroRua.Ida)
|
|
{
|
|
RuaEsquerda = Trajetoria[IdxRuaAtual];
|
|
}
|
|
else if (DirecaoCaminho == DirecaoCarroRua.Volta)
|
|
{
|
|
if (IdxRuaAtual > 0)
|
|
{
|
|
(GPSModel pontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual - 1], TrajetoriaTipo.Geral);
|
|
double distanciaP0 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoMaisProximo);
|
|
if (distanciaP0 < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua)
|
|
{
|
|
Trajetoria[IdxRuaAtual - 1].ForEach(x => RuaEsquerda.Add(x));
|
|
RuaEsquerda.Reverse();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (RuaEsquerda.Any())
|
|
{
|
|
distancia = DistanciaAteRua(RuaEsquerda, VariaveisEquipamento.LarguraEsquerda);
|
|
}
|
|
else
|
|
{
|
|
distancia = DistanciaEntreRuas(Trajetoria) / 2;
|
|
}
|
|
|
|
return distancia;
|
|
}
|
|
}
|
|
public double DistanciaDireita
|
|
{
|
|
get
|
|
{
|
|
if (IdxRuaAtual >= Trajetoria.Count())
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
double distancia = 0;
|
|
|
|
List<GPSModel> RuaDireita = new List<GPSModel>();
|
|
if (Trajetoria.Any())
|
|
{
|
|
if (DirecaoCaminho == DirecaoCarroRua.Volta)
|
|
{
|
|
Trajetoria[IdxRuaAtual].ForEach(x => RuaDireita.Add(x));
|
|
RuaDireita.Reverse();
|
|
}
|
|
else if (DirecaoCaminho == DirecaoCarroRua.Ida)
|
|
{
|
|
if (IdxRuaAtual > 0)
|
|
{
|
|
(GPSModel pontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual - 1], TrajetoriaTipo.Geral);
|
|
double distanciaP0 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoMaisProximo);
|
|
if (distanciaP0 < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua)
|
|
{
|
|
RuaDireita = Trajetoria[IdxRuaAtual - 1];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (RuaDireita.Any())
|
|
{
|
|
distancia = DistanciaAteRua(RuaDireita, VariaveisEquipamento.LarguraDireita);
|
|
}
|
|
else
|
|
{
|
|
distancia = DistanciaEntreRuas(Trajetoria) / 2;
|
|
}
|
|
|
|
return distancia;
|
|
}
|
|
}
|
|
|
|
public void btnCarregar_Click(object sender, EventArgs e, string caminhoArquivo = "", bool monitoramento = false)
|
|
{
|
|
if (monitoramento)
|
|
{
|
|
mapaService.CarregarMapaMonitoramento(MapasVariaveisModel.NomeArquivoMapaInterativo, MapasVariaveisModel.NomeArquivoMapaInterativoSaida, caminhoArquivo);
|
|
}
|
|
else
|
|
{
|
|
mapaService.CarregarMapa(MapasVariaveisModel.NomeArquivoMapaInterativo, MapasVariaveisModel.NomeArquivoMapaInterativoSaida, caminhoArquivo);
|
|
}
|
|
if (mapaService.MapaUrl != "")
|
|
{
|
|
if (picLoading == null)
|
|
{
|
|
picLoading = new PictureBox();
|
|
}
|
|
picLoading.Dock = DockStyle.Fill;
|
|
picLoading.BackColor = System.Drawing.Color.Transparent;
|
|
picLoading.Image = Resources.loading;
|
|
picLoading.Name = "picLoading";
|
|
picLoading.SizeMode = PictureBoxSizeMode.Zoom;
|
|
picLoading.Visible = true;
|
|
pnlMapa.Controls.Add(picLoading);
|
|
|
|
Timer tmr = new Timer() { Interval = 500 };
|
|
tmr.Tick += Tmr_Tick;
|
|
tmr.Start();
|
|
}
|
|
}
|
|
|
|
public void CarregarMapaGPS()
|
|
{
|
|
if (picLoading == null)
|
|
{
|
|
picLoading = new PictureBox()
|
|
{
|
|
SizeMode = PictureBoxSizeMode.Zoom,
|
|
Location = new System.Drawing.Point(0, 0),
|
|
Size = new System.Drawing.Size(pnlMapa.Width, pnlMapa.Height)
|
|
};
|
|
pnlMapa.Controls.Add(picLoading);
|
|
}
|
|
mapaService.CarregarMapaGPS(MapasVariaveisModel.NomeArquivoMapaGPS);
|
|
Timer tmr = new Timer() { Interval = 500 };
|
|
tmr.Tick += Tmr_Tick;
|
|
tmr.Start();
|
|
}
|
|
|
|
private void Tmr_Tick(object sender, EventArgs e)
|
|
{
|
|
if (mapaService.pythonProcess.HasExited)
|
|
{
|
|
((Timer)sender).Stop();
|
|
picLoading.Visible = false;
|
|
mapaService.Carregado = true;
|
|
|
|
if (mapaService.NomeArquivoMapa == MapasVariaveisModel.NomeArquivoMapaInterativo)
|
|
{
|
|
mapaService.CarregarDadosMapa();
|
|
if (lblMapa != null)
|
|
{
|
|
lblMapa.Text = "Mapa carregado: " + mapaService.NomeArquivos;
|
|
}
|
|
|
|
PopularTrajetoria();
|
|
}
|
|
|
|
IniciarProcessamento();
|
|
}
|
|
}
|
|
|
|
public void PopularTrajetoria()
|
|
{
|
|
if (mapaService.DadosMapa != null)
|
|
{
|
|
double OrientacaoRuas = -999;
|
|
//DistanciaTotal = 0;
|
|
/*if (!RuasPercorrer.Any())
|
|
{
|
|
RuasPercorrer = mapaService.DadosMapa.features.Select(x => x.geometry.id).ToList();
|
|
}*/
|
|
Trajetoria = new List<List<GPSModel>>();
|
|
|
|
foreach (var ID in RuasPercorrer)
|
|
{
|
|
var rua = mapaService.DadosMapa.features.FirstOrDefault(x => ID == x.geometry.id);
|
|
if (rua != null)
|
|
{
|
|
Trajetoria.Add(new List<GPSModel>());
|
|
foreach (var ponto in rua.geometry.coordinates)
|
|
{
|
|
GPSModel item = new GPSModel()
|
|
{
|
|
Longitude = ponto[0],
|
|
Latitude = ponto[1],
|
|
};
|
|
Trajetoria[Trajetoria.Count - 1].Add(item);
|
|
/*if (Trajetoria[Trajetoria.Count - 1].Count > 1)
|
|
{
|
|
DistanciaTotal += GPSService.DistanciaEntrePontos(Trajetoria[Trajetoria.Count - 1][Trajetoria[Trajetoria.Count - 1].Count - 2], item);
|
|
}*/
|
|
}
|
|
|
|
var Rua = Trajetoria[Trajetoria.Count - 1];
|
|
if (OrientacaoRuas == -999)
|
|
{
|
|
OrientacaoRuas = GPSService.CalcularOrientacao(Rua[Rua.Count - 1], Rua[Rua.Count - 2]);
|
|
}
|
|
else
|
|
{
|
|
double MargemErroAngulo = 45;
|
|
double orientacaoRua = GPSService.CalcularOrientacao(Rua[Rua.Count - 1], Rua[Rua.Count - 2]);
|
|
double anguloOposto = (orientacaoRua + 180) % 360;
|
|
if ((anguloOposto + MargemErroAngulo) >= OrientacaoRuas && (anguloOposto - MargemErroAngulo) <= OrientacaoRuas)
|
|
{
|
|
Rua.Reverse();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void IniciarProcessamento()
|
|
{
|
|
//APIService.Inicializar();
|
|
|
|
chromiumWebBrowser = new ChromiumWebBrowser(mapaService.MapaUrl);
|
|
pnlMapa.Controls.Add(chromiumWebBrowser);
|
|
chromiumWebBrowser.Dock = DockStyle.Fill;
|
|
}
|
|
|
|
public void PararProcessamento()
|
|
{
|
|
//APIService.Parar();
|
|
//GPSService.PararRecepcaoDados();
|
|
}
|
|
|
|
|
|
public double DistanciaEntreRuas(List<List<GPSModel>> ruas)
|
|
{
|
|
double distanciaTotal = 0;
|
|
|
|
if (ruas.Count < 2)
|
|
{
|
|
// Se houver menos de duas ruas, não há distância a calcular
|
|
return distanciaTotal;
|
|
}
|
|
|
|
double menorDistancia = double.MaxValue;
|
|
|
|
for (int i = 0; i < ruas.Count - 1; i++)
|
|
{
|
|
List<GPSModel> ruaAtual = ruas[i];
|
|
List<GPSModel> proximaRua = ruas[i + 1];
|
|
|
|
// Calcule a distância entre as duas ruas (rua atual e próxima rua)
|
|
// Aqui você pode usar o método GPSService.CalcularDistanciaPontos
|
|
// ou uma função personalizada para calcular a distância entre as ruas
|
|
|
|
|
|
foreach (GPSModel pontoAtual in ruaAtual)
|
|
{
|
|
foreach (GPSModel pontoProxima in proximaRua)
|
|
{
|
|
// Calcular a distância entre pontoAtual e pontoProxima
|
|
double distancia = GPSService.DistanciaEntrePontos(pontoAtual, pontoProxima);
|
|
|
|
// Verifique se esta é a menor distância
|
|
if (distancia < menorDistancia)
|
|
{
|
|
menorDistancia = distancia;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Adiciona a menor distância encontrada à distância total
|
|
distanciaTotal += menorDistancia;
|
|
}
|
|
|
|
double distanciaMedia = distanciaTotal / ruas.Count();
|
|
|
|
return menorDistancia;
|
|
}
|
|
|
|
public double DistanciaAteRua(List<GPSModel> Rua, double larguraRobo)
|
|
{
|
|
GPSModel pontoAtual = GPSService.UltimaLeitura;
|
|
|
|
(GPSModel P0, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Rua, TrajetoriaTipo.Geral);
|
|
int idxP0 = Rua.IndexOf(P0);
|
|
bool PontoProximo = idxP0 < Rua.Count() - 1;
|
|
bool PontoAnterior = idxP0 > 0;
|
|
GPSModel P1 = PontoProximo ? Rua[idxP0 + 1] : PontoAnterior ? Rua[idxP0 - 1] : pontoAtual;
|
|
|
|
double a = GPSService.DistanciaEntrePontos(pontoAtual, P0);
|
|
double b = GPSService.DistanciaEntrePontos(pontoAtual, P1);
|
|
double c = GPSService.DistanciaEntrePontos(P0, P1);
|
|
|
|
double distancia = FuncoesMatematicas.CalcularAlturaTriangulo(a, b, c);
|
|
|
|
distancia -= (larguraRobo / 100.0);
|
|
|
|
return distancia;
|
|
}
|
|
|
|
|
|
public void GerarTrajetoriaDinamica()
|
|
{
|
|
double distanciaEntreRuas = DistanciaEntreRuas(Trajetoria) / 2;
|
|
List<GPSModel> Caminho = new List<GPSModel>();
|
|
|
|
DirecaoCarroRua direcaoRua = TrajetoriaDinamica.Any() ? TrajetoriaDinamica[0].DirecaoTrecho : ExtremoMaisProximo == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
|
|
|
for (int i = IdxRuaAtual; i < Trajetoria.Count(); i++)
|
|
{
|
|
List<GPSModel> RuaAtual = Trajetoria.Skip(i).First();
|
|
|
|
double anguloProjetar = GPSService.CalcularOrientacao(RuaAtual.First(), RuaAtual.Skip(1).First()) + 90;
|
|
|
|
List<GPSModel> RuaProjetada = ProjetarRua(RuaAtual, distanciaEntreRuas, anguloProjetar);
|
|
|
|
if (Caminho.Any())
|
|
{
|
|
double dist0 = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], RuaProjetada[0]);
|
|
double dist1 = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], RuaProjetada[RuaProjetada.Count() - 1]);
|
|
direcaoRua = dist0 < dist1 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
|
}
|
|
|
|
RuaProjetada.ForEach(x =>
|
|
{
|
|
x.idxRua = i;
|
|
x.DirecaoTrecho = direcaoRua;
|
|
});
|
|
if (direcaoRua == DirecaoCarroRua.Volta)
|
|
{
|
|
RuaProjetada.Reverse();
|
|
}
|
|
double anguloProjetar1 = GPSService.CalcularOrientacao(RuaProjetada.Skip(1).First(), RuaProjetada.First());
|
|
GPSModel PrimeiroPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.First(), 3, anguloProjetar1);
|
|
double anguloProjetar2 = GPSService.CalcularOrientacao(RuaProjetada.Skip(RuaProjetada.Count() - 2).First(), RuaProjetada.Last());
|
|
GPSModel UltimoPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.Last(), 3, anguloProjetar2);
|
|
|
|
if (!Caminho.Any())
|
|
{
|
|
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(RuaProjetada, TrajetoriaTipo.Rua);
|
|
int idxPontoMaisProximo = RuaProjetada.IndexOf(PontoMaisProximo);
|
|
|
|
GPSModel inicio = GPSService.UltimaLeitura;
|
|
|
|
List<GPSModel> _caminho = new List<GPSModel>();
|
|
if (idxPontoMaisProximo == 0 && aproximando)
|
|
{
|
|
//_caminho.Add(PrimeiroPonto);
|
|
List<GPSModel> _pontos = GPSService.InterpolarPontos(inicio, PrimeiroPonto, 1.0);
|
|
if (_pontos.Count() == 1 || GPSService.CompararDirecaoTrajeto(_pontos, RuaProjetada))
|
|
{
|
|
_caminho.AddRange(_pontos);
|
|
}
|
|
}
|
|
|
|
//bool dentroRua = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.DentroDaRua;
|
|
bool dentroRua = Variaveis.OperacaoEmAndamento.Mapa.DentroDaRua;
|
|
|
|
// Se estiver manobrando e a manobra ainda não tiver sido concluída, OU, o ponto mais próximo é o ultimo ponto da rua, e já esta fora da rua
|
|
if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ManobraConcluida || ((idxPontoMaisProximo == (RuaProjetada.Count() - 1)) && !dentroRua))
|
|
{
|
|
_caminho.Add(inicio);
|
|
}
|
|
else
|
|
{
|
|
_caminho.AddRange(RuaProjetada.Skip(idxPontoMaisProximo));
|
|
_caminho.Add(UltimoPonto);
|
|
}
|
|
_caminho.ForEach(x => x.DirecaoTrecho = direcaoRua);
|
|
Caminho.AddRange(_caminho);
|
|
|
|
//direcaoRua = direcaoRua == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
|
|
}
|
|
else
|
|
{
|
|
List<GPSModel> _caminho = new List<GPSModel>();
|
|
// 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 = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], PrimeiroPonto);
|
|
bool distanciaMinima = distancia < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua * 2;
|
|
|
|
// Verifica se o angulo formado entre a rua anterior e a rua atual é grande o bastante para gerar a curva de manobra
|
|
double anguloEntreRuas = GPSService.CalcularOrientacao(Caminho.Last(), PrimeiroPonto);
|
|
double anguloFinalRuaAnterior = GPSService.CalcularOrientacao(Caminho.Count() > 1 ? Caminho[Caminho.Count() - 2] : Caminho.First(), Caminho.Last());
|
|
double difAngulo = anguloFinalRuaAnterior - anguloEntreRuas;
|
|
bool anguloMinimo = difAngulo >= -45 && difAngulo <= 45;
|
|
|
|
if (distanciaMinima && !anguloMinimo)
|
|
{
|
|
GPSModel P0 = CalcularPontoControle(Caminho.Last(), PrimeiroPonto, distanciaEntreRuas * 2);
|
|
List<GPSModel> CurvaConexao = GerarCurvaConexao(Caminho.Last(), PrimeiroPonto, P0, 6);
|
|
|
|
_caminho.AddRange(CurvaConexao);
|
|
_caminho.ForEach(x => x.DirecaoTrecho = DirecaoCarroRua.Manobra);
|
|
}
|
|
else
|
|
{
|
|
_caminho.Add(PrimeiroPonto);
|
|
}
|
|
//_caminho.AddRange(GPSService.InterpolarPontos(inicio, PrimeiroPonto, 1.0));
|
|
_caminho.AddRange(RuaProjetada);
|
|
_caminho.Add(UltimoPonto);
|
|
_caminho.ForEach(x => x.DirecaoTrecho = x.DirecaoTrecho != DirecaoCarroRua.Manobra ? direcaoRua : x.DirecaoTrecho);
|
|
Caminho.AddRange(_caminho);
|
|
|
|
//direcaoRua = direcaoRua == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
|
|
}
|
|
}
|
|
|
|
|
|
// Limpa a trajetória dinâmica existente e adiciona os novos pontos
|
|
TrajetoriaDinamica.Clear();
|
|
Caminho.ForEach(x =>
|
|
{
|
|
// Impede inclusão de pontos duplicados
|
|
if (!TrajetoriaDinamica.Contains(x))
|
|
{
|
|
TrajetoriaDinamica.Add(x);
|
|
}
|
|
});
|
|
Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoriaDinamica = 0;
|
|
|
|
if (Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.DesvioNecessario)
|
|
{
|
|
AjustarTrajetoriaParaDesvio(Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.ObstaculoCritico);
|
|
}
|
|
|
|
GPSService.AtualizarTrajetoriaDinamica();
|
|
}
|
|
|
|
public List<GPSModel> ProjetarRua(List<GPSModel> ruaOriginal, double offset, double anguloOffset)
|
|
{
|
|
// Certifique-se de que a lista da rua original não está vazia
|
|
if (ruaOriginal == null || ruaOriginal.Count == 0)
|
|
{
|
|
return new List<GPSModel>();
|
|
}
|
|
|
|
List<GPSModel> RuaProjetada = new List<GPSModel>();
|
|
|
|
foreach (var Ponto in ruaOriginal)
|
|
{
|
|
int idx = ruaOriginal.IndexOf(Ponto);
|
|
GPSModel Ponto2 = idx + 1 < ruaOriginal.Count() ? ruaOriginal[idx + 1] : ruaOriginal[idx - 1];
|
|
double angulo = 0;
|
|
if (idx + 1 < ruaOriginal.Count())
|
|
{
|
|
angulo = GPSService.CalcularOrientacao(Ponto, Ponto2);
|
|
}
|
|
else
|
|
{
|
|
angulo = GPSService.CalcularOrientacao(Ponto2, Ponto);
|
|
}
|
|
RuaProjetada.Add(GPSService.ProjetarPontoDeslocado(Ponto, offset, angulo + 90));
|
|
}
|
|
|
|
return RuaProjetada;
|
|
}
|
|
|
|
|
|
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 static GPSModel CalcularPontoControle(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
|
|
{
|
|
double angulo = GPSService.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
|
|
double lat = (ultimoPontoRuaAtual.Latitude + primeiroPontoProximaRua.Latitude) / 2;
|
|
double lon = (ultimoPontoRuaAtual.Longitude + primeiroPontoProximaRua.Longitude) / 2;
|
|
GPSModel pontoMedio = new GPSModel()
|
|
{
|
|
Latitude = lat,
|
|
Longitude = lon,
|
|
};
|
|
// Projetar um ponto a uma certa distância na direção calculada
|
|
return GPSService.ProjetarPontoDeslocado(pontoMedio, distancia, angulo + 270);
|
|
}
|
|
|
|
public static List<GPSModel> GerarCurvaConexao(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, GPSModel pontoControle, int numeroPontos)
|
|
{
|
|
List<GPSModel> pontosCurva = new List<GPSModel>();
|
|
for (int i = 0; i <= numeroPontos; i++)
|
|
{
|
|
double t = i / (double)numeroPontos;
|
|
pontosCurva.Add(CalcularBezierQuadratica(ultimoPontoRuaAtual, pontoControle, primeiroPontoProximaRua, t));
|
|
}
|
|
return pontosCurva;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// Método para gerar o caminho entre dois pontos
|
|
public void GerarCaminhoEntreDoisPontos(GPSModel destino)
|
|
{
|
|
//PopularTrajetoriaComOffset(destino);
|
|
//return;
|
|
// Inicialização do caminho com o ponto de início
|
|
GPSModel inicio = GPSService.UltimaLeitura;
|
|
|
|
// Gera waypoints intermediários
|
|
var caminho = GPSService.InterpolarPontos(inicio, destino, 1.0); // 1 metro de distância entre os pontos
|
|
|
|
// Limpa a trajetória dinâmica existente e adiciona os novos pontos
|
|
TrajetoriaDinamica.Clear();
|
|
caminho.ForEach(x => TrajetoriaDinamica.Add(x));
|
|
|
|
if (Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.DesvioNecessario)
|
|
{
|
|
AjustarTrajetoriaParaDesvio(Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.ObstaculoCritico);
|
|
}
|
|
|
|
GPSService.AtualizarTrajetoriaDinamica();
|
|
}
|
|
|
|
public void AjustarTrajetoriaParaDesvio(SonarObstaculoDetectadoModel obstaculo)
|
|
{
|
|
GPSModel posicaoAtual = GPSService.UltimaLeitura;
|
|
|
|
// Identificar o waypoint mais próximo para iniciar o desvio
|
|
int indiceInicioDesvio = EncontrarIndiceInicioDesvio(TrajetoriaDinamica, posicaoAtual, ((double)obstaculo.DistanciaMedia / 100));
|
|
|
|
// Calcular os waypoints de desvio
|
|
var waypointsDesvio = CalcularWaypointsDesvio(posicaoAtual, obstaculo, indiceInicioDesvio);
|
|
|
|
// Determinar o intervalo de waypoints a serem removidos
|
|
int indiceFinalDesvio = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, waypointsDesvio.Last(), ((double)obstaculo.DistanciaMedia / 100) + ((double)obstaculo.DeslocamentoNecessarioDesvio * 2 / 100));
|
|
int quantidadeWaypointsRemover = indiceFinalDesvio - indiceInicioDesvio;
|
|
|
|
// Remover waypoints obsoletos
|
|
if (quantidadeWaypointsRemover > 0)
|
|
{
|
|
TrajetoriaDinamica.RemoveRange(indiceInicioDesvio + 1, quantidadeWaypointsRemover);
|
|
}
|
|
|
|
// Inserir waypoints de desvio na trajetória
|
|
TrajetoriaDinamica.InsertRange(indiceInicioDesvio + 1, waypointsDesvio);
|
|
}
|
|
|
|
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 = GPSService.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 int EncontrarIndiceInicioDesvio(List<GPSModel> trajetoria, GPSModel posicaoAtual, double distanciaObstaculo)
|
|
{
|
|
// Este é um método simplificado. Você precisará implementar a lógica para encontrar o waypoint mais adequado.
|
|
return trajetoria.FindIndex(wp => GPSService.DistanciaEntrePontos(wp, posicaoAtual) < distanciaObstaculo);
|
|
}
|
|
|
|
private List<GPSModel> CalcularWaypointsDesvio(GPSModel posicaoAtual, SonarObstaculoDetectadoModel obstaculo, int indiceInicioDesvio)
|
|
{
|
|
List<GPSModel> waypointsDesvio = new List<GPSModel>();
|
|
|
|
double anguloCaminho = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
|
|
|
|
// Calcular o ângulo e distância para o desvio
|
|
double anguloDesvio = anguloCaminho + ((obstaculo.DirecaoDesvio == Enums.Direcao.Direita ? 270 : 90) + obstaculo.AnguloParaDesvio);
|
|
double deslocamento = (double)obstaculo.DistanciaMedia / 100; // Math.Sqrt(Math.Pow(((double)obstaculo.DeslocamentoNecessarioDesvio / 100), 2) + Math.Pow(((double)obstaculo.DistanciaMedia / 100), 2));
|
|
|
|
// Waypoint de início do desvio
|
|
GPSModel waypointInicioDesvio = CalcularNovoWaypoint(posicaoAtual, anguloDesvio, deslocamento);
|
|
waypointsDesvio.Add(waypointInicioDesvio);
|
|
|
|
// Waypoint de contorno do obstáculo
|
|
double anguloContorno = 270 - anguloCaminho; // Ajustar conforme a orientação do desvio
|
|
GPSModel waypointContornoObstaculo = CalcularNovoWaypoint(waypointInicioDesvio, anguloContorno, ((double)obstaculo.Largura / 100)); //((double)obstaculo.Largura / 100)
|
|
waypointsDesvio.Add(waypointContornoObstaculo);
|
|
|
|
// Waypoint de retorno à trajetória
|
|
double anguloRetorno = anguloCaminho + ((obstaculo.DirecaoDesvio == Enums.Direcao.Direita ? 90 : 270) - obstaculo.AnguloParaDesvio); //anguloDesvio - obstaculo.AnguloParaDesvio - 180;// Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + (obstaculo.AnguloParaDesvio); // Pode precisar ajustar baseado na orientação do desvio
|
|
GPSModel waypointRetorno = CalcularNovoWaypoint(waypointContornoObstaculo, anguloRetorno, deslocamento);
|
|
waypointsDesvio.Add(waypointRetorno);
|
|
|
|
return waypointsDesvio;
|
|
}
|
|
|
|
private GPSModel CalcularNovoWaypoint(GPSModel posicaoAtual, double angulo, double distancia)
|
|
{
|
|
double anguloRad = FuncoesMatematicas.ConvertDegreesToRadians(angulo);
|
|
double deltaLatitude = Math.Cos(anguloRad) * distancia / 111111; // Conversão para graus
|
|
double deltaLongitude = Math.Sin(anguloRad) * distancia / (111111 * Math.Cos(FuncoesMatematicas.ConvertDegreesToRadians(posicaoAtual.Latitude)));
|
|
|
|
return new GPSModel
|
|
{
|
|
Latitude = posicaoAtual.Latitude + deltaLatitude,
|
|
Longitude = posicaoAtual.Longitude + deltaLongitude
|
|
};
|
|
}
|
|
|
|
|
|
|
|
public void PopularTrajetoriaComOffset(GPSModel destino)
|
|
{
|
|
if (mapaService.DadosMapa != null)
|
|
{
|
|
GPSModel inicio = GPSService.UltimaLeitura;
|
|
|
|
// Coleta os primeiros pontos de cada rua com base na orientação e posição atual do robô
|
|
var pontosExtremosComOffset = new List<GPSModel>();
|
|
|
|
foreach (var rua in mapaService.DadosMapa.features)
|
|
{
|
|
var primeiroPonto = new GPSModel()
|
|
{
|
|
Latitude = rua.geometry.coordinates.First()[1],
|
|
Longitude = rua.geometry.coordinates.First()[0]
|
|
};
|
|
var ultimoPonto = new GPSModel()
|
|
{
|
|
Latitude = rua.geometry.coordinates.Last()[1],
|
|
Longitude = rua.geometry.coordinates.Last()[0]
|
|
};
|
|
GPSModel pontoInicial = new GPSModel();
|
|
GPSModel pontoSecundario = new GPSModel();
|
|
|
|
if (GPSService.DistanciaEntrePontos(inicio, primeiroPonto) < GPSService.DistanciaEntrePontos(inicio, ultimoPonto))
|
|
{
|
|
pontoInicial = primeiroPonto;
|
|
pontoSecundario = new GPSModel()
|
|
{
|
|
Latitude = rua.geometry.coordinates.Skip(1).First()[1],
|
|
Longitude = rua.geometry.coordinates.Skip(1).First()[0]
|
|
};
|
|
}
|
|
else
|
|
{
|
|
pontoInicial = ultimoPonto;
|
|
pontoSecundario = new GPSModel()
|
|
{
|
|
Latitude = rua.geometry.coordinates.Skip(rua.geometry.coordinates.Count - 2).First()[1],
|
|
Longitude = rua.geometry.coordinates.Skip(rua.geometry.coordinates.Count - 2).First()[0]
|
|
};
|
|
}
|
|
|
|
var orientacaoRua = GPSService.CalcularOrientacao(pontoInicial, pontoSecundario);
|
|
|
|
// Calcula o ponto com offset baseado no ângulo oposto à orientação da rua
|
|
var anguloOffset = (orientacaoRua + 180) % 360;
|
|
var distanciaOffset = 10; // Defina o offset desejado aqui
|
|
|
|
// Adiciona o ponto com offset à lista
|
|
pontosExtremosComOffset.Add(new GPSModel
|
|
{
|
|
Latitude = pontoInicial.Latitude,// + (Math.Cos(anguloOffset * (Math.PI / 180)) * distanciaOffset),
|
|
Longitude = pontoInicial.Longitude// + (Math.Sin(anguloOffset * (Math.PI / 180)) * distanciaOffset),
|
|
});
|
|
}
|
|
|
|
var pontosRelevantes = FiltrarPontosRelevantes(inicio, destino, pontosExtremosComOffset);
|
|
|
|
// Ordena os pontos com offset pela proximidade com a posição atual
|
|
var pontosOrdenados = pontosRelevantes.OrderBy(p => GPSService.DistanciaEntrePontos(inicio, p)).ToList();
|
|
|
|
// Agora, conecta os pontos ordenados para formar a trajetória
|
|
TrajetoriaDinamica.Clear();
|
|
GPSModel pontoAnterior = inicio;
|
|
foreach (var ponto in pontosOrdenados)
|
|
{
|
|
//var caminho = GPSService.InterpolarPontos(pontoAnterior, ponto, 1.0);
|
|
//caminho.ForEach(x => TrajetoriaDinamica.Add(x));
|
|
//pontoAnterior = ponto;
|
|
TrajetoriaDinamica.Add(ponto);
|
|
}
|
|
|
|
// Atualiza a trajetória dinâmica
|
|
GPSService.AtualizarTrajetoriaDinamica();
|
|
}
|
|
}
|
|
private List<GPSModel> FiltrarPontosRelevantes(GPSModel inicio, GPSModel destino, List<GPSModel> todasRuas)
|
|
{
|
|
var ruasRelevantes = new List<GPSModel>();
|
|
|
|
// Define os limites do quadrante
|
|
double minLatitude = Math.Min(inicio.Latitude, destino.Latitude);
|
|
double maxLatitude = Math.Max(inicio.Latitude, destino.Latitude);
|
|
double minLongitude = Math.Min(inicio.Longitude, destino.Longitude);
|
|
double maxLongitude = Math.Max(inicio.Longitude, destino.Longitude);
|
|
|
|
// Filtra previamente os pontos que estão dentro do quadrante definido
|
|
var pontosNoQuadrante = todasRuas.Where(ponto => ponto.Latitude >= minLatitude && ponto.Latitude <= maxLatitude
|
|
&& ponto.Longitude >= minLongitude && ponto.Longitude <= maxLongitude).ToList();
|
|
|
|
// Continua a filtragem dos pontos relevantes como antes, mas agora apenas com os pontos dentro do quadrante
|
|
GPSModel pontoMaisProximoInicio = IdentificarPontoMaisProximo(inicio, pontosNoQuadrante);
|
|
if (pontoMaisProximoInicio != null && !ruasRelevantes.Contains(pontoMaisProximoInicio))
|
|
{
|
|
ruasRelevantes.Add(pontoMaisProximoInicio);
|
|
}
|
|
|
|
GPSModel ultimoPontoRelevante = pontoMaisProximoInicio;
|
|
|
|
foreach (var ponto in pontosNoQuadrante)
|
|
{
|
|
if (ponto.Equals(inicio) || ponto.Equals(pontoMaisProximoInicio)) continue;
|
|
|
|
double margem = 20;
|
|
double distanciaPontoAnterior = GPSService.DistanciaEntrePontos(ultimoPontoRelevante, destino);
|
|
double distanciaPontoAtual = GPSService.DistanciaEntrePontos(ponto, destino);
|
|
double diferenca = (distanciaPontoAnterior - distanciaPontoAtual);
|
|
bool relevante = diferenca > 0 && diferenca < margem;
|
|
|
|
if (relevante || distanciaPontoAtual == 0 || true)
|
|
{
|
|
ruasRelevantes.Add(ponto);
|
|
ultimoPontoRelevante = ponto;
|
|
}
|
|
|
|
if (distanciaPontoAtual == 0)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
return ruasRelevantes;
|
|
}
|
|
|
|
// Método auxiliar para identificar o ponto mais próximo a um dado ponto de referência
|
|
private GPSModel IdentificarPontoMaisProximo(GPSModel referencia, List<GPSModel> pontos)
|
|
{
|
|
GPSModel pontoMaisProximo = null;
|
|
double distanciaMinima = double.MaxValue;
|
|
|
|
foreach (var ponto in pontos)
|
|
{
|
|
double distancia = GPSService.DistanciaEntrePontos(referencia, ponto);
|
|
if (distancia < distanciaMinima)
|
|
{
|
|
distanciaMinima = distancia;
|
|
pontoMaisProximo = ponto;
|
|
}
|
|
}
|
|
|
|
return pontoMaisProximo;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
public class MapaFeatureCollectionModel
|
|
{
|
|
public string type { get; set; }
|
|
public List<MapaFeatureModel> features { get; set; }
|
|
}
|
|
|
|
public class MapaFeatureModel
|
|
{
|
|
public string type { get; set; }
|
|
public MapaFeaturePropertiesModel properties { get; set; }
|
|
public MapaFeatureGeometryModel geometry { get; set; }
|
|
}
|
|
|
|
public class MapaFeaturePropertiesModel
|
|
{
|
|
public string Id { get; set; }
|
|
public string Name { get; set; }
|
|
public double Length { get; set; }
|
|
public double Dist1 { get; set; }
|
|
public double Dist2 { get; set; }
|
|
}
|
|
|
|
public class MapaFeatureGeometryModel
|
|
{
|
|
public string id { get; set; }
|
|
public string type { get; set; }
|
|
public List<List<double>> coordinates { get; set; }
|
|
}
|
|
|
|
|
|
|
|
}
|