459 lines
20 KiB
C#
459 lines
20 KiB
C#
using AgroBase.Properties;
|
|
using AgroBase.Services;
|
|
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.Enuns;
|
|
|
|
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 double DistanciaTotal { get; set; } = 0;
|
|
|
|
public void btnCarregar_Click(object sender, EventArgs e)
|
|
{
|
|
mapaService.CarregarMapa(MapasVariaveisModel.NomeArquivoMapaInterativo, MapasVariaveisModel.NomeArquivoMapaInterativoSaida);
|
|
|
|
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();
|
|
GPSService.IniciarRecepcaoDados();
|
|
|
|
chromiumWebBrowser = new ChromiumWebBrowser(mapaService.MapaUrl);
|
|
pnlMapa.Controls.Add(chromiumWebBrowser);
|
|
chromiumWebBrowser.Dock = DockStyle.Fill;
|
|
}
|
|
|
|
public void PararProcessamento()
|
|
{
|
|
APIService.Parar();
|
|
GPSService.PararRecepcaoDados();
|
|
}
|
|
|
|
|
|
|
|
// 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>();
|
|
|
|
// Calcular o ângulo e distância para o desvio
|
|
double anguloDesvio = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + ((obstaculo.DirecaoDesvio == Enuns.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 - Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.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 = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + ((obstaculo.DirecaoDesvio == Enuns.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 = FuncoesGlobais.ConvertDegreesToRadians(angulo);
|
|
double deltaLatitude = Math.Cos(anguloRad) * distancia / 111111; // Conversão para graus
|
|
double deltaLongitude = Math.Sin(anguloRad) * distancia / (111111 * Math.Cos(FuncoesGlobais.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; }
|
|
}
|
|
|
|
|
|
|
|
}
|