From 5f04f9c5c67b8493140fc277623bd33c299d78c7 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Mon, 21 Sep 2026 10:38:11 -0300 Subject: [PATCH] revisao do retorno a base quando alinhado com a entrada do corredor e do mesmo lado que a base, inicializacao limpa e sincrona, inclusao do botao de referenciamento no menu da ihm, aleterado padroes da nova operacao --- .../IHM/Operacao/ucOperacaoParametros.cs | 1 + AgroBase/AgroBase/Forms/IHM/frmIHM.cs | 116 ++++- AgroBase/AgroBase/Forms/IHM/frmMenu.cs | 102 +++- AgroBase/AgroBase/Forms/IHM/frmOperacao.cs | 6 +- AgroBase/AgroBase/Forms/frmInstancial.cs | 6 +- .../Models/Operacoes/OperacaoModel.cs | 46 +- .../Models/TrajetoriaMapaOperacaoModel.cs | 444 +++++++++++++++++- 7 files changed, 660 insertions(+), 61 deletions(-) diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs index 12cb609ac..0a6b7428b 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs @@ -1,6 +1,7 @@ using AgroBase.Forms.IHM.Controls; using AgroBase.Forms.IHM.Operacao.Parametros; using AgroBase.Models; +using AgroBase.Models.Operacoes; using System; using System.Collections.Generic; using System.Drawing; diff --git a/AgroBase/AgroBase/Forms/IHM/frmIHM.cs b/AgroBase/AgroBase/Forms/IHM/frmIHM.cs index 0197d457e..0519099c1 100644 --- a/AgroBase/AgroBase/Forms/IHM/frmIHM.cs +++ b/AgroBase/AgroBase/Forms/IHM/frmIHM.cs @@ -6,6 +6,8 @@ using System; using System.Collections.Generic; using System.Drawing; using System.Linq; +using System.Diagnostics; +using System.Threading; using System.Threading.Tasks; using System.Windows.Forms; @@ -17,6 +19,46 @@ namespace AgroBase.Forms.IHM private OperacaoModel op => Variaveis.OperacaoEmAndamento; private Form _telaAtual; + private int _atualizacaoPendente; + private bool _consumoEstadoLiberado; + private System.Windows.Forms.Timer _pulsoUi; + private System.Threading.Timer _monitorUi; + private long _ultimoPulsoUi; + private int _monitorOcupado; + private bool _uiAtrasada; + private long _ultimoLogMonitor; + + private void IniciarDiagnosticoUi() + { + Interlocked.Exchange(ref _ultimoPulsoUi, Stopwatch.GetTimestamp()); + _pulsoUi = new System.Windows.Forms.Timer { Interval = 1000 }; + _pulsoUi.Tick += (s, e) => Interlocked.Exchange(ref _ultimoPulsoUi, Stopwatch.GetTimestamp()); + _pulsoUi.Start(); + _monitorUi = new System.Threading.Timer(_ => + { + if (Interlocked.Exchange(ref _monitorOcupado, 1) != 0) return; + try + { + if (Variaveis.Fechando) return; + long agora = Stopwatch.GetTimestamp(); + double atraso = (agora - Interlocked.Read(ref _ultimoPulsoUi)) / (double)Stopwatch.Frequency; + bool atrasada = atraso >= 5; + if (atrasada != _uiAtrasada || (agora - _ultimoLogMonitor) / (double)Stopwatch.Frequency >= 30) + { + _uiAtrasada = atrasada; + _ultimoLogMonitor = agora; + Variaveis.MostrarLog($"[IHM/UI] {(atrasada ? "ATRASO" : "RESPONSIVA")} | ultimoPulso={atraso:F1}s | startup={frmInstancial.StartupConcluido}"); + } + } + catch { /* Diagnóstico não pode derrubar o processo. */ } + finally { Volatile.Write(ref _monitorOcupado, 0); } + }, null, 2000, 2000); + Disposed += (s, e) => + { + _monitorUi?.Dispose(); + _pulsoUi?.Dispose(); + }; + } public frmMenu frmMenu = new frmMenu(); private frmAjustes _frmAjustes; @@ -61,6 +103,8 @@ namespace AgroBase.Forms.IHM private void FrmMenu_TelaSolicitada(object sender, TelaSolicitadaEventArgs e) { + if (!_consumoEstadoLiberado) return; + switch (e.Tela) { case TipoTelaIHM.Operacao: @@ -426,6 +470,8 @@ namespace AgroBase.Forms.IHM private void frmIHM_Load(object sender, EventArgs e) { lblVersao.Text = "IHM Rover • v " + Variaveis.Versao; + IniciarDiagnosticoUi(); + AtualizarControlesUI(); AbrirTela(frmMenu); if (tmrDispositivos != null) @@ -447,29 +493,55 @@ namespace AgroBase.Forms.IHM private Task tmrDispositivos_Tick() { - try - { - if (IsDisposed || Disposing || !IsHandleCreated) - return Task.CompletedTask; - - if (InvokeRequired) - { - BeginInvoke(new Action(AtualizarControlesUI)); - } - else - { - AtualizarControlesUI(); - } - } - catch (Exception ex) - { - Variaveis.MostrarLog("Erro no tmrDispositivos_Tick: " + ex.Message); - } - + AtualizarControlesUI(); return Task.CompletedTask; } public void AtualizarControlesUI() + { + if (IsDisposed || Disposing || !IsHandleCreated || Variaveis.Fechando) return; + if (Interlocked.Exchange(ref _atualizacaoPendente, 1) != 0) return; + if (InvokeRequired) + { + try { BeginInvoke(new Action(AtualizarControlesProtegidos)); } + catch { Volatile.Write(ref _atualizacaoPendente, 0); } + return; + } + AtualizarControlesProtegidos(); + } + + private void AtualizarControlesProtegidos() + { + var tempo = Stopwatch.StartNew(); + try + { + if (IsDisposed || Disposing || Variaveis.Fechando) return; + // Latch: perda posterior de prontidão deve continuar visível na IHM. + if (!_consumoEstadoLiberado && frmInstancial.StartupConcluido && VariaveisEquipamento.Pronto) + { + _consumoEstadoLiberado = true; + Variaveis.MostrarLog("[IHM_STARTUP] LIBERADA: startup concluído e núcleo pronto."); + } + if (!_consumoEstadoLiberado) + { + AtualizarCabecalhoOperacao("INICIALIZANDO", "Inicializando equipamento. Aguarde...", HmiTheme.Warning); + DefinirAcoesHabilitadas(false, false, false); + foreach (var card in new[] { cardBateria, cardReservatorio, cardTemperatura, cardRede, cardGnss, cardBase }) + AtualizarCard(card, "--", HmiTheme.TextMuted); + return; + } + AtualizarControlesCompletos(); + } + catch (Exception ex) { Variaveis.MostrarLog("[IHM/UI] ERRO atualização: " + ex); } + finally + { + Volatile.Write(ref _atualizacaoPendente, 0); + if (tempo.ElapsedMilliseconds >= 500) + Variaveis.MostrarLog($"[IHM/UI] ATUALIZACAO_LENTA | duracao={tempo.ElapsedMilliseconds}ms | tela={_telaAtual?.Name}"); + } + } + + private void AtualizarControlesCompletos() { var operacao = Variaveis.OperacaoEmAndamento; var dadosOperacao = operacao?.Sensoriamento?.Operacao; @@ -552,7 +624,7 @@ namespace AgroBase.Forms.IHM bool operacaoPausada = ObterPausaParaExibicao(pausaModelo); bool operacaoEmergencia = ObterEmergenciaParaExibicao(emergenciaModelo); bool operacaoIniciada = dadosOperacao?.OperacaoIniciada ?? false; - AtualizarStatusOperacao(statusOperacao, operacaoPausada, operacaoEmergencia, (int)dadosOperacao.TempoAguardandoSeg); + AtualizarStatusOperacao(statusOperacao, operacaoPausada, operacaoEmergencia, (int)(dadosOperacao?.TempoAguardandoSeg ?? 0)); AtualizarAcoesOperacao(statusOperacao, operacaoIniciada, operacaoPausada, operacaoEmergencia); // ========================================================= @@ -560,7 +632,7 @@ namespace AgroBase.Forms.IHM // ========================================================= if (_telaAtual != null && !_telaAtual.IsDisposed) { - (_telaAtual as ITelaNavegavel).AtualizarDadosTela(); + (_telaAtual as ITelaNavegavel)?.AtualizarDadosTela(); } } @@ -666,7 +738,7 @@ namespace AgroBase.Forms.IHM if (card == null) return; - if (card.Valor == valor || card.CorValor == cor) + if (card.Valor == valor && card.CorValor == cor) return; ExecutarNaUI(delegate diff --git a/AgroBase/AgroBase/Forms/IHM/frmMenu.cs b/AgroBase/AgroBase/Forms/IHM/frmMenu.cs index 6ac5b9f75..f2e8d3497 100644 --- a/AgroBase/AgroBase/Forms/IHM/frmMenu.cs +++ b/AgroBase/AgroBase/Forms/IHM/frmMenu.cs @@ -15,6 +15,7 @@ namespace AgroBase.Forms.IHM public partial class frmMenu : Form, ITelaNavegavel { private bool _sincronizando; + private bool _referenciamentoEmAndamento; /// /// Estes eventos deixam os atalhos de campo prontos para receber @@ -52,7 +53,9 @@ namespace AgroBase.Forms.IHM }); btnDebugMode.Click += BtnDebugMode_Click; - btnAjustarLeverArm.Click += BtnAjustarLeverArm_Click; + btnAjustarLeverArm.Texto = "Referenciar robô"; + btnAjustarLeverArm.IconText = "↻"; + btnAjustarLeverArm.Click += BtnReferenciarRobo_Click; rowFalhas.Click += LblCodigoFalha_Click; } @@ -73,8 +76,19 @@ namespace AgroBase.Forms.IHM AtualizarControlesUI(); } + private bool _startupVisualLiberado; public void AtualizarControlesUI() { + if (!_startupVisualLiberado) + { + _startupVisualLiberado = frmInstancial.StartupConcluido && VariaveisEquipamento.Pronto; + if (!_startupVisualLiberado) + { + lblRoverProntidao.Text = "Inicializando equipamento..."; + lblStatus.Text = "Aguardando inicialização do núcleo."; + return; + } + } bool mqttConectado = Variaveis.MqttServiceLocal != null && Variaveis.MqttServiceLocal.StatusConexao(); bool iaConectada = VariaveisOperacao.Operadores != null && VariaveisOperacao.Operadores.Conectado; var operacao = Variaveis.OperacaoEmAndamento; @@ -134,7 +148,7 @@ namespace AgroBase.Forms.IHM private bool VerificaEquipamentoIniciado() { - if (!VariaveisEquipamento.Pronto) + if (!frmInstancial.StartupConcluido || !VariaveisEquipamento.Pronto) { FuncoesIHM.MostrarAlerta( "Equipamento iniciando", @@ -297,18 +311,82 @@ namespace AgroBase.Forms.IHM btnDebugMode.Texto = Variaveis.DebugMode ? "Desligar Debug Mode" : "Ligar Debug Mode"; } - private void BtnAjustarLeverArm_Click(object sender, EventArgs e) + private string MotivoBloqueioReferenciamento(OperacaoModel operacao) { - if (AjustarLeverArmSolicitado != null) - { - AjustarLeverArmSolicitado(this, EventArgs.Empty); - return; - } + if (!frmInstancial.StartupConcluido || Variaveis.Fechando) + return "Aguarde a inicialização do equipamento."; + if (operacao?.Controle == null || operacao.Parametros?.Controle == null || + operacao.Sensoriamento?.Operacao == null || operacao.DispMvd?.Dados?.Modulos == null) + return "Dados de controle ou módulos indisponíveis."; + if (operacao.CalibrandoRuntime || MKS057DCanService.Referenciando) + return "Já existe calibragem ou referenciamento em andamento."; + var estado = operacao.Sensoriamento.Operacao; + if (estado.Emergencia) + return "Resolva a emergência antes de referenciar."; + if (estado.OperacaoIniciada && !estado.Pausa) + return "Pause a operação e aguarde o robô parar antes de referenciar."; + if (Math.Abs(operacao.Controle.PercentualVelocidadeSP) > 0 || + Math.Abs(operacao.Controle.RPM_SP) > 0) + return "Neutralize os comandos de movimento antes de referenciar."; + var modulos = operacao.DispMvd.Dados.Modulos.ToList(); + if (modulos.Count == 0 || modulos.Any(m => m == null || !m.Conectado || + m.MovMotor == null || m.DirMotor == null || !m.DirMotor.Inicializado || !m.DirMotor.Comandar)) + return "Todos os módulos devem estar conectados e a direção inicializada e habilitada."; + if (modulos.Any(m => double.IsNaN(m.MovMotor.VelocidadeInstantanea) || + double.IsInfinity(m.MovMotor.VelocidadeInstantanea) || + Math.Abs(m.MovMotor.VelocidadeInstantanea) > 0.05)) + return "Aguarde todas as rodas pararem antes de referenciar."; + return null; + } - ExibirAtalhoNaoVinculado( - "Ajustar Lever Arm", - "O layout já está pronto. Vincule a tela ou o diálogo no evento " + - "AjustarLeverArmSolicitado."); + private async void BtnReferenciarRobo_Click(object sender, EventArgs e) + { + if (_referenciamentoEmAndamento) return; + _referenciamentoEmAndamento = true; + btnAjustarLeverArm.Enabled = false; + try + { + var operacao = Variaveis.OperacaoEmAndamento; + string motivo = MotivoBloqueioReferenciamento(operacao); + if (motivo != null) + { + FuncoesIHM.MostrarAlerta("Referenciamento indisponível", motivo, MessageBoxIcon.Warning); + return; + } + if (!FuncoesIHM.ConfirmarAcao("Referenciar robô", + "Refazer a referência de todas as rodas? A direção irá se movimentar. " + + "Mantenha o robô parado e a área das rodas livre.", MessageBoxIcon.Question)) return; + + // A confirmação abre um diálogo: revalidar o estado antes de executar. + if (!ReferenceEquals(operacao, Variaveis.OperacaoEmAndamento)) return; + motivo = MotivoBloqueioReferenciamento(operacao); + if (motivo != null) + { + FuncoesIHM.MostrarAlerta("Referenciamento indisponível", motivo, MessageBoxIcon.Warning); + return; + } + btnAjustarLeverArm.Texto = "Referenciando..."; + Variaveis.MostrarLog("[IHM/REFERENCIAR] Solicitado referenciamento de todas as rodas pelo menu."); + // Mesma rotina usada pelo R3; evita o desvio para movimento combinado. + bool resultado = await operacao.DispMvd.Dados.RealizarReferenciamentoDirecional(); + Variaveis.MostrarLog($"[IHM/REFERENCIAR] Rotina retornou resultado={resultado}; confira os logs de cada módulo."); + // Não retomar a operação nem liberar pausa automaticamente. + } + catch (Exception ex) + { + Variaveis.MostrarLog("[IHM/REFERENCIAR] Falha: " + ex); + if (!IsDisposed && !Disposing) + FuncoesIHM.MostrarAlerta("Falha no referenciamento", ex.Message, MessageBoxIcon.Error); + } + finally + { + _referenciamentoEmAndamento = false; + if (!IsDisposed && !Disposing) + { + btnAjustarLeverArm.Texto = "Referenciar robô"; + btnAjustarLeverArm.Enabled = true; + } + } } private void LblCodigoFalha_Click(object sender, EventArgs e) diff --git a/AgroBase/AgroBase/Forms/IHM/frmOperacao.cs b/AgroBase/AgroBase/Forms/IHM/frmOperacao.cs index 638d300fe..6fa708874 100644 --- a/AgroBase/AgroBase/Forms/IHM/frmOperacao.cs +++ b/AgroBase/AgroBase/Forms/IHM/frmOperacao.cs @@ -94,14 +94,14 @@ namespace AgroBase.Forms.IHM AtualizarParametrosTela ), op, recarregarMapa); } + catch (ObjectDisposedException) + { + } catch (InvalidOperationException) { // O formulário pode estar encerrando entre a verificação // e o BeginInvoke. A próxima abertura sincroniza a fonte. } - catch (ObjectDisposedException) - { - } return; } diff --git a/AgroBase/AgroBase/Forms/frmInstancial.cs b/AgroBase/AgroBase/Forms/frmInstancial.cs index f4a9133eb..f34f69127 100644 --- a/AgroBase/AgroBase/Forms/frmInstancial.cs +++ b/AgroBase/AgroBase/Forms/frmInstancial.cs @@ -1,4 +1,4 @@ -using AgroBase.Forms.IHM; +using AgroBase.Forms.IHM; using AgroBase.Models; using AgroBase.Services; using System; @@ -27,7 +27,9 @@ namespace AgroBase.Forms private static CancellationTokenSource _appCts; - private static bool _startupConcluido; + private static volatile bool _startupConcluido; + // Prontidão visual não altera os intertravamentos operacionais. + public static bool StartupConcluido => _startupConcluido; private static bool _shutdownIniciado; private bool _allowClose; diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index b638ddbfb..0f5a75062 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -662,18 +662,18 @@ namespace AgroBase.Models DirecionalAutomatico = true, PulverizadorAutomatico = true, - FrenagemAutomaticaAoParar = true, + FrenagemAutomaticaAoParar = false, ImuParadaPorInclinacao = true, ImuAuxilioMovimento = true, - ImuAuxilioDirecional = true, - ImuDirecionalPreferirArco = true, - OakParadaPorObstaculo = true, - DirAuxilioSonar = true, + ImuAuxilioDirecional = false, + ImuDirecionalPreferirArco = false, + OakParadaPorObstaculo = false, + DirAuxilioSonar = false, MovAuxilioSonar = true, MpcHorizonte = 4.0, AnteciparManobraCorredorM = 0.0, - MovVelocidadeSErvasPercent = 50, + MovVelocidadeSErvasPercent = 45, MovVelocidadeCErvasPercent = 20, DirAnguloMaximo = 30, DirVelocidadeMovimento = 80, @@ -685,8 +685,8 @@ namespace AgroBase.Models AtuDuracaoAtuacao = 0, AtuPercentualErvasBicoOff = 2, AtuPercentualErvasBicoOn = 1, - AtuPercentualInicioPulverizacao = 85, - AtuPressaoLinha = 22, + AtuPercentualInicioPulverizacao = 70, + AtuPressaoLinha = 35, AtuAgitadorModo = ModoAgitadorCalda.Continuo, AtuModoControle = ModoControleBomba.PID, AtuModeloCabeca = CabecasModelo.Target, @@ -715,7 +715,7 @@ namespace AgroBase.Models new OperacaoModulosMandatoriosModel() { Dispositivo = T_Code.Sen, - Mandatorio = true, + Mandatorio = false, Utilizar = true, }, new OperacaoModulosMandatoriosModel() @@ -1050,31 +1050,31 @@ namespace AgroBase.Models DirecionalAutomatico = true, PulverizadorAutomatico = false, - FrenagemAutomaticaAoParar = true, + FrenagemAutomaticaAoParar = false, ImuParadaPorInclinacao = true, ImuAuxilioMovimento = true, - ImuAuxilioDirecional = true, - ImuDirecionalPreferirArco = true, - OakParadaPorObstaculo = true, - DirAuxilioSonar = true, + ImuAuxilioDirecional = false, + ImuDirecionalPreferirArco = false, + OakParadaPorObstaculo = false, + DirAuxilioSonar = false, MovAuxilioSonar = true, MpcHorizonte = 4.0, AnteciparManobraCorredorM = 0, - MovVelocidadeSErvasPercent = 60, + MovVelocidadeSErvasPercent = 50, MovVelocidadeCErvasPercent = 20, DirAnguloMaximo = 30, DirVelocidadeMovimento = 80, DirTipoMovimento = TiposControladorDirecional.MPC, - DistanciaManobra = 3.0, + DistanciaManobra = 2.2, AtuAlturaAreaPulverizacao = 10, AtuDuracaoAtuacao = 0, AtuPercentualErvasBicoOff = 2, AtuPercentualErvasBicoOn = 1, - AtuPercentualInicioPulverizacao = 85, - AtuPressaoLinha = 18, + AtuPercentualInicioPulverizacao = 70, + AtuPressaoLinha = 22, AtuAgitadorModo = ModoAgitadorCalda.SemAgitacao, AtuModoControle = ModoControleBomba.PID, AtuModeloCabeca = CabecasModelo.Target, @@ -1103,13 +1103,13 @@ namespace AgroBase.Models new OperacaoModulosMandatoriosModel() { Dispositivo = T_Code.Sen, - Mandatorio = true, + Mandatorio = false, Utilizar = true, }, new OperacaoModulosMandatoriosModel() { Dispositivo = T_Code.Atu, - Mandatorio = true, + Mandatorio = false, Utilizar = true, }, new OperacaoModulosMandatoriosModel() @@ -1119,6 +1119,12 @@ namespace AgroBase.Models Utilizar = true, }, new OperacaoModulosMandatoriosModel() + { + Dispositivo = T_Code.Cam, + Mandatorio = false, + Utilizar = true, + }, + new OperacaoModulosMandatoriosModel() { Dispositivo = T_Code.Snr, Mandatorio = true, diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index 8b709955d..67687d036 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -111,6 +111,18 @@ namespace AgroBase.Models private double ErroOrientacaoMaximoRecuperacaoRetornoGraus { get; set; } = 65.0; private double ToleranciaParametroRecuperacaoRetorno { get; set; } = 0.25; + // RetornoBase V6 - tentativa de escape cinematicamente viavel quando + // rover e base estao na mesma cabeceira. O envelope convexo continua + // sendo uma barreira conservadora para rotas retas/perimetrais, mas + // deixa de ocultar uma curva curta que caiba na area livre da borda. + private double DistanciaMaximaCabecieraRetornoM { get; set; } = 6.0; + // As ruas do mapa sao polilinhas sem largura agronomica confiavel. + // Portanto a barreira autoritativa e NAO CRUZAR a linha. Os 5 cm + // abaixo servem apenas para rejeitar tangencias numericas/raspadas; + // nao tentam representar a largura fisica da cultura. + private double MargemLinhaCulturaRetornoM { get; set; } = 0.05; + private double ComprimentoMaximoRelativoEscapeRetorno { get; set; } = 4.0; + private int MaxPontosEntradaRecuperacaoTransicao { get; set; } = 12; private int ConfirmacoesMinimasEntradaProximoCorredor { get; set; } = 2; @@ -6118,7 +6130,8 @@ namespace AgroBase.Models int ultimoCorredor = -1; - for (int i = 0; i < pontos.Count; i++) + // Ponto 0 é a poição do robô, não considera distancia entre pontos + for (int i = 1; i < pontos.Count; i++) { var ponto = pontos[i]; @@ -6128,7 +6141,7 @@ namespace AgroBase.Models if (ponto.idxPonto != i) throw new InvalidOperationException($"Indice global inconsistente na trajetoria: {ponto.idxPonto}/{i}."); - if (i > 0) + if (i > 1) { double salto = GPSUtils.DistanciaEntrePontos(pontos[i - 1].Posicao, ponto.Posicao); @@ -6494,6 +6507,44 @@ namespace AgroBase.Models op.Sensoriamento.Operacao.OperacaoIniciada = operacaoIniciadaAnterior.Value; } + /* + * Falha fechada do retorno. + * + * A rota agricola anterior e preservada apenas para + * diagnostico/retentativa. Ela NAO pode voltar a ser uma + * rota automatica liberada depois que o retorno falhou. + * O operador ainda pode assumir o modo manual, reposicionar + * o rover e solicitar um novo retorno a partir da nova pose. + */ + try + { + op?.DefinirPausa(true); + } + catch (Exception exPausa) + { + Variaveis.MostrarLog( + "[RETORNO] Falha ao solicitar pausa local: " + + exPausa.Message + ); + } + + try + { + RedisService.AtualizarCampos( + CtxKey.DadosOperacao, + ("configurado", false), + ("liberado", false), + ("motivo_nao_liberado", "Retorno a base rejeitado: " + ex.Message) + ); + } + catch (Exception exRedis) + { + Variaveis.MostrarLog( + "[RETORNO] Falha ao publicar bloqueio no Redis: " + + exRedis.Message + ); + } + Variaveis.MostrarLog("[RETORNO] Trajetoria rejeitada: " + ex.Message); op?.Sensoriamento?.InserirLog( T_Code.Trj, @@ -7032,6 +7083,43 @@ namespace AgroBase.Models ); break; } + + + /* + * CASO 4 - ESCAPE DE CABECEIRA + * + * O caso perimetral pode ser recusado quando o rover ainda esta + * matematicamente dentro do envelope convexo, mesmo estando na + * area livre da cabeceira. Nesse ponto tentamos uma Dubins a + * partir da pose REAL do rover ate a base. Cada candidato e + * amostrado e validado contra as polilinhas das ruas; nenhuma + * curva que cruze uma linha de cultura e aceita. + */ + if (!trecho.Any()) + { + string diagnosticoEscape; + var escape = GerarEscapeSeguroCabeciera( + posicaoAtual, + baseGps, + out diagnosticoEscape + ); + + if (escape.Any()) + { + trecho.AddRange(escape); + Variaveis.MostrarLog( + "[RETORNO] Escape seguro de cabeceira aceito | " + + diagnosticoEscape + ); + } + else + { + Variaveis.MostrarLog( + "[RETORNO] Escape de cabeceira indisponivel | " + + diagnosticoEscape + ); + } + } pontos.AddRange(trecho); return (trecho.Any(), pontos); @@ -7178,6 +7266,358 @@ namespace AgroBase.Models return lista; } + private List GerarEscapeSeguroCabeciera( + GPSModel posRobo, + GPSModel posBase, + out string diagnostico + ) + { + diagnostico = "nao avaliado"; + + if (!PontoGpsValido(posRobo) || !PontoGpsValido(posBase)) + { + diagnostico = "coordenada do rover ou da base invalida"; + return new List(); + } + + if (CorredorAtual?.Dentro == true) + { + diagnostico = "rover ainda confirmado dentro de corredor"; + return new List(); + } + + double distanciaCabeciera = DistanciaAteExtremidadeMaisProximaRua(posRobo); + if ( + double.IsNaN(distanciaCabeciera) || + double.IsInfinity(distanciaCabeciera) || + distanciaCabeciera > DistanciaMaximaCabecieraRetornoM + ) + { + diagnostico = $"fora da zona de cabeceira ({distanciaCabeciera:F2}m)"; + return new List(); + } + + double headingInicial = posRobo.AnguloCarroDefinido; + if (double.IsNaN(headingInicial) || double.IsInfinity(headingInicial)) + headingInicial = posRobo.OrientacaoReal; + + if (double.IsNaN(headingInicial) || double.IsInfinity(headingInicial)) + { + diagnostico = "heading atual invalido"; + return new List(); + } + + double headingFinal = GPSUtils.CalcularOrientacao(posRobo, posBase); + double distanciaDireta = GPSUtils.DistanciaEntrePontos(posRobo, posBase); + double entreEixosM = Math.Max( + 0.30, + VariaveisEquipamento.DistanciaEntreEixosCm / 100.0 + ); + + // Mesmo raio conservador ja usado nas ferraduras validadas. + const double anguloProjetoGraus = 28.0; + double raioSeguro = Math.Max( + 0.75, + (entreEixosM / + (2.0 * Math.Tan(anguloProjetoGraus * Math.PI / 180.0))) * + 1.05 + ); + + var candidatos = GerarCurvasDubinsCandidatas( + posRobo, + headingInicial, + posBase, + headingFinal, + raioSeguro, + Math.Max(0.15, DistanciaEntrePontosCurva) + ); + + int rejeitadosGeometria = 0; + int rejeitadosComprimento = 0; + + foreach (var candidato in candidatos.OrderBy(x => x.custo)) + { + if ( + candidato.pontos == null || + candidato.pontos.Count < 2 || + candidato.pontos.Any(x => !PontoGpsValido(x)) + ) + { + continue; + } + + double limiteComprimento = + distanciaDireta * ComprimentoMaximoRelativoEscapeRetorno + + 10.0; + + if (candidato.custo > limiteComprimento) + { + rejeitadosComprimento++; + continue; + } + + string motivoGeometria; + if (!TrajetoEscapeLivreDaCultura(candidato.pontos, out motivoGeometria)) + { + rejeitadosGeometria++; + continue; + } + + diagnostico = + $"dubins={candidato.tipo}, raio={raioSeguro:F2}m, " + + $"comprimento={candidato.custo:F2}m, " + + $"cabeciera={distanciaCabeciera:F2}m, " + + $"heading={headingInicial:F1}->{headingFinal:F1}graus"; + + return candidato.pontos; + } + + diagnostico = + $"nenhum candidato seguro; total={candidatos.Count}, " + + $"geometria={rejeitadosGeometria}, " + + $"comprimento={rejeitadosComprimento}, " + + $"cabeciera={distanciaCabeciera:F2}m"; + + return new List(); + } + + private double DistanciaAteExtremidadeMaisProximaRua(GPSModel ponto) + { + if (RuasPlantacao == null || RuasPlantacao.Count == 0) + return double.PositiveInfinity; + + double melhor = double.PositiveInfinity; + + foreach (var rua in RuasPlantacao) + { + if (rua == null || rua.Count == 0) + continue; + + melhor = Math.Min( + melhor, + GPSUtils.DistanciaEntrePontos(ponto, rua[0]) + ); + melhor = Math.Min( + melhor, + GPSUtils.DistanciaEntrePontos(ponto, rua[rua.Count - 1]) + ); + } + + return melhor; + } + + private List<(string tipo, double custo, List pontos)> + GerarCurvasDubinsCandidatas( + GPSModel inicio, + double headingInicioGraus, + GPSModel fim, + double headingFimGraus, + double raioM, + double espacamentoM + ) + { + double refLat = inicio.Latitude; + double refLon = inicio.Longitude; + var fimXY = LatLonToXY(fim, refLat, refLon); + + double theta0 = Mod2Pi( + Math.PI / 2.0 - headingInicioGraus * Math.PI / 180.0 + ); + double theta1 = Mod2Pi( + Math.PI / 2.0 - headingFimGraus * Math.PI / 180.0 + ); + double dx = fimXY.x; + double dy = fimXY.y; + double d = Math.Sqrt(dx * dx + dy * dy) / raioM; + double theta = Mod2Pi(Math.Atan2(dy, dx)); + double alpha = Mod2Pi(theta0 - theta); + double beta = Mod2Pi(theta1 - theta); + + var parametros = new List<(string tipo, double t, double p, double q, double custo)>(); + AdicionarDubins(parametros, "LSL", DubinsLSL(alpha, beta, d), raioM); + AdicionarDubins(parametros, "RSR", DubinsRSR(alpha, beta, d), raioM); + AdicionarDubins(parametros, "LSR", DubinsLSR(alpha, beta, d), raioM); + AdicionarDubins(parametros, "RSL", DubinsRSL(alpha, beta, d), raioM); + AdicionarDubins(parametros, "RLR", DubinsRLR(alpha, beta, d), raioM); + AdicionarDubins(parametros, "LRL", DubinsLRL(alpha, beta, d), raioM); + + var resultado = new List<(string tipo, double custo, List pontos)>(); + + foreach (var candidato in parametros) + { + double[] segmentos = { candidato.t, candidato.p, candidato.q }; + var pontos = new List { ClonarPontoGeometrico(inicio) }; + double x = 0.0; + double y = 0.0; + double th = theta0; + double passo = Math.Max(0.10, espacamentoM); + + for (int seg = 0; seg < 3; seg++) + { + char tipoSegmento = candidato.tipo[seg]; + double restante = segmentos[seg] * raioM; + + while (restante > 1e-6) + { + double ds = Math.Min(passo, restante); + restante -= ds; + + if (tipoSegmento == 'S') + { + x += Math.Cos(th) * ds; + y += Math.Sin(th) * ds; + } + else + { + double k = tipoSegmento == 'L' + ? 1.0 / raioM + : -1.0 / raioM; + double dth = k * ds; + x += (Math.Sin(th + dth) - Math.Sin(th)) / k; + y += -(Math.Cos(th + dth) - Math.Cos(th)) / k; + th = Mod2Pi(th + dth); + } + + pontos.Add(XYToLatLon(x, y, refLat, refLon)); + } + } + + if (pontos.Count > 0) + pontos[pontos.Count - 1] = ClonarPontoGeometrico(fim); + + resultado.Add((candidato.tipo, candidato.custo, pontos)); + } + + return resultado; + } + + private bool TrajetoEscapeLivreDaCultura( + List trajeto, + out string motivo + ) + { + motivo = null; + + if (trajeto == null || trajeto.Count < 2) + { + motivo = "trajeto vazio"; + return false; + } + + if (RuasPlantacao == null || RuasPlantacao.Count == 0) + { + motivo = "mapa de ruas indisponivel"; + return false; + } + + GPSModel referencia = trajeto[0]; + double distanciaAcumulada = 0.0; + double distanciaInicialLinha = DistanciaMinimaAteLinhasCultura( + trajeto[0] + ); + + for (int i = 1; i < trajeto.Count; i++) + { + GPSModel a = trajeto[i - 1]; + GPSModel b = trajeto[i]; + distanciaAcumulada += GPSUtils.DistanciaEntrePontos(a, b); + + foreach (var rua in RuasPlantacao) + { + if (rua == null || rua.Count < 2) + continue; + + for (int j = 1; j < rua.Count; j++) + { + var aXY = LatLonToXY(a, referencia.Latitude, referencia.Longitude); + var bXY = LatLonToXY(b, referencia.Latitude, referencia.Longitude); + var cXY = LatLonToXY(rua[j - 1], referencia.Latitude, referencia.Longitude); + var dXY = LatLonToXY(rua[j], referencia.Latitude, referencia.Longitude); + + if (SegmentosIntersectam(aXY, bXY, cXY, dXY)) + { + motivo = $"segmento {i - 1}->{i} cruza linha de cultura"; + return false; + } + } + } + + double distanciaLinha = DistanciaMinimaAteLinhasCultura(b); + double margemExigida = MargemLinhaCulturaRetornoM; + + // O rover pode iniciar proximo da ultima linha. Durante o + // primeiro metro aceitamos a distancia inicial, mas nunca uma + // aproximacao adicional relevante. Depois disso vale a margem + // integral configurada. + if (distanciaAcumulada <= 1.0) + margemExigida = Math.Min( + margemExigida, + Math.Max(0.10, distanciaInicialLinha - 0.05) + ); + + if (distanciaLinha + 1e-6 < margemExigida) + { + motivo = + $"distancia insuficiente da cultura: " + + $"{distanciaLinha:F2}m < {margemExigida:F2}m"; + return false; + } + } + + return true; + } + + private double DistanciaMinimaAteLinhasCultura(GPSModel ponto) + { + double melhor = double.PositiveInfinity; + + foreach (var rua in RuasPlantacao) + { + if (rua == null || rua.Count < 2) + continue; + + for (int i = 1; i < rua.Count; i++) + { + melhor = Math.Min( + melhor, + DistanciaPontoSegmentoMetros( + ponto, + rua[i - 1], + rua[i] + ) + ); + } + } + + return melhor; + } + + private double DistanciaPontoSegmentoMetros( + GPSModel ponto, + GPSModel inicio, + GPSModel fim + ) + { + var p = LatLonToXY(ponto, ponto.Latitude, ponto.Longitude); + var a = LatLonToXY(inicio, ponto.Latitude, ponto.Longitude); + var b = LatLonToXY(fim, ponto.Latitude, ponto.Longitude); + double dx = b.x - a.x; + double dy = b.y - a.y; + double norma2 = dx * dx + dy * dy; + + if (norma2 <= 1e-12) + return Math.Sqrt(a.x * a.x + a.y * a.y); + + double t = ((p.x - a.x) * dx + (p.y - a.y) * dy) / norma2; + t = Math.Max(0.0, Math.Min(1.0, t)); + double qx = a.x + t * dx; + double qy = a.y + t * dy; + return Math.Sqrt( + Math.Pow(p.x - qx, 2) + + Math.Pow(p.y - qy, 2) + ); + } + #endregion #region CASO 2