diff --git a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.Designer.cs b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.Designer.cs index 938a5b0d6..72463f329 100644 --- a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.Designer.cs +++ b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.Designer.cs @@ -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; } } \ No newline at end of file diff --git a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs index 214738474..e6dd6c168 100644 --- a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs +++ b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs @@ -166,7 +166,7 @@ namespace AgroBase.Forms simulacao = new List(); } - 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() : _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); + } } diff --git a/AgroBase/AgroBase/Models/Enums.cs b/AgroBase/AgroBase/Models/Enums.cs index d0c648cad..c2e6967fd 100644 --- a/AgroBase/AgroBase/Models/Enums.cs +++ b/AgroBase/AgroBase/Models/Enums.cs @@ -197,7 +197,8 @@ CaminhandoRua = 2, SaindoRua = 3, Manobrando = 4, - Direcionando = 5 + Direcionando = 5, + RetornandoBase = 6 } public enum DirecaoCarroRua diff --git a/AgroBase/AgroBase/Models/GPSModel.cs b/AgroBase/AgroBase/Models/GPSModel.cs index 21269edb1..33c88d321 100644 --- a/AgroBase/AgroBase/Models/GPSModel.cs +++ b/AgroBase/AgroBase/Models/GPSModel.cs @@ -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 diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index 58c271f50..f0efa22fe 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -1674,6 +1674,13 @@ namespace AgroBase.Models return; } + if (controleBase.Dispositivo == T_Code.Trj) + { + var (posicaoBase, pontosRetorno) = ((JObject)controleBase._comp_value).ToObject<(GPSModel, List)>(); + 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, diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index ab31e3073..cab4cca40 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -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> 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 waypointsManual = null) + { + if (baseGps == null) + throw new ArgumentNullException(nameof(baseGps)); + + bool valido = false; + List pontosRetorno; + + // ========================= + // MODO MANUAL (operador) + // ========================= + if (waypointsManual != null && waypointsManual.Count > 0) + { + pontosRetorno = new List(); + 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 pontos, string motivo) + { + var traj = new List(); + + // 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 pontos) GerarTrajetoriaRetornoAutomatico(GPSModel baseGps) + { + var pontos = new List(); + + 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 trecho = new List(); + 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(); + 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 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 GerarTrechoReto(GPSModel a, GPSModel b, double espacamento_m) + { + var lista = new List(); + + 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 GerarTrajetoriaViaCorredor(GPSModel posRobo, GPSModel posBase) + { + var candidatos = EncontrarCorredoresProximos(posRobo, posBase); + if (candidatos.Count == 0) return new List(); + + (int idxCorredor, TipoCondicaoRoboRua condicaoCarro) = SelecionarMelhorCorredor(candidatos, posRobo); + if (idxCorredor < 0) return new List(); + + var pontos = new List(); + 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 GerarTrajetoriaPerimetral(GPSModel posRobo, GPSModel posBase, double margem_m = 12.0) + { + // 1) Coleta todos os pontos do mapa (corredores) + var ptsMapa = new List(); + 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(); + + // 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(); + + // 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(); + + // 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 ConstruirPerimetroConvexoComOffset(List 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(); + + // 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(); + 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 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 ExtrairArcoPerimetro(List perim, int idxIni, int idxFim, bool sentidoHorario) + { + var arco = new List(); + 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 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 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) diff --git a/AgroBase/AgroBase/Services/GPSService.cs b/AgroBase/AgroBase/Services/GPSService.cs index d42a00f88..6ed68cef0 100644 --- a/AgroBase/AgroBase/Services/GPSService.cs +++ b/AgroBase/AgroBase/Services/GPSService.cs @@ -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); } } diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index a9bd797bc..271ea65a2 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -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, diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py index 706aa8463..39292a183 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py @@ -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) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py index deba8110d..6bc84e8ea 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py @@ -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: diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py index 3ba644820..dc6cc9fd2 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py @@ -56,6 +56,7 @@ class StatusCarroMapa(IntEnum): SaindoRua = 3, Manobrando = 4 Direcionando = 5 + RetornandoBase = 6 class T_Code(IntEnum): Vzo = -1