using AgroBase.Models; using AgroBase.Services; using Newtonsoft.Json; using System; using System.Collections.Generic; using System.IO; using System.Linq; using System.Threading.Tasks; using System.Windows.Forms; using static AgroBase.Models.Enums; namespace AgroBase.Forms { public partial class frmSimulacaoMapaGPS : Form { AsyncTaskTimerModel tmrLeitura; MapaDinamicoModel MapaDinamico; bool parar = false; double tempoGps = (1000.0 / GPSService.TaxaAmostragemHz); int ticksControleMax = 0; int ticksControle = 0; List passosOperacao = new List(); int idxPassoOperacao = 0; double velocidadeCarroMs = 0.0; DateTime UltimaAtualizacaoGps = DateTime.MinValue; Queue historicoAngulosControle = new Queue(); List simulacao = new List(); public frmSimulacaoMapaGPS() { InitializeComponent(); // Ativa o double buffering this.DoubleBuffered = true; this.SetStyle(ControlStyles.AllPaintingInWmPaint, true); this.SetStyle(ControlStyles.UserPaint, true); this.SetStyle(ControlStyles.OptimizedDoubleBuffer, true); MapaDinamico = new MapaDinamicoModel(this, pnlZoomMapa); } private void frmSimulacaoMapaGPS_Load(object sender, EventArgs e) { cmbTipoControleDirecional.Items.Clear(); cmbTipoControleDirecional.Items.AddRange(Enum.GetNames(typeof(Enums.TiposControladorDirecional))); cmbTipoControleDirecional.SelectedIndex = (int)Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional; cmbStatusCorredor.Items.Clear(); cmbStatusCorredor.Items.AddRange(Enum.GetNames(typeof(Enums.StatusCarroMapa))); cmbStatusCorredor.SelectedIndex = (int)StatusCarroMapa.Parado; Variaveis.OperacaoEmAndamento.ReiniciarSimulacao(); tmrLeitura = new AsyncTaskTimerModel("tmrLeitura", tmrLeitura_Tick, (int)tempoGps, this); tmrLeitura.Start(); txtTempoGPS.Text = tempoGps.ToString(); Variaveis.OperacaoEmAndamento.Mapa = new MapasModel() { pnlMapa = pnlMapa, }; } private void frmSimulacaoMapaGPS_FormClosing(object sender, FormClosingEventArgs e) { tmrLeitura.Dispose(); Variaveis.OperacaoEmAndamento.Iniciado = false; } DateTime t0 = DateTime.Now; private async Task tmrLeitura_Tick() { double latencia_tick = (DateTime.Now - t0).TotalSeconds; t0 = DateTime.Now; int.TryParse(txtTempoGPS.Text, out int tempo); if (tempo > 0) { tempoGps = tempo; } Func funcAtt = new Func(() => { AtualizarDadosTela(); return true; }); Task.Run(() => FuncoesGlobais.ExecutarMetodoComVerificacaoCrossThread(this, funcAtt)); if (chbAutomatico.Checked) { btnAcrescentarGPS_Click(btnAcrescentarGPS, new EventArgs()); } DateTime t1 = DateTime.Now; double latencia_exec = (t1 - t0).TotalSeconds; double frequencia_exec = 1.0 / Math.Max(latencia_exec, 1e-6); double frequencia_tick = 1.0 / Math.Max(latencia_tick, 1e-6); //Console.WriteLine($"Tempo por tick GPS: {Math.Round(latencia_exec, 4)} s, Frequencia: {Math.Round(frequencia_exec, 2)} Hz | Tempo entre tick GPS: {Math.Round(latencia_tick, 4)}, Frequencia: {Math.Round(frequencia_tick, 2)} Hz"); double novoDelay = Math.Max(tempoGps - (latencia_exec * 1000.0), 1); tmrLeitura.SetInterval((int)novoDelay); } bool AtualizandoTela = false; private void AtualizarDadosTela() { if (AtualizandoTela) return; AtualizandoTela = true; var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento; var _Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria; lblStatusOperacao.Text = "Operação: " + Enum.GetName(typeof(StatusOperacao), _Sensoriamento.StatusOperacao); lblTipoMovimento.Text = _Sensoriamento.Controle.TipoMovimento.ToString(); if (_Trajetoria == null) { Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas(); return; } lblStatus.Text = "Carro: " + Enum.GetName(typeof(StatusCarroMapa), _Sensoriamento.Trajetoria.StatusCarro); lblDirecao.Text = "Direção: " + Enum.GetName(typeof(DirecaoCarroRua), _Sensoriamento.Trajetoria.DirecaoCaminho); lblDistanciaOperacao.Text = "Distância Operação: " + _Sensoriamento.Trajetoria.DistanciaPercorrida.ToString("0.00") + " m de " + _Sensoriamento.Trajetoria.DistanciaTotal.ToString("0.00") + " m"; lblDistanciaFinal.Text = "Distância Restante: " + _Sensoriamento.Trajetoria.DistanciaRestante.ToString("0.00") + " m"; lblDistanciaLateral.Text = "Esq: " + _Sensoriamento.Trajetoria.DistanciaEsquerda.ToString("0.00") + " m - Dir: " + _Sensoriamento.Trajetoria.DistanciaDireita.ToString("0.00") + " m"; lblRua.Text = "Corredor: " + (_Sensoriamento.Trajetoria.CorredorAtual.Dentro ? "Dentro" : "Fora"); lblMargem.Text = "Margem: " + (_Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não"); lblProximoPonto.Text = "Próximo Ponto: " + (_Sensoriamento.Trajetoria.ProximoPonto.idxPonto); lblAproximando.Text = (_Sensoriamento.Trajetoria.ProximoPonto.Aproximando ? "Aproximando" : "Afastando"); lblDistProx.Text = "Distância Próximo Ponto: " + _Sensoriamento.Trajetoria.ProximoPonto.DistanciaTrajeto.ToString("0.00") + " m"; lblDistAnt.Text = "Distância Ponto Anterior: " + _Sensoriamento.Trajetoria.PontoMaisProximo.DistanciaTrajeto.ToString("0.00") + " m"; lblTempoRestante.Text = "Tempo Restante: " + _Sensoriamento.Trajetoria.TempoEstimadoRestante; lblTempoOperacao.Text = "Tempo: " + _Sensoriamento.TempoDecorrido.ToString("HH:mm:ss") + " de " + _Sensoriamento.Trajetoria.TempoEstimadoOperacao; lblPercentualRua.Text = "Rua: " + _Sensoriamento.Trajetoria.CorredorAtual.Progresso.ToString("0.00") + "%"; lblPercentualOperacao.Text = "Operação: " + _Sensoriamento.Trajetoria.ProgressoTrajeto.ToString("0.00") + "%"; pnlOrientacaoTrajeto.Invalidate(); pnlAnguloControle.Invalidate(); pnlOrientacaoCarro.Invalidate(); if (_Sensoriamento.Controle.SimulacaoMPC != null) { simulacao = _Sensoriamento.Controle.SimulacaoMPC; } if (_Trajetoria?.CorredorAtual != null) { MapaDinamico.AtualizarDados( KinectService.Iniciado && KinectService.Leitura.StatusDetecao ? KinectService.Leitura.ObstaculoCritico : null, (float)_Sensoriamento.Trajetoria.AnguloCaminho, (float)_Sensoriamento.Gps.AnguloCarroDefinido, _Sensoriamento.Gps, _Trajetoria._TrajetoriaDinamica, _Trajetoria.CorredorAtual.Pontos, Variaveis.OperacaoEmAndamento.GPSTrajetoria, _Trajetoria.RuasPlantacao, simulacao ); } if (!Variaveis.OperacaoEmAndamento.Iniciado) { if (btnIniciarSimulacao.Text.Contains("Parar")) { parar = true; btnIniciarSimulacao_Click(new object(), new EventArgs()); parar = false; } Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas(); } AtualizandoTela = false; } private void btnCarregar_Click(object sender, EventArgs e) { Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click((Control) sender, e); } private void btnIniciarGPS_Click(object sender, EventArgs e) { AtualizarPosicaoGPS(); btnAcrescentarGPS.Enabled = true; txtAnguloGPS.Enabled = true; txtDistanciaGPS.Enabled = true; btnIniciarGPS.Enabled = false; txtLatitude.ReadOnly = true; txtLongitude.ReadOnly = true; } private void AtualizarPosicaoGPS() { GPSService.PenultimaLeitura = new GPSModel() { DataHora = GPSService.UltimaLeitura.DataHora, Longitude = GPSService.UltimaLeitura.Longitude, Latitude = GPSService.UltimaLeitura.Latitude, }; if (!GPSService.Iniciado || (txtLongitude.Text != "" && txtLatitude.Text != "")) { GPSService.UltimaLeitura = new GPSModel() { DataHora = DateTime.Now, Longitude = double.Parse(txtLongitude.Text.Replace(".", ",")), Latitude = double.Parse(txtLatitude.Text.Replace(".", ",")), }; } GPSService.AtualizarCoordenadasGPS(); } private async void btnAcrescentarGPS_Click(object sender, EventArgs e) { GPSModel novaPosicao = new GPSModel(); double velocidadeMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle); velocidadeCarroMs = FuncoesMatematicas.ConverteMsParaKmh(velocidadeMs); Variaveis.OperacaoEmAndamento.DispMvd.Dados.VelocidadeMedia = velocidadeMs; double tempoPassado = !chbAutomatico.Checked || UltimaAtualizacaoGps == DateTime.MinValue ? // esta no modo manual (tempoGps / 1000.0) : // 1 tick do tempo gps (DateTime.Now - UltimaAtualizacaoGps).TotalSeconds; // tempo decorrido entre ticks do timer de leitura // Obtém os parâmetros double anguloAtual = double.Parse(txtAnguloGPS.Text); var _Gps = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps; double anguloControle = 0.0; TipoMovimentoDirecional tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento; var tipoControle = Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional; if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando) { novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)]; idxPassoOperacao++; } else { if (Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Aguardando) { return; } anguloControle = double.Parse(txtAnguloControle.Text); double.TryParse(txtErroTempoComando.Text, out double fatorErro); int ticksMax = (int)(ticksControleMax * Math.Max(1, fatorErro)); bool atualizarAnguloControle = (ticksControle >= ticksMax); if (atualizarAnguloControle) { ticksControle = 0; } else { ticksControle++; } if (atualizarAnguloControle) { btnCalcularAngulo_Click(new object(), new EventArgs()); anguloControle = double.Parse(txtAnguloControle.Text); } } // Calcula a nova posicao do robo //GPSModel pos = NovaPosicaoSimulacao(velocidadeMs, tempoPassado); if (true || novaPosicao == null) { novaPosicao = VariaveisEquipamento.SimularNovaPosicao(tipoMovimento, anguloControle, velocidadeMs, tempoPassado, anguloAtual, _Gps); } //Console.WriteLine($"C#: {DadosPosicao(novaPosicao)}"); //Console.WriteLine($"Py: {DadosPosicao(pos)}"); // Atualiza a interface gráfica txtAnguloGPS.Text = novaPosicao.OrientacaoReal.ToString("0.00"); txtVelocidade.Text = velocidadeCarroMs.ToString("0.00"); txtDistanciaGPS.Text = novaPosicao.Distancia.ToString("0.0000"); txtLatitude.Text = novaPosicao.Latitude.ToString(); txtLongitude.Text = novaPosicao.Longitude.ToString(); UltimaAtualizacaoGps = DateTime.Now; AtualizarPosicaoGPS(); Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados(); if (!chbAutomatico.Checked) { AtualizarDadosTela(); } } private GPSModel NovaPosicaoSimulacao(double velocidadeMs, double tempo) { if (Variaveis.OperacaoEmAndamento.Controle.SimulacaoMPC.Count() > 1) { double distancia = velocidadeMs * tempo; MPCSimulacaoModel posicao = Variaveis.OperacaoEmAndamento.Controle.SimulacaoMPC[1]; GPSModel novaPosicao = new GPSModel() { Latitude = posicao.latitude, Longitude = posicao.longitude, OrientacaoReal = posicao.orientacao, Distancia = distancia, }; return novaPosicao; } return null; } private string DadosPosicao(GPSModel pos) { if (pos == null) return ""; return $"Lat: {pos.Latitude}, Long: {pos.Longitude}, Theta: {pos.OrientacaoReal}, Distancia: {pos.Distancia}"; } private void btnIniciarSimulacao_Click(object sender, EventArgs e) { if (!Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Any()) { MessageBox.Show("Selecione as ruas para realizar a operação!"); return; } if (!Variaveis.OperacaoEmAndamento.Iniciado && !parar) { Variaveis.OperacaoEmAndamento.IniciarSimulacao(); } btnIniciarSimulacao.Text = Variaveis.OperacaoEmAndamento.Iniciado ? "Parar Simulação" : "Iniciar Simulação"; if (Variaveis.OperacaoEmAndamento.Iniciado) { GPSService.PenultimaLeitura.Latitude = GPSService.UltimaLeitura.Latitude; GPSService.PenultimaLeitura.Longitude = GPSService.UltimaLeitura.Longitude; //Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica(); AtualizarDadosTela(); } } private void btnCarregarMapa_Click(object sender, EventArgs e) { Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click((Control)sender, e); } private void btnCarregarOperacao_Click(object sender, EventArgs e) { passosOperacao = new List(); txtTempoGPS.Text = tempoGps.ToString(); idxPassoOperacao = 0; lblCarregado.Text = "Simulador"; OpenFileDialog ofd = new OpenFileDialog(); ofd.Filter = "Arquivos de Operação|*.lgop"; ofd.InitialDirectory = Application.StartupPath + "\\" + Variaveis.CaminhoOperacoes; if (ofd.ShowDialog() == DialogResult.OK) { OperacaoDadosModel data = JsonConvert.DeserializeObject(File.ReadAllText(ofd.FileName)); passosOperacao = JsonConvert.DeserializeObject(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, T_Code.Gps.ToString())).ToList(); txtTempoGPS.Text = "1000"; lblCarregado.Text = "Operação"; } } private void btnCarregarTrajetoria_Click(object sender, EventArgs e) { passosOperacao = new List(); txtTempoGPS.Text = tempoGps.ToString(); idxPassoOperacao = 0; lblCarregado.Text = "Simulador"; OpenFileDialog ofd = new OpenFileDialog(); ofd.Filter = "Arquivos de Trajetória|*.json"; ofd.InitialDirectory = Application.StartupPath + "\\" + Variaveis.CaminhoMapasConvertidos; if (ofd.ShowDialog() == DialogResult.OK) { var DadosMapa = JsonConvert.DeserializeObject(File.ReadAllText(ofd.FileName)); passosOperacao = DadosMapa.features[0].geometry.coordinates .Select(x => new GPSModel() { Latitude = x[1], Longitude = x[0] }) .ToList(); txtTempoGPS.Text = "200"; lblCarregado.Text = "Trajetória"; } } private void pnlOrientacaoTrajeto_Paint(object sender, PaintEventArgs e) { try { double angulo = Variaveis.OperacaoEmAndamento.Trajetoria.AnguloCaminho; double anguloCorredor = Variaveis.OperacaoEmAndamento.Trajetoria.AnguloMedioCorredor; FuncoesGlobais.DesenharInclinacao((Panel)sender, e, (float)angulo, 90, (float)anguloCorredor); } catch { } } private void pnlOrientacaoCarro_Paint(object sender, PaintEventArgs e) { try { double angulo = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps?.AnguloCarroDefinido ?? 0; FuncoesGlobais.DesenharInclinacao((Panel)sender, e, (float)angulo, 90); } catch { } } private void pnlAnguloControle_Paint(object sender, PaintEventArgs e) { try { double angulo = Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle; double dif = Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria.ErroOrientacaoAngularProximoPonto; FuncoesGlobais.DesenharInclinacao((Panel)sender, e, (float)angulo, 90, (float)dif); } catch { } } private void btnAtualizarLeitura_Click(object sender, EventArgs e) { AtualizarDadosTela(); } private void btnCalcularAngulo_Click(object sender, EventArgs e) { //Variaveis.OperacaoEmAndamento.OpMapaGPS.CalculaDadosMovimentacaoAutonoma(); int errosConsiderar = int.Parse(txtQtdErro.Text); double anguloControle = Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle; if (historicoAngulosControle.Count >= errosConsiderar || errosConsiderar == 0) historicoAngulosControle.Dequeue(); // Remove o valor mais antigo historicoAngulosControle.Enqueue(anguloControle); double anguloAtrasado = errosConsiderar == 0 ? anguloControle : historicoAngulosControle.Peek(); // Valor de 5 ciclos atrás txtAnguloControle.Text = anguloAtrasado.ToString("0.00"); } private void chbSonarVirando_CheckedChanged(object sender, EventArgs e) { } private void chbAutomatico_CheckedChanged(object sender, EventArgs e) { UltimaAtualizacaoGps = DateTime.MinValue; } private void btnReiniciar_Click(object sender, EventArgs e) { Variaveis.OperacaoEmAndamento.ReiniciarSimulacao(); frmSimulacaoMapaGPS frm = new frmSimulacaoMapaGPS(); frm.Show(); this.Close(); } private void cmbTipoControleDirecional_SelectedIndexChanged(object sender, EventArgs e) { Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional = (TiposControladorDirecional)cmbTipoControleDirecional.SelectedIndex; } private void cmbStatusCorredor_SelectedIndexChanged(object sender, EventArgs e) { RedisService.AtualizarCampos( CtxKey.DadosVisualWorker, ("segmentacao.status_corredor", cmbStatusCorredor.SelectedIndex) ); } } }