diff --git a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs index dfc2d723d..ffe742810 100644 --- a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs +++ b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs @@ -4,11 +4,13 @@ using Newtonsoft.Json; using Newtonsoft.Json.Linq; using System; using System.Collections.Generic; +using System.Threading; using System.IO; using System.Linq; using System.Threading.Tasks; using System.Windows.Forms; using static AgroBase.Models.Enums; +using System.Diagnostics; namespace AgroBase.Forms { @@ -31,6 +33,15 @@ namespace AgroBase.Forms List simulacao = new List(); + + private int _atualizacaoTelaPendente; + private int _tickEmExecucao; + + private readonly Stopwatch _cronometroTela = Stopwatch.StartNew(); + + private const int IntervaloTelaMs = 100; // 10 FPS para a UI + + public frmSimulacaoMapaGPS() { InitializeComponent(); @@ -70,58 +81,96 @@ namespace AgroBase.Forms private void frmSimulacaoMapaGPS_FormClosing(object sender, FormClosingEventArgs e) { tmrLeitura?.Dispose(); + MapaDinamico?.Dispose(); Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.OperacaoIniciada = 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) + if (Interlocked.Exchange(ref _tickEmExecucao, 1) == 1) + return; + + try { - tempoGps = tempo; + int tempo = 0; + bool automatico = false; + + // Esses componentes pertencem à UI. + if (!IsDisposed && IsHandleCreated) + { + Invoke(new Action(() => + { + int.TryParse(txtTempoGPS.Text, out tempo); + automatico = chbAutomatico.Checked; + })); + } + + if (tempo > 0) + tempoGps = tempo; + + if (automatico) + { + // Temporariamente mantém o passo na UI porque o método ainda + // acessa vários TextBox diretamente. + Invoke(new Action(() => + { + btnAcrescentarGPS_Click( + btnAcrescentarGPS, + EventArgs.Empty + ); + })); + } + + if (_cronometroTela.ElapsedMilliseconds >= IntervaloTelaMs) + { + SolicitarAtualizacaoTela(); + _cronometroTela.Restart(); + } + + // Não reduza para 1 ms quando estiver atrasado. + tmrLeitura.SetInterval(Math.Max((int)tempoGps, 10)); + } + finally + { + Interlocked.Exchange(ref _tickEmExecucao, 0); } - if (!AtualizandoTela) + await Task.CompletedTask; + } + private void SolicitarAtualizacaoTela() + { + if (IsDisposed || !IsHandleCreated) + return; + + if (Interlocked.Exchange(ref _atualizacaoTelaPendente, 1) == 1) + return; + + BeginInvoke(new Action(() => { - Func funcAtt = new Func(() => + try { 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); + } + catch (Exception ex) + { + Variaveis.MostrarLog( + "[Simulacao] Erro ao atualizar tela: " + ex + ); + } + finally + { + Interlocked.Exchange(ref _atualizacaoTelaPendente, 0); + } + })); } - - bool AtualizandoTela = false; private void AtualizarDadosTela() { - if (AtualizandoTela) - return; - AtualizandoTela = true; - var _Controle = Variaveis.OperacaoEmAndamento.Controle; var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento; var _Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria; if (_Trajetoria == null) { - AtualizandoTela = false; return; } @@ -132,13 +181,11 @@ namespace AgroBase.Forms if (_Trajetoria == null) { Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas(); - AtualizandoTela = false; return; } if (_Sensoriamento.Trajetoria == null) { - AtualizandoTela = false; return; } @@ -202,8 +249,6 @@ namespace AgroBase.Forms Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas(); } - - AtualizandoTela = false; } @@ -239,20 +284,36 @@ namespace AgroBase.Forms private void AtualizarPosicaoGPS() { - GPSService.PenultimaLeitura = new GPSModel() - { - DataHora = GPSService.UltimaLeitura.DataHora, - Longitude = GPSService.UltimaLeitura.Longitude, - LongitudeAnt = GPSService.UltimaLeitura.LongitudeAnt, - Latitude = GPSService.UltimaLeitura.Latitude, - LatitudeAnt = GPSService.UltimaLeitura.LatitudeAnt, - Heartbeat = GPSService.UltimaLeitura.Heartbeat, - OrientacaoReal = GPSService.UltimaLeitura.OrientacaoReal - }; - if (!GPSService.Iniciado || (txtLongitude.Text != "" && txtLatitude.Text != "")) { - GPSService.UltimaLeitura = posicaoAtual; + double orientacaoReal = GPSUtils.NormalizarAngulo(posicaoAtual.OrientacaoReal); + + GPSService.UltimaLeitura.Momento = posicaoAtual.Momento; + GPSService.UltimaLeitura.DataHora = posicaoAtual.DataHora; + GPSService.UltimaLeitura.UltimoComandoRespondido = posicaoAtual.UltimoComandoRespondido; + + GPSService.UltimaLeitura.Latitude = posicaoAtual.Latitude; + GPSService.UltimaLeitura.Longitude = posicaoAtual.Longitude; + GPSService.UltimaLeitura.LatitudeAnt = posicaoAtual.LatitudeAnt; + GPSService.UltimaLeitura.LongitudeAnt = posicaoAtual.LongitudeAnt; + + GPSService.UltimaLeitura.OrientacaoReal = orientacaoReal; + GPSService.UltimaLeitura.AnguloCarroDefinido = orientacaoReal; + GPSService.UltimaLeitura.TipoOrientacao = "A"; + + GPSService.UltimaLeitura.OrientacaoMovimento = + double.IsNaN(posicaoAtual.OrientacaoMovimento) || double.IsInfinity(posicaoAtual.OrientacaoMovimento) + ? orientacaoReal + : GPSUtils.NormalizarAngulo(posicaoAtual.OrientacaoMovimento); + + GPSService.UltimaLeitura.Distancia = posicaoAtual.Distancia; + GPSService.UltimaLeitura.Velocidade = posicaoAtual.Velocidade; + + GPSService.UltimaLeitura.TimestampOri = posicaoAtual.TimestampOri.Clone(); + GPSService.UltimaLeitura.TimestampPos = posicaoAtual.TimestampPos.Clone(); + + GPSService.UltimaLeitura.Heartbeat = posicaoAtual.Heartbeat; + GPSService.UltimaLeitura.Inicializado = true; } GPSService.AtualizarAtrasoPosicoes(Convert.ToInt32(txtDelayPosicao.Text)); @@ -277,7 +338,7 @@ namespace AgroBase.Forms // Obtém os parâmetros double anguloAtual = double.Parse(txtAnguloGPS.Text); - var _Gps = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps; + var _Gps = GPSService.UltimaLeitura; double anguloControle = 0.0; TipoMovimentoDirecional tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento; @@ -328,8 +389,8 @@ namespace AgroBase.Forms txtAnguloGPS.Text = posicaoAtual.OrientacaoReal.ToString("0.00"); txtVelocidade.Text = veloccidade.ToString("0.00"); txtDistanciaGPS.Text = posicaoAtual.Distancia.ToString("0.0000"); - txtLatitude.Text = posicaoAtual.Latitude.ToString(); - txtLongitude.Text = posicaoAtual.Longitude.ToString(); + txtLatitude.Text = posicaoAtual.LatitudeAnt.ToString(); + txtLongitude.Text = posicaoAtual.LongitudeAnt.ToString(); UltimaAtualizacaoGps = DateTime.Now; @@ -570,7 +631,7 @@ namespace AgroBase.Forms Heartbeat = GPSService.UltimaLeitura.Heartbeat, OrientacaoReal = double.Parse(txtAnguloGPS.Text.Replace(".", ",")), }; - GPSService.EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.AnguloCarroDefinido, false, 0x01); + GPSService.EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.AnguloCarroDefinido, false, 0x02); } private void btnLiberarCorredor_Click(object sender, EventArgs e) diff --git a/AgroBase/AgroBase/Models/MapaDinamicoModel.cs b/AgroBase/AgroBase/Models/MapaDinamicoModel.cs index 90d895550..9719d1dd6 100644 --- a/AgroBase/AgroBase/Models/MapaDinamicoModel.cs +++ b/AgroBase/AgroBase/Models/MapaDinamicoModel.cs @@ -2,745 +2,1785 @@ using System; using System.Collections.Generic; using System.Drawing; -using System.Linq; +using System.Drawing.Drawing2D; +using System.Reflection; +using System.Threading; using System.Windows.Forms; namespace AgroBase.Models { - public class MapaDinamicoModel + /// + /// Renderizador leve do mapa dinâmico. + /// + /// Princípios: + /// 1. AtualizarDados cria um snapshot próprio dos dados. + /// 2. O Paint nunca percorre listas vivas da operação. + /// 3. Nenhuma lista ou modelo temporário é criado dentro do Paint. + /// 4. Recursos GDI são reutilizados e descartados no Dispose. + /// 5. Invalidate é agrupado para não inundar a thread da interface. + /// + public sealed class MapaDinamicoModel : IDisposable { - private double _zoom = 1000000.0; // Nível de zoom - private const double ZoomFactor = 1.1; // Fator de zoom - private bool _isDragging = false; // Controle de arrasto - private PointF _startPoint = new PointF(); // Posição inicial do mouse - private PointF _mapOffset = new PointF(); // Deslocamento do mapa + #region Configurações - private GPSModel _posicaoAtual { get; set; } = new GPSModel(); - private List> RuasPlantacao { get; set; } = new List>(); - private List TrajetoriaDinamica { get; set; } = new List(); - private List TrajetoriaRuaProjetada { get; set; } = new List(); - private List TrajetoriaRobo { get; set; } = new List(); - public List TrajetoriaSimuladaMPC { get; set; } = new List(); - private Obstaculo obstaculoDetectado { get; set; } - private float AnguloCaminho { get; set; } - private float AnguloCarro { get; set; } + private const double ZoomInicial = 1_000_000.0; + private const double ZoomMinimo = 10_000.0; + private const double ZoomMaximo = 100_000_000.0; + private const double FatorZoom = 1.1; - private Panel pnlZoomMapa; - private Form parent; + private const int MaxPontosTrilhaMapa = 2_000; - // toggles e estado - private bool _travarNoRobo = true; // follow cam - private bool _suavizarFollow = true; // suaviza o panning - private float _followAlpha = 0.25f; // 0.15–0.35 bom - private PointF _userPan = PointF.Empty; // pan manual quando _travarNoRobo=false - private PointF _panStart; + // Acima dessa quantidade, reduz os pontos visuais. + // As linhas continuam sendo desenhadas normalmente. + private const int LimitePontosVisuais = 500; - private static float Lerp(float a, float b, float t) => a + (b - a) * t; - private static PointF Lerp(PointF a, PointF b, float t) => new PointF(Lerp(a.X, b.X, t), Lerp(a.Y, b.Y, t)); + private const float TamanhoPonto = 5f; - public MapaDinamicoModel(Form _parent, Panel pnl) + #endregion + + #region Estado e sincronização + + private readonly object _stateLock = new object(); + + private RenderState _state = RenderState.Empty; + + private readonly Panel _painel; + private readonly Form _parent; + + private double _zoom = ZoomInicial; + + private bool _disposed; + private int _invalidatePendente; + + #endregion + + #region Navegação do mapa + + private bool _arrastando; + private PointF _inicioArrasto; + private PointF _inicioPan; + + private PointF _mapOffset; + private PointF _userPan; + + private bool _travarNoRobo = true; + private bool _suavizarFollow = true; + + private float _followAlpha = 0.25f; + + #endregion + + #region Recursos gráficos reutilizados + + private readonly Pen _penRuaPlantacao; + private readonly Pen _penTrajetoriaDinamica; + private readonly Pen _penRuaProjetada; + private readonly Pen _penTrilhaRobo; + private readonly Pen _penMpc; + private readonly Pen _penRobo; + private readonly Pen _penFrenteRobo; + + private readonly Pen _penAntenaPrevista; + private readonly Pen _penAntenaCorrigida; + private readonly Pen _penAntenaBruta; + private readonly Pen _penPrevistaCorrigida; + private readonly Pen _penPrevistaBruta; + + private readonly Pen _penMargemPadrao; + private readonly Pen _penMargemRobo; + private readonly Pen _penMargemBorda; + private readonly Pen _penMargemRua; + private readonly Pen _penMargemCurva; + + private readonly Brush _brushRuaPlantacao; + private readonly Brush _brushTrajetoriaDinamica; + private readonly Brush _brushRuaProjetada; + private readonly Brush _brushTrilhaRobo; + private readonly Brush _brushMpc; + + private readonly Brush _brushCentroRobo; + private readonly Brush _brushAntenaPrevista; + private readonly Brush _brushAntenaCorrigida; + private readonly Brush _brushAntenaBruta; + + #endregion + + #region Compatibilidade pública + + private List _trajetoriaSimuladaMpc = + new List(); + + /// + /// Mantida para compatibilidade com código externo. + /// O renderizador não utiliza diretamente essa referência. + /// + public List TrajetoriaSimuladaMPC { - this.pnlZoomMapa = pnl; - - this.pnlZoomMapa.Paint += PnlZoomMapa_Paint; - this.pnlZoomMapa.MouseWheel += PnlZoomMapa_MouseWheel; - this.pnlZoomMapa.MouseDown += PnlZoomMapa_MouseDown; - this.pnlZoomMapa.MouseMove += PnlZoomMapa_MouseMove; - this.pnlZoomMapa.MouseUp += PnlZoomMapa_MouseUp; - - parent = _parent; - - pnlZoomMapa.GetType().GetMethod("SetStyle", System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic).Invoke(pnlZoomMapa, new object[] { ControlStyles.UserPaint | ControlStyles.AllPaintingInWmPaint | ControlStyles.OptimizedDoubleBuffer, true }); + get + { + lock (_stateLock) + { + return new List(_trajetoriaSimuladaMpc); + } + } + set + { + lock (_stateLock) + { + _trajetoriaSimuladaMpc = + value != null + ? new List(value) + : new List(); + } + } } + #endregion + + public MapaDinamicoModel(Form parent, Panel painel) + { + if (parent == null) + throw new ArgumentNullException(nameof(parent)); + + if (painel == null) + throw new ArgumentNullException(nameof(painel)); + + _parent = parent; + _painel = painel; + + ConfigurarDoubleBuffer(_painel); + + _painel.Paint += PnlZoomMapa_Paint; + _painel.MouseWheel += PnlZoomMapa_MouseWheel; + _painel.MouseDown += PnlZoomMapa_MouseDown; + _painel.MouseMove += PnlZoomMapa_MouseMove; + _painel.MouseUp += PnlZoomMapa_MouseUp; + _painel.MouseLeave += PnlZoomMapa_MouseLeave; + + _penRuaPlantacao = new Pen(Color.Black, 1.2f); + _penTrajetoriaDinamica = new Pen(Color.MediumPurple, 1.5f); + _penRuaProjetada = new Pen(Color.Blue, 1.5f); + _penTrilhaRobo = new Pen(Color.OrangeRed, 1.8f); + + _penMpc = new Pen(Color.LimeGreen, 2f) + { + DashStyle = DashStyle.Dash + }; + + _penRobo = new Pen(Color.ForestGreen, 2f); + _penFrenteRobo = new Pen(Color.ForestGreen, 2f); + + _penAntenaPrevista = new Pen(Color.DeepSkyBlue, 1.5f) + { + DashStyle = DashStyle.Dash + }; + + _penAntenaCorrigida = new Pen(Color.LimeGreen, 1.5f); + + _penAntenaBruta = new Pen(Color.OrangeRed, 1f) + { + DashStyle = DashStyle.Dot + }; + + _penPrevistaCorrigida = new Pen(Color.MediumPurple, 1f) + { + DashStyle = DashStyle.Dash + }; + + _penPrevistaBruta = new Pen(Color.SandyBrown, 1f) + { + DashStyle = DashStyle.Dash + }; + + _penMargemPadrao = CriarPenMargem(Color.Purple); + _penMargemRobo = CriarPenMargem(Color.Red); + _penMargemBorda = CriarPenMargem(Color.Blue); + _penMargemRua = CriarPenMargem(Color.Green); + _penMargemCurva = CriarPenMargem(Color.Orange); + + _brushRuaPlantacao = new SolidBrush(Color.Blue); + _brushTrajetoriaDinamica = new SolidBrush(Color.Purple); + _brushRuaProjetada = new SolidBrush(Color.Green); + _brushTrilhaRobo = new SolidBrush(Color.Orange); + _brushMpc = new SolidBrush(Color.LimeGreen); + + _brushCentroRobo = new SolidBrush(Color.WhiteSmoke); + _brushAntenaPrevista = new SolidBrush(Color.DeepSkyBlue); + _brushAntenaCorrigida = new SolidBrush(Color.LimeGreen); + _brushAntenaBruta = new SolidBrush(Color.OrangeRed); + } + + #region API pública + public void AtualizarDados( - Obstaculo obstaculo, - float anguloCaminho, - float anguloCarro, - GPSModel posicaoAtual, - List trajetoriaDinamica, - List trajetoriaRua, - List trajetoriaRobo, + Obstaculo obstaculo, + float anguloCaminho, + float anguloCarro, + GPSModel posicaoAtual, + List trajetoriaDinamica, + List trajetoriaRua, + List trajetoriaRobo, List> ruasPlantacao, List trajetoriaSimulada = null) { - obstaculoDetectado = obstaculo; - AnguloCaminho = anguloCaminho; - AnguloCarro = anguloCarro; - _posicaoAtual = posicaoAtual; - TrajetoriaDinamica = trajetoriaDinamica; - TrajetoriaRuaProjetada = trajetoriaRua; - TrajetoriaRobo = trajetoriaRobo; - RuasPlantacao = ruasPlantacao; - TrajetoriaSimuladaMPC = trajetoriaSimulada ?? new List(); + if (_disposed) + return; - pnlZoomMapa.Invalidate(); + RenderState novoEstado; + + try + { + novoEstado = CriarSnapshot( + obstaculo, + anguloCaminho, + anguloCarro, + posicaoAtual, + trajetoriaDinamica, + trajetoriaRua, + trajetoriaRobo, + ruasPlantacao, + trajetoriaSimulada + ); + } + catch (Exception ex) + { + Variaveis.MostrarLog( + "[MapaDinamicoModel.AtualizarDados] " + + "Erro ao criar snapshot: " + ex + ); + + return; + } + + if (novoEstado == null || !novoEstado.Posicao.Valida) + { + return; + } + + lock (_stateLock) + { + _state = novoEstado; + + _trajetoriaSimuladaMpc = + trajetoriaSimulada != null + ? CopiarListaSegura(trajetoriaSimulada) + : new List(); + } + + SolicitarRedesenho(); } public void AlterarTravaRover(bool travar) { + if (_disposed) + return; + if (_travarNoRobo && !travar) - { _userPan = _mapOffset; - } _travarNoRobo = travar; + + SolicitarRedesenho(); } - private void PnlZoomMapa_MouseUp(object sender, MouseEventArgs e) + public void AlterarSuavizacaoFollow(bool suavizar) { - if (e.Button == MouseButtons.Left) + _suavizarFollow = suavizar; + SolicitarRedesenho(); + } + + public void AlterarFollowAlpha(float alpha) + { + _followAlpha = Limitar(alpha, 0.01f, 1f); + } + + public void ResetarVisualizacao() + { + _zoom = ZoomInicial; + + _mapOffset = PointF.Empty; + _userPan = PointF.Empty; + + SolicitarRedesenho(); + } + + #endregion + + #region Snapshot + + private static RenderState CriarSnapshot( + Obstaculo obstaculo, + float anguloCaminho, + float anguloCarro, + GPSModel posicaoAtual, + List trajetoriaDinamica, + List trajetoriaRua, + List trajetoriaRobo, + List> ruasPlantacao, + List trajetoriaSimulada) + { + PosicaoRender posicao = CriarSnapshotPosicao(posicaoAtual); + + TrajectoryRenderPoint[] dinamica = + CriarSnapshotTrajetoria(trajetoriaDinamica); + + TrajectoryRenderPoint[] ruaProjetada = + CriarSnapshotTrajetoria(trajetoriaRua); + + GeoRenderPoint[] trilha = + CriarSnapshotTrilha( + trajetoriaRobo, + MaxPontosTrilhaMapa + ); + + GeoRenderPoint[][] ruas = + CriarSnapshotRuas(ruasPlantacao); + + GeoRenderPoint[] mpc = + CriarSnapshotMpc(trajetoriaSimulada); + + return new RenderState( + obstaculo, + anguloCaminho, + anguloCarro, + posicao, + dinamica, + ruaProjetada, + trilha, + ruas, + mpc + ); + } + + private static PosicaoRender CriarSnapshotPosicao(GPSModel origem) + { + if (origem == null) + return PosicaoRender.Empty; + + double latitudeAtual = origem.Latitude; + double longitudeAtual = origem.Longitude; + + // Sem uma posição atual real, não existe referência segura + // para desenhar o mapa. + if (!CoordenadaGeograficaValida( + latitudeAtual, + longitudeAtual)) { - _isDragging = false; // Termina o arrasto - parent.Cursor = Cursors.Default; + return PosicaoRender.Empty; } + + double origemLatitude; + double origemLongitude; + + // Usa Lat0/Lon0 somente quando o PAR é realmente válido. + if (CoordenadaGeograficaValida( + origem.Lat0, + origem.Lon0)) + { + origemLatitude = origem.Lat0; + origemLongitude = origem.Lon0; + } + else + { + // Fallback seguro: posição atual. + origemLatitude = latitudeAtual; + origemLongitude = longitudeAtual; + } + + double latitudeAntenaBruta; + double longitudeAntenaBruta; + + if (CoordenadaGeograficaValida( + origem.LatitudeAnt, + origem.LongitudeAnt)) + { + latitudeAntenaBruta = origem.LatitudeAnt; + longitudeAntenaBruta = origem.LongitudeAnt; + } + else + { + latitudeAntenaBruta = latitudeAtual; + longitudeAntenaBruta = longitudeAtual; + } + + return new PosicaoRender( + latitudeAtual, + longitudeAtual, + latitudeAntenaBruta, + longitudeAntenaBruta, + origemLatitude, + origemLongitude + ); } - private void PnlZoomMapa_MouseMove(object sender, MouseEventArgs e) + private static bool CoordenadaGeograficaValida(double latitude, double longitude) { - var panel1 = (Panel)sender; - - if (_isDragging) + if (double.IsNaN(latitude) || + double.IsInfinity(latitude) || + double.IsNaN(longitude) || + double.IsInfinity(longitude)) { - if (!_travarNoRobo) + return false; + } + + if (latitude < -90.0 || latitude > 90.0) + return false; + + if (longitude < -180.0 || longitude > 180.0) + return false; + + // No seu sistema, 0,0 representa GPS ainda não inicializado. + if (Math.Abs(latitude) < 0.0000001 && + Math.Abs(longitude) < 0.0000001) + { + return false; + } + + return true; + } + + private static TrajectoryRenderPoint[] CriarSnapshotTrajetoria( + IList origem) + { + if (origem == null || origem.Count == 0) + return Array.Empty(); + + for (int tentativa = 0; tentativa < 3; tentativa++) + { + try { - var dx = e.X - _startPoint.X; - var dy = e.Y - _startPoint.Y; + int count = origem.Count; - _userPan = new PointF(_panStart.X + dx, _panStart.Y + dy); - _mapOffset = _userPan; // feedback imediato + var resultado = + new TrajectoryRenderPoint[count]; - panel1.Invalidate(); - parent.Cursor = Cursors.Cross; + int gravados = 0; + + for (int i = 0; i < count; i++) + { + PontoTrajetoriaModel ponto = origem[i]; + + if (ponto == null || ponto.Posicao == null) + continue; + + resultado[gravados++] = + new TrajectoryRenderPoint( + ponto.Posicao.Latitude, + ponto.Posicao.Longitude, + ponto.Tipo, + ponto.DistanciaMargem, + ponto.PontoLigacao, + ponto.PontoBorda + ); + } + + if (gravados == resultado.Length) + return resultado; + + Array.Resize(ref resultado, gravados); + return resultado; } - // Se quiser permitir “pan” mesmo com follow ON, - // você pode: (a) ignorar, ou (b) desligar o follow ao iniciar o drag. - } - } - - private void PnlZoomMapa_MouseDown(object sender, MouseEventArgs e) - { - if (e.Button == MouseButtons.Left) - { - if (_travarNoRobo) + catch (ArgumentOutOfRangeException) { - // congela a vista atual e desliga follow automaticamente - _userPan = _mapOffset; - _travarNoRobo = false; + Thread.Yield(); + } + catch (InvalidOperationException) + { + Thread.Yield(); } - - _isDragging = true; - _startPoint = e.Location; // Salva a posição inicial do mouse - _panStart = _userPan; // baseline do pan - parent.Cursor = Cursors.Hand; } + + return Array.Empty(); } - private void PnlZoomMapa_MouseWheel(object sender, MouseEventArgs e) + private static GeoRenderPoint[] CriarSnapshotTrilha( + IList origem, + int limite) { - Panel panel1 = (Panel)sender; + if (origem == null || origem.Count == 0) + return Array.Empty(); - if (e.Delta > 0) + for (int tentativa = 0; tentativa < 3; tentativa++) { - _zoom *= ZoomFactor; // Aumenta o zoom - } - else if (e.Delta < 0) - { - _zoom /= ZoomFactor; // Diminui o zoom + try + { + int count = origem.Count; + int inicio = Math.Max(0, count - limite); + + var resultado = + new GeoRenderPoint[count - inicio]; + + int gravados = 0; + + for (int i = inicio; i < count; i++) + { + GPSModel ponto = origem[i]; + + if (ponto == null) + continue; + + resultado[gravados++] = + new GeoRenderPoint( + ponto.Latitude, + ponto.Longitude + ); + } + + if (gravados == resultado.Length) + return resultado; + + Array.Resize(ref resultado, gravados); + return resultado; + } + catch (ArgumentOutOfRangeException) + { + Thread.Yield(); + } + catch (InvalidOperationException) + { + Thread.Yield(); + } } - panel1.Invalidate(); + return Array.Empty(); } - private void PnlZoomMapa_Paint_bkp(object sender, PaintEventArgs e) + private static GeoRenderPoint[][] CriarSnapshotRuas( + IList> origem) { + if (origem == null || origem.Count == 0) + return Array.Empty(); + + for (int tentativa = 0; tentativa < 3; tentativa++) + { + try + { + int count = origem.Count; + + var resultado = + new GeoRenderPoint[count][]; + + for (int i = 0; i < count; i++) + { + resultado[i] = + CriarSnapshotTrilha( + origem[i], + int.MaxValue + ); + } + + return resultado; + } + catch (ArgumentOutOfRangeException) + { + Thread.Yield(); + } + catch (InvalidOperationException) + { + Thread.Yield(); + } + } + + return Array.Empty(); + } + + private static GeoRenderPoint[] CriarSnapshotMpc( + IList origem) + { + if (origem == null || origem.Count == 0) + return Array.Empty(); + + for (int tentativa = 0; tentativa < 3; tentativa++) + { + try + { + int count = origem.Count; + + var resultado = + new GeoRenderPoint[count]; + + for (int i = 0; i < count; i++) + { + MPCSimulacaoModel ponto = origem[i]; + + resultado[i] = + new GeoRenderPoint( + ponto.latitude, + ponto.longitude + ); + } + + return resultado; + } + catch (ArgumentOutOfRangeException) + { + Thread.Yield(); + } + catch (InvalidOperationException) + { + Thread.Yield(); + } + } + + return Array.Empty(); + } + + private static List CopiarListaSegura(IList origem) + { + if (origem == null || origem.Count == 0) + return new List(); + + for (int tentativa = 0; tentativa < 3; tentativa++) + { + try + { + int count = origem.Count; + var resultado = new List(count); + + for (int i = 0; i < count; i++) + resultado.Add(origem[i]); + + return resultado; + } + catch (ArgumentOutOfRangeException) + { + Thread.Yield(); + } + catch (InvalidOperationException) + { + Thread.Yield(); + } + } + + return new List(); + } + + #endregion + + #region Desenho principal + + private void PnlZoomMapa_Paint( + object sender, + PaintEventArgs e) + { + if (_disposed) + return; + + RenderState state; + + lock (_stateLock) + { + state = _state; + } + + if (state == null || !state.Posicao.Valida) + return; + try { - Panel panel1 = (Panel)sender; - Graphics g = e.Graphics; - // Defina as dimensões do panel - int width = panel1.Width; - int height = panel1.Height; + g.SmoothingMode = SmoothingMode.AntiAlias; + g.PixelOffsetMode = PixelOffsetMode.HighQuality; - // Coordenadas centrais (onde o robô estará), ajustadas pelo deslocamento do mapa - float centerX = width / 2 + _mapOffset.X; - float centerY = height / 2 + _mapOffset.Y; + float baseCenterX = _painel.ClientSize.Width / 2f; + float baseCenterY = _painel.ClientSize.Height / 2f; - // Desenha os pontos da rua com plantacao - foreach (var _rua in RuasPlantacao) - { - var rua = _rua.Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) { Posicao = x }).ToList(); - DrawTrajectory(g, rua, centerX, centerY, Color.Blue, Pens.Black); - } - - // Desenha os pontos da trajetória projetada - DrawTrajectory(g, TrajetoriaDinamica, centerX, centerY, Color.Purple, Pens.MediumPurple, true); - - // Desenha os pontos da trajetória planejada da rua atual - DrawTrajectory(g, TrajetoriaRuaProjetada, centerX, centerY, Color.Green, Pens.Blue); - - // Desenha os pontos da trajetória percorrida - var _trajetoriaRobo = TrajetoriaRobo.Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) { Posicao = x }).ToList(); - DrawTrajectory(g, _trajetoriaRobo, centerX, centerY, Color.Orange, Pens.OrangeRed); - - // Desenha a posição atual do robô (ponto vermelho) - //float PxToCm = DrawRobot(g, centerX, centerY, Color.Red, Color.Red, (float)VariaveisEquipamento.LarguraEsquerda, (float)VariaveisEquipamento.LarguraDireita, (float)VariaveisEquipamento.ComprimentoFrente, (float)VariaveisEquipamento.ComprimentoTras, AnguloCarro); - - // === NOVO: pega pixels do CENTRO (corrigido) e da ANTENA (bruto) === - // Use sua própria função de conversão se já tiver. - //PointF pCtr = LatLonToPanel(_posicaoAtual.Latitude, _posicaoAtual.Longitude, centerX, centerY); - //PointF pAnt = LatLonToPanel(_posicaoAtual.LatitudeAnt, _posicaoAtual.LongitudeAnt, centerX, centerY); - // desenha o robô centrado no ponto corrigido e marca a antena bruta - //float PxToCm = DrawRobotNew( - // g, - // pCtr.X, pCtr.Y, - // Color.ForestGreen, // cor do contorno do corpo - // (float)VariaveisEquipamento.LarguraEsquerda, - // (float)VariaveisEquipamento.LarguraDireita, - // (float)VariaveisEquipamento.ComprimentoFrente, - // (float)VariaveisEquipamento.ComprimentoTras, - // AnguloCarro, // em rad - // pAnt.X, pAnt.Y, // antena "bruta" (opcional) - // desenharAntenaEsperada: true // mostra também a antena “prevista” - //); - - - // 1) centro em pixels (seu “centro de giro” no painel) - PointF pCtr = LatLonToPanel(_posicaoAtual.Latitude, _posicaoAtual.Longitude, centerX, centerY); - // 2) antenas (corrigida e bruta) - double latCorr = _posicaoAtual.Latitude; // já com lever arm aplicado - double lonCorr = _posicaoAtual.Longitude; - double latBrut = _posicaoAtual.LatitudeAnt; // GNGGA bruto - double lonBrut = _posicaoAtual.LongitudeAnt; - AntenaDiagPx diag = DrawRobotAndAntennaDiagnostics( - g, - pCtr.X, pCtr.Y, - AnguloCarro, // em rad conforme seu desenho - (lat, lon) => LatLonToPanel(lat, lon, centerX, centerY), - latCorr, lonCorr, - latBrut, lonBrut, - Color.ForestGreen + AtualizarOffsetFollow( + state, + baseCenterX, + baseCenterY ); - // Se quiser logar os offsets em px: - var offPrev_vs_Corr = new PointF(diag.Corrigida.X - diag.Prevista.X, diag.Corrigida.Y - diag.Prevista.Y); - var offPrev_vs_Brut = new PointF(diag.Bruta.X - diag.Prevista.X, diag.Bruta.Y - diag.Prevista.Y); + float centerX = baseCenterX + _mapOffset.X; + float centerY = baseCenterY + _mapOffset.Y; - if (obstaculoDetectado != null) - { - //DrawObstacle(g, centerX, centerY, Color.Cyan, Color.Cyan, AnguloCaminho - (float)obstaculoDetectado.AnguloParaDesvio, obstaculoDetectado, PxToCm); - } + DesenharRuasPlantacao( + g, + state, + centerX, + centerY + ); - // Desenha a simulação do MPC (linha tracejada verde-limão) - if (TrajetoriaSimuladaMPC != null && TrajetoriaSimuladaMPC.Count > 1) - { - var _trajetoriaSimulada = TrajetoriaSimuladaMPC - .Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) { Posicao = new GPSModel() { Latitude = x.latitude, Longitude = x.longitude } }) - .ToList(); + DesenharTrajetoria( + g, + state.TrajetoriaDinamica, + state.Posicao, + centerX, + centerY, + _penTrajetoriaDinamica, + _brushTrajetoriaDinamica, + desenharMargens: true + ); - Pen penTracejada = new Pen(Color.LimeGreen, 2); - penTracejada.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash; + DesenharTrajetoria( + g, + state.TrajetoriaRuaProjetada, + state.Posicao, + centerX, + centerY, + _penRuaProjetada, + _brushRuaProjetada, + desenharMargens: false + ); - DrawTrajectory(g, _trajetoriaSimulada, centerX, centerY, Color.LimeGreen, penTracejada); - } + DesenharGeoTrajetoria( + g, + state.TrajetoriaRobo, + state.Posicao, + centerX, + centerY, + _penTrilhaRobo, + _brushTrilhaRobo + ); + + DesenharRoboEDiagnostico( + g, + state, + centerX, + centerY + ); + + DesenharGeoTrajetoria( + g, + state.TrajetoriaMpc, + state.Posicao, + centerX, + centerY, + _penMpc, + _brushMpc + ); } catch (Exception ex) { - Variaveis.MostrarLog("[MapaDinamicoModel.PnlZoomMapa_Paint] Erro ao atualizar mapa dinamico: " + ex.Message); + Variaveis.MostrarLog( + "[MapaDinamicoModel.Paint] " + + "Erro ao renderizar mapa: " + ex + ); } } - private void PnlZoomMapa_Paint(object sender, PaintEventArgs e) + private void AtualizarOffsetFollow( + RenderState state, + float baseCenterX, + float baseCenterY) { - try + if (!_travarNoRobo) { - var panel1 = (Panel)sender; - Graphics g = e.Graphics; + _mapOffset = _userPan; + return; + } - float baseCX = panel1.Width / 2f; - float baseCY = panel1.Height / 2f; + PointF roverSemOffset = LatLonToPanel( + state.Posicao.Latitude, + state.Posicao.Longitude, + state.Posicao, + baseCenterX, + baseCenterY + ); - // Centro do robô sem offset (para calcular follow) - PointF pCtrNoOffset = LatLonToPanel(_posicaoAtual.Latitude, _posicaoAtual.Longitude, baseCX, baseCY); + PointF offsetDesejado = new PointF( + baseCenterX - roverSemOffset.X, + baseCenterY - roverSemOffset.Y + ); - // Offset necessário para centralizar o robô - PointF offFollow = new PointF(baseCX - pCtrNoOffset.X, baseCY - pCtrNoOffset.Y); + if (_suavizarFollow) + { + _mapOffset = new PointF( + Lerp( + _mapOffset.X, + offsetDesejado.X, + _followAlpha + ), + Lerp( + _mapOffset.Y, + offsetDesejado.Y, + _followAlpha + ) + ); + } + else + { + _mapOffset = offsetDesejado; + } + } - if (_travarNoRobo) - { - _mapOffset = _suavizarFollow - ? new PointF( - _mapOffset.X + (_followAlpha * (offFollow.X - _mapOffset.X)), - _mapOffset.Y + (_followAlpha * (offFollow.Y - _mapOffset.Y)) - ) - : offFollow; - } - else - { - // mapa travado: usa o pan do usuário; NÃO recalcula aqui - _mapOffset = _userPan; - } + private void DesenharRuasPlantacao( + Graphics g, + RenderState state, + float centerX, + float centerY) + { + GeoRenderPoint[][] ruas = state.RuasPlantacao; - float centerX = baseCX + _mapOffset.X; - float centerY = baseCY + _mapOffset.Y; - - // === desenha tudo usando centerX/centerY === - foreach (var _rua in RuasPlantacao) - { - var rua = _rua.Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) { Posicao = x }).ToList(); - DrawTrajectory(g, rua, centerX, centerY, Color.Blue, Pens.Black); - } - - DrawTrajectory(g, TrajetoriaDinamica, centerX, centerY, Color.Purple, Pens.MediumPurple, true); - DrawTrajectory(g, TrajetoriaRuaProjetada, centerX, centerY, Color.Green, Pens.Blue); - - var _trajetoriaRobo = TrajetoriaRobo.Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) { Posicao = x }).ToList(); - DrawTrajectory(g, _trajetoriaRobo, centerX, centerY, Color.Orange, Pens.OrangeRed); - - // Centro (ENU->px) e antenas - PointF pCtr = LatLonToPanel(_posicaoAtual.Latitude, _posicaoAtual.Longitude, centerX, centerY); - - double latCorr = _posicaoAtual.Latitude; - double lonCorr = _posicaoAtual.Longitude; - double latBrut = _posicaoAtual.LatitudeAnt; - double lonBrut = _posicaoAtual.LongitudeAnt; - - AntenaDiagPx diag = DrawRobotAndAntennaDiagnostics( + for (int i = 0; i < ruas.Length; i++) + { + DesenharGeoTrajetoria( g, - pCtr.X, pCtr.Y, - AnguloCarro, // RAD - (lat, lon) => LatLonToPanel(lat, lon, centerX, centerY), - latCorr, lonCorr, - latBrut, lonBrut, - Color.ForestGreen + ruas[i], + state.Posicao, + centerX, + centerY, + _penRuaPlantacao, + _brushRuaPlantacao + ); + } + } + + private void DesenharTrajetoria( + Graphics g, + TrajectoryRenderPoint[] trajectory, + PosicaoRender posicao, + float centerX, + float centerY, + Pen linePen, + Brush pointBrush, + bool desenharMargens) + { + if (trajectory == null || trajectory.Length == 0) + return; + + int passoPontos = + CalcularPassoPontos(trajectory.Length); + + bool possuiAnterior = false; + PointF anterior = PointF.Empty; + + for (int i = 0; i < trajectory.Length; i++) + { + TrajectoryRenderPoint trajPoint = trajectory[i]; + + if (!CoordenadaValida(trajPoint.Latitude) || + !CoordenadaValida(trajPoint.Longitude)) + { + possuiAnterior = false; + continue; + } + + PointF atual = LatLonToPanel( + trajPoint.Latitude, + trajPoint.Longitude, + posicao, + centerX, + centerY ); - if (TrajetoriaSimuladaMPC != null && TrajetoriaSimuladaMPC.Count > 1) + if (possuiAnterior) + g.DrawLine(linePen, anterior, atual); + + if (desenharMargens && + trajPoint.Tipo != Enums.TipoPontoRua.Indefinido) { - var _trajetoriaSimulada = TrajetoriaSimuladaMPC - .Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) { Posicao = new GPSModel() { Latitude = x.latitude, Longitude = x.longitude } }) - .ToList(); - - var penTracejada = new Pen(Color.LimeGreen, 2) { DashStyle = System.Drawing.Drawing2D.DashStyle.Dash }; - DrawTrajectory(g, _trajetoriaSimulada, centerX, centerY, Color.LimeGreen, penTracejada); + DesenharMargem( + g, + trajPoint, + atual + ); } - } - catch (Exception ex) - { - Variaveis.MostrarLog("[MapaDinamicoModel.PnlZoomMapa_Paint] Erro ao atualizar mapa dinamico: " + ex.Message); - } - } - - private void DrawTrajectory(Graphics g, List trajectory, float centerX, float centerY, Color pointColor, Pen linePen, bool drawMargin = false) - { - if (trajectory == null) - { - trajectory = new List(); - } - - try - { - for (int i = 0; i < trajectory.Count; i++) + if (i % passoPontos == 0 || + i == trajectory.Length - 1) { - var trajPoint = trajectory[i]; - // Calcule a posição no panel - //float x = centerX + (float)((trajPoint.Posicao.Longitude - _posicaoAtual.Longitude) * _zoom); - //float y = centerY - (float)((trajPoint.Posicao.Latitude - _posicaoAtual.Latitude) * _zoom); - PointF p = LatLonToPanel(trajPoint.Posicao.Latitude, trajPoint.Posicao.Longitude, centerX, centerY); - float x = p.X; - float y = p.Y; - - if (drawMargin && trajPoint.Tipo != Enums.TipoPontoRua.Indefinido) - { - double margemMetros = trajPoint.DistanciaMargem * 10; - // Converter metros para pixels usando a mesma escala do robô - float PxToCm = (float)margemMetros * (float)_zoom / 1e7f * 1.19f; - float diametroPixels = ((float)margemMetros * PxToCm); // Conversão correta para pixels - - Color corBorda = - trajPoint.Tipo == Enums.TipoPontoRua.PosicaoRobo ? Color.Red : - trajPoint.PontoLigacao ? Color.Red : - trajPoint.PontoBorda ? Color.Blue : - trajPoint.Tipo == Enums.TipoPontoRua.Rua ? Color.Green : - trajPoint.Tipo == Enums.TipoPontoRua.CruvaEntreCorredores ? Color.Orange : - pointColor; - - // Desenha a margem como um círculo ao redor do ponto - using (Pen margemPen = new Pen(Color.FromArgb(150, corBorda), 1)) // Transparente - { - g.DrawEllipse(margemPen, x - diametroPixels / 2, y - diametroPixels / 2, diametroPixels, diametroPixels); - } - } - - // Desenha o ponto da trajetória - DrawPoint(g, x, y, pointColor); - - // Desenha a linha conectando os pontos - if (i > 0) - { - var prevPoint = trajectory[i - 1]; - //float prevX = centerX + (float)((prevPoint.Posicao.Longitude - _posicaoAtual.Longitude) * _zoom); - //float prevY = centerY - (float)((prevPoint.Posicao.Latitude - _posicaoAtual.Latitude) * _zoom); - PointF pv = LatLonToPanel(prevPoint.Posicao.Latitude, prevPoint.Posicao.Longitude, centerX, centerY); - float prevX = pv.X; - float prevY = pv.Y; - - g.DrawLine(linePen, prevX, prevY, x, y); - } + DesenharPonto( + g, + atual, + pointBrush + ); } - } - catch - { - } - - } - - private void DrawPoint(Graphics g, float x, float y, Color color) - { - float size = 5; - using (Brush brush = new SolidBrush(color)) - { - g.FillEllipse(brush, x - size / 2, y - size / 2, size, size); + anterior = atual; + possuiAnterior = true; } } - private float DrawRobot(Graphics g, float x, float y, Color pointColor, Color borderColor, float larguraEsquerda, float larguraDireita, float comprimentoFrente, float comprimentoTras, float angle) - { - float zoom = (float)_zoom; - - // Calcular as dimensões do robô em centímetros - float larguraTotal = larguraEsquerda + larguraDireita; // Largura total em cm - float comprimentoTotal = comprimentoFrente + comprimentoTras; // Comprimento total em cm - - // Calcular os deslocamentos do GPS em relação ao centro do robô - float deslocamentoX = (larguraTotal / 2.0f) - larguraDireita; // Deslocamento proporcional em X - float deslocamentoY = (comprimentoTotal / 2.0f) - comprimentoFrente; // Deslocamento proporcional em Y - - // Ajustar as dimensões para o espaço de pixels considerando o zoom - float larguraPixel = larguraTotal * zoom / 1e7f; // Escala do zoom em pixels - float comprimentoPixel = comprimentoTotal * zoom / 1e7f; // Escala do zoom em pixels - - // Calcular os deslocamentos em pixels - float deslocamentoXPixel = deslocamentoX * zoom / 1e7f; - float deslocamentoYPixel = deslocamentoY * zoom / 1e7f; - - // Coordenadas ajustadas para o centro do robô - float centroRoboX = x; - float centroRoboY = y; - - // Calcular a posição do ponto GPS em relação ao centro do robô - PointF pontoGPS = CalcularPontoRotacionado(centroRoboX, centroRoboY, deslocamentoXPixel, -deslocamentoYPixel, angle); - - // Calcular o offset para deslocar o conjunto - float offsetX = pontoGPS.X - centroRoboX; - float offsetY = pontoGPS.Y - centroRoboY; - - // Desenhar o ponto GPS - DrawPoint(g, pontoGPS.X - offsetX, pontoGPS.Y - offsetY, pointColor); - - // Calcular os vértices do retângulo representando o robô - PointF[] corners = new PointF[4]; - corners[0] = CalcularPontoRotacionado(centroRoboX - offsetX, centroRoboY - offsetY, -larguraPixel / 2, -comprimentoPixel / 2, angle); - corners[1] = CalcularPontoRotacionado(centroRoboX - offsetX, centroRoboY - offsetY, larguraPixel / 2, -comprimentoPixel / 2, angle); - corners[2] = CalcularPontoRotacionado(centroRoboX - offsetX, centroRoboY - offsetY, larguraPixel / 2, comprimentoPixel / 2, angle); - corners[3] = CalcularPontoRotacionado(centroRoboX - offsetX, centroRoboY - offsetY, -larguraPixel / 2, comprimentoPixel / 2, angle); - - // Desenhar o contorno do robô - using (Pen pen = new Pen(borderColor, 2)) - { - g.DrawPolygon(pen, corners); - } - - // Calcular o ponto final da linha indicando a frente do robô - float linhaFrenteComprimento = comprimentoFrente * zoom / 1e7f; // Ajustar para pixels considerando o zoom - PointF frenteLinha = CalcularPontoRotacionado(pontoGPS.X - offsetX, pontoGPS.Y - offsetY, 0, -linhaFrenteComprimento, angle); - - // Desenhar a linha do GPS até a frente do robô - using (Pen frontPen = new Pen(borderColor, 2)) - { - g.DrawLine(frontPen, pontoGPS.X - offsetX, pontoGPS.Y - offsetY, frenteLinha.X, frenteLinha.Y); - } - - // Calcular a relação pixel para cm baseado na largura total - float PxToCm = larguraTotal / larguraPixel; - - return PxToCm; - } - - private float DrawRobotNew( + private void DesenharGeoTrajetoria( Graphics g, - float xCtr, float yCtr, // centro (corrigido) em pixels - Color bodyBorder, - float larguraEsquerdaCm, float larguraDireitaCm, - float compFrenteCm, float compTrasCm, - float angleRad, // mesmo sistema do seu theta (0=Norte) - float? xAnt = null, float? yAnt = null, // antena bruta (pixels) - opcional - bool desenharAntenaEsperada = false // desenhar antena “prevista” a partir do centro - ) + GeoRenderPoint[] trajectory, + PosicaoRender posicao, + float centerX, + float centerY, + Pen linePen, + Brush pointBrush) { - float zoom = (float)_zoom; + if (trajectory == null || trajectory.Length == 0) + return; - // dimensões em cm - float larguraTotalCm = larguraEsquerdaCm + larguraDireitaCm; - float comprimentoTotalCm = compFrenteCm + compTrasCm; + int passoPontos = + CalcularPassoPontos(trajectory.Length); - // escala -> pixels (mantenha sua convenção) - float cmToPx = zoom / 1e7f; // mesma que você já usa - float larguraPx = larguraTotalCm * cmToPx; - float comprimentoPx = comprimentoTotalCm * cmToPx; + bool possuiAnterior = false; + PointF anterior = PointF.Empty; - // retângulo do robô centrado no (xCtr,yCtr) - PointF[] corners = new PointF[4]; - corners[0] = CalcularPontoRotacionado(xCtr, yCtr, -larguraPx / 2f, -comprimentoPx / 2f, angleRad); - corners[1] = CalcularPontoRotacionado(xCtr, yCtr, +larguraPx / 2f, -comprimentoPx / 2f, angleRad); - corners[2] = CalcularPontoRotacionado(xCtr, yCtr, +larguraPx / 2f, +comprimentoPx / 2f, angleRad); - corners[3] = CalcularPontoRotacionado(xCtr, yCtr, -larguraPx / 2f, +comprimentoPx / 2f, angleRad); - - using (var pen = new Pen(bodyBorder, 2f)) - g.DrawPolygon(pen, corners); - - // setinha de “frente” partindo do centro (opcional) - float frenteLenPx = compFrenteCm * cmToPx; - PointF pontaFrente = CalcularPontoRotacionado(xCtr, yCtr, 0f, -frenteLenPx, angleRad); - using (var penFront = new Pen(bodyBorder, 2f)) - g.DrawLine(penFront, xCtr, yCtr, pontaFrente.X, pontaFrente.Y); - - // === Antena bruta (se veio lat/lon da antena) === - if (xAnt.HasValue && yAnt.HasValue) + for (int i = 0; i < trajectory.Length; i++) { - // ligue centro ↔ antena (visual da alavanca) - using (var penLink = new Pen(Color.DarkGray, 1.5f) { DashStyle = System.Drawing.Drawing2D.DashStyle.Dash }) - g.DrawLine(penLink, xCtr, yCtr, xAnt.Value, yAnt.Value); + GeoRenderPoint trajPoint = trajectory[i]; - // ponto da antena (bruto) - DrawPoint(g, xAnt.Value, yAnt.Value, Color.OrangeRed); // cor diferente + if (!CoordenadaValida(trajPoint.Latitude) || + !CoordenadaValida(trajPoint.Longitude)) + { + possuiAnterior = false; + continue; + } + + PointF atual = LatLonToPanel( + trajPoint.Latitude, + trajPoint.Longitude, + posicao, + centerX, + centerY + ); + + if (possuiAnterior) + g.DrawLine(linePen, anterior, atual); + + if (i % passoPontos == 0 || + i == trajectory.Length - 1) + { + DesenharPonto( + g, + atual, + pointBrush + ); + } + + anterior = atual; + possuiAnterior = true; } - - // === Antena “esperada” calculada a partir do centro (sanity check) === - if (desenharAntenaEsperada) - { - // offsets em cm do CENTRO para a antena (mesma lógica que você já tinha) - float dxCm = (larguraTotalCm / 2f) - larguraDireitaCm; // +esquerda - float dyCm = (comprimentoTotalCm / 2f) - compFrenteCm; // +trás - - // para o sistema de tela, seu Y “frente” foi -dy (igual ao que você fazia) - float dxPx = dxCm * cmToPx; - float dyPx = dyCm * cmToPx; - - PointF antenaPrevista = CalcularPontoRotacionado(xCtr, yCtr, dxPx, -dyPx, angleRad); - DrawPoint(g, antenaPrevista.X, antenaPrevista.Y, Color.DeepSkyBlue); - - // e ligue também, para comparar “previsto” x “bruto” - using (var penLink2 = new Pen(Color.DeepSkyBlue, 1f)) - g.DrawLine(penLink2, xCtr, yCtr, antenaPrevista.X, antenaPrevista.Y); - } - - // relação px->cm (se você usa depois) - float pxToCm = larguraTotalCm / Math.Max(larguraPx, 1e-6f); - return pxToCm; } - // Opcional: um contêiner simples para você logar os pixels depois - public struct AntenaDiagPx - { - public PointF Centro; // centro do robô (retângulo) - public PointF Prevista; // antena prevista a partir do centro (pelas medidas) - public PointF Corrigida; // antena corrigida (lever arm aplicado) - public PointF Bruta; // antena bruta (GNGGA) - } - - // ===================== MÉTODO PRINCIPAL ===================== - public AntenaDiagPx DrawRobotAndAntennaDiagnostics( + private void DesenharMargem( Graphics g, - float xCtrPx, float yCtrPx, // centro do robô em pixels (centro de giro) - float angleRad, // heading (mesmo sistema do seu desenho: 0°=N, Y para "frente" = -Y) - Func latlonToPanel, // conversor lat/lon -> pixels - double latAntCorr, double lonAntCorr, // antena corrigida (lever arm já aplicado) - double latAntBruta, double lonAntBruta, // antena bruta (GNGGA) - Color corBordaRobo - ) + TrajectoryRenderPoint ponto, + PointF local) { - // --- 1) Dimensões vindas do seu equipamento (em cm) --- - float L_esq_cm = (float)VariaveisEquipamento.LarguraEsquerda; - float L_dir_cm = (float)VariaveisEquipamento.LarguraDireita; - float F_cm = (float)VariaveisEquipamento.ComprimentoFrente; - float T_cm = (float)VariaveisEquipamento.ComprimentoTras; + double margemMetros = ponto.DistanciaMargem * 10.0; - float larguraTotalCm = L_esq_cm + L_dir_cm; - float comprimentoTotalCm = F_cm + T_cm; + if (margemMetros <= 0 || + double.IsNaN(margemMetros) || + double.IsInfinity(margemMetros)) + { + return; + } - // --- 2) Conversão cm -> px (mesma convenção que você já usa) --- - float zoom = (float)_zoom; - float cmToPx = zoom / 1e7f; - float larguraPx = Math.Max(1e-6f, larguraTotalCm * cmToPx); - float comprimentoPx = Math.Max(1e-6f, comprimentoTotalCm * cmToPx); + float pxToCm = + (float)margemMetros * + (float)_zoom / + 10_000_000f * + 1.19f; - // --- 3) Desenhar o retângulo do robô centrado no (xCtrPx, yCtrPx) --- - PointF[] corners = new PointF[4]; - corners[0] = CalcularPontoRotacionado(xCtrPx, yCtrPx, -larguraPx / 2f, -comprimentoPx / 2f, angleRad); - corners[1] = CalcularPontoRotacionado(xCtrPx, yCtrPx, +larguraPx / 2f, -comprimentoPx / 2f, angleRad); - corners[2] = CalcularPontoRotacionado(xCtrPx, yCtrPx, +larguraPx / 2f, +comprimentoPx / 2f, angleRad); - corners[3] = CalcularPontoRotacionado(xCtrPx, yCtrPx, -larguraPx / 2f, +comprimentoPx / 2f, angleRad); + float diametroPixels = + (float)margemMetros * pxToCm; - using (var penBody = new Pen(corBordaRobo, 2f)) - g.DrawPolygon(penBody, corners); + if (diametroPixels <= 0.1f || + float.IsNaN(diametroPixels) || + float.IsInfinity(diametroPixels)) + { + return; + } - // Seta da frente (opcional) - float frenteLenPx = F_cm * cmToPx; - PointF pontaFrente = CalcularPontoRotacionado(xCtrPx, yCtrPx, 0f, -frenteLenPx, angleRad); - using (var penFront = new Pen(corBordaRobo, 2f)) - g.DrawLine(penFront, xCtrPx, yCtrPx, pontaFrente.X, pontaFrente.Y); + Pen pen = ObterPenMargem(ponto); - // --- 4) Antena PREVISTA a partir do centro (onde deveria estar na carcaça) --- - // Offsets CENTRO -> ANTENA em cm: - // +dx = esquerda, -dx = direita ; +dy = trás, -dy = frente - float dxCm = (larguraTotalCm / 2f) - L_dir_cm; - float dyCm = (comprimentoTotalCm / 2f) - F_cm; + g.DrawEllipse( + pen, + local.X - (diametroPixels / 2f), + local.Y - (diametroPixels / 2f), + diametroPixels, + diametroPixels + ); + } - // Para a tela: "frente" é -dy (mesma convenção do seu código) - float dxPx = dxCm * cmToPx; - float dyPx = dyCm * cmToPx; + private Pen ObterPenMargem( + TrajectoryRenderPoint ponto) + { + if (ponto.Tipo == Enums.TipoPontoRua.PosicaoRobo) + return _penMargemRobo; - PointF antenaPrevistaPx = CalcularPontoRotacionado(xCtrPx, yCtrPx, dxPx, dyPx, angleRad); + if (ponto.PontoLigacao) + return _penMargemRobo; - // --- 5) Antena CORRIGIDA (lat/lon após lever arm) --- - PointF antenaCorrigidaPx = latlonToPanel(latAntCorr, lonAntCorr); + if (ponto.PontoBorda) + return _penMargemBorda; - // --- 6) Antena BRUTA (lat/lon direto do GNGGA) --- - PointF antenaBrutaPx = latlonToPanel(latAntBruta, lonAntBruta); + if (ponto.Tipo == Enums.TipoPontoRua.Rua) + return _penMargemRua; - // --- 7) Plots e ligações (cores distintas) --- - // Centro - DrawPoint(g, xCtrPx, yCtrPx, Color.WhiteSmoke); + if (ponto.Tipo == + Enums.TipoPontoRua.CruvaEntreCorredores) + { + return _penMargemCurva; + } - // Antena prevista (onde deveria estar fisicamente) - DrawPoint(g, antenaPrevistaPx.X, antenaPrevistaPx.Y, Color.DeepSkyBlue); - using (var linkPrev = new Pen(Color.DeepSkyBlue, 1.5f) { DashStyle = System.Drawing.Drawing2D.DashStyle.Dash }) - g.DrawLine(linkPrev, xCtrPx, yCtrPx, antenaPrevistaPx.X, antenaPrevistaPx.Y); + return _penMargemPadrao; + } - // Antena corrigida (o que você "quer atingir" após correção) - DrawPoint(g, antenaCorrigidaPx.X, antenaCorrigidaPx.Y, Color.LimeGreen); - using (var linkCorr = new Pen(Color.LimeGreen, 1.5f)) - g.DrawLine(linkCorr, xCtrPx, yCtrPx, antenaCorrigidaPx.X, antenaCorrigidaPx.Y); + #endregion - // Antena bruta (de onde realmente vem o GNGGA) - DrawPoint(g, antenaBrutaPx.X, antenaBrutaPx.Y, Color.OrangeRed); - using (var linkBruta = new Pen(Color.OrangeRed, 1.0f) { DashStyle = System.Drawing.Drawing2D.DashStyle.Dot }) - g.DrawLine(linkBruta, xCtrPx, yCtrPx, antenaBrutaPx.X, antenaBrutaPx.Y); + #region Rover e antenas - // Opcional: linhas entre prevista x corrigida x bruta (comparação direta) - using (var penPC = new Pen(Color.MediumPurple, 1.0f) { DashStyle = System.Drawing.Drawing2D.DashStyle.Dash }) - g.DrawLine(penPC, antenaPrevistaPx, antenaCorrigidaPx); - using (var penPB = new Pen(Color.SandyBrown, 1.0f) { DashStyle = System.Drawing.Drawing2D.DashStyle.Dash }) - g.DrawLine(penPB, antenaPrevistaPx, antenaBrutaPx); + private void DesenharRoboEDiagnostico( + Graphics g, + RenderState state, + float centerX, + float centerY) + { + // Posição corrigida pelo lever arm. + PointF posicaoCorrigidaPx = LatLonToPanel( + state.Posicao.Latitude, + state.Posicao.Longitude, + state.Posicao, + centerX, + centerY + ); + + // Posição física/raw da antena. + PointF posicaoRawPx = LatLonToPanel( + state.Posicao.LatitudeAntenaBruta, + state.Posicao.LongitudeAntenaBruta, + state.Posicao, + centerX, + centerY + ); + + DrawRobotAndGpsPositions( + g, + posicaoRawPx, + posicaoCorrigidaPx, + state.AnguloCarro + ); + } + + public AntenaDiagPx DrawRobotAndGpsPositions( + Graphics g, + PointF posicaoRawPx, + PointF posicaoCorrigidaPx, + float headingDeg) + { + /* + * Convenção geométrica local: + * + * X negativo = esquerda + * X positivo = direita + * + * Y negativo = frente + * Y positivo = trás + * + * A posição (0,0) local é a antena física/raw. + */ + + float larguraEsquerdaCm = + (float)VariaveisEquipamento.LarguraEsquerda; + + float larguraDireitaCm = + (float)VariaveisEquipamento.LarguraDireita; + + float comprimentoFrenteCm = + (float)VariaveisEquipamento.ComprimentoFrente; + + float comprimentoTrasCm = + (float)VariaveisEquipamento.ComprimentoTras; + + float larguraTotalCm = + larguraEsquerdaCm + larguraDireitaCm; + + float comprimentoTotalCm = + comprimentoFrenteCm + comprimentoTrasCm; + + float cmToPx = + (float)_zoom / 10_000_000f; + + float larguraPx = Math.Max( + 0.000001f, + larguraTotalCm * cmToPx + ); + + float comprimentoPx = Math.Max( + 0.000001f, + comprimentoTotalCm * cmToPx + ); + + /* + * Calcula o centro geométrico do rover em relação à antena. + * + * Exemplo: + * esquerda = 44 + * direita = 44 + * + * centro lateral = (44 - 44) / 2 = 0 cm + * + * frente = 7 + * trás = 107 + * + * centro longitudinal = (107 - 7) / 2 = 50 cm para trás + */ + + float centroLocalXCm = + (larguraDireitaCm - larguraEsquerdaCm) / 2f; + + float centroLocalYCm = + (comprimentoTrasCm - comprimentoFrenteCm) / 2f; + + float centroLocalXPx = + centroLocalXCm * cmToPx; + + float centroLocalYPx = + centroLocalYCm * cmToPx; + + PointF centroRoverPx = CalcularPontoRotacionado( + posicaoRawPx.X, + posicaoRawPx.Y, + centroLocalXPx, + centroLocalYPx, + headingDeg + ); + + /* + * Os quatro cantos são calculados diretamente a partir + * da antena física. Isso evita depender do ponto corrigido + * para posicionar o corpo do rover. + */ + + PointF frenteEsquerda = CalcularPontoRotacionado( + posicaoRawPx.X, + posicaoRawPx.Y, + -larguraEsquerdaCm * cmToPx, + -comprimentoFrenteCm * cmToPx, + headingDeg + ); + + PointF frenteDireita = CalcularPontoRotacionado( + posicaoRawPx.X, + posicaoRawPx.Y, + larguraDireitaCm * cmToPx, + -comprimentoFrenteCm * cmToPx, + headingDeg + ); + + PointF trasDireita = CalcularPontoRotacionado( + posicaoRawPx.X, + posicaoRawPx.Y, + larguraDireitaCm * cmToPx, + comprimentoTrasCm * cmToPx, + headingDeg + ); + + PointF trasEsquerda = CalcularPontoRotacionado( + posicaoRawPx.X, + posicaoRawPx.Y, + -larguraEsquerdaCm * cmToPx, + comprimentoTrasCm * cmToPx, + headingDeg + ); + + PointF[] corpoRover = + { + frenteEsquerda, + frenteDireita, + trasDireita, + trasEsquerda + }; + + g.DrawPolygon( + _penRobo, + corpoRover + ); + + /* + * Linha que indica a frente. + * Ela sai do centro geométrico e termina no meio da face frontal. + */ + + PointF meioFrente = new PointF( + (frenteEsquerda.X + frenteDireita.X) / 2f, + (frenteEsquerda.Y + frenteDireita.Y) / 2f + ); + + g.DrawLine( + _penFrenteRobo, + centroRoverPx, + meioFrente + ); + + /* + * Vetor entre: + * + * RAW = posição física da antena; + * CORRIGIDA = posição depois da aplicação do lever arm. + */ + + g.DrawLine( + _penAntenaBruta, + posicaoRawPx, + posicaoCorrigidaPx + ); + + // Ponto raw da antena física. + DesenharPonto( + g, + posicaoRawPx, + _brushAntenaBruta + ); + + // Ponto corrigido, referência usada pelo sistema. + DesenharPonto( + g, + posicaoCorrigidaPx, + _brushAntenaCorrigida + ); - // --- 8) Retornar as posições para log/depuração --- return new AntenaDiagPx { - Centro = new PointF(xCtrPx, yCtrPx), - Prevista = antenaPrevistaPx, - Corrigida = antenaCorrigidaPx, - Bruta = antenaBrutaPx + CentroRover = centroRoverPx, + PosicaoCorrigida = posicaoCorrigidaPx, + PosicaoRaw = posicaoRawPx }; } - - private PointF CalcularPontoRotacionado(float centroX, float centroY, float offsetX, float offsetY, float angle) + public struct AntenaDiagPx { - // Converter o ângulo para radianos - float rad = angle * (float)Math.PI / 180; + /// + /// Centro geométrico calculado do corpo do rover. + /// + public PointF CentroRover; - // Calcular as novas coordenadas após rotação - float cos = (float)Math.Cos(rad); - float sin = (float)Math.Sin(rad); + /// + /// Latitude/Longitude após a compensação do lever arm. + /// É a referência usada pelo restante do sistema. + /// + public PointF PosicaoCorrigida; - float novoX = centroX + (offsetX * cos - offsetY * sin); - float novoY = centroY + (offsetX * sin + offsetY * cos); - - return new PointF(novoX, novoY); + /// + /// LatitudeAnt/LongitudeAnt recebidas diretamente do GPS. + /// É a posição física da antena. + /// + public PointF PosicaoRaw; } - private void DrawObstacle(Graphics g, float xRobot, float yRobot, Color pointColor, Color borderColor, float angle, Obstaculo obstaculo, float PxToCm) + #endregion + + #region Mouse, zoom e pan + + private void PnlZoomMapa_MouseDown( + object sender, + MouseEventArgs e) { - GPSModel posicaoRobo = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps; - GPSModel posicaoObstaculo = GPSUtils.GerarPontoDeslocado(posicaoRobo, angle, obstaculo.DistanciaMedia_mm / 1000); + if (_disposed || e.Button != MouseButtons.Left) + return; - float x = xRobot + (float)((posicaoObstaculo.Longitude - posicaoRobo.Longitude) * _zoom); - float y = yRobot - (float)((posicaoObstaculo.Latitude - posicaoRobo.Latitude) * _zoom); - - // Desenhar o ponto GPS - DrawPoint(g, x, y, pointColor); - - - int agPlus = Convert.ToInt32(135 + angle); - - float largura = obstaculo.Largura_mm / 20; - - (float xET, float yET) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 90); - (float xDT, float yDT) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 0); - (float xEF, float yEF) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 180); - (float xDF, float yDF) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 270); - - // Calcular os cantos do retângulo considerando o ângulo - PointF[] corners = new PointF[4]; - - // Cantos do retângulo com as novas medidas - corners[0] = new PointF(xET, yET); // Superior esquerdo - corners[1] = new PointF(xDT, yDT); // Superior direito - corners[2] = new PointF(xDF, yDF); // Inferior direito - corners[3] = new PointF(xEF, yEF); // Inferior esquerdo - - // Desenhar o contorno do robô - using (Pen pen = new Pen(borderColor, 2)) + if (_travarNoRobo) { - g.DrawPolygon(pen, corners); + _userPan = _mapOffset; + _travarNoRobo = false; + } + + _arrastando = true; + _inicioArrasto = e.Location; + _inicioPan = _userPan; + + _parent.Cursor = Cursors.Hand; + } + + private void PnlZoomMapa_MouseMove( + object sender, + MouseEventArgs e) + { + if (_disposed || !_arrastando) + return; + + float dx = e.X - _inicioArrasto.X; + float dy = e.Y - _inicioArrasto.Y; + + _userPan = new PointF( + _inicioPan.X + dx, + _inicioPan.Y + dy + ); + + _mapOffset = _userPan; + + _parent.Cursor = Cursors.Cross; + + SolicitarRedesenho(); + } + + private void PnlZoomMapa_MouseUp( + object sender, + MouseEventArgs e) + { + if (e.Button != MouseButtons.Left) + return; + + FinalizarArrasto(); + } + + private void PnlZoomMapa_MouseLeave( + object sender, + EventArgs e) + { + if (_arrastando && + Control.MouseButtons == MouseButtons.None) + { + FinalizarArrasto(); } } - private void DrawFrontLine(Graphics g, float x, float y, float xDF, float yDF, float xEF, float yEF, Color borderColor) + private void FinalizarArrasto() { - // Calcular o ponto médio entre DF e EF - float midX = (xDF + xEF) / 2; - float midY = (yDF + yEF) / 2; + _arrastando = false; - // Desenhar a linha do ponto central para a frente do robô - using (Pen frontPen = new Pen(borderColor, 2)) - { - g.DrawLine(frontPen, x, y, midX, midY); - } + if (!_parent.IsDisposed) + _parent.Cursor = Cursors.Default; } - private (float, float) CalcularPontosLateraisRobo(GPSModel posicaoRobo, float Largura, float Comprimento, float x, float y, int offAngulo) + private void PnlZoomMapa_MouseWheel( + object sender, + MouseEventArgs e) { - float hP = (float)Math.Sqrt(Math.Pow(Largura, 2) + Math.Pow(Comprimento, 2)) / 100f; - GPSModel pP = GPSUtils.GerarPontoDeslocado(posicaoRobo, offAngulo, hP); - float xP = x + (float)((pP.Longitude - posicaoRobo.Longitude) * _zoom); - float yP = y - (float)((pP.Latitude - posicaoRobo.Latitude) * _zoom); + if (_disposed) + return; - return (xP, yP); + if (e.Delta > 0) + _zoom *= FatorZoom; + else if (e.Delta < 0) + _zoom /= FatorZoom; + + _zoom = Limitar( + _zoom, + ZoomMinimo, + ZoomMaximo + ); + + SolicitarRedesenho(); } + #endregion + #region Conversões e utilidades - - private PointF LatLonToPanel(double lat, double lon, float centerX, float centerY) + private PointF LatLonToPanel( + double latitude, + double longitude, + PosicaoRender origem, + float centerX, + float centerY) { - float x = centerX + (float)((lon - _posicaoAtual.Lon0) * _zoom); - float y = centerY - (float)((lat - _posicaoAtual.Lat0) * _zoom); + float x = + centerX + + (float)((longitude - origem.OrigemLongitude) * _zoom); - // Usa a mesma origem ENU do GeoLeverArm - var (xE, yN) = GPSService.LeverArm.GeodeticToENU(lat, lon, _posicaoAtual.Lat0, _posicaoAtual.Lon0); - float scale = (float)_zoom / 1e7f; - - float px = centerX + (float)(xE * scale); // Leste para +X - float py = centerY - (float)(yN * scale); // Norte para -Y (tela cresce para baixo) + float y = + centerY - + (float)((latitude - origem.OrigemLatitude) * _zoom); return new PointF(x, y); } + private static PointF CalcularPontoRotacionado( + float centroX, + float centroY, + float offsetX, + float offsetY, + float angleDeg) + { + float rad = + angleDeg * + ((float)Math.PI / 180f); + + float cos = (float)Math.Cos(rad); + float sin = (float)Math.Sin(rad); + + return new PointF( + centroX + ((offsetX * cos) - (offsetY * sin)), + centroY + ((offsetX * sin) + (offsetY * cos)) + ); + } + + private static int CalcularPassoPontos(int quantidade) + { + if (quantidade <= LimitePontosVisuais) + return 1; + + return Math.Max( + 1, + quantidade / LimitePontosVisuais + ); + } + + private static void DesenharPonto( + Graphics g, + PointF ponto, + Brush brush) + { + if (float.IsNaN(ponto.X) || + float.IsInfinity(ponto.X) || + float.IsNaN(ponto.Y) || + float.IsInfinity(ponto.Y)) + { + return; + } + + float metade = TamanhoPonto / 2f; + + g.FillEllipse( + brush, + ponto.X - metade, + ponto.Y - metade, + TamanhoPonto, + TamanhoPonto + ); + } + + private void SolicitarRedesenho() + { + if (_disposed || + _painel.IsDisposed || + !_painel.IsHandleCreated) + { + return; + } + + // Já existe um Invalidate aguardando a UI. + if (Interlocked.Exchange( + ref _invalidatePendente, + 1) == 1) + { + return; + } + + try + { + _painel.BeginInvoke(new Action(delegate + { + Interlocked.Exchange( + ref _invalidatePendente, + 0 + ); + + if (!_disposed && + !_painel.IsDisposed) + { + _painel.Invalidate(); + } + })); + } + catch (InvalidOperationException) + { + Interlocked.Exchange( + ref _invalidatePendente, + 0 + ); + } + catch (Exception) + { + Interlocked.Exchange( + ref _invalidatePendente, + 0 + ); + } + } + + private static void ConfigurarDoubleBuffer(Control control) + { + try + { + MethodInfo setStyle = + control.GetType().GetMethod( + "SetStyle", + BindingFlags.Instance | + BindingFlags.NonPublic + ); + + if (setStyle == null) + return; + + setStyle.Invoke( + control, + new object[] + { + ControlStyles.UserPaint | + ControlStyles.AllPaintingInWmPaint | + ControlStyles.OptimizedDoubleBuffer, + true + } + ); + } + catch + { + // O controle ainda funciona sem esse ajuste. + } + } + + private static Pen CriarPenMargem(Color color) + { + return new Pen( + Color.FromArgb(150, color), + 1f + ); + } + + private static bool CoordenadaValida(double valor) + { + return !double.IsNaN(valor) && + !double.IsInfinity(valor); + } + + private static float Lerp( + float atual, + float destino, + float alpha) + { + return atual + + ((destino - atual) * alpha); + } + + private static float Limitar( + float valor, + float minimo, + float maximo) + { + return Math.Max( + minimo, + Math.Min(maximo, valor) + ); + } + + private static double Limitar( + double valor, + double minimo, + double maximo) + { + return Math.Max( + minimo, + Math.Min(maximo, valor) + ); + } + + #endregion + + #region Modelos internos de renderização + + private sealed class RenderState + { + public static readonly RenderState Empty = + new RenderState( + null, + 0f, + 0f, + PosicaoRender.Empty, + Array.Empty(), + Array.Empty(), + Array.Empty(), + Array.Empty(), + Array.Empty() + ); + + public readonly Obstaculo Obstaculo; + public readonly float AnguloCaminho; + public readonly float AnguloCarro; + + public readonly PosicaoRender Posicao; + + public readonly TrajectoryRenderPoint[] + TrajetoriaDinamica; + + public readonly TrajectoryRenderPoint[] + TrajetoriaRuaProjetada; + + public readonly GeoRenderPoint[] + TrajetoriaRobo; + + public readonly GeoRenderPoint[][] + RuasPlantacao; + + public readonly GeoRenderPoint[] + TrajetoriaMpc; + + public RenderState( + Obstaculo obstaculo, + float anguloCaminho, + float anguloCarro, + PosicaoRender posicao, + TrajectoryRenderPoint[] trajetoriaDinamica, + TrajectoryRenderPoint[] trajetoriaRuaProjetada, + GeoRenderPoint[] trajetoriaRobo, + GeoRenderPoint[][] ruasPlantacao, + GeoRenderPoint[] trajetoriaMpc) + { + Obstaculo = obstaculo; + AnguloCaminho = anguloCaminho; + AnguloCarro = anguloCarro; + + Posicao = posicao; + + TrajetoriaDinamica = + trajetoriaDinamica ?? + Array.Empty(); + + TrajetoriaRuaProjetada = + trajetoriaRuaProjetada ?? + Array.Empty(); + + TrajetoriaRobo = + trajetoriaRobo ?? + Array.Empty(); + + RuasPlantacao = + ruasPlantacao ?? + Array.Empty(); + + TrajetoriaMpc = + trajetoriaMpc ?? + Array.Empty(); + } + } + + private struct PosicaoRender + { + public static readonly PosicaoRender Empty = + new PosicaoRender( + 0, + 0, + 0, + 0, + 0, + 0 + ); + + public readonly double Latitude; + public readonly double Longitude; + + public readonly double LatitudeAntenaBruta; + public readonly double LongitudeAntenaBruta; + + public readonly double OrigemLatitude; + public readonly double OrigemLongitude; + + public bool Valida + { + get + { + return CoordenadaGeograficaValida( + Latitude, + Longitude + ) && + CoordenadaGeograficaValida( + OrigemLatitude, + OrigemLongitude + ); + } + } + + public PosicaoRender( + double latitude, + double longitude, + double latitudeAntenaBruta, + double longitudeAntenaBruta, + double origemLatitude, + double origemLongitude) + { + Latitude = latitude; + Longitude = longitude; + + LatitudeAntenaBruta = latitudeAntenaBruta; + LongitudeAntenaBruta = longitudeAntenaBruta; + + OrigemLatitude = origemLatitude; + OrigemLongitude = origemLongitude; + } + } + + private struct GeoRenderPoint + { + public readonly double Latitude; + public readonly double Longitude; + + public GeoRenderPoint( + double latitude, + double longitude) + { + Latitude = latitude; + Longitude = longitude; + } + } + + private struct TrajectoryRenderPoint + { + public readonly double Latitude; + public readonly double Longitude; + + public readonly Enums.TipoPontoRua Tipo; + + public readonly double DistanciaMargem; + public readonly bool PontoLigacao; + public readonly bool PontoBorda; + + public TrajectoryRenderPoint( + double latitude, + double longitude, + Enums.TipoPontoRua tipo, + double distanciaMargem, + bool pontoLigacao, + bool pontoBorda) + { + Latitude = latitude; + Longitude = longitude; + + Tipo = tipo; + + DistanciaMargem = distanciaMargem; + PontoLigacao = pontoLigacao; + PontoBorda = pontoBorda; + } + } + + #endregion + + #region Dispose + + public void Dispose() + { + if (_disposed) + return; + + _disposed = true; + + _painel.Paint -= PnlZoomMapa_Paint; + _painel.MouseWheel -= PnlZoomMapa_MouseWheel; + _painel.MouseDown -= PnlZoomMapa_MouseDown; + _painel.MouseMove -= PnlZoomMapa_MouseMove; + _painel.MouseUp -= PnlZoomMapa_MouseUp; + _painel.MouseLeave -= PnlZoomMapa_MouseLeave; + + _penRuaPlantacao.Dispose(); + _penTrajetoriaDinamica.Dispose(); + _penRuaProjetada.Dispose(); + _penTrilhaRobo.Dispose(); + _penMpc.Dispose(); + _penRobo.Dispose(); + _penFrenteRobo.Dispose(); + + _penAntenaPrevista.Dispose(); + _penAntenaCorrigida.Dispose(); + _penAntenaBruta.Dispose(); + _penPrevistaCorrigida.Dispose(); + _penPrevistaBruta.Dispose(); + + _penMargemPadrao.Dispose(); + _penMargemRobo.Dispose(); + _penMargemBorda.Dispose(); + _penMargemRua.Dispose(); + _penMargemCurva.Dispose(); + + _brushRuaPlantacao.Dispose(); + _brushTrajetoriaDinamica.Dispose(); + _brushRuaProjetada.Dispose(); + _brushTrilhaRobo.Dispose(); + _brushMpc.Dispose(); + + _brushCentroRobo.Dispose(); + _brushAntenaPrevista.Dispose(); + _brushAntenaCorrigida.Dispose(); + _brushAntenaBruta.Dispose(); + } + + #endregion } -} +} \ No newline at end of file diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index 05f0ab26b..56661e998 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -554,7 +554,6 @@ namespace AgroBase.Models }; op.Controle = new OperacaoControleModel() { - TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, Angulo = 0, EmFreio = false, PercentualVelocidadeSP = 0, @@ -615,7 +614,7 @@ namespace AgroBase.Models DirAuxilioSonar = true, MovAuxilioSonar = true, MpcHorizonte = 4.0, - AnteciparManobraCorredorM = 10.0, + AnteciparManobraCorredorM = 0.0, MovVelocidadeSErvasPercent = 50, MovVelocidadeCErvasPercent = 20, @@ -695,7 +694,6 @@ namespace AgroBase.Models }; op.Controle = new OperacaoControleModel() { - TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, Angulo = 0, EmFreio = false, PercentualVelocidadeSP = 0, @@ -836,7 +834,6 @@ namespace AgroBase.Models }; op.Controle = new OperacaoControleModel() { - TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, Angulo = 0, EmFreio = false, PercentualVelocidadeSP = 0, @@ -894,7 +891,7 @@ namespace AgroBase.Models DirAuxilioSonar = true, MovAuxilioSonar = true, MpcHorizonte = 4.0, - AnteciparManobraCorredorM = 10.0, + AnteciparManobraCorredorM = 0.0, MovVelocidadeSErvasPercent = 40, MovVelocidadeCErvasPercent = 20, @@ -944,7 +941,6 @@ namespace AgroBase.Models }; op.Controle = new OperacaoControleModel() { - TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, Angulo = 0, EmFreio = false, PercentualVelocidadeSP = 0, @@ -1059,7 +1055,6 @@ namespace AgroBase.Models op.Parametros.ParametrosMandatorios = new List(); op.Controle = new OperacaoControleModel() { - TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, Angulo = 0, EmFreio = false, PercentualVelocidadeSP = 0, @@ -1093,7 +1088,7 @@ namespace AgroBase.Models break; } - + op.Controle.DefinirTipoMovimento(TipoMovimentoDirecional.RodasDianteiras, op.DispMvd?.Dados?.Modulos); op.Parametros.ModulosMandatorios.ForEach(m => m.ComponentesEmUso = op.PreencherComponentesEmUso(m.Dispositivo)); if (CarregarModulos) { @@ -1799,40 +1794,10 @@ namespace AgroBase.Models var dispMvd = DispMvd; - if (dispMvd?.Dados?.Modulos != null && value != Controle.TipoMovimento) + if (dispMvd?.Dados?.Modulos != null) // && value != Controle.TipoMovimento { - switch (value) - { - case TipoMovimentoDirecional.RodasDianteiras: - dispMvd.Dados.Modulos.ForEach(x => - { - x.MovMotor.Comandar = true; - x.DirMotor.Comandar = x.Modulo_ID.Contains("F"); - x.DirMotor.Angulo_SP = x.DirMotor.Comandar ? Controle.Angulo : 0; - }); - break; - - case TipoMovimentoDirecional.RodasTraseiras: - dispMvd.Dados.Modulos.ForEach(x => - { - x.MovMotor.Comandar = true; - x.DirMotor.Comandar = x.Modulo_ID.Contains("T"); - x.DirMotor.Angulo_SP = x.DirMotor.Comandar ? Controle.Angulo : 0; - }); - break; - - default: - dispMvd.Dados.Modulos.ForEach(x => - { - x.MovMotor.Comandar = true; - x.DirMotor.Comandar = true; - x.DirMotor.Angulo_SP = Controle.Angulo; - }); - break; - } + Controle.DefinirTipoMovimento(value, dispMvd.Dados.Modulos); } - - Controle.TipoMovimento = value; } public void AtualizaInformacoesControleOperacao() @@ -1958,7 +1923,7 @@ namespace AgroBase.Models //Variaveis.MostrarLog($"Angulo SP alterado de {_ControleAnterior.Angulo:F2} para {_Controle.Angulo:F2}"); _ControleAnterior.Angulo = _Controle.Angulo; - _ControleAnterior.TipoMovimento = _Controle.TipoMovimento; + _ControleAnterior.DefinirTipoMovimento(_Controle.TipoMovimento); RedisService.AtualizarCampos( CtxKey.DadosControle, @@ -3640,7 +3605,45 @@ namespace AgroBase.Models } public bool EmFreio { get; set; } = false; public double AlturaBarra { get; set; } - public TipoMovimentoDirecional TipoMovimento { get; set; } = TipoMovimentoDirecional.Diagnostico; + [JsonProperty] + public TipoMovimentoDirecional TipoMovimento { get; private set; } = TipoMovimentoDirecional.Diagnostico; + public void DefinirTipoMovimento(TipoMovimentoDirecional value, List modulos = null) + { + if (modulos != null) + { + switch (value) + { + case TipoMovimentoDirecional.RodasDianteiras: + modulos.ForEach(x => + { + x.MovMotor.Comandar = true; + x.DirMotor.Comandar = x.Modulo_ID.Contains("F"); + x.DirMotor.Angulo_SP = x.DirMotor.Comandar ? Angulo : 0; + }); + break; + + case TipoMovimentoDirecional.RodasTraseiras: + modulos.ForEach(x => + { + x.MovMotor.Comandar = true; + x.DirMotor.Comandar = x.Modulo_ID.Contains("T"); + x.DirMotor.Angulo_SP = x.DirMotor.Comandar ? Angulo : 0; + }); + break; + + default: + modulos.ForEach(x => + { + x.MovMotor.Comandar = true; + x.DirMotor.Comandar = true; + x.DirMotor.Angulo_SP = Angulo; + }); + break; + } + } + + TipoMovimento = value; + } public List TiposControle { get; set; } = new List(); public List SimulacaoMPC { get; set; } = new List(); public double Latencia { get; set; } @@ -3648,6 +3651,10 @@ namespace AgroBase.Models public Dictionary DebugCustoMpc { get; set; } = new Dictionary(); public List Motivos { get; set; } = new List(); + public string UltimaSessaoComando { get; set; } + public long? UltimaSeqMovimentoAplicada { get; set; } + public long? UltimaSeqDirecionalAplicada { get; set; } + public void Reiniciar() { PercentualVelocidadeSP = 0; @@ -3668,6 +3675,8 @@ namespace AgroBase.Models Angulo = Angulo, PercentualVelocidadeSP = PercentualVelocidadeSP, + _pctVelSp = _pctVelSp, + TipoMovimento = TipoMovimento, AlturaBarra = AlturaBarra, heartbeat = heartbeat, ticks_sem_resposta = ticks_sem_resposta, @@ -3687,7 +3696,11 @@ namespace AgroBase.Models }).ToList() ?? new List(), SimulacaoMPC = new List(SimulacaoMPC ?? new List()), DebugCustoMpc = new Dictionary(DebugCustoMpc ?? new Dictionary()), - Motivos = new List(Motivos ?? new List()) + Motivos = new List(Motivos ?? new List()), + + UltimaSessaoComando = UltimaSessaoComando, + UltimaSeqMovimentoAplicada = UltimaSeqMovimentoAplicada, + UltimaSeqDirecionalAplicada = UltimaSeqDirecionalAplicada }; return clone; @@ -3787,19 +3800,33 @@ namespace AgroBase.Models tempoDecorridoSeg = Math.Max(0, (fim - Operacao.DataInicio).TotalSeconds); } - double tempoAguardandoRaw = RedisService.GetField(CtxKey.DadosOperacao, "tempo_aguardando", 0.0); + double tempoAguardandoInicioTs = RedisService.GetField(CtxKey.DadosOperacao, "tempo_aguardando", 0.0); double tempoAguardarInicio = RedisService.GetField(CtxKey.DadosOperacao, "tempo_aguardar_inicio_operacao", op.TempoIniciarOperacao); - double agoraUnix = DateTimeOffset.Now.ToUnixTimeMilliseconds() / 1000.0; - double tempoAguardandoSeg; - if (tempoAguardandoRaw > 1_000_000_000) + double agoraUnix = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0; + double tempoRestanteInicioSeg = 0.0; + if (tempoAguardandoInicioTs > 1_000_000_000) { - // Redis está mandando timestamp Unix de início da espera. - tempoAguardandoSeg = Math.Max(0.0, agoraUnix - tempoAguardandoRaw); + /* + * tempo_aguardando é o timestamp Unix + * de quando a espera começou. + */ + double tempoDecorridoEsperaSeg = Math.Max(0.0, agoraUnix - tempoAguardandoInicioTs); + tempoRestanteInicioSeg = Math.Max(0.0, tempoAguardarInicio - tempoDecorridoEsperaSeg); + } + else if (tempoAguardandoInicioTs > 0.0) + { + /* + * Compatibilidade com formato antigo: + * considera o valor como tempo já decorrido. + */ + tempoRestanteInicioSeg = Math.Max(0.0, tempoAguardarInicio - tempoAguardandoInicioTs); } else { - // Compatibilidade: caso algum fluxo antigo mande duração em segundos. - tempoAguardandoSeg = Math.Max(0.0, tempoAguardandoRaw); + /* + * Ainda não existe timestamp de espera. + */ + tempoRestanteInicioSeg = Math.Max(0.0, tempoAguardarInicio); } op.TempoIniciarOperacao = Math.Max(0, Convert.ToInt32(Math.Round(tempoAguardarInicio))); @@ -3819,7 +3846,7 @@ namespace AgroBase.Models DataInicio = Operacao.DataInicio, DataFim = Operacao.DataFim, TempoDecorridoSeg = tempoDecorridoSeg, - TempoAguardandoSeg = tempoAguardandoSeg, + TempoAguardandoSeg = tempoRestanteInicioSeg, TipoCanAtivo = CanManager.TipoServico, OperacaoLiberada = RedisService.GetField(CtxKey.DadosOperacao, "liberado", false), @@ -3835,10 +3862,7 @@ namespace AgroBase.Models ModsCalibragemView = Operacao.ModsCalibragem?.ToDictionary(x => x.Key.ToString(), x => new Dictionary(x.Value ?? new Dictionary())) ?? new Dictionary>(), }; - if (operacaoAtualizada.StatusOperacaoAtual != Operacao.StatusOperacaoAnterior) - { - AudioAlertaService.AlertaSonoroPorStatus(operacaoAtualizada.StatusOperacaoAtual); - } + bool statusMudou = operacaoAtualizada.StatusOperacaoAtual != Operacao.StatusOperacaoAnterior; operacaoAtualizada.StatusOperacaoAnterior = operacaoAtualizada.StatusOperacaoAtual; var saude = OperadorSaude.ModulosSaude; @@ -3877,6 +3901,11 @@ namespace AgroBase.Models StatusModulo.Operante; Operacao = operacaoAtualizada; + + if (statusMudou) + { + AudioAlertaService.AlertaSonoroPorStatus(operacaoAtualizada.StatusOperacaoAtual); + } } // Atu diff --git a/AgroBase/AgroBase/Models/Operadores/ManagerWorkerModel.cs b/AgroBase/AgroBase/Models/Operadores/ManagerWorkerModel.cs index f5b276e01..93bd001ef 100644 --- a/AgroBase/AgroBase/Models/Operadores/ManagerWorkerModel.cs +++ b/AgroBase/AgroBase/Models/Operadores/ManagerWorkerModel.cs @@ -49,6 +49,11 @@ namespace AgroBase.Models.Operadores public int versao_comando { get; set; } = 1; public double ts_comando { get; set; } = 0; + public string sessao_comando { get; set; } + + public long? seq_movimento { get; set; } + public long? seq_direcional { get; set; } + public bool atualizar_movimento { get; set; } = true; public bool atualizar_direcional { get; set; } = true; diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index e731770ee..8e40bdffc 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -21,16 +21,24 @@ namespace AgroBase.Models #region PARAMETROS - public double AnguloAberturaCurva { get; set; } = 45; // Angulo usado para deslocar o ponto de curva - public double DistanciaProjecaoRua { get; set; } = 1.5; // Distancia para projetar o primeiro ponto para fora do corredor + public double AnguloAberturaCurva { get; set; } = 25; // Angulo usado para deslocar o ponto de curva + public double DistanciaProjecaoRua { get; set; } = (VariaveisEquipamento.DistanciaEntreEixos / 100.0 / 2.0) + 2.0; // Distancia para projetar o primeiro ponto para fora do corredor public static double DistanciaEntrePontos { get; set; } = 0.8; // Distancia entre os pontos dentro do corredor public static double DistanciaEntrePontosCurva { get; set; } = 0.25; // Distancia entre os pontos durante a curva entre corredores private double DistanciaManobraEntreRuas { get; set; } = 3.0; // Distancia máxima para gerar a curva de conexão entre os corredores private bool EspacamentoPrimeirosPontosProjecao { get; set; } = false; // Projetar os primeiros pontos com distancia menor entre eles - private double DistanciaPrimeirosPontosProjecao { get; set; } = 3.0; // Distancia máxima para projetar os primeiros pontos com distancia menor public static double LarguraCorredorPadrao { get; set; } = 1.5; // Largura de um corredor padrão private int JanelaAtualizacaoDePontos { get; set; } = 20; // Define o tamanho da janela de pontos a atualizar public bool VerificacaoInicialMeioRuaConcluida { get; set; } = false; // Realiza a verificacao inicial para saber se o robo deve comecar a operacao do meio da rua + private int JanelaBuscaAvancoPontos { get; set; } = 14; + private double MargemExtraAvancoM { get; set; } = 0.35; + private double DistanciaConclusaoTrajetoriaM { get; set; } = 3.0; + private double DistanciaRestanteMaximaParaTrocarCorredorM { get; set; } = 1.0; + private int PontosRestantesMaximosParaTrocarCorredor { get; set; } = 0; + private int MaxTentativasRecuperacaoTrajetoria { get; set; } = 3; + private double ToleranciaLateralRecuperacaoM { get; set; } = 1.5; + private double ToleranciaLongitudinalAntesPontoM { get; set; } = 0.10; + private int MaxPontosRecuperadosPorCiclo { get; set; } = 8; #endregion @@ -324,24 +332,31 @@ namespace AgroBase.Models public PontoTrajetoriaModel PontoAtual { get; private set; } private void DefinirPontoAtual() { - // ✅ Evita processamento desnecessário se a lista for nula ou vazia if (!_TrajetoriaFixaDefinida) { PontoAtual = null; return; } - // Busca eficiente: percorre do começo ao fim para achar o primeiro não visitado - for (int i = idxFinalJanela; i >= idxInicialJanela; i--) + int idxUltimoVisitado = -1; + + for (int i = _TrajetoriaFixa.Count - 1; i >= 0; i--) { if (_TrajetoriaFixa[i].Visitado) { - PontoAtual = _TrajetoriaFixa[i]; - return; + idxUltimoVisitado = i; + break; } } - PontoAtual = null; + if (idxUltimoVisitado >= 0) + { + PontoAtual = _TrajetoriaFixa[idxUltimoVisitado]; + return; + } + + PontoAtual = _TrajetoriaFixa[0]; + PontoAtual.Visitado = true; } [JsonProperty] public PontoTrajetoriaModel ProximoPonto { get; private set; } @@ -353,18 +368,20 @@ namespace AgroBase.Models return; } - if (PontoAtual?.idxPonto == _TrajetoriaFixa.Count - 1) + if (PontoAtual == null) { - ProximoPonto = PontoAtual; - } - else - { - int idx = _TrajetoriaFixa.IndexOf(PontoAtual) + 1; - ProximoPonto = _TrajetoriaFixa[idx]; + DefinirPontoAtual(); } - /*// Busca eficiente: percorre do começo ao fim para achar o primeiro não visitado - for (int i = idxInicialJanela; i <= idxFinalJanela; i++) + if (PontoAtual == null) + { + ProximoPonto = null; + return; + } + + int idxAtual = Math.Max(0, PontoAtual.idxPonto); + + for (int i = idxAtual + 1; i < _TrajetoriaFixa.Count; i++) { if (!_TrajetoriaFixa[i].Visitado) { @@ -373,8 +390,7 @@ namespace AgroBase.Models } } - ProximoPonto = PontoAtual; // Se não houver pontos não visitados, retorna o último ponto visitado - */ + ProximoPonto = PontoAtual; } [JsonProperty] public PontoTrajetoriaModel PontoMaisProximo { get; private set; } @@ -457,11 +473,11 @@ namespace AgroBase.Models { StatusAtual = StatusCarroMapa.CaminhandoRua; } - else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaEntrada)) + else if (_TrajetoriaJanela.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaEntrada)) { StatusAtual = StatusCarroMapa.EntrandoRua; } - else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaSaida)) + else if (_TrajetoriaJanela.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaSaida)) { StatusAtual = StatusCarroMapa.SaindoRua; } @@ -600,7 +616,52 @@ namespace AgroBase.Models public bool TrajeotiraConcluida { get; private set; } private void AtualizarTrajetoriaConcluida() { - TrajeotiraConcluida = _TrajetoriaDinamica.All(x => x.Visitado); + if (!_TrajetoriaFixaDefinida) + { + TrajeotiraConcluida = false; + return; + } + + TrajeotiraConcluida = _TrajetoriaFixa.All(x => x.Visitado); + } + + private bool TentarConcluirTrajetoriaPorFimFisico() + { + if (!_TrajetoriaFixaDefinida) + return false; + + var ultimo = _TrajetoriaFixa.LastOrDefault(); + if (ultimo == null) + return false; + + ultimo.AtualizarPropriedades(); + + int idxUltimoVisitado = ObterIdxUltimoVisitado(); + bool pertoDoFimPorIndice = idxUltimoVisitado >= _TrajetoriaFixa.Count - 3; + + bool pertoDoUltimoPonto = + ultimo.DistanciaAtual <= DistanciaConclusaoTrajetoriaM; + + bool passouDoUltimoPonto = + !ultimo.Aproximando && + ultimo.DistanciaAnterior <= DistanciaConclusaoTrajetoriaM && + ultimo.DistanciaAtual <= DistanciaConclusaoTrajetoriaM + 2.0; + + if (pertoDoFimPorIndice && (pertoDoUltimoPonto || passouDoUltimoPonto)) + { + MarcarVisitadoAte(_TrajetoriaFixa.Count - 1); + + Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Operante, + 100, + $"[TRJ] Trajetória concluída por fim físico. dist_ultimo={ultimo.DistanciaAtual:F2}m" + ); + + return true; + } + + return false; } private void AtualizarErroCombinado() @@ -653,69 +714,158 @@ namespace AgroBase.Models private void MarcarPontosIntermediariosNaoVisitados() { - if (!_TrajetoriaFixaDefinida) return; + if (!_TrajetoriaFixaDefinida || PontoAtual == null) + return; - for (int i = PontoAtual.idxPonto; i >= Math.Max(0, PontoAtual.idxPonto - JanelaAtualizacaoDePontos); i--) + int idxFinal = Math.Max(0, Math.Min(PontoAtual.idxPonto, _TrajetoriaFixa.Count - 1)); + + for (int i = 0; i <= idxFinal; i++) { - var ponto = _TrajetoriaFixa[i]; - if (!ponto.Visitado) - { - ponto.Visitado = true; - } + if (!_TrajetoriaFixa[i].Visitado) + _TrajetoriaFixa[i].Visitado = true; } } - - private void AtualizarPropriedadesPontosTrajetoria() + private void AtualizarPropriedadesPontos(int idxInicial, int idxFinal) { - if (!_TrajetoriaFixaDefinida) return; + if (!_TrajetoriaFixaDefinida) + return; - // Atualiza apenas os pontos dentro do intervalo definido - foreach (var ponto in _TrajetoriaJanela ?? new List()) + idxInicial = Math.Max(0, idxInicial); + idxFinal = Math.Min(_TrajetoriaFixa.Count - 1, idxFinal); + + for (int i = idxInicial; i <= idxFinal; i++) { - ponto.AtualizarPropriedades(); + _TrajetoriaFixa[i].AtualizarPropriedades(); + } + } + + private void MarcarVisitadoAte(int idx) + { + if (!_TrajetoriaFixaDefinida) + return; + + idx = Math.Max(0, Math.Min(idx, _TrajetoriaFixa.Count - 1)); + + for (int i = 0; i <= idx; i++) + { + _TrajetoriaFixa[i].Visitado = true; + } + } + + private int ObterIdxUltimoVisitado() + { + if (!_TrajetoriaFixaDefinida) + return -1; + + for (int i = _TrajetoriaFixa.Count - 1; i >= 0; i--) + { + if (_TrajetoriaFixa[i].Visitado) + return i; } - AtualizarTrajetoriaConcluida(); + return -1; } + private void AtualizarDadosTrajetoria() { - if (!_TrajetoriaFixaDefinida) return; + if (!_TrajetoriaFixaDefinida) + return; AtualizarTempoEntreLeituras(); AtualizarDistanciaMaximaEntreLeituras(); - Inicio: + for (int tentativa = 0; tentativa < MaxTentativasRecuperacaoTrajetoria; tentativa++) + { + DefinirPontoAtual(); + + if (PontoAtual == null) + return; + + AtualizarCorredorAtual(); + DefinirProximoPonto(); + AtualizarIndicesJanela(); AtualizarTrajetoriaJanela(); + AtualizarPropriedadesPontos(idxInicialJanela, idxFinalJanela); VerificaFimCorredorSegmentacao(); + if (VerificaInicioOperacaoMeioRua(true)) + continue; + + MarcarPontosIntermediariosNaoVisitados(); + + //bool recuperouAvanco = RecuperarAvancoPorPontoFuturo(); + bool recuperouAvanco = RecuperarPontosDeixadosParaTras(); + DefinirPontoAtual(); AtualizarCorredorAtual(); - - //if (AutonomiaCorredor.Liberado) DefinirProximoPonto(); - if (PontoAtual == null) goto Inicio; - if (VerificaInicioOperacaoMeioRua(true)) goto Inicio; + bool recuperouTransicao = RecuperarTransicaoEntreCorredores(); + + DefinirPontoAtual(); + AtualizarCorredorAtual(); + DefinirProximoPonto(); + + /* + * Uma troca de corredor encerra as recuperações deste ciclo. + * + * Sem isso, o próximo giro do for pode executar + * RecuperarAvancoPorPontoFuturo() já no corredor novo + * e confirmar mais pontos imediatamente. + */ + if (recuperouTransicao) + { + break; + } + + if (!recuperouAvanco) + { + break; + } + } + + if (PontoAtual == null) + return; + + DefinirProximoPonto(); + + if (ProximoPonto == null) + return; + + AtualizarIndicesJanela(); + AtualizarTrajetoriaJanela(); + AtualizarPropriedadesPontos(idxInicialJanela, idxFinalJanela); MarcarPontosIntermediariosNaoVisitados(); + + TentarConcluirTrajetoriaPorFimFisico(); + + DefinirPontoAtual(); + AtualizarCorredorAtual(); + DefinirProximoPonto(); + + if (ProximoPonto == null) + return; + DefinirPontoMaisProximo(); AtualizarAnguloMedioCorredor(); AtualizarAnguloCaminho(); AtualizarDistanciasLaterais(); AtualizarNaMargemDoCorredor(); - AtualizarManobrandoEntreRuas(); AtualizarStatusAtual(); - AtualizarDistanciaRestante(); + AtualizarManobrandoEntreRuas(); AtualizarPercentualTrajetoria(); AtualizarTempoEstimado(); AtualizarErroCombinado(); AtualizarTrajetoriaDinamica(); + AtualizarDistanciaRestante(); + AtualizarTrajetoriaConcluida(); AtualizarDistanciaErroMapaPlantacao(); } @@ -999,7 +1149,7 @@ namespace AgroBase.Models if (!(op.Parametros?.ControleAutomatico ?? false)) return; - AtualizarPropriedadesPontosTrajetoria(); + //AtualizarPropriedadesPontosTrajetoria(); AtualizarDadosTrajetoria(); @@ -1014,11 +1164,15 @@ namespace AgroBase.Models { var op = Variaveis.OperacaoEmAndamento; - if (!op.Sensoriamento.Operacao.OperacaoIniciada) + if (op == null || + !op.Sensoriamento.Operacao.OperacaoIniciada) + { return; + } + + const int minTicksSemResposta = 500; + const int maxTicksSemResposta = 2000; - int min_ticks_sem_resposta = 500; - int max_ticks_sem_resposta = 2000; ManagerWorkerMessageResponseComandoModel novoComando = null; try @@ -1033,91 +1187,618 @@ namespace AgroBase.Models } catch (Exception ex) { - // Se quiser, mantém silencioso. Mas em fase de campo eu logaria limitado. - // Variaveis.MostrarLog($"Erro ao receber novo comando: {ex.Message}"); + Variaveis.MostrarLog("[ManagerWorker] Erro ao ler comando: " + ex.Message); } DateTime agora = DateTime.Now; + var controle = op.Controle; - var _Controle = op.Controle; + if (controle == null) + return; - bool heartbeatMudou = - novoComando != null && - novoComando.heartbeat != _Controle.heartbeat; + bool recebeuComando = novoComando != null; + bool heartbeatMudou = recebeuComando && novoComando.heartbeat != controle.heartbeat; + bool sessaoMudou = recebeuComando && !string.IsNullOrWhiteSpace(novoComando.sessao_comando) && !string.Equals(controle.UltimaSessaoComando, novoComando.sessao_comando, StringComparison.Ordinal); + bool movimentoMudou = recebeuComando && (Math.Abs(controle.PercentualVelocidadeSP - novoComando.percentual_velocidade) >= 0.001 || controle.EmFreio != novoComando.em_freio); + bool direcionalMudou = recebeuComando && (Math.Abs(controle.Angulo - novoComando.angulo) >= 0.001 || controle.TipoMovimento != novoComando.tipo_movimento); - bool movimentoMudou = - novoComando != null && - novoComando.atualizar_movimento && + bool seqMovimentoMudou = + recebeuComando && + novoComando.seq_movimento.HasValue && ( - _Controle.PercentualVelocidadeSP != novoComando.percentual_velocidade || - _Controle.EmFreio != novoComando.em_freio + sessaoMudou || + !controle + .UltimaSeqMovimentoAplicada + .HasValue || + controle + .UltimaSeqMovimentoAplicada + .Value != + novoComando.seq_movimento.Value ); - bool direcionalMudou = - novoComando != null && - novoComando.atualizar_direcional && + bool seqDirecionalMudou = + recebeuComando && + novoComando.seq_direcional.HasValue && ( - _Controle.Angulo != novoComando.angulo || - _Controle.TipoMovimento != novoComando.tipo_movimento + sessaoMudou || + !controle + .UltimaSeqDirecionalAplicada + .HasValue || + controle + .UltimaSeqDirecionalAplicada + .Value != + novoComando.seq_direcional.Value ); - bool mangerDestravou = - novoComando != null && - _Controle.ticks_sem_resposta.AddMilliseconds(min_ticks_sem_resposta) < agora && - heartbeatMudou; + bool managerDestravou = recebeuComando && controle.ticks_sem_resposta != DateTime.MinValue && controle.ticks_sem_resposta.AddMilliseconds(minTicksSemResposta) < agora && heartbeatMudou; - if (novoComando != null && (movimentoMudou || direcionalMudou || mangerDestravou)) + bool possuiComandoNovo = + recebeuComando && + ( + sessaoMudou || + seqMovimentoMudou || + seqDirecionalMudou || + movimentoMudou || + direcionalMudou || + managerDestravou + ); + + if (possuiComandoNovo) { - ManagerWorkerControlParser.AplicarComandoControle( - novoComando, - forcarAplicacao: mangerDestravou - ); + ManagerWorkerControlParser.AplicarComandoControle(novoComando, forcarAplicacao: managerDestravou || sessaoMudou); } - if (novoComando != null && !heartbeatMudou) + /* + * Watchdog: + * + * Sem mensagem ou heartbeat parado: + * começa/continua contagem. + * + * Heartbeat mudou: + * limpa contagem. + */ + if (!recebeuComando || !heartbeatMudou) { - if (_Controle.ticks_sem_resposta == DateTime.MinValue) + if (controle.ticks_sem_resposta == DateTime.MinValue) { - _Controle.ticks_sem_resposta = agora; + controle.ticks_sem_resposta = agora; } } else { - _Controle.ticks_sem_resposta = DateTime.MinValue; + controle.ticks_sem_resposta = DateTime.MinValue; } - if (novoComando != null) + if (recebeuComando) { - _Controle.heartbeat = novoComando.heartbeat; - _Controle.Motivos = (novoComando.motivos ?? new string[0]).ToList(); + controle.heartbeat = novoComando.heartbeat; + controle.Motivos = (novoComando.motivos ?? Array.Empty()).ToList(); } - if (_Controle.ticks_sem_resposta != DateTime.MinValue) + + if (controle.ticks_sem_resposta == DateTime.MinValue) { - if (_Controle.ticks_sem_resposta.AddMilliseconds(max_ticks_sem_resposta) < agora) + return; + } + + bool venceuTimeoutMaximo = controle.ticks_sem_resposta.AddMilliseconds(maxTicksSemResposta) < agora; + bool venceuTimeoutMinimo = controle.ticks_sem_resposta.AddMilliseconds(minTicksSemResposta) < agora; + + if (venceuTimeoutMaximo) + { + if (op.Controle?.RPM_SP > 0) { - if (op.Controle?.RPM_SP > 0) - { - op.Sensoriamento?.InserirLog(T_Code.Npc, StatusModulo.Falha, 0, "Muito tempo sem receber novo comando do Manager Worker! Parando Movimento!"); - Variaveis.MostrarLog("Muito tempo sem receber novo comando do Manager Worker! Parando Movimento!"); - RedisService.AtualizarCampos( - CtxKey.DadosControle, - ("velocidade_sp", 0) - ); - } + op.Sensoriamento?.InserirLog( + T_Code.Npc, + StatusModulo.Falha, + 0, + "Muito tempo sem receber novo comando " + + "do Manager Worker! Parando movimento!" + ); + + Variaveis.MostrarLog( + "Muito tempo sem receber novo comando " + + "do Manager Worker! Parando movimento!" + ); + + RedisService.AtualizarCampos( + CtxKey.DadosControle, + ("velocidade_sp", 0) + ); } - else if (_Controle.ticks_sem_resposta.AddMilliseconds(min_ticks_sem_resposta) < agora) + + return; + } + + if (venceuTimeoutMinimo) + { + double velocidadeSegura = op.Parametros.Controle.MovVelocidadeCErvasPercent; + if (op.Controle?.RPM_SP > velocidadeSegura) { - if (op.Controle?.RPM_SP > op.Parametros.Controle.MovVelocidadeCErvasPercent) + op.Sensoriamento?.InserirLog( + T_Code.Npc, + StatusModulo.Alerta, + 80, + "Muito tempo sem receber novo comando " + + "do Manager Worker! Reduzindo velocidade!" + ); + + Variaveis.MostrarLog( + "Muito tempo sem receber novo comando " + + "do Manager Worker! Reduzindo velocidade!" + ); + + RedisService.AtualizarCampos( + CtxKey.DadosControle, + ( + "velocidade_sp", + velocidadeSegura + ) + ); + } + } + } + + + private bool RecuperarAvancoPorPontoFuturo() + { + if (!_TrajetoriaFixaDefinida) + return false; + + int idxAtual = PontoAtual?.idxPonto ?? ObterIdxUltimoVisitado(); + if (idxAtual < 0) + idxAtual = 0; + + int idxFim = Math.Min(_TrajetoriaFixa.Count - 1, idxAtual + JanelaBuscaAvancoPontos); + + int idxCorredorAtual = CorredorAtual?.Idx ?? PontoAtual?.idxCorredor ?? 0; + + int idxPrimeiroNaoVisitado = _TrajetoriaFixa.FindIndex(Math.Max(0, idxAtual + 1), x => !x.Visitado); + + int idxBarreiraEstrutural = -1; + + if (idxPrimeiroNaoVisitado >= 0) + { + for (int i = idxPrimeiroNaoVisitado; + i <= idxFim; + i++) + { + var ponto = _TrajetoriaFixa[i]; + + if (ponto.idxCorredor != idxCorredorAtual) + break; + + /* + * Não pode escolher um ponto depois de uma borda ou ligação + * que ainda não foi confirmada. + */ + if (idxBarreiraEstrutural >= 0 && i > idxBarreiraEstrutural) + break; + + bool pontoEstrutural = + ponto.PontoBorda || + ponto.PontoLigacao || + ponto.Tipo == TipoPontoRua.BordaSaida || + ponto.Tipo == TipoPontoRua.LigacaoSaida || + ponto.Tipo == TipoPontoRua.BordaEntrada || + ponto.Tipo == TipoPontoRua.LigacaoEntrada; + + if (pontoEstrutural) { - op.Sensoriamento?.InserirLog(T_Code.Npc, StatusModulo.Alerta, 80, "Muito tempo sem receber novo comando do Manager Worker! Reduzindo Velocidade!"); - Variaveis.MostrarLog("Muito tempo sem receber novo comando do Manager Worker! Reduzindo Velocidade!"); - RedisService.AtualizarCampos( - CtxKey.DadosControle, - ("velocidade_sp", op.Parametros.Controle.MovVelocidadeCErvasPercent) - ); + idxBarreiraEstrutural = i; + break; } } } + + int idxMelhor = -1; + double melhorDist = double.MaxValue; + + AtualizarPropriedadesPontos(idxAtual + 1, idxFim); + + for (int i = idxAtual + 1; i <= idxFim; i++) + { + var ponto = _TrajetoriaFixa[i]; + + // Recuperação local não pode trocar corredor. + // A troca de corredor fica exclusivamente em RecuperarTransicaoEntreCorredores(). + if (ponto.idxCorredor != idxCorredorAtual) + continue; + + double margemBase = ponto.LarguraCorredor * 0.8; + double margem = Math.Max( + margemBase + MargemExtraAvancoM, + DistanciaMaximaEntreLeituras + margemBase + ); + + if (ponto.DistanciaAtual <= margem && ponto.DistanciaAtual < melhorDist) + { + idxMelhor = i; + melhorDist = ponto.DistanciaAtual; + } + } + + if (idxMelhor > idxAtual) + { + MarcarVisitadoAte(idxMelhor); + DefinirPontoAtual(); + DefinirProximoPonto(); + + Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Operante, + 100, + $"[TRJ] Avanço recuperado por ponto futuro no mesmo corredor. idx={idxMelhor}, dist={melhorDist:F2}m" + ); + + return true; + } + + return false; + } + + private bool RecuperarTransicaoEntreCorredores() + { + if (!_TrajetoriaFixaDefinida || + CorredorAtual == null || + CorredorAtual.Ultimo || + _Corredores == null) + { + return false; + } + + int idxCorredorAtual = CorredorAtual.Idx; + int idxProximoCorredor = idxCorredorAtual + 1; + + if (idxProximoCorredor < 0 || + idxProximoCorredor >= _Corredores.Count) + { + return false; + } + + var pontosCorredorAtual = CorredorAtual.Pontos? + .OrderBy(x => x.idxPontoCorredor) + .ToList(); + + if (pontosCorredorAtual == null || + pontosCorredorAtual.Count == 0) + { + return false; + } + + /* + * Atualiza somente a região final do corredor atual. + * Isso evita decidir com DistanciaAtual/Aproximando antigos. + */ + foreach (var ponto in pontosCorredorAtual + .Skip(Math.Max(0, pontosCorredorAtual.Count - 8))) + { + ponto.AtualizarPropriedades(); + } + + CorredorAtual.AtualizarDados(); + + var ultimoPontoCorredorAtual = + pontosCorredorAtual[pontosCorredorAtual.Count - 1]; + + int pontosRestantesCorredorAtual = + pontosCorredorAtual.Count(x => !x.Visitado); + + /* + * Regra soberana: + * + * Com PontosRestantesMaximosParaTrocarCorredor = 0, + * nenhum ponto do corredor atual pode ficar pendente. + * + * BordaSaida não conclui o corredor se LigacaoSaida + * ainda não foi visitada. + */ + bool corredorAtualConcluido = + pontosRestantesCorredorAtual <= + PontosRestantesMaximosParaTrocarCorredor && + ultimoPontoCorredorAtual.Visitado; + + if (!corredorAtualConcluido) + { + return false; + } + + var proximoCorredor = + _Corredores[idxProximoCorredor]; + + var primeiroPontoProximoCorredor = + proximoCorredor.Pontos? + .OrderBy(x => x.idxPontoCorredor) + .FirstOrDefault(); + + if (primeiroPontoProximoCorredor == null) + { + return false; + } + + /* + * Só observa o PRIMEIRO ponto do próximo corredor. + * + * Não procura o mais próximo entre vários pontos, + * porque isso permitia pular: + * + * LigacaoEntrada -> BordaEntrada -> ponto interno. + */ + primeiroPontoProximoCorredor.AtualizarPropriedades(); + + double distanciaEntrada = + primeiroPontoProximoCorredor.DistanciaAtual; + + bool entradaProximoCorredorAlcancada = + distanciaEntrada <= + DistanciaRestanteMaximaParaTrocarCorredorM; + + if (!entradaProximoCorredorAlcancada) + { + return false; + } + + /* + * Marca somente o primeiro ponto do novo corredor. + * + * Nunca utilizar MarcarVisitadoAte(idxMelhor) aqui, + * pois ele pode confirmar pontos internos não percorridos. + */ + primeiroPontoProximoCorredor.Visitado = true; + + DefinirPontoAtual(); + AtualizarCorredorAtual(); + DefinirProximoPonto(); + + Variaveis + .OperacaoEmAndamento + .Sensoriamento? + .InserirLog( + T_Code.Trj, + StatusModulo.Operante, + 100, + "[TRJ] Transição confirmada de forma estrita. " + + $"corredor={idxCorredorAtual}->{idxProximoCorredor}, " + + $"primeiro_idx={primeiroPontoProximoCorredor.idxPonto}, " + + $"dist={distanciaEntrada:F2}m, " + + $"restantes_anterior={pontosRestantesCorredorAtual}" + ); + + return true; + } + + private bool RecuperarPontosDeixadosParaTras() + { + if (!_TrajetoriaFixaDefinida || + GPSPosicaoAtual == null || + PontoAtual == null) + { + return false; + } + + int idxAtual = Math.Max( + 0, + Math.Min( + PontoAtual.idxPonto, + _TrajetoriaFixa.Count - 1 + ) + ); + + int idxCorredorAtual = + CorredorAtual?.Idx ?? + PontoAtual.idxCorredor; + + int idxPrimeiroNaoVisitado = + _TrajetoriaFixa.FindIndex( + idxAtual + 1, + x => !x.Visitado + ); + + if (idxPrimeiroNaoVisitado < 0) + return false; + + bool recuperou = false; + int quantidadeRecuperada = 0; + + int idxLimite = Math.Min( + _TrajetoriaFixa.Count - 1, + idxPrimeiroNaoVisitado + + MaxPontosRecuperadosPorCiclo - 1 + ); + + for (int i = idxPrimeiroNaoVisitado; + i <= idxLimite; + i++) + { + var ponto = _TrajetoriaFixa[i]; + + /* + * Recuperação local nunca atravessa corredor. + */ + if (ponto.idxCorredor != idxCorredorAtual) + break; + + /* + * Borda e ligação são confirmadas pelos fluxos + * específicos de entrada, saída e transição. + */ + if (EhPontoEstrutural(ponto)) + break; + + if (i + 1 >= _TrajetoriaFixa.Count) + break; + + var proximo = _TrajetoriaFixa[i + 1]; + + if (proximo.idxCorredor != idxCorredorAtual) + break; + + ponto.AtualizarPropriedades(); + proximo.AtualizarPropriedades(); + + var progresso = + CalcularProgressoNoSegmento( + ponto.Posicao, + proximo.Posicao, + GPSPosicaoAtual + ); + + /* + * avançoLongitudinal: + * + * < 0 -> rover ainda está antes do ponto + * = 0 -> rover está na perpendicular do ponto + * > 0 -> ponto ficou atrás do rover + */ + bool pontoFicouParaTras = + progresso.avancoLongitudinalM >= + -ToleranciaLongitudinalAntesPontoM && + progresso.distanciaLateralM <= + ObterToleranciaLateralRecuperacao(ponto); + + if (!pontoFicouParaTras) + { + /* + * Regra fundamental: + * se este ponto ainda não ficou para trás, + * nenhum ponto posterior pode ser recuperado. + */ + break; + } + + ponto.Visitado = true; + + recuperou = true; + quantidadeRecuperada++; + } + + if (!recuperou) + return false; + + DefinirPontoAtual(); + AtualizarCorredorAtual(); + DefinirProximoPonto(); + + Variaveis + .OperacaoEmAndamento + .Sensoriamento? + .InserirLog( + T_Code.Trj, + StatusModulo.Operante, + 100, + "[TRJ] Pontos deixados para trás recuperados. " + + $"qtd={quantidadeRecuperada}, " + + $"novo_atual={PontoAtual?.idxPonto}, " + + $"proximo={ProximoPonto?.idxPonto}" + ); + + return true; + } + private static bool EhPontoEstrutural(PontoTrajetoriaModel ponto) + { + if (ponto == null) + return true; + + return + ponto.PontoBorda || + ponto.PontoLigacao || + ponto.Tipo == TipoPontoRua.BordaEntrada || + ponto.Tipo == TipoPontoRua.BordaSaida || + ponto.Tipo == TipoPontoRua.LigacaoEntrada || + ponto.Tipo == TipoPontoRua.LigacaoSaida; + } + private double ObterToleranciaLateralRecuperacao(PontoTrajetoriaModel ponto) + { + double margemPonto = + Math.Max( + 0.30, + ponto?.DistanciaMargem ?? 0.70 + ); + + /* + * Não deixa uma largura muito grande do corredor + * transformar a recuperação em teletransporte lateral. + */ + return Math.Min( + ToleranciaLateralRecuperacaoM, + margemPonto + MargemExtraAvancoM + ); + } + private static (double avancoLongitudinalM, double distanciaLateralM, double parametroSegmento) CalcularProgressoNoSegmento(GPSModel inicio, GPSModel fim, GPSModel robo) + { + if (inicio == null || fim == null || robo == null) + { + return ( + double.NegativeInfinity, + double.PositiveInfinity, + 0.0 + ); + } + + double latRefRad = + inicio.Latitude * + Math.PI / 180.0; + + const double metrosPorGrauLat = + 110540.0; + + double metrosPorGrauLon = + 111320.0 * + Math.Cos(latRefRad); + + double vx = + (fim.Longitude - inicio.Longitude) * + metrosPorGrauLon; + + double vy = + (fim.Latitude - inicio.Latitude) * + metrosPorGrauLat; + + double wx = + (robo.Longitude - inicio.Longitude) * + metrosPorGrauLon; + + double wy = + (robo.Latitude - inicio.Latitude) * + metrosPorGrauLat; + + double comprimentoQuadrado = + vx * vx + vy * vy; + + if (comprimentoQuadrado < 1e-8) + { + return ( + double.NegativeInfinity, + GPSUtils.DistanciaEntrePontos( + inicio, + robo + ), + 0.0 + ); + } + + double comprimento = + Math.Sqrt(comprimentoQuadrado); + + double parametro = + (wx * vx + wy * vy) / + comprimentoQuadrado; + + double avancoLongitudinal = + (wx * vx + wy * vy) / + comprimento; + + double produtoVetorial = + vx * wy - vy * wx; + + double distanciaLateral = + Math.Abs(produtoVetorial) / + comprimento; + + return ( + avancoLongitudinal, + distanciaLateral, + parametro + ); } @@ -2657,11 +3338,22 @@ namespace AgroBase.Models var op = Variaveis.OperacaoEmAndamento; if (!op.Trajetoria._TrajetoriaFixaDefinida) + return; + + var pontoAtual = op.Trajetoria.PontoAtual; + if (pontoAtual == null) { + DistanciaTrajeto = DistanciaAtual; + return; + } + + var primeiroNaoVisitado = op.Trajetoria._TrajetoriaFixa.FirstOrDefault(x => !x.Visitado); + if (primeiroNaoVisitado == null) + { + DistanciaTrajeto = 0; return; } - var pontoAtual = op.Trajetoria.PontoAtual; int idxPontoAtual = pontoAtual.idxPonto + 1; int qtdPontos = Math.Abs(idxPonto - idxPontoAtual) + 1; // Sempre positivo @@ -2669,7 +3361,7 @@ namespace AgroBase.Models { //DistanciaTrajeto = pontoAtual.Aproximando ? pontoAtual.DistanciaAtual : -pontoAtual.DistanciaAtual; //DistanciaTrajeto = GPSUtils.DistanciaEntrePontos(op.Trajetoria._TrajetoriaDinamica.First().Posicao, Posicao); - DistanciaTrajeto = GPSUtils.DistanciaEntrePontos(op.Trajetoria._TrajetoriaFixa.Where(x => !x.Visitado).First().Posicao, Posicao); + DistanciaTrajeto = GPSUtils.DistanciaEntrePontos(primeiroNaoVisitado.Posicao, Posicao); return; } diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index a59e65c9c..83682402c 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -27,7 +27,7 @@ namespace AgroBase.Models { public class Variaveis { - public static readonly bool IniciarWorkers = true; + public static readonly bool IniciarWorkers = false; public static readonly bool Producao = false; public static bool DebugMode { get; set; } = true; public static bool Fechando { get; set; } = false; @@ -859,8 +859,8 @@ namespace AgroBase.Models public static string TopicoMqttParametros { get; } = $"agrobot/v1/rover//parameters"; public static double LarguraEsquerda { get; } = 44.0; // 62 public static double LarguraDireita { get; } = 44.0; // 22 - public static double ComprimentoFrente { get; } = 7.0; // 7 - public static double ComprimentoTras { get; } = 107.0; // 107 + public static double ComprimentoFrente { get; } = 90.0; // 7 + public static double ComprimentoTras { get; } = 40.0; // 107 public static double DistanciaEntreEixos { get; } = 92.0; public static double LeverArmFrontalCm { get; } = 37.0; // cm public static double LeverArmLateralCm { get; } = 0.0; // cm @@ -882,7 +882,7 @@ namespace AgroBase.Models public static int QuantidadeBicosPulverizadores { get; set; } = 7; public static double AlturaBarraPulverizadoraCm { get; set; } = 80; public static double ComprimentoBarraPulverizadoraCm { get; set; } = 103.7; - public static double DistanciaEntreBicosCm { get; set; } = 50; + public static double DistanciaEntreBicosCm { get; set; } = 21; public static double DensidadeHerbicida { get; set; } = 1.0; public static double CapacidadeReservatorio { get; set; } = 60; public static double PressaoMinimaCavitacao { get; set; } = 10.0; @@ -962,8 +962,8 @@ namespace AgroBase.Models { TipoMovimentoDirecional.MovimentoLateral, 90 }, { TipoMovimentoDirecional.Diagnostico, 25 }, }; - public static double AnguloInclinacaoRollMax { get; set; } = 30.0; // Frontal - public static double AnguloInclinacaoPitchMax { get; set; } = 45.0; // Lateral + public static double AnguloInclinacaoRollMax { get; set; } = 45.0; // Frontal + public static double AnguloInclinacaoPitchMax { get; set; } = 25.0; // Lateral public static string VozAlerta { get; set; } = "Microsoft Maria Desktop"; @@ -1170,32 +1170,51 @@ namespace AgroBase.Models double dLat = (dy / R_earth) * 180.0 / Math.PI; double dLon = (dx / (R_earth * Math.Cos(pos.Latitude * Math.PI / 180.0))) * 180.0 / Math.PI; - double latitude = pos.Latitude + dLat; - double longitude = pos.Longitude + dLon; + double latitude = pos.LatitudeAnt + dLat; + double longitude = pos.LongitudeAnt + dLon; + + DateTime agora = DateTime.Now; + double agoraMono = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; + + double orientacaoRealDeg = GPSUtils.NormalizarAngulo(theta * 180.0 / Math.PI); + double orientacaoMovimentoDeg = GPSUtils.NormalizarAngulo(heading * 180.0 / Math.PI); GPSModel novaPosicao = new GPSModel { - Momento = DateTime.Now, + Momento = agora, + DataHora = agora, + UltimoComandoRespondido = agora, Lat0 = GPSService.UltimaLeitura.Lat0, Lon0 = GPSService.UltimaLeitura.Lon0, - Latitude = latitude, LatitudeAnt = latitude, - Longitude = longitude, LongitudeAnt = longitude, - OrientacaoReal = GPSUtils.NormalizarAngulo(theta * 180.0 / Math.PI), + OrientacaoReal = orientacaoRealDeg, + AnguloCarroDefinido = orientacaoRealDeg, + OrientacaoMovimento = orientacaoMovimentoDeg, + TipoOrientacao = "A", Distancia = velocidadeMs * tempoDelta, Velocidade = velocidadeMs, - Heartbeat = ultimaPosicao.Heartbeat, + Heartbeat = ultimaPosicao.Heartbeat + 1, TimestampOri = ultimaPosicao.TimestampOri.Clone(), TimestampPos = ultimaPosicao.TimestampPos.Clone(), }; + var corrigida = + GPSService.LeverArm.FixLeverArmLatLon_Fast( + novaPosicao.LatitudeAnt, + novaPosicao.LongitudeAnt, + novaPosicao.OrientacaoReal, + 5 + ); + novaPosicao.Latitude = corrigida.lat; + novaPosicao.Longitude = corrigida.lon; + //(double latCor, double lonCorr) = GeoLeverArm.FixLeverArmLatLon_Fast(latitude, longitude, novaPosicao.OrientacaoReal); //novaPosicao.Latitude = latCor; //novaPosicao.Longitude = lonCorr; - novaPosicao.TimestampOri.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; - novaPosicao.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; + novaPosicao.TimestampOri.valor = agoraMono; + novaPosicao.TimestampPos.valor = agoraMono; return novaPosicao; } diff --git a/AgroBase/AgroBase/Services/AudioAlertaService.cs b/AgroBase/AgroBase/Services/AudioAlertaService.cs index 7fc2ea09f..8aad7f1c3 100644 --- a/AgroBase/AgroBase/Services/AudioAlertaService.cs +++ b/AgroBase/AgroBase/Services/AudioAlertaService.cs @@ -94,7 +94,7 @@ namespace AgroBase.Services // ------- helpers locais ------- string SegundosFmt(double s) { - int v = (int)Math.Round(s); + int v = Math.Max(0, (int)Math.Ceiling(s)); return v == 1 ? "1 segundo" : $"{v} segundos"; } diff --git a/AgroBase/AgroBase/Services/GPSService.cs b/AgroBase/AgroBase/Services/GPSService.cs index 009010738..c9472b9f6 100644 --- a/AgroBase/AgroBase/Services/GPSService.cs +++ b/AgroBase/AgroBase/Services/GPSService.cs @@ -2189,28 +2189,15 @@ namespace AgroBase.Services AtualizarTrajetoriaDinamica(); - int enderecoEquipamento = 1; + int enderecoEquipamento = 0x02; - EnviarCoordenadasParaMapa( - UltimaLeitura.Latitude, - UltimaLeitura.Longitude, - UltimaLeitura.AnguloCarroDefinido, - op.Sensoriamento.Operacao.OperacaoIniciada, - enderecoEquipamento - ); + EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, UltimaLeitura.AnguloCarroDefinido, op.Sensoriamento.Operacao.OperacaoIniciada, enderecoEquipamento); GPSModel posicaoBase = VariaveisOperacao.ObterPosicaoBaseSnapshot(); if ((posicaoBase?.UltimoComandoRespondido ?? DateTime.MinValue) > DateTime.UtcNow.AddSeconds(-60)) { - int enderecoBase = 0; - - EnviarCoordenadasParaMapa( - posicaoBase.Latitude, - posicaoBase.Longitude, - posicaoBase.OrientacaoReal, - false, - enderecoBase - ); + int enderecoBase = 0x01; + EnviarCoordenadasParaMapa(posicaoBase.Latitude, posicaoBase.Longitude, posicaoBase.OrientacaoReal, false, enderecoBase); } PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado; @@ -2375,23 +2362,10 @@ namespace AgroBase.Services { if (UltimasLeituras.Count < 2) { - double ang = - GPSUtils.CalcularOrientacao( - PenultimaLeitura, - UltimaLeitura - ); - - double d = - GPSUtils.DistanciaEntrePontos( - PenultimaLeitura, - UltimaLeitura - ); - - PenultimaLeitura.OrientacaoMovimento = - UltimaLeitura.OrientacaoMovimento; - - PenultimaLeitura.Distancia = - UltimaLeitura.Distancia; + double ang = GPSUtils.CalcularOrientacao(PenultimaLeitura, UltimaLeitura); + double d = GPSUtils.DistanciaEntrePontos(PenultimaLeitura, UltimaLeitura); + PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento; + PenultimaLeitura.Distancia = UltimaLeitura.Distancia; UltimaLeitura.OrientacaoMovimento = ang; UltimaLeitura.Distancia = d; @@ -2402,24 +2376,19 @@ namespace AgroBase.Services double sumY = 0; double distAcum = 0; - for (int i = 0; - i < UltimasLeituras.Count - 1; - i++) + for (int i = 0; i < UltimasLeituras.Count - 1; i++) { GPSModel a = UltimasLeituras[i]; GPSModel b = UltimasLeituras[i + 1]; - double angSeg = - GPSUtils.CalcularOrientacao(a, b); + double angSeg = GPSUtils.CalcularOrientacao(a, b); - double dSeg = - GPSUtils.DistanciaEntrePontos(a, b); + double dSeg = GPSUtils.DistanciaEntrePontos(a, b); if (dSeg <= 0) continue; - double rad = - angSeg * Math.PI / 180.0; + double rad = angSeg * Math.PI / 180.0; sumX += Math.Cos(rad) * dSeg; sumY += Math.Sin(rad) * dSeg; @@ -2430,44 +2399,22 @@ namespace AgroBase.Services if (distAcum <= 0) { - anguloMovimento = - UltimaLeitura.OrientacaoReal; + anguloMovimento = UltimaLeitura.OrientacaoReal; } else { - anguloMovimento = - Math.Atan2(sumY, sumX) * - 180.0 / Math.PI; - - anguloMovimento = - GPSUtils.NormalizarAngulo( - anguloMovimento - ); + anguloMovimento = Math.Atan2(sumY, sumX) * 180.0 / Math.PI; + anguloMovimento = GPSUtils.NormalizarAngulo(anguloMovimento); } - GPSModel first = - UltimasLeituras[0]; + GPSModel first = UltimasLeituras[0]; + GPSModel last = UltimasLeituras[UltimasLeituras.Count - 1]; - GPSModel last = - UltimasLeituras[ - UltimasLeituras.Count - 1 - ]; - - double distLinear = - GPSUtils.DistanciaEntrePontos( - first, - last - ); - - PenultimaLeitura.OrientacaoMovimento = - UltimaLeitura.OrientacaoMovimento; - - PenultimaLeitura.Distancia = - UltimaLeitura.Distancia; - - UltimaLeitura.OrientacaoMovimento = - anguloMovimento; + double distLinear = GPSUtils.DistanciaEntrePontos(first, last); + PenultimaLeitura.OrientacaoMovimento = UltimaLeitura.OrientacaoMovimento; + PenultimaLeitura.Distancia = UltimaLeitura.Distancia; + UltimaLeitura.OrientacaoMovimento = anguloMovimento; UltimaLeitura.Distancia = distLinear; } diff --git a/AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs index e373792bd..b397522c3 100644 --- a/AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs @@ -236,6 +236,10 @@ namespace AgroBase.Services.Operadores versao_comando = GetInt(dictParams, "versao_comando", 1), ts_comando = GetDouble(dictParams, "ts_comando", 0.0), + sessao_comando = dictParams.Value("sessao_comando"), + seq_movimento = dictParams["seq_movimento"]?.Value(), + seq_direcional = dictParams["seq_direcional"]?.Value(), + // Default true para compatibilidade com contrato antigo atualizar_movimento = GetBool(dictParams, "atualizar_movimento", true), atualizar_direcional = GetBool(dictParams, "atualizar_direcional", true), @@ -283,33 +287,80 @@ namespace AgroBase.Services.Operadores var op = Variaveis.OperacaoEmAndamento; - var _Controle = op.Controle; - var pControle = op.Parametros.Controle; - - if (_Controle == null || pControle == null) + if (op == null) return; - _Controle.Motivos = (novoComando.motivos ?? new string[0]).ToList(); + var controle = op.Controle; + var parametros = op.Parametros?.Controle; - bool podeAtualizarMovimento = - novoComando.atualizar_movimento && - pControle.MovimentoAutomatico; + if (controle == null || parametros == null) + return; - bool podeAtualizarDirecional = - novoComando.atualizar_direcional && - pControle.DirecionalAutomatico; + controle.Motivos = (novoComando.motivos ?? Array.Empty()).ToList(); - bool movimentoMudou = - _Controle.PercentualVelocidadeSP != novoComando.percentual_velocidade || - _Controle.EmFreio != novoComando.em_freio; + bool sessaoMudou = !string.IsNullOrWhiteSpace(novoComando.sessao_comando) && !string.Equals(controle.UltimaSessaoComando, novoComando.sessao_comando, StringComparison.Ordinal); - bool direcionalMudou = - _Controle.Angulo != novoComando.angulo || - _Controle.TipoMovimento != novoComando.tipo_movimento; + bool movimentoMudou = Math.Abs(controle.PercentualVelocidadeSP - novoComando.percentual_velocidade) >= 0.001 || controle.EmFreio != novoComando.em_freio; - if (podeAtualizarMovimento && (movimentoMudou || forcarAplicacao)) + bool direcionalMudou = Math.Abs(controle.Angulo - novoComando.angulo) >= 0.001 || controle.TipoMovimento != novoComando.tipo_movimento; + + bool seqMovimentoMudou = + novoComando.seq_movimento.HasValue && + ( + sessaoMudou || + !controle.UltimaSeqMovimentoAplicada.HasValue || + controle.UltimaSeqMovimentoAplicada.Value != + novoComando.seq_movimento.Value + ); + + bool seqDirecionalMudou = + novoComando.seq_direcional.HasValue && + ( + sessaoMudou || + !controle.UltimaSeqDirecionalAplicada.HasValue || + controle.UltimaSeqDirecionalAplicada.Value != + novoComando.seq_direcional.Value + ); + + /* + * Compatibilidade: + * + * Mensagem nova: + * sequência é a autoridade. + * + * Mensagem antiga: + * usa flag e mudança de valor. + */ + bool mensagemPossuiSequenciaMov = novoComando.seq_movimento.HasValue; + bool mensagemPossuiSequenciaDir = novoComando.seq_direcional.HasValue; + + bool aplicarMovimento = + parametros.MovimentoAutomatico && + ( + forcarAplicacao || + movimentoMudou || + seqMovimentoMudou || + ( + !mensagemPossuiSequenciaMov && + novoComando.atualizar_movimento + ) + ); + + bool aplicarDirecional = + parametros.DirecionalAutomatico && + ( + forcarAplicacao || + direcionalMudou || + seqDirecionalMudou || + ( + !mensagemPossuiSequenciaDir && + novoComando.atualizar_direcional + ) + ); + + if (aplicarMovimento) { - var tipoMov = _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov); + var tipoMov = controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov); if (tipoMov != null) { @@ -319,15 +370,21 @@ namespace AgroBase.Services.Operadores : Direcao.Parado; } - _Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade; - _Controle.EmFreio = pControle.FrenagemAutomaticaAoParar && novoComando.em_freio; + controle.PercentualVelocidadeSP = novoComando.percentual_velocidade; - op.AtualizarDadosControle(false, new List() { T_Code.Mov }); + controle.EmFreio = parametros.FrenagemAutomaticaAoParar && novoComando.em_freio; + + op.AtualizarDadosControle(false, new List { T_Code.Mov }); + + if (novoComando.seq_movimento.HasValue) + { + controle.UltimaSeqMovimentoAplicada = novoComando.seq_movimento.Value; + } } - if (podeAtualizarDirecional && (direcionalMudou || forcarAplicacao)) + if (aplicarDirecional) { - var tipoDir = _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir); + var tipoDir = controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir); if (tipoDir != null) { @@ -339,26 +396,38 @@ namespace AgroBase.Services.Operadores : Direcao.Parado; } - _Controle.Angulo = novoComando.angulo; - _Controle.TipoMovimento = novoComando.tipo_movimento; + controle.Angulo = novoComando.angulo; - _Controle.SimulacaoMPC = novoComando.simulacao? - .Where(x => x != null && x.Length >= 3) - .Select(x => new MPCSimulacaoModel() - { - latitude = x[0], - longitude = x[1], - orientacao = x[2] - }) - .ToList() ?? new List(); + op.DefinirTipoMovimentoControle(novoComando.tipo_movimento); - _Controle.Latencia = novoComando.latencia; - _Controle.ErroLateral = novoComando.erro_lateral; - _Controle.DebugCustoMpc = - novoComando.debug_custo ?? - new Dictionary(); + controle.SimulacaoMPC = novoComando.simulacao? + .Where(x => x != null && x.Length >= 3) + .Select( + x => new MPCSimulacaoModel + { + latitude = x[0], + longitude = x[1], + orientacao = x[2] + } + ) + .ToList() + ?? new List(); - op.AtualizarDadosControle(false, new List() { T_Code.Dir }); + controle.Latencia = novoComando.latencia; + controle.ErroLateral = novoComando.erro_lateral; + controle.DebugCustoMpc = novoComando.debug_custo ?? new Dictionary(); + + op.AtualizarDadosControle(false, new List { T_Code.Dir }); + + if (novoComando.seq_direcional.HasValue) + { + controle.UltimaSeqDirecionalAplicada = novoComando.seq_direcional.Value; + } + } + + if (!string.IsNullOrWhiteSpace(novoComando.sessao_comando)) + { + controle.UltimaSessaoComando = novoComando.sessao_comando; } } diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py index 52b30cc25..ce5c72422 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py @@ -1,4 +1,5 @@ import time +import uuid from shared.enums import TipoMovimentoDirecional from shared.contexto_global_redis import ContextoGlobalRedis @@ -16,7 +17,11 @@ class ProcessadorEmAndamento(ProcessadorBase): def __init__(self): self.filtro_mov = FiltroVelocidade(alpha=0.7) - ang_max = ContextoGlobalRedis.get_controle().get("angulo_max", 30) + controle = ContextoGlobalRedis.get_controle() or {} + ang_max = self._to_float( + controle.get("angulo_max", 30.0), + 30.0 + ) self.pid_dir = PIDAdaptativo( kp_base=0.55, @@ -31,7 +36,14 @@ class ProcessadorEmAndamento(ProcessadorBase): agora = time.perf_counter() - # Cadências independentes + # Identifica esta execução do Manager. + # Se o Python reiniciar, o C# perceberá uma sessão nova. + self.sessao_comando = uuid.uuid4().hex + + # Sequências monotônicas por módulo. + self.seq_movimento = 0 + self.seq_direcional = 0 + self.ultimo_envio_envelope = agora self.ultimo_envio_movimento = agora self.ultimo_envio_direcional = agora @@ -40,81 +52,107 @@ class ProcessadorEmAndamento(ProcessadorBase): self.intervalo_movimento_s = 0.25 self.intervalo_direcional_s = 0.30 - # Memória local para evitar publicar movimento idêntico o tempo todo self.ultima_velocidade_enviada = None self.ultimo_freio_enviado = None + self.ultimo_angulo_enviado = None self.ultimo_tipo_enviado = None - # Evita spam de log de bloqueio self.ultimo_log_bloqueio = 0.0 self.ultima_msg_bloqueio = "" self.regras = RegrasTaticas() - def processar(self): - try: - agora = time.perf_counter() + mostrar_log( + "🧭 ProcessadorEmAndamento criado | " + f"id={id(self)} | " + f"sessao={self.sessao_comando} | " + f"perf={agora:.4f}" + ) - _controle = ContextoGlobalRedis.get_controle() + def processar(self): + agora = time.perf_counter() + + try: + controle = ContextoGlobalRedis.get_controle() or {} movimento_automatico = bool( - _controle.get("movimento_automatico", False) + controle.get("movimento_automatico", False) ) direcional_automatico = bool( - _controle.get("direcional_automatico", False) + controle.get("direcional_automatico", False) ) - # Se nenhum controle automático está ativo, este processador não comanda nada. if not movimento_automatico and not direcional_automatico: return None - valores_atuais = self._ler_valores_atuais(_controle) + estado = self._ler_valores_atuais(controle) + + velocidade = estado["velocidade"] + frear = estado["frear"] + + angulo = estado["angulo"] + tipo_movimento = estado["tipo_movimento"] + + simulacao = estado["simulacao"] + latencia = estado["latencia"] + erro_lateral = estado["erro_lateral"] + debug_custo = estado["debug_custo"] motivos = [] erro_geral = False - velocidade = valores_atuais["velocidade"] - frear = valores_atuais["frear"] - angulo = valores_atuais["angulo"] - tipo_movimento = valores_atuais["tipo_movimento"] - simulacao = valores_atuais["simulacao"] - latencia = valores_atuais["latencia"] - erro_lateral = valores_atuais["erro_lateral"] - debug_custo = valores_atuais["debug_custo"] - atualizar_movimento = False atualizar_direcional = False # ============================================================ - # 1) Regras táticas globais + # 1. Bloqueios táticos # ============================================================ - motivos_parada, freio_necessario = self.regras.verifica_controle_liberado() + motivos_parada, freio_necessario = ( + self.regras.verifica_controle_liberado() + ) + motivos_parada = motivos_parada or [] if motivos_parada: self._logar_bloqueio(motivos_parada) - # Bloqueio global: para movimento e centraliza direcional, - # mas respeitando quais módulos estão em automático. - atualizar_movimento = movimento_automatico - atualizar_direcional = direcional_automatico - - if atualizar_movimento: + if movimento_automatico: velocidade = 0.0 frear = bool(freio_necessario) - if atualizar_direcional: + atualizar_movimento = ( + self._movimento_mudou( + velocidade, + frear + ) + or self._venceu_intervalo_movimento(agora) + ) + + if direcional_automatico: angulo = 0.0 - tipo_movimento = TipoMovimentoDirecional.RodasDianteiras.value + tipo_movimento = ( + TipoMovimentoDirecional + .RodasDianteiras + .value + ) + simulacao = [] latencia = 0.0 erro_lateral = 0.0 debug_custo = {} - return self._publicar_comando( + atualizar_direcional = ( + self._direcional_mudou( + angulo, + tipo_movimento + ) + or self._venceu_intervalo_direcional(agora) + ) + + return self._publicar_se_necessario( agora=agora, velocidade=velocidade, frear=frear, @@ -131,134 +169,69 @@ class ProcessadorEmAndamento(ProcessadorBase): ) # ============================================================ - # 2) Movimento automático + # 2. Movimento # ============================================================ if movimento_automatico: - comando_mov = definir_comando_mov(self.filtro_mov) + comando_mov = definir_comando_mov( + self.filtro_mov + ) - if isinstance(comando_mov, dict): - velocidade_calculada = self._to_float( - comando_mov.get("velocidade", velocidade), - velocidade - ) - - freio_calculado = bool( - comando_mov.get("frear", False) - ) - - velocidade = velocidade_calculada - frear = freio_calculado - - movimento_mudou = self._movimento_mudou( - velocidade, - frear - ) - - envio_movimento_periodico = ( - agora - self.ultimo_envio_movimento - ) >= self.intervalo_movimento_s - - atualizar_movimento = ( - movimento_mudou or - envio_movimento_periodico - ) - - else: - # Falha ao calcular movimento automático: falha segura. + if not isinstance(comando_mov, dict): erro_geral = True - motivos.append("Falha ao definir comando de movimento automático") + + motivos.append( + "Falha ao definir comando de movimento automático" + ) velocidade = 0.0 frear = True atualizar_movimento = True + else: + velocidade = self._to_float( + comando_mov.get( + "velocidade", + velocidade + ), + velocidade + ) + + frear = bool( + comando_mov.get( + "frear", + frear + ) + ) + + atualizar_movimento = ( + self._movimento_mudou( + velocidade, + frear + ) + or self._venceu_intervalo_movimento(agora) + ) + # ============================================================ - # 3) Direcional automático + # 3. Direcional # ============================================================ if direcional_automatico: - envio_direcional_necessario = ( - agora - self.ultimo_envio_direcional - ) >= self.intervalo_direcional_s + envio_periodico_necessario = ( + self._venceu_intervalo_direcional(agora) + ) comando_dir = definir_comando_dir( self.pid_dir, - envio_direcional_necessario + envio_periodico_necessario ) - if isinstance(comando_dir, dict): - erro_dir = bool(comando_dir.get("erro", False)) - parada_necessaria = bool( - comando_dir.get("parada_necessaria", False) - ) - - if erro_dir: - erro_geral = True - motivos.extend( - comando_dir.get( - "motivos", - ["Erro ao definir comando direcional automático"] - ) or [] - ) - - # Se o direcional falhou, não deixa o robô seguir andando - # sob movimento automático. - if movimento_automatico: - velocidade = 0.0 - frear = True - atualizar_movimento = True - - # Direcional vai para posição segura. - angulo = 0.0 - tipo_movimento = TipoMovimentoDirecional.RodasDianteiras.value - simulacao = [] - latencia = self._to_float(comando_dir.get("latencia", 0.0), 0.0) - erro_lateral = self._to_float(comando_dir.get("erro_lateral", 0.0), 0.0) - debug_custo = comando_dir.get("debug_custo", {}) or {} - atualizar_direcional = True - - else: - enviar_direcional = bool( - comando_dir.get("enviar_comando", False) - ) - - if enviar_direcional: - angulo = self._to_float( - comando_dir.get("angulo", angulo), - angulo - ) - - tipo_movimento = self._to_int( - comando_dir.get("tipo", tipo_movimento), - tipo_movimento - ) - - simulacao = comando_dir.get("simulacao", []) or [] - latencia = self._to_float( - comando_dir.get("latencia", latencia), - latencia - ) - erro_lateral = self._to_float( - comando_dir.get("erro_lateral", erro_lateral), - erro_lateral - ) - debug_custo = comando_dir.get("debug_custo", {}) or {} - - atualizar_direcional = True - - if parada_necessaria: - motivos.extend(comando_dir.get("motivos", []) or []) - - if movimento_automatico: - velocidade = 0.0 - frear = True - atualizar_movimento = True - - else: - # Falha ao calcular direcional automático: falha segura. + if not isinstance(comando_dir, dict): erro_geral = True - motivos.append("Falha ao definir comando direcional automático") + + motivos.append( + "Falha ao definir comando direcional automático" + ) if movimento_automatico: velocidade = 0.0 @@ -266,35 +239,204 @@ class ProcessadorEmAndamento(ProcessadorBase): atualizar_movimento = True angulo = 0.0 - tipo_movimento = TipoMovimentoDirecional.RodasDianteiras.value + tipo_movimento = ( + TipoMovimentoDirecional + .RodasDianteiras + .value + ) + simulacao = [] latencia = 0.0 erro_lateral = 0.0 debug_custo = {} + atualizar_direcional = True - # ============================================================ - # 4) Publicação do envelope - # ============================================================ + else: + erro_dir = bool( + comando_dir.get("erro", False) + ) - precisa_keepalive = ( - agora - self.ultimo_envio_envelope - ) >= self.intervalo_keepalive_s + if erro_dir: + erro_geral = True - deve_publicar = ( - atualizar_movimento or - atualizar_direcional or - precisa_keepalive - ) + motivos.extend( + comando_dir.get( + "motivos", + [ + "Erro ao definir comando " + "direcional automático" + ] + ) or [] + ) - if not deve_publicar: - return None + if movimento_automatico: + velocidade = 0.0 + frear = True + atualizar_movimento = True - # Keepalive puro: Manager vivo, mas nenhum módulo precisa reaplicar. - if precisa_keepalive and not atualizar_movimento and not atualizar_direcional: - motivos = [] + angulo = 0.0 + tipo_movimento = ( + TipoMovimentoDirecional + .RodasDianteiras + .value + ) - return self._publicar_comando( + simulacao = [] + + latencia = self._to_float( + comando_dir.get( + "latencia", + 0.0 + ), + 0.0 + ) + + erro_lateral = self._to_float( + comando_dir.get( + "erro_lateral", + 0.0 + ), + 0.0 + ) + + debug_custo = ( + comando_dir.get( + "debug_custo", + {} + ) + or {} + ) + + atualizar_direcional = True + + else: + angulo_calculado = self._to_float( + comando_dir.get( + "angulo", + angulo + ), + angulo + ) + + tipo_calculado = self._to_int( + comando_dir.get( + "tipo", + tipo_movimento + ), + tipo_movimento + ) + + simulacao_calculada = ( + comando_dir.get( + "simulacao", + [] + ) + or [] + ) + + latencia_calculada = self._to_float( + comando_dir.get( + "latencia", + latencia + ), + latencia + ) + + erro_lateral_calculado = self._to_float( + comando_dir.get( + "erro_lateral", + erro_lateral + ), + erro_lateral + ) + + debug_calculado = ( + comando_dir.get( + "debug_custo", + {} + ) + or {} + ) + + solicitou_envio_mpc = bool( + comando_dir.get( + "enviar_comando", + False + ) + ) + + direcional_mudou = ( + self._direcional_mudou( + angulo_calculado, + tipo_calculado + ) + ) + + envio_periodico = ( + self._venceu_intervalo_direcional( + agora + ) + ) + + atualizar_direcional = ( + solicitou_envio_mpc + or direcional_mudou + or envio_periodico + ) + + angulo = angulo_calculado + tipo_movimento = tipo_calculado + simulacao = simulacao_calculada + latencia = latencia_calculada + erro_lateral = erro_lateral_calculado + debug_custo = debug_calculado + + if not isinstance(debug_custo, dict): + debug_custo = {} + + debug_custo[ + "decisao_envio_direcional" + ] = { + "solicitou_envio_mpc": + solicitou_envio_mpc, + + "direcional_mudou": + direcional_mudou, + + "envio_periodico": + envio_periodico, + + "atualizar_direcional": + atualizar_direcional, + + "angulo_calculado": + round(angulo, 3), + + "angulo_ultimo_enviado": ( + None + if self.ultimo_angulo_enviado is None + else round( + self.ultimo_angulo_enviado, + 3 + ) + ), + + "tipo_calculado": + int(tipo_movimento), + + "tipo_ultimo_enviado": ( + self.ultimo_tipo_enviado + ), + + "seq_direcional_atual": + self.seq_direcional, + + "sessao_comando": + self.sessao_comando, + } + + return self._publicar_se_necessario( agora=agora, velocidade=velocidade, frear=frear, @@ -310,30 +452,86 @@ class ProcessadorEmAndamento(ProcessadorBase): atualizar_direcional=atualizar_direcional, ) - except Exception as e: - mostrar_log(f"Erro no processador EmAndamento: {e}") + except Exception as ex: + mostrar_log( + f"Erro no processador EmAndamento: {ex}" + ) - # Falha no orquestrador em operação automática: - # tenta publicar parada segura. try: - return comando_controle( - percentual_velocidade=0.0, + return self._publicar_comando( + agora=agora, + velocidade=0.0, frear=True, angulo=0.0, - tipo_movimento=TipoMovimentoDirecional.RodasDianteiras.value, + tipo_movimento=( + TipoMovimentoDirecional + .RodasDianteiras + .value + ), simulacao=[], erro=True, latencia=0.0, erro_lateral=0.0, debug_custo={}, - motivos=[f"Erro no processador EmAndamento: {e}"], + motivos=[ + f"Erro no processador EmAndamento: {ex}" + ], atualizar_movimento=True, atualizar_direcional=True, ) - except Exception as e2: - mostrar_log(f"Erro ao publicar parada segura no EmAndamento: {e2}") + + except Exception as ex2: + mostrar_log( + "Erro ao publicar parada segura no " + f"EmAndamento: {ex2}" + ) + return None + def _publicar_se_necessario( + self, + *, + agora, + velocidade, + frear, + angulo, + tipo_movimento, + simulacao, + erro, + latencia, + erro_lateral, + debug_custo, + motivos, + atualizar_movimento, + atualizar_direcional, + ): + precisa_keepalive = ( + agora - self.ultimo_envio_envelope + ) >= self.intervalo_keepalive_s + + if not ( + atualizar_movimento + or atualizar_direcional + or precisa_keepalive + ): + return None + + return self._publicar_comando( + agora=agora, + velocidade=velocidade, + frear=frear, + angulo=angulo, + tipo_movimento=tipo_movimento, + simulacao=simulacao, + erro=erro, + latencia=latencia, + erro_lateral=erro_lateral, + debug_custo=debug_custo, + motivos=motivos, + atualizar_movimento=atualizar_movimento, + atualizar_direcional=atualizar_direcional, + ) + def _publicar_comando( self, *, @@ -351,6 +549,14 @@ class ProcessadorEmAndamento(ProcessadorBase): atualizar_movimento, atualizar_direcional, ): + # A sequência aumenta somente quando aquele módulo + # deve reaplicar o comando. + if atualizar_movimento: + self.seq_movimento += 1 + + if atualizar_direcional: + self.seq_direcional += 1 + payload = comando_controle( percentual_velocidade=velocidade, frear=frear, @@ -362,77 +568,189 @@ class ProcessadorEmAndamento(ProcessadorBase): erro_lateral=erro_lateral, debug_custo=debug_custo, motivos=motivos or [], + atualizar_movimento=atualizar_movimento, atualizar_direcional=atualizar_direcional, + + seq_movimento=self.seq_movimento, + seq_direcional=self.seq_direcional, + sessao_comando=self.sessao_comando, ) self.ultimo_envio_envelope = agora if atualizar_movimento: self.ultimo_envio_movimento = agora - self.ultima_velocidade_enviada = velocidade - self.ultimo_freio_enviado = frear + self.ultima_velocidade_enviada = float( + velocidade + ) + self.ultimo_freio_enviado = bool(frear) if atualizar_direcional: self.ultimo_envio_direcional = agora - self.ultimo_angulo_enviado = angulo - self.ultimo_tipo_enviado = tipo_movimento + self.ultimo_angulo_enviado = float(angulo) + self.ultimo_tipo_enviado = int( + tipo_movimento + ) return payload - def _ler_valores_atuais(self, _controle): + def _venceu_intervalo_movimento(self, agora): + return ( + agora - self.ultimo_envio_movimento + ) >= self.intervalo_movimento_s + + def _venceu_intervalo_direcional(self, agora): + return ( + agora - self.ultimo_envio_direcional + ) >= self.intervalo_direcional_s + + def _ler_valores_atuais(self, controle): return { "velocidade": self._to_float( - _controle.get("velocidade_sp", 0.0), - 0.0 - ), - "frear": bool( - _controle.get("em_freio", False) - ), - "angulo": self._to_float( - _controle.get("angulo_sp", 0.0), - 0.0 - ), - "tipo_movimento": self._to_int( - _controle.get( - "tipo_movimento_direcional", - TipoMovimentoDirecional.RodasDianteiras.value + controle.get( + "velocidade_sp", + 0.0 ), - TipoMovimentoDirecional.RodasDianteiras.value + 0.0 ), - "simulacao": _controle.get("simulacao", []) or [], + + "frear": bool( + controle.get( + "em_freio", + False + ) + ), + + "angulo": self._to_float( + controle.get( + "angulo_sp", + 0.0 + ), + 0.0 + ), + + "tipo_movimento": self._to_int( + controle.get( + "tipo_movimento_direcional", + TipoMovimentoDirecional + .RodasDianteiras + .value + ), + TipoMovimentoDirecional + .RodasDianteiras + .value + ), + + "simulacao": ( + controle.get( + "simulacao", + [] + ) + or [] + ), + "latencia": self._to_float( - _controle.get("latencia", 0.0), + controle.get( + "latencia", + 0.0 + ), 0.0 ), + "erro_lateral": self._to_float( - _controle.get("erro_lateral", 0.0), + controle.get( + "erro_lateral", + 0.0 + ), 0.0 ), - "debug_custo": _controle.get("debug_custo", {}) or {}, + + "debug_custo": ( + controle.get( + "debug_custo", + {} + ) + or {} + ), } - def _movimento_mudou(self, velocidade, frear): + def _movimento_mudou( + self, + velocidade, + frear, + tolerancia_velocidade=0.05 + ): if self.ultima_velocidade_enviada is None: return True if self.ultimo_freio_enviado is None: return True - if bool(frear) != bool(self.ultimo_freio_enviado): + if bool(frear) != bool( + self.ultimo_freio_enviado + ): return True return abs( - float(velocidade) - float(self.ultima_velocidade_enviada) - ) >= 0.05 + float(velocidade) + - float(self.ultima_velocidade_enviada) + ) >= tolerancia_velocidade + + def _direcional_mudou( + self, + angulo, + tipo_movimento, + tolerancia_angulo=0.10 + ): + if self.ultimo_angulo_enviado is None: + return True + + if self.ultimo_tipo_enviado is None: + return True + + tipo_atual = self._to_int( + tipo_movimento, + TipoMovimentoDirecional + .RodasDianteiras + .value + ) + + tipo_anterior = self._to_int( + self.ultimo_tipo_enviado, + TipoMovimentoDirecional + .RodasDianteiras + .value + ) + + if tipo_atual != tipo_anterior: + return True + + return abs( + float(angulo) + - float(self.ultimo_angulo_enviado) + ) >= tolerancia_angulo def _logar_bloqueio(self, motivos): agora = time.perf_counter() - msg = " | ".join(map(str, motivos)) + mensagem = " | ".join( + map(str, motivos) + ) - if msg != self.ultima_msg_bloqueio or (agora - self.ultimo_log_bloqueio) >= 1.0: - mostrar_log(f"⛔ Controle bloqueado: {msg}") - self.ultima_msg_bloqueio = msg + mudou = ( + mensagem != self.ultima_msg_bloqueio + ) + + venceu_intervalo = ( + agora - self.ultimo_log_bloqueio + ) >= 1.0 + + if mudou or venceu_intervalo: + mostrar_log( + f"⛔ Controle bloqueado: {mensagem}" + ) + + self.ultima_msg_bloqueio = mensagem self.ultimo_log_bloqueio = agora @staticmethod @@ -440,7 +758,9 @@ class ProcessadorEmAndamento(ProcessadorBase): try: if valor is None: return float(padrao) + return float(valor) + except Exception: return float(padrao) @@ -454,5 +774,6 @@ class ProcessadorEmAndamento(ProcessadorBase): return int(valor.value) return int(valor) + except Exception: - return int(padrao) + return int(padrao) \ No newline at end of file diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py index 10f4c6fc4..4f64bc1f0 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py @@ -29,8 +29,11 @@ def comando_controle( erro_lateral: float = 0.0, debug_custo=None, motivos=None, - atualizar_movimento: bool = True, - atualizar_direcional: bool = True, + atualizar_movimento: bool = False, + atualizar_direcional: bool = False, + seq_movimento: int = 0, + seq_direcional: int = 0, + sessao_comando: str =None, ): _controle = ContextoGlobalRedis.get_controle() @@ -47,9 +50,14 @@ def comando_controle( hb_novo = _proximo_heartbeat(hb_atual) payload = { - "versao_comando": 2, + "versao_comando": 3, "ts_comando": time.time(), + "sessao_comando": sessao_comando, + + "seq_movimento": int(seq_movimento), + "seq_direcional": int(seq_direcional), + # Flags novas do contrato "atualizar_movimento": bool(atualizar_movimento), "atualizar_direcional": bool(atualizar_direcional),