Adicionado funcao de retorno a base
This commit is contained in:
Diego Freitas
parent
a6d3b8e348
commit
8d537383fb
|
|
@ -105,6 +105,9 @@
|
|||
this.lblTempoRestante = new System.Windows.Forms.Label();
|
||||
this.lblPercentualRua = new System.Windows.Forms.Label();
|
||||
this.lblPercentualOperacao = new System.Windows.Forms.Label();
|
||||
this.btnRetornarBase = new System.Windows.Forms.Button();
|
||||
this.btnFixarBase = new System.Windows.Forms.Button();
|
||||
this.btnLiberarCorredor = new System.Windows.Forms.Button();
|
||||
this.pnlOpcoes.SuspendLayout();
|
||||
this.gpbSonar.SuspendLayout();
|
||||
this.gpbGPS.SuspendLayout();
|
||||
|
|
@ -114,6 +117,9 @@
|
|||
//
|
||||
// pnlOpcoes
|
||||
//
|
||||
this.pnlOpcoes.Controls.Add(this.btnLiberarCorredor);
|
||||
this.pnlOpcoes.Controls.Add(this.btnFixarBase);
|
||||
this.pnlOpcoes.Controls.Add(this.btnRetornarBase);
|
||||
this.pnlOpcoes.Controls.Add(this.lblDelayPosicao);
|
||||
this.pnlOpcoes.Controls.Add(this.txtDelayPosicao);
|
||||
this.pnlOpcoes.Controls.Add(this.lblCarregado);
|
||||
|
|
@ -988,6 +994,39 @@
|
|||
this.lblPercentualOperacao.TabIndex = 42;
|
||||
this.lblPercentualOperacao.Text = "Operação: 0,00%";
|
||||
//
|
||||
// btnRetornarBase
|
||||
//
|
||||
this.btnRetornarBase.Location = new System.Drawing.Point(388, 44);
|
||||
this.btnRetornarBase.Margin = new System.Windows.Forms.Padding(2);
|
||||
this.btnRetornarBase.Name = "btnRetornarBase";
|
||||
this.btnRetornarBase.Size = new System.Drawing.Size(55, 27);
|
||||
this.btnRetornarBase.TabIndex = 46;
|
||||
this.btnRetornarBase.Text = "R Base";
|
||||
this.btnRetornarBase.UseVisualStyleBackColor = true;
|
||||
this.btnRetornarBase.Click += new System.EventHandler(this.btnRetornarBase_Click);
|
||||
//
|
||||
// btnFixarBase
|
||||
//
|
||||
this.btnFixarBase.Location = new System.Drawing.Point(388, 13);
|
||||
this.btnFixarBase.Margin = new System.Windows.Forms.Padding(2);
|
||||
this.btnFixarBase.Name = "btnFixarBase";
|
||||
this.btnFixarBase.Size = new System.Drawing.Size(55, 27);
|
||||
this.btnFixarBase.TabIndex = 47;
|
||||
this.btnFixarBase.Text = "F Base";
|
||||
this.btnFixarBase.UseVisualStyleBackColor = true;
|
||||
this.btnFixarBase.Click += new System.EventHandler(this.btnFixarBase_Click);
|
||||
//
|
||||
// btnLiberarCorredor
|
||||
//
|
||||
this.btnLiberarCorredor.Location = new System.Drawing.Point(388, 75);
|
||||
this.btnLiberarCorredor.Margin = new System.Windows.Forms.Padding(2);
|
||||
this.btnLiberarCorredor.Name = "btnLiberarCorredor";
|
||||
this.btnLiberarCorredor.Size = new System.Drawing.Size(55, 20);
|
||||
this.btnLiberarCorredor.TabIndex = 48;
|
||||
this.btnLiberarCorredor.Text = "Liberar";
|
||||
this.btnLiberarCorredor.UseVisualStyleBackColor = true;
|
||||
this.btnLiberarCorredor.Click += new System.EventHandler(this.btnLiberarCorredor_Click);
|
||||
//
|
||||
// frmSimulacaoMapaGPS
|
||||
//
|
||||
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
|
||||
|
|
@ -1113,5 +1152,8 @@
|
|||
private System.Windows.Forms.Label label8;
|
||||
private System.Windows.Forms.ComboBox cmbStatusCorredor;
|
||||
private System.Windows.Forms.CheckBox chbAcompanharCarro;
|
||||
private System.Windows.Forms.Button btnRetornarBase;
|
||||
private System.Windows.Forms.Button btnFixarBase;
|
||||
private System.Windows.Forms.Button btnLiberarCorredor;
|
||||
}
|
||||
}
|
||||
|
|
@ -166,7 +166,7 @@ namespace AgroBase.Forms
|
|||
simulacao = new List<MPCSimulacaoModel>();
|
||||
}
|
||||
|
||||
if (_Trajetoria?.CorredorAtual != null)
|
||||
if (_Trajetoria?.CorredorAtual != null || (_Trajetoria?.RetornandoBase ?? false))
|
||||
{
|
||||
MapaDinamico.AtualizarDados(
|
||||
KinectService.Iniciado && KinectService.Leitura.StatusDetecao ? KinectService.Leitura.ObstaculoCritico : null,
|
||||
|
|
@ -174,7 +174,7 @@ namespace AgroBase.Forms
|
|||
(float)_Sensoriamento.Gps.AnguloCarroDefinido,
|
||||
_Sensoriamento.Gps,
|
||||
_Trajetoria._TrajetoriaDinamica,
|
||||
_Trajetoria.CorredorAtual.Pontos,
|
||||
_Trajetoria.RetornandoBase ? new List<PontoTrajetoriaModel>() : _Trajetoria.CorredorAtual.Pontos,
|
||||
Variaveis.OperacaoEmAndamento.GPSTrajetoria,
|
||||
_Trajetoria.RuasPlantacao,
|
||||
simulacao
|
||||
|
|
@ -223,9 +223,9 @@ namespace AgroBase.Forms
|
|||
btnAcrescentarGPS.Enabled = true;
|
||||
txtAnguloGPS.Enabled = true;
|
||||
txtDistanciaGPS.Enabled = true;
|
||||
btnIniciarGPS.Enabled = false;
|
||||
txtLatitude.ReadOnly = true;
|
||||
txtLongitude.ReadOnly = true;
|
||||
//btnIniciarGPS.Enabled = false;
|
||||
//txtLatitude.ReadOnly = true;
|
||||
//txtLongitude.ReadOnly = true;
|
||||
}
|
||||
|
||||
private void AtualizarPosicaoGPS()
|
||||
|
|
@ -537,6 +537,37 @@ namespace AgroBase.Forms
|
|||
{
|
||||
MapaDinamico.AlterarTravaRover(chbAcompanharCarro.Checked);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
private void btnRetornarBase_Click(object sender, EventArgs e)
|
||||
{
|
||||
Variaveis.OperacaoEmAndamento.Trajetoria.IniciarRetornoBase(VariaveisOperacao.PosicaoBase);
|
||||
}
|
||||
|
||||
private void btnFixarBase_Click(object sender, EventArgs e)
|
||||
{
|
||||
VariaveisOperacao.PosicaoBase = new GPSModel()
|
||||
{
|
||||
DataHora = DateTime.Now,
|
||||
Lon0 = double.Parse(txtLongitude.Text.Replace(".", ",")),
|
||||
LongitudeAnt = double.Parse(txtLongitude.Text.Replace(".", ",")),
|
||||
Longitude = double.Parse(txtLongitude.Text.Replace(".", ",")),
|
||||
LatitudeAnt = double.Parse(txtLatitude.Text.Replace(".", ",")),
|
||||
Lat0 = double.Parse(txtLatitude.Text.Replace(".", ",")),
|
||||
Latitude = double.Parse(txtLatitude.Text.Replace(".", ",")),
|
||||
Heartbeat = GPSService.UltimaLeitura.Heartbeat,
|
||||
OrientacaoReal = double.Parse(txtAnguloGPS.Text.Replace(".", ",")),
|
||||
};
|
||||
GPSService.EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.AnguloCarroDefinido, false, Variaveis.LoraBaseParametros.address);
|
||||
}
|
||||
|
||||
private void btnLiberarCorredor_Click(object sender, EventArgs e)
|
||||
{
|
||||
int idx_corredor = (Variaveis.OperacaoEmAndamento.Trajetoria.CorredorAtual?.Idx ?? 0) + (Variaveis.OperacaoEmAndamento.Trajetoria.CorredorAtual?.Concluido ?? false ? 1 : 0);
|
||||
Variaveis.OperacaoEmAndamento.Trajetoria.AtualizarDadosAutonomiaCorredor(idxCorredor: idx_corredor, bat_liberada: true, herb_liberado: true);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -197,7 +197,8 @@
|
|||
CaminhandoRua = 2,
|
||||
SaindoRua = 3,
|
||||
Manobrando = 4,
|
||||
Direcionando = 5
|
||||
Direcionando = 5,
|
||||
RetornandoBase = 6
|
||||
}
|
||||
|
||||
public enum DirecaoCarroRua
|
||||
|
|
|
|||
|
|
@ -431,6 +431,17 @@ namespace AgroBase.Models
|
|||
return (x, y);
|
||||
}
|
||||
|
||||
public static (double lat, double lon) ConverterMetrosParaLatLong(double x, double y, GPSModel referencia)
|
||||
{
|
||||
double lat0 = FuncoesMatematicas.GrausParaRadianos(referencia.Latitude);
|
||||
double lon0 = FuncoesMatematicas.GrausParaRadianos(referencia.Longitude);
|
||||
|
||||
double lat = (y / RaioDaTerra) + lat0;
|
||||
double lon = (x / (RaioDaTerra * Math.Cos(lat0))) + lon0;
|
||||
|
||||
return (lat * 180.0 / Math.PI, lon * 180.0 / Math.PI);
|
||||
}
|
||||
|
||||
public static double ConvertToDecimalDegrees(string nmeaCoordinate, string direction, int digits)
|
||||
{
|
||||
// Divide a string em graus e minutos
|
||||
|
|
|
|||
|
|
@ -1674,6 +1674,13 @@ namespace AgroBase.Models
|
|||
return;
|
||||
}
|
||||
|
||||
if (controleBase.Dispositivo == T_Code.Trj)
|
||||
{
|
||||
var (posicaoBase, pontosRetorno) = ((JObject)controleBase._comp_value).ToObject<(GPSModel, List<GPSModel>)>();
|
||||
Variaveis.OperacaoEmAndamento.Trajetoria?.IniciarRetornoBase(posicaoBase, pontosRetorno);
|
||||
return;
|
||||
}
|
||||
|
||||
Variaveis.OperacaoEmAndamento.Emergencia = controleBase.Emergencia;
|
||||
if (Variaveis.OperacaoEmAndamento.Emergencia) return;
|
||||
|
||||
|
|
@ -2481,6 +2488,7 @@ namespace AgroBase.Models
|
|||
var dadosTrajetoria = new OperacaoSensoriamentoLogTrajetoriaModel()
|
||||
{
|
||||
StatusCarro = _Trajetoria?.StatusAtual ?? StatusCarroMapa.Parado,
|
||||
TrajetoriaConcluida = _Trajetoria?.TrajeotiraConcluida ?? false,
|
||||
DistanciaDireita = _Trajetoria?.DistanciaDireita ?? 0,
|
||||
DistanciaEsquerda = _Trajetoria?.DistanciaEsquerda ?? 0,
|
||||
ProgressoTrajeto = FuncoesMatematicas.Clamp(_Trajetoria?.PercentualTrajetoria ?? 0, 0, 100),
|
||||
|
|
@ -2846,6 +2854,7 @@ namespace AgroBase.Models
|
|||
public class OperacaoSensoriamentoLogTrajetoriaModel
|
||||
{
|
||||
public StatusCarroMapa StatusCarro { get; set; }
|
||||
public bool TrajetoriaConcluida { get; set; }
|
||||
public double ProgressoTrajeto { get; set; }
|
||||
public double AnguloCaminho { get; set; }
|
||||
public double AnguloMedioCorredor { get; set; }
|
||||
|
|
@ -2886,6 +2895,7 @@ namespace AgroBase.Models
|
|||
return new OperacaoSensoriamentoLogTrajetoriaModel()
|
||||
{
|
||||
StatusCarro = StatusCarro,
|
||||
TrajetoriaConcluida = TrajetoriaConcluida,
|
||||
AnguloCaminho = AnguloCaminho,
|
||||
AnguloMedioCorredor = AnguloMedioCorredor,
|
||||
DirecaoCaminho = DirecaoCaminho,
|
||||
|
|
|
|||
|
|
@ -33,15 +33,18 @@ namespace AgroBase.Models
|
|||
#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()
|
||||
{
|
||||
double dt = Math.Min((1.0 / GPSService.TaxaAmostragemHz), (UltimaAtualizacaoDados - (Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps?.Momento ?? DateTime.MinValue)).TotalSeconds);
|
||||
TempoEntreLeituras = dt;
|
||||
UltimaAtualizacaoDados = Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura.Gps?.Momento ?? DateTime.MinValue;
|
||||
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; }
|
||||
|
|
@ -70,7 +73,7 @@ namespace AgroBase.Models
|
|||
// 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.Count - 1]; // Acessa diretamente o ultimo ponto da rua
|
||||
return RuasPlantacao[CorredorAtual.Idx][RuasPlantacao[CorredorAtual.Idx].Count - 1]; // Acessa diretamente o ultimo ponto da rua
|
||||
}
|
||||
else if (CorredorAtual == null && RuasPlantacao.Count > 0)
|
||||
{
|
||||
|
|
@ -420,35 +423,49 @@ namespace AgroBase.Models
|
|||
private void AtualizarStatusAtual()
|
||||
{
|
||||
StatusAtual = StatusCarroMapa.Parado;
|
||||
if (ProximoPonto != null && CorredorAtual != null && Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.EmAndamento)
|
||||
if (ProximoPonto != null)
|
||||
{
|
||||
if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas)
|
||||
if (CorredorAtual != null)
|
||||
{
|
||||
StatusAtual = StatusCarroMapa.Direcionando;
|
||||
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 (!CorredorAtual.Dentro && (PontoAtual.PontoBorda || PontoAtual.PontoLigacao || ProximoPonto.Tipo == TipoPontoRua.LigacaoEntrada || PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores))
|
||||
else if (RetornandoBase)
|
||||
{
|
||||
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;
|
||||
StatusAtual = StatusCarroMapa.RetornandoBase;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -456,9 +473,10 @@ namespace AgroBase.Models
|
|||
public double DistanciaTotal { get; private set; }
|
||||
[JsonProperty]
|
||||
public double DistanciaPercorrida { get; private set; }
|
||||
private void AtualizarDistanciaPercorrida()
|
||||
public void AtualizarDistanciaPercorrida(double distancia)
|
||||
{
|
||||
DistanciaPercorrida = _Corredores?.Sum(x => x.DistanciaPercorridaTotal) ?? 0;
|
||||
DistanciaPercorrida += distancia;
|
||||
CorredorAtual?.AtualizarDistancias(distancia);
|
||||
}
|
||||
[JsonProperty]
|
||||
public double DistanciaRestante { get; private set; }
|
||||
|
|
@ -564,6 +582,12 @@ namespace AgroBase.Models
|
|||
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()
|
||||
{
|
||||
|
|
@ -635,6 +659,8 @@ namespace AgroBase.Models
|
|||
{
|
||||
ponto.AtualizarPropriedades();
|
||||
}
|
||||
|
||||
AtualizarTrajetoriaConcluida();
|
||||
}
|
||||
|
||||
private void AtualizarDadosTrajetoria()
|
||||
|
|
@ -667,7 +693,6 @@ namespace AgroBase.Models
|
|||
AtualizarNaMargemDoCorredor();
|
||||
AtualizarManobrandoEntreRuas();
|
||||
AtualizarStatusAtual();
|
||||
AtualizarDistanciaPercorrida();
|
||||
AtualizarDistanciaRestante();
|
||||
AtualizarPercentualTrajetoria();
|
||||
AtualizarTempoEstimado();
|
||||
|
|
@ -1413,8 +1438,6 @@ namespace AgroBase.Models
|
|||
DistanciaTotal = distanciaTotal,
|
||||
Pontos = x.ToList(),
|
||||
QtdPontos = x.Count(),
|
||||
DistanciaPercorridaCorredor = 0,
|
||||
DistanciaPercorridaTotal = 0,
|
||||
idxRuaDireita = idxDir + incDir,
|
||||
idxRuaEsquerda = idxEsq + incEsq,
|
||||
FatorLarguraCorredor = CorredoresLarguras[x.Key] / LarguraCorredorPadrao
|
||||
|
|
@ -1428,6 +1451,8 @@ namespace AgroBase.Models
|
|||
|
||||
DistanciaTotal = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo).Select(x => x.Posicao).ToList());
|
||||
|
||||
RetornandoBase = false;
|
||||
|
||||
AutonomiaCorredor.Iniciar();
|
||||
|
||||
AtualizarDadosTrajetoria();
|
||||
|
|
@ -1589,6 +1614,600 @@ namespace AgroBase.Models
|
|||
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
|
||||
|
|
@ -1596,8 +2215,18 @@ namespace AgroBase.Models
|
|||
public int Idx { get; set; }
|
||||
public double Largura { get; set; }
|
||||
public double DistanciaTotal { get; set; }
|
||||
public double DistanciaPercorridaCorredor { get; set; }
|
||||
public double DistanciaPercorridaTotal { 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]
|
||||
|
|
@ -1896,6 +2525,11 @@ namespace AgroBase.Models
|
|||
Iniciado = true;
|
||||
}
|
||||
|
||||
public void Parar()
|
||||
{
|
||||
Iniciado = false;
|
||||
}
|
||||
|
||||
public void AtualizarDados(int idxCorredor, bool bateria_liberada, bool reservatorio_liberado)
|
||||
{
|
||||
if (!Iniciado)
|
||||
|
|
|
|||
|
|
@ -987,14 +987,7 @@ namespace AgroBase.Services
|
|||
_GPSTrajetoria[penultimo] // Penúltimo ponto
|
||||
);
|
||||
|
||||
if (CorredorAtual != null)
|
||||
{
|
||||
CorredorAtual.DistanciaPercorridaTotal += distancia;
|
||||
if (_Trajetoria.CorredorAtual.Dentro)
|
||||
{
|
||||
CorredorAtual.DistanciaPercorridaCorredor += distancia;
|
||||
}
|
||||
}
|
||||
_Trajetoria?.AtualizarDistanciaPercorrida(distancia);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -127,6 +127,7 @@ namespace AgroBase.Services.Operadores
|
|||
("Trajetoria", _t != null ? new
|
||||
{
|
||||
status = (int)_t.StatusCarro,
|
||||
concluido = _t.TrajetoriaConcluida,
|
||||
manobrando = _t.ManobrandoEntreRuas,
|
||||
erro_angular = _t.ErroOrientacaoAngularCombinado,
|
||||
erro_angular_caminho = _t.ErroOrientacaoAngularCaminho,
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ def definir_comando(parada_necessaria: bool, erro: bool, filtro_vel: FiltroVeloc
|
|||
|
||||
if status_carro == StatusCarroMapa.Parado:
|
||||
velocidade_sp = 0
|
||||
elif status_carro == StatusCarroMapa.Direcionando:
|
||||
elif status_carro in [StatusCarroMapa.Direcionando, StatusCarroMapa.RetornandoBase]:
|
||||
velocidade_sp = calcular_velocidade_relativa(vel_com_ervas, vel_sem_ervas, ang_max, erro_orientacao, k=0.85, curva=0.8, dead=10.0)
|
||||
elif status_carro in [StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando]:
|
||||
velocidade_sp = min(vel_min, vel_com_ervas)
|
||||
|
|
|
|||
|
|
@ -480,8 +480,9 @@ class ContextoGlobalRedis:
|
|||
return
|
||||
|
||||
# Ultimo corredor ja foi concluido
|
||||
_traj_concluida = cls.get_contexto().get("Trajetoria", {}).get("concluido", False)
|
||||
_corredor = cls.get_contexto().get("Trajetoria", {}).get("CorredorAtual", {})
|
||||
if _corredor.get("ultimo", False) and _corredor.get("concluido", False):
|
||||
if _traj_concluida or (_corredor.get("ultimo", False) and _corredor.get("concluido", False)):
|
||||
if status_atual != StatusOperacao.Concluido:
|
||||
cls.atualizar_ctx_dict(CtxKey.DadosOperacao, status=StatusOperacao.Concluido.value)
|
||||
if iniciado and not finalizando:
|
||||
|
|
|
|||
|
|
@ -56,6 +56,7 @@ class StatusCarroMapa(IntEnum):
|
|||
SaindoRua = 3,
|
||||
Manobrando = 4
|
||||
Direcionando = 5
|
||||
RetornandoBase = 6
|
||||
|
||||
class T_Code(IntEnum):
|
||||
Vzo = -1
|
||||
|
|
|
|||
Loading…
Reference in New Issue