diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosAtuador.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosAtuador.cs index 261b3105a..58a07ccde 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosAtuador.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosAtuador.cs @@ -1,4 +1,4 @@ -using AgroBase.Models; +using AgroBase.Models; using System; using System.Windows.Forms; @@ -52,6 +52,8 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros private void CarregarDadosTela() { + _carregandoDados = true; + try { var parametros = op?.Parametros?.Controle; @@ -97,9 +99,12 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros parametros.AtuAgitadorModo = seletorModoAgitador.SelectedMode; - agitador.Kp = (double)paramKp.Value; - agitador.Ki = (double)paramKi.Value; - agitador.Kd = (double)paramKd.Value; + if (agitador != null) + { + agitador.Kp = (double)paramKp.Value; + agitador.Ki = (double)paramKi.Value; + agitador.Kd = (double)paramKd.Value; + } parametros.AtuDuracaoAtuacao = (int)paramDuracaoAtuacao.Value; parametros.AtuPressaoLinha = (double)paramPressaoLinha.Value; diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs index 1012f467e..19869ad75 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs @@ -1,4 +1,4 @@ -using AgroBase.Forms.IHM.Controls; +using AgroBase.Forms.IHM.Controls; using AgroBase.Models; using System; using System.Collections.Generic; @@ -14,6 +14,7 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros private bool _carregandoDados; private bool _mapaExistenteCarregado; + private bool _hidratandoMapaExistente; public ucParametrosMapa() { @@ -56,8 +57,12 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros if (mesmaInstanciaMapa && !recarregarMapa) { _mapaExistenteCarregado = true; - lblMapaPlaceholder.Visible = _mapa.mapaService?.DadosMapa == null; - SincronizarParametrosComMapa(); + lblMapaPlaceholder.Visible = + (_mapa.mapaService?.DadosMapa ?? op.Parametros?.Mapa) == null; + + // Ao apenas reanexar a mesma instância, nunca sobrescrevemos + // Parametros.RuasPercorrer com uma lista runtime vazia. + SincronizarParametrosComMapa(permitirSelecaoVazia: false); AtualizarResumo(); return; } @@ -72,17 +77,28 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros try { - lblMapaPlaceholder.Visible = _mapa?.mapaService?.DadosMapa == null; + MapaFeatureCollectionModel mapaRuntime = + _mapa?.mapaService?.DadosMapa; - SincronizarParametrosComMapa(); - AtualizarResumo(); + lblMapaPlaceholder.Visible = + (mapaRuntime ?? op?.Parametros?.Mapa) == null; + /* + * Primeiro preservamos o snapshot da operação. Se o MapasModel + * runtime ainda não foi hidratado, copiar dele agora apagaria + * Mapa/RuasPercorrer dos parâmetros. + */ if (!_mapaExistenteCarregado && op?.Parametros?.Mapa != null) { _mapaExistenteCarregado = true; - + AtualizarResumo(); CarregarMapaExistenteAsync(); } + else + { + SincronizarParametrosComMapa(permitirSelecaoVazia: false); + AtualizarResumo(); + } } finally { @@ -92,28 +108,57 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros private async void CarregarMapaExistenteAsync() { + if (_hidratandoMapaExistente) + return; + + _hidratandoMapaExistente = true; + try { if (_mapa == null || op?.Parametros?.Mapa == null) - { return; - } - List ruasSelecionadas = op.Parametros.RuasPercorrer != null ? new List(op.Parametros.RuasPercorrer) : new List(); + /* + * Snapshot antes do primeiro await. Eventos do mapa podem ocorrer + * durante CarregarMapaAsync; eles não podem apagar esta seleção. + */ + List ruasSelecionadas = + op.Parametros.RuasPercorrer != null + ? new List(op.Parametros.RuasPercorrer) + : new List(); - await _mapa.CarregarMapaAsync(op.Parametros.Mapa, "Mapa da operação"); + await _mapa.CarregarMapaAsync( + op.Parametros.Mapa, + "Mapa da operação" + ); _mapa.TipoMapa = op.Parametros.TipoMapa; - await _mapa.DefinirRuasSelecionadasAsync(op, ruasSelecionadas, false); + await _mapa.DefinirRuasSelecionadasAsync( + op, + ruasSelecionadas, + false + ); + + /* + * Agora o runtime está hidratado. Sincronizamos uma única vez, + * inclusive aceitando lista vazia quando ela realmente veio dos + * parâmetros da operação. + */ + SincronizarParametrosComMapa(permitirSelecaoVazia: true); lblMapaPlaceholder.Visible = false; - AtualizarResumo(); } catch (Exception ex) { - Variaveis.MostrarLog("[ucParametrosMapa.CarregarMapaExistente] " + ex.Message); + Variaveis.MostrarLog( + "[ucParametrosMapa.CarregarMapaExistente] " + ex.Message + ); + } + finally + { + _hidratandoMapaExistente = false; } } @@ -136,6 +181,9 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros if (IsDisposed || Disposing) return; + if (_hidratandoMapaExistente) + return; + if (InvokeRequired) { BeginInvoke(new Action( @@ -150,13 +198,13 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros lblMapaPlaceholder.Visible = false; - SincronizarParametrosComMapa(); + SincronizarParametrosComMapa(permitirSelecaoVazia: true); AtualizarResumo(); } private void Mapa_RuasSelecionadasAlteradas(object sender, EventArgs e) { - if (_carregandoDados) + if (_carregandoDados || _hidratandoMapaExistente) return; if (IsDisposed || Disposing) @@ -174,27 +222,52 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros return; } - SincronizarParametrosComMapa(); + SincronizarParametrosComMapa(permitirSelecaoVazia: true); AtualizarResumo(); } - private void SincronizarParametrosComMapa() + private void SincronizarParametrosComMapa(bool permitirSelecaoVazia) { if (op?.Parametros == null || _mapa == null) - { return; - } - op.Parametros.Mapa = _mapa.mapaService?.DadosMapa; + MapaFeatureCollectionModel dadosMapa = + _mapa.mapaService?.DadosMapa; + + /* + * A tela de parâmetros não é a fonte primária do mapa durante uma + * operação em andamento. Se o WebView/MapasModel ainda não carregou, + * mantemos intacto o snapshot que já veio em op.Parametros. + */ + if (dadosMapa == null) + return; + + op.Parametros.Mapa = dadosMapa; op.Parametros.TipoMapa = _mapa.TipoMapa; - op.Parametros.RuasPercorrer = _mapa.RuasPercorrer != null ? _mapa.RuasPercorrer.ToList() : new List(); + + List ruasRuntime = + _mapa.RuasPercorrer != null + ? _mapa.RuasPercorrer.ToList() + : new List(); + + if (permitirSelecaoVazia || ruasRuntime.Count > 0) + op.Parametros.RuasPercorrer = ruasRuntime; } private void AtualizarResumo() { - MapaFeatureCollectionModel dadosMapa = _mapa?.mapaService?.DadosMapa; - List features = dadosMapa?.features ?? new List(); - List selecionadas = _mapa?.RuasPercorrer ?? new List(); + MapaFeatureCollectionModel dadosMapa = + _mapa?.mapaService?.DadosMapa ?? + op?.Parametros?.Mapa; + + List features = + dadosMapa?.features ?? + new List(); + + List selecionadas = + (_mapa?.RuasPercorrer?.Count ?? 0) > 0 + ? _mapa.RuasPercorrer + : (op?.Parametros?.RuasPercorrer ?? new List()); int quantidadeTotal = features.Count; int quantidadeSelecionada = selecionadas.Count; double distanciaTotalMapa = features diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs index 4c36e710d..f6b563c14 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs @@ -1,4 +1,4 @@ -using AgroBase.Forms.IHM.Controls; +using AgroBase.Forms.IHM.Controls; using AgroBase.Models; using System; using System.Collections.Generic; @@ -193,6 +193,8 @@ namespace AgroBase.Forms.IHM.Operacao pnlAlertaAutonomia.Visible = false; pnlStatusAutonomia.Visible = true; + btnLiberarSupervisao.Enabled = false; + pnlStatusAutonomia.BringToFront(); } @@ -204,6 +206,36 @@ namespace AgroBase.Forms.IHM.Operacao pnlAlertaAutonomia.BringToFront(); } + private static bool PodeLiberarSupervisao(AutonomiaCorredorModel autonomia) + { + if (autonomia == null || autonomia.Liberado) + return false; + + bool bateriaBloqueando = + autonomia.DadosBateriaValidos && + !autonomia.BateriaSuficiente && + !autonomia.BateriaOverrideAplicado; + + bool herbicidaBloqueando = + autonomia.PulverizacaoExigida && + autonomia.DadosHerbicidaValidos && + !autonomia.HerbicidaSuficiente && + !autonomia.HerbicidaOverrideAplicado; + + /* + * AguardandoLiberacaoManual representa uma trava persistente que + * precisa de reconhecimento humano mesmo se o recurso já recuperou. + * Critico representa uma insuficiência atual e também deve permitir + * a decisão supervisionada, sem obrigar o operador a esperar o latch. + */ + return + autonomia.Status == AutonomiaCorredorStatus.AguardandoLiberacaoManual || + ( + autonomia.Status == AutonomiaCorredorStatus.Critico && + (bateriaBloqueando || herbicidaBloqueando) + ); + } + private int ObterIndiceCorredorAvaliado() { int indiceAtual = @@ -227,8 +259,44 @@ namespace AgroBase.Forms.IHM.Operacao private void btnLiberarSupervisao_Click(object sender, EventArgs e) { + var autonomia = op?.Trajetoria?.AutonomiaCorredor; + + if (!PodeLiberarSupervisao(autonomia)) + return; + int idxCorredor = ObterIndiceCorredorAvaliado(); - op?.Trajetoria?.AtualizarDadosAutonomiaCorredor(idxCorredor: idxCorredor, bat_liberada: true, herb_liberado: true); + + bool bateriaBloqueando = + autonomia.DadosBateriaValidos && + !autonomia.BateriaSuficiente && + !autonomia.BateriaOverrideAplicado; + + bool herbicidaBloqueando = + autonomia.PulverizacaoExigida && + autonomia.DadosHerbicidaValidos && + !autonomia.HerbicidaSuficiente && + !autonomia.HerbicidaOverrideAplicado; + + bool aguardandoReconhecimento = + autonomia.Status == AutonomiaCorredorStatus.AguardandoLiberacaoManual; + + /* + * Em AguardandoLiberacaoManual o motivo original da trava pode já + * ter se recuperado, e o modelo não expõe publicamente qual latch + * ficou pendente. Nesse caso reconhecemos ambos, como a implementação + * antiga já fazia. + * + * Em Critico liberamos apenas o recurso que realmente está bloqueando. + */ + bool liberarBateria = aguardandoReconhecimento || bateriaBloqueando; + bool liberarHerbicida = aguardandoReconhecimento || herbicidaBloqueando; + + op?.Trajetoria?.AtualizarDadosAutonomiaCorredor( + idxCorredor: idxCorredor, + bat_liberada: liberarBateria, + herb_liberado: liberarHerbicida + ); + AtualizarResultadoAutonomia(op?.Trajetoria?.AutonomiaCorredor); } @@ -467,13 +535,21 @@ namespace AgroBase.Forms.IHM.Operacao bool operacaoIniciada = op?.Sensoriamento?.Operacao?.OperacaoIniciada ?? false; - bool aguardandoLiberacao = autonomia.Status == AutonomiaCorredorStatus.AguardandoLiberacaoManual && !autonomia.Liberado; + bool podeLiberarSupervisao = PodeLiberarSupervisao(autonomia); - if (operacaoIniciada && aguardandoLiberacao) + btnLiberarSupervisao.Enabled = + operacaoIniciada && + podeLiberarSupervisao; + + if (operacaoIniciada && podeLiberarSupervisao) { AtualizarTextoAlertaAutonomia(autonomia); MostrarAlertaAutonomia(); + + // MostrarStatusAutonomia() desabilita o botão, então restauramos + // explicitamente o estado após trazer o painel de alerta. + btnLiberarSupervisao.Enabled = true; return; } @@ -483,11 +559,13 @@ namespace AgroBase.Forms.IHM.Operacao private void AtualizarTextoAlertaAutonomia(AutonomiaCorredorModel autonomia) { bool bateriaBloqueando = + autonomia.DadosBateriaValidos && !autonomia.BateriaSuficiente && !autonomia.BateriaOverrideAplicado; bool herbicidaBloqueando = autonomia.PulverizacaoExigida && + autonomia.DadosHerbicidaValidos && !autonomia.HerbicidaSuficiente && !autonomia.HerbicidaOverrideAplicado; @@ -644,10 +722,7 @@ namespace AgroBase.Forms.IHM.Operacao lblStatusAvaliacao.Text = !string.IsNullOrWhiteSpace(autonomia.Motivo) ? autonomia.Motivo : "Autonomia atualizada"; lblStatusAvaliacao.ForeColor = autonomia.Liberado ? corCorredor : HmiTheme.TextMuted; - if (autonomia.Liberado) - MostrarStatusAutonomia(); - else - MostrarAlertaAutonomia(); + AtualizarPainelAutonomia(autonomia); } diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs index 7bbaf041a..18bb0734d 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs @@ -1,4 +1,4 @@ -using AgroBase.Forms.IHM.Controls; +using AgroBase.Forms.IHM.Controls; using AgroBase.Forms.IHM.Operacao.Parametros; using AgroBase.Models; using System; @@ -155,18 +155,36 @@ namespace AgroBase.Forms.IHM.Operacao if (operacaoParametrizando == null) return; - if (operacaoParametrizando.Parametros.Modo == ModoOperacao.MapaGPS && (operacaoParametrizando.Mapa.RuasPercorrer?.Count ?? 0) == 0) + var op = Variaveis.OperacaoEmAndamento; + bool operacaoIniciada = + op?.Sensoriamento?.Operacao?.OperacaoIniciada ?? false; + + /* + * A exigência de mapa/ruas é válida para parametrizar uma operação + * nova. Durante uma operação já iniciada, o botão Salvar realiza + * atualização parcial (pulverizador, agitador, velocidades etc.) e + * não deve ser bloqueado por uma validação estrutural do mapa. + */ + if (!operacaoIniciada && + operacaoParametrizando.Parametros.Modo == ModoOperacao.MapaGPS) { - FuncoesIHM.MostrarAlerta( - "Trajetória não parametrizada", - $"Para salvar os parâmetros da operação {operacaoParametrizando.Sensoriamento.Operacao.Modo}, é necessário configurar o mapa e as ruas a serem percorridas!", - MessageBoxIcon.Warning - ); - return; + int qtdRuas = + operacaoParametrizando.Parametros?.RuasPercorrer?.Count + ?? operacaoParametrizando.Mapa?.RuasPercorrer?.Count + ?? 0; + + if (qtdRuas == 0) + { + FuncoesIHM.MostrarAlerta( + "Trajetória não parametrizada", + $"Para salvar os parâmetros da operação {operacaoParametrizando.Sensoriamento.Operacao.Modo}, é necessário configurar o mapa e as ruas a serem percorridas!", + MessageBoxIcon.Warning + ); + return; + } } - var op = Variaveis.OperacaoEmAndamento; - if (op?.Sensoriamento?.Operacao?.OperacaoIniciada ?? false) + if (operacaoIniciada) op?.AtualizarParametrosParciais(operacaoParametrizando.Parametros, op); else OperacaoModel.CarregarParamerosOperacaoBase(operacaoParametrizando.Parametros); diff --git a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs index 02f18dcfc..4ace1aea8 100644 --- a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs +++ b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs @@ -772,7 +772,7 @@ namespace AgroBase.Forms lblDistProx.Text = $"Distância Próximo Ponto: {(proximoPonto?.DistanciaAtual ?? 0):F2} m"; lblDistAnt.Text = - $"Dist Esquerda: {dadosTrajetoria.DistanciaEsquerda * 100:F0} cm | Dist Direita: {dadosTrajetoria.DistanciaDireita * 100:F0} cm"; + $"Esq: {dadosTrajetoria.DistanciaEsquerda * 100:F0} cm | Dir: {dadosTrajetoria.DistanciaDireita * 100:F0} cm | Err: {(dadosTrajetoria.DistanciaDireita - dadosTrajetoria.DistanciaEsquerda) * 100:F0} cm"; lblTempoRestante.Text = $"Tempo Restante: {dadosTrajetoria.TempoEstimadoRestante}"; lblTempoOperacao.Text = @@ -1446,6 +1446,15 @@ namespace AgroBase.Forms Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1); AtualizarDadosTela(); + + for (int idxCorredor = 0; idxCorredor < Variaveis.OperacaoEmAndamento.Trajetoria.Corredores.Count; idxCorredor++) + { + Variaveis.OperacaoEmAndamento.Trajetoria.AtualizarDadosAutonomiaCorredor( + idxCorredor: idxCorredor, + bat_liberada: true, + herb_liberado: true + ); + } } } catch (Exception ex) diff --git a/AgroBase/AgroBase/Models/Modules/AtuadorModel.cs b/AgroBase/AgroBase/Models/Modules/AtuadorModel.cs index 91352cb9b..239b4bf97 100644 --- a/AgroBase/AgroBase/Models/Modules/AtuadorModel.cs +++ b/AgroBase/AgroBase/Models/Modules/AtuadorModel.cs @@ -2599,12 +2599,24 @@ namespace AgroBase.Models.Modules if (op?.DispAtu?.Dados == null) return tasks; - tasks.Add(ExecutarCalibragemProtegidaAsync(forcar)); + // Mantido por compatibilidade com os chamadores antigos. + // Task deriva de Task, então o contrato List continua válido. + tasks.Add(RealizarCalibragemComResultadoAsync(forcar)); return tasks; } - private async Task ExecutarCalibragemProtegidaAsync(bool forcar) + public Task RealizarCalibragemComResultadoAsync(bool forcar) + { + var op = Variaveis.OperacaoEmAndamento; + + if (op?.DispAtu?.Dados == null) + return Task.FromResult(false); + + return ExecutarCalibragemProtegidaAsync(forcar); + } + + private async Task ExecutarCalibragemProtegidaAsync(bool forcar) { var op = Variaveis.OperacaoEmAndamento; @@ -2637,11 +2649,15 @@ namespace AgroBase.Models.Modules op.Sensoriamento.InserirLog(Dispositivo, StatusModulo.Alerta, 50, $"Teste do módulo {Modulo_ID} finalizado com pendências: " + string.Join(", ", v)); } + + return sucesso; } catch (OperationCanceledException ex) { Variaveis.MostrarLog($"[AtuadorModel] [RealizarCalibragemAsync] Calibragem cancelada: {ex.Message}"); op?.Sensoriamento?.InserirLog(Dispositivo, StatusModulo.Alerta, 50, $"Calibragem do módulo {Modulo_ID} cancelada: {ex.Message}"); + + return false; } catch (Exception ex) { diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index 365e19aeb..f7a2bdbec 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -1,4 +1,4 @@ -using AgroBase.Comum; +using AgroBase.Comum; using AgroBase.Forms; using AgroBase.Forms.Operacoes; using AgroBase.Models.Modules; @@ -349,6 +349,12 @@ namespace AgroBase.Models private static string _discoverySessionId = Guid.NewGuid().ToString("N"); private static long _telemetrySequence = 0; private readonly SemaphoreSlim _calibragemGate = new SemaphoreSlim(1, 1); + + // A calibração inicial é uma etapa transacional da partida da operação. + // Depois que ela efetivamente entrou em calibração e terminou, não permitimos + // que o timer periódico pare o rover novamente para repetir a mesma rotina. + private volatile bool _calibragemInicialConcluida = false; + public bool CalibrandoRuntime { get @@ -361,6 +367,12 @@ namespace AgroBase.Models private readonly TimeSpan _cooldownCalibragem = TimeSpan.FromSeconds(30); private bool PodeTentarCalibragemAutomatica() { + // Depois da calibração inicial efetivamente executada, qualquer pendência + // deve permanecer como diagnóstico/alerta. Não paramos o rover novamente + // no meio da operação para recalibrar automaticamente. + if (_calibragemInicialConcluida) + return false; + return DateTime.Now - _ultimaTentativaCalibragem > _cooldownCalibragem; } private void RegistrarTentativaCalibragem() @@ -1395,6 +1407,11 @@ namespace AgroBase.Models op.Sensoriamento.UltimoRegistroLog = agora; op.idxLog = 0; + + // Cada nova operação ganha um ciclo de calibração novo. + op._calibragemInicialConcluida = false; + op._ultimaTentativaCalibragem = DateTime.MinValue; + op.IniciarTimerLogsOperacao(); RedisService.Publish(CmdKey.ManagerWorkerRx, JsonConvert.SerializeObject(new { cmd = ManagerWorkerCommandType.IniciarMPC })); @@ -1665,6 +1682,12 @@ namespace AgroBase.Models if (!await _calibragemGate.WaitAsync(0)) return; + // A calibração forçada do início também conta como tentativa. + // Isso evita que o timer periódico considere o cooldown vencido logo + // após a partida da operação. + if (forcar) + op.RegistrarTentativaCalibragem(); + TipoMovimentoDirecional tipoControle = op.Controle.TipoMovimento; _pausaAntesCalibragem = op.Sensoriamento.Operacao.Pausa; bool entrouEmCalibragem = false; @@ -1771,9 +1794,16 @@ namespace AgroBase.Models op.Sensoriamento.InserirLog(T_Code.Mod, StatusModulo.Operante, 100, "Calibragem dos módulos iniciada..."); var tasks = new List(); + Task taskCalibragemAtu = null; if (calibragemAtuNecessaria) - tasks.AddRange(dispAtu.Dados.RealizarCalibragemAsync(forcar)); + { + // Para o ATU consumimos o resultado real da calibração. + // Uma Task concluída não significa necessariamente que bomba/bicos + // passaram nos testes. + taskCalibragemAtu = dispAtu.Dados.RealizarCalibragemComResultadoAsync(forcar); + tasks.Add(taskCalibragemAtu); + } if (calibragemDirNecessaria) tasks.AddRange(dispMvd.Dados.RealizarCalibragemDirAsync(forcar)); @@ -1799,12 +1829,56 @@ namespace AgroBase.Models { await allTasks; - Variaveis.MostrarLog( - "[OperacaoModel.RealizarCalibragemInicialAsync] " + - "Tasks de calibragem dos módulos concluídas." - ); + bool atuSucesso = + taskCalibragemAtu == null || + (taskCalibragemAtu.Status == TaskStatus.RanToCompletion && taskCalibragemAtu.Result); - op.Sensoriamento.InserirLog(T_Code.Mod, StatusModulo.Operante, 100, "Calibragem finalizada."); + var pendencias = op.Sensoriamento?.Operacao?.ModsCalibragem? + .SelectMany(modulo => + (modulo.Value ?? new Dictionary()) + .Where(item => !item.Value) + .Select(item => $"{modulo.Key}:{item.Key}") + ) + .ToList() + ?? new List(); + + bool calibragemComPendencias = !atuSucesso || pendencias.Any(); + + if (calibragemComPendencias) + { + string detalhe = pendencias.Any() + ? string.Join(", ", pendencias) + : "ATU reportou pendência sem item individual identificado"; + + string mensagem = + $"Calibragem inicial concluída com pendências: {detalhe}. " + + "A operação será liberada sem nova recalibração automática nesta execução."; + + Variaveis.MostrarLog( + "[OperacaoModel.RealizarCalibragemInicialAsync] " + mensagem + ); + + op.Sensoriamento.InserirLog( + T_Code.Mod, + StatusModulo.Alerta, + 50, + mensagem + ); + } + else + { + Variaveis.MostrarLog( + "[OperacaoModel.RealizarCalibragemInicialAsync] " + + "Calibragem dos módulos concluída com sucesso." + ); + + op.Sensoriamento.InserirLog( + T_Code.Mod, + StatusModulo.Operante, + 100, + "Calibragem finalizada com sucesso." + ); + } } else { @@ -1877,6 +1951,17 @@ namespace AgroBase.Models ); } + if (forcar && entrouEmCalibragem) + { + op._calibragemInicialConcluida = true; + + Variaveis.MostrarLog( + "[OperacaoModel.RealizarCalibragemInicialAsync] " + + "Ciclo de calibração inicial encerrado. Recalibração automática " + + "durante esta operação ficará desabilitada." + ); + } + try { _calibragemGate.Release(); @@ -3291,10 +3376,9 @@ namespace AgroBase.Models op.RegistrarTentativaCalibragem(); _ = Task.Run(async () => await op.RealizarCalibragemInicialAsync(false)); } - if (op.CalibrandoRuntime && op.TodosModulosMandatoriosCalibrados()) - { - op.DefinirEstadoCalibragemRuntime(false); - } + + // O estado Calibrando pertence exclusivamente à rotina de calibração. + // O timer não deve liberar a operação antes das Tasks terminarem. } } @@ -4140,6 +4224,9 @@ namespace AgroBase.Models OperacaoLiberacaoDebug = RedisService.GetField(CtxKey.DadosOperacao, "operacao_liberacao_debug"), StatusOperacaoAtual = (StatusOperacao)RedisService.GetField(CtxKey.DadosOperacao, "status", 0), + MotivoStatusOperacao = RedisService.GetField(CtxKey.DadosOperacao, "motivo_status_operacao"), + UltimoMotivoParada = RedisService.GetField(CtxKey.DadosOperacao, "ultimo_motivo_parada"), + ModulosMandatorios = new List(op.Parametros.ModulosMandatorios ?? new List()), ParametrosMandatorios = new List(op.Parametros.ParametrosMandatorios ?? new List()), DispositivosMapeados = new List(SerialService.DispositivosMapeados), @@ -5375,6 +5462,8 @@ namespace AgroBase.Models public StatusOperacao StatusOperacaoAnterior { get; set; } public string ErroOperacaoLiberada { get; set; } public JObject OperacaoLiberacaoDebug { get; set; } + public string MotivoStatusOperacao { get; set; } + public string UltimoMotivoParada { get; set; } public bool OperacaoConfigurada { get; set; } public bool OperacaoLiberada { get; set; } public bool OperacaoIniciada { get; set; } @@ -5439,6 +5528,8 @@ namespace AgroBase.Models Status = Status, ErroOperacaoLiberada = ErroOperacaoLiberada, OperacaoLiberacaoDebug = OperacaoLiberacaoDebug, + MotivoStatusOperacao = MotivoStatusOperacao, + UltimoMotivoParada = UltimoMotivoParada, OperacaoConfigurada = OperacaoConfigurada, OperacaoLiberada = OperacaoLiberada, OperacaoIniciada = OperacaoIniciada, diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs index 56bb79b52..4a0e7d842 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs @@ -712,6 +712,8 @@ namespace AgroBase.Models.Operacoes Iniciada = op?.Sensoriamento?.Operacao?.OperacaoIniciada ?? false, Liberada = op?.Sensoriamento?.Operacao?.OperacaoLiberada ?? false, Status = op?.Sensoriamento?.Operacao?.StatusOperacaoAtual ?? StatusOperacao.Erro, + UltimoMotivoParada = op?.Sensoriamento?.Operacao?.UltimoMotivoParada, + MotivoStatusOperacao = op?.Sensoriamento?.Operacao?.MotivoStatusOperacao, Modo = parametros?.Modo ?? ModoOperacao.NaoDefinido, ImpedimentoErro = op?.Sensoriamento?.Operacao?.ErroOperacaoLiberada, TempoAguardandoSegs = op?.Sensoriamento?.Operacao?.TempoAguardandoSeg ?? 0, @@ -1497,6 +1499,8 @@ namespace AgroBase.Models.Operacoes public ModoOperacao Modo { get; set; } public StatusOperacao Status { get; set; } public string ImpedimentoErro { get; set; } + public string MotivoStatusOperacao { get; set; } + public string UltimoMotivoParada { get; set; } public bool Liberada { get; set; } public bool Iniciada { get; set; } public bool Emergencia { get; set; } diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index 6749963d7..56578c0b4 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -114,8 +114,108 @@ namespace AgroBase.Models private int MaxPontosEntradaRecuperacaoTransicao { get; set; } = 12; private int ConfirmacoesMinimasEntradaProximoCorredor { get; set; } = 2; + // V11 - watchdog de progresso da trajetória. + // + // Filosofia: + // - NÃO julga saúde por repetição de comando/ângulo. Uma rua reta pode + // operar por muito tempo com ângulo 0 e velocidade constante. + // - julga progresso FÍSICO + GEOMÉTRICO: o rover precisa estar andando + // e, ao mesmo tempo, reduzir a distância restante / distância ao + // próximo ponto ou avançar o prefixo autoritativo. + // - Suspeita é passiva. Reaquisição apenas limita a velocidade e dá + // tempo para as recuperações estritas já existentes agirem. + // - se mesmo assim não houver progresso, a saída é falha fechada: + // velocidade 0 + operação não liberada. + public bool WatchdogProgressoHabilitado { get; set; } = true; + public double WatchdogVelocidadeMinimaMs { get; set; } = 0.12; + public double WatchdogProgressoRestanteMinimoM { get; set; } = 0.22; + public double WatchdogProgressoAlvoMinimoM { get; set; } = 0.15; + public double WatchdogTempoMinimoSuspeitaS { get; set; } = 2.50; + public double WatchdogTempoMinimoReaquisicaoS { get; set; } = 3.75; + public double WatchdogTempoMaximoReaquisicaoS { get; set; } = 4.00; + public double WatchdogDistanciaMaximaReaquisicaoM { get; set; } = 1.50; + public double WatchdogTempoMaximoSemMovimentoS { get; set; } = 6.00; + public double WatchdogDistanciaMinimaMovimentoM { get; set; } = 0.15; + public double WatchdogAnguloMinimoInversaoGraus { get; set; } = 3.0; + public int WatchdogInversoesParaReaquisicao { get; set; } = 4; + #endregion + public enum EstadoWatchdogTrajetoria + { + Desarmado = 0, + Normal = 1, + Suspeita = 2, + Reaquisicao = 3, + ParadaSegura = 4 + } + + [JsonProperty] + public EstadoWatchdogTrajetoria WatchdogEstado { get; private set; } = EstadoWatchdogTrajetoria.Desarmado; + + [JsonProperty] + public string WatchdogMotivo { get; private set; } = string.Empty; + + [JsonProperty] + public double WatchdogDistanciaFisicaJanelaM { get; private set; } = 0.0; + + [JsonProperty] + public int WatchdogInversoesAnguloJanela { get; private set; } = 0; + + [JsonIgnore] + private DateTime _watchdogInicioJanelaUtc = DateTime.MinValue; + + [JsonIgnore] + private DateTime _watchdogInicioReaquisicaoUtc = DateTime.MinValue; + + [JsonIgnore] + private double _watchdogDistanciaFisicaInicioReaquisicaoM = 0.0; + + [JsonIgnore] + private int _watchdogIdxPontoAtualRef = -1; + + [JsonIgnore] + private int _watchdogIdxProximoPontoRef = -1; + + [JsonIgnore] + private int _watchdogIdxCorredorRef = -1; + + [JsonIgnore] + private int _watchdogQtdPontosRef = -1; + + [JsonIgnore] + private StatusCarroMapa _watchdogStatusRef = StatusCarroMapa.Parado; + + [JsonIgnore] + private double _watchdogDistanciaRestanteRefM = double.NaN; + + [JsonIgnore] + private double _watchdogDistanciaProximoRefM = double.NaN; + + [JsonIgnore] + private double _watchdogUltimoAnguloComandoGraus = 0.0; + + [JsonIgnore] + private bool _watchdogUltimoAnguloValido = false; + + [JsonIgnore] + private double _watchdogUltimoTimestampGps = double.NaN; + + [JsonIgnore] + private DateTime _watchdogUltimoMomentoGps = DateTime.MinValue; + + [JsonIgnore] + private double _watchdogUltimaLatitudeGps = double.NaN; + + [JsonIgnore] + private double _watchdogUltimaLongitudeGps = double.NaN; + + [JsonIgnore] + private DateTime _watchdogUltimaReducaoVelocidadeUtc = DateTime.MinValue; + + [JsonIgnore] + private DateTime _watchdogUltimaReafirmacaoParadaUtc = DateTime.MinValue; + public DateTime UltimaAtualizacaoDados { get; set; } = DateTime.MinValue; public bool RetornandoBase { get; set; } public AutonomiaCorredorModel AutonomiaCorredor { get; set; } @@ -861,6 +961,549 @@ namespace AgroBase.Models var _trajetoria = (_TrajetoriaDinamica ?? new List()).Select(x => x.Posicao).ToList(); DistanciaRestante = GPSUtils.DistanciaDoTrecho(_trajetoria); } + private bool WatchdogNovaAmostraGps() + { + var gps = GPSPosicaoAtual; + if (gps == null) + return false; + + double ts = gps.TimestampPos?.valor ?? double.NaN; + + if (ValorFinito(ts)) + { + if (ValorFinito(_watchdogUltimoTimestampGps) && + Math.Abs(ts - _watchdogUltimoTimestampGps) <= ToleranciaCoordenada) + { + return false; + } + + _watchdogUltimoTimestampGps = ts; + _watchdogUltimoMomentoGps = gps.Momento; + _watchdogUltimaLatitudeGps = gps.Latitude; + _watchdogUltimaLongitudeGps = gps.Longitude; + return true; + } + + if (gps.Momento != DateTime.MinValue) + { + if (gps.Momento == _watchdogUltimoMomentoGps) + return false; + + _watchdogUltimoMomentoGps = gps.Momento; + _watchdogUltimaLatitudeGps = gps.Latitude; + _watchdogUltimaLongitudeGps = gps.Longitude; + return true; + } + + if (ValorFinito(_watchdogUltimaLatitudeGps) && + ValorFinito(_watchdogUltimaLongitudeGps) && + Math.Abs(gps.Latitude - _watchdogUltimaLatitudeGps) <= ToleranciaCoordenada && + Math.Abs(gps.Longitude - _watchdogUltimaLongitudeGps) <= ToleranciaCoordenada) + { + return false; + } + + _watchdogUltimaLatitudeGps = gps.Latitude; + _watchdogUltimaLongitudeGps = gps.Longitude; + return true; + } + + private double ObterDistanciaMinimaSuspeitaWatchdogM() + { + switch (StatusAtual) + { + case StatusCarroMapa.Manobrando: + case StatusCarroMapa.EntrandoRua: + case StatusCarroMapa.SaindoRua: + return 1.00; + + case StatusCarroMapa.CaminhandoRua: + return 1.20; + + case StatusCarroMapa.Direcionando: + case StatusCarroMapa.RetornandoBase: + return 1.50; + + default: + return 1.50; + } + } + + private void ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria novoEstado, + string motivo, + bool registrarRecuperacao = false + ) + { + var estadoAnterior = WatchdogEstado; + + _watchdogInicioJanelaUtc = DateTime.UtcNow; + _watchdogInicioReaquisicaoUtc = DateTime.MinValue; + _watchdogDistanciaFisicaInicioReaquisicaoM = 0.0; + + _watchdogIdxPontoAtualRef = PontoAtual?.idxPonto ?? -1; + _watchdogIdxProximoPontoRef = ProximoPonto?.idxPonto ?? -1; + _watchdogIdxCorredorRef = CorredorAtual?.Idx ?? -1; + _watchdogQtdPontosRef = _TrajetoriaFixa?.Count ?? 0; + _watchdogStatusRef = StatusAtual; + + _watchdogDistanciaRestanteRefM = DistanciaRestante; + _watchdogDistanciaProximoRefM = + (GPSPosicaoAtual != null && ProximoPonto?.Posicao != null) + ? GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, ProximoPonto.Posicao) + : double.NaN; + + WatchdogDistanciaFisicaJanelaM = 0.0; + WatchdogInversoesAnguloJanela = 0; + + _watchdogUltimoAnguloComandoGraus = op?.Controle?.Angulo ?? 0.0; + _watchdogUltimoAnguloValido = Math.Abs(_watchdogUltimoAnguloComandoGraus) >= WatchdogAnguloMinimoInversaoGraus; + + WatchdogEstado = novoEstado; + WatchdogMotivo = motivo ?? string.Empty; + + if (registrarRecuperacao && + estadoAnterior != EstadoWatchdogTrajetoria.Normal && + estadoAnterior != EstadoWatchdogTrajetoria.Desarmado) + { + string msg = + $"[TRJ/WATCHDOG] Progresso recuperado. " + + $"estado={estadoAnterior}->Normal, " + + $"idx={_watchdogIdxPontoAtualRef}, " + + $"status={StatusAtual}."; + + Variaveis.MostrarLog(msg); + op?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Operante, + 100, + msg + ); + } + } + + private void DesarmarWatchdog(string motivo, bool limparParadaSegura) + { + if (WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura && + !limparParadaSegura) + { + return; + } + + ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria.Desarmado, + motivo ?? string.Empty, + registrarRecuperacao: false + ); + } + + private void EntrarSuspeitaWatchdog(string motivo) + { + if (WatchdogEstado == EstadoWatchdogTrajetoria.Suspeita || + WatchdogEstado == EstadoWatchdogTrajetoria.Reaquisicao || + WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura) + { + return; + } + + WatchdogEstado = EstadoWatchdogTrajetoria.Suspeita; + WatchdogMotivo = motivo; + + string msg = + $"[TRJ/WATCHDOG] SUSPEITA de falta de progresso. " + + $"idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + + $"status={StatusAtual}, fisico={WatchdogDistanciaFisicaJanelaM:F2}m, " + + $"inversoes={WatchdogInversoesAnguloJanela}, motivo={motivo}"; + + Variaveis.MostrarLog(msg); + op?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Alerta, + 80, + msg + ); + } + + private void LimitarVelocidadeReaquisicaoWatchdog() + { + if ((DateTime.UtcNow - _watchdogUltimaReducaoVelocidadeUtc).TotalSeconds < 0.50) + return; + + var parametrosControle = op?.Parametros?.Controle; + var controle = op?.Controle; + + if (parametrosControle == null || controle == null) + return; + + double velocidadeSeguraPercent = parametrosControle.MovVelocidadeCErvasPercent; + + if (!ValorFinito(velocidadeSeguraPercent) || + velocidadeSeguraPercent <= 0 || + controle.PercentualVelocidadeSP <= velocidadeSeguraPercent + 0.1) + { + return; + } + + RedisService.AtualizarCampos( + CtxKey.DadosControle, + ("velocidade_sp", velocidadeSeguraPercent) + ); + + _watchdogUltimaReducaoVelocidadeUtc = DateTime.UtcNow; + } + + private void EntrarReaquisicaoWatchdog(string motivo) + { + if (WatchdogEstado == EstadoWatchdogTrajetoria.Reaquisicao || + WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura) + { + return; + } + + WatchdogEstado = EstadoWatchdogTrajetoria.Reaquisicao; + WatchdogMotivo = motivo; + _watchdogInicioReaquisicaoUtc = DateTime.UtcNow; + _watchdogDistanciaFisicaInicioReaquisicaoM = WatchdogDistanciaFisicaJanelaM; + + // Não troca status, não muda geometria e não marca waypoint. + // As recuperações estritas do fluxo normal continuam soberanas. + // A única intervenção é limitar a velocidade ao perfil seguro que + // já existe no equipamento, caso o Python esteja mandando mais. + LimitarVelocidadeReaquisicaoWatchdog(); + + string msg = + $"[TRJ/WATCHDOG] REAQUISICAO armada. " + + $"idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + + $"status={StatusAtual}, fisico={WatchdogDistanciaFisicaJanelaM:F2}m, " + + $"inversoes={WatchdogInversoesAnguloJanela}, motivo={motivo}"; + + Variaveis.MostrarLog(msg); + op?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Alerta, + 70, + msg + ); + } + + private void AcionarParadaSeguraWatchdog(string motivo) + { + if (WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura) + return; + + WatchdogEstado = EstadoWatchdogTrajetoria.ParadaSegura; + WatchdogMotivo = motivo; + + RedisService.AtualizarCampos( + CtxKey.DadosControle, + ("velocidade_sp", 0) + ); + + RedisService.AtualizarCampos( + CtxKey.DadosOperacao, + ("liberado", false), + ("motivo_nao_liberado", "Watchdog de trajetória: " + motivo) + ); + + _watchdogUltimaReafirmacaoParadaUtc = DateTime.UtcNow; + + string msg = + $"[TRJ/WATCHDOG] PARADA SEGURA. " + + $"idx={PontoAtual?.idxPonto}, prox={ProximoPonto?.idxPonto}, " + + $"status={StatusAtual}, fisico={WatchdogDistanciaFisicaJanelaM:F2}m, " + + $"inversoes={WatchdogInversoesAnguloJanela}, motivo={motivo}"; + + Variaveis.MostrarLog(msg); + op?.Sensoriamento?.InserirLog( + T_Code.Trj, + StatusModulo.Falha, + 0, + msg + ); + } + + private void ReafirmarParadaSeguraWatchdog() + { + if ((DateTime.UtcNow - _watchdogUltimaReafirmacaoParadaUtc).TotalSeconds < 0.50) + return; + + RedisService.AtualizarCampos( + CtxKey.DadosControle, + ("velocidade_sp", 0) + ); + + _watchdogUltimaReafirmacaoParadaUtc = DateTime.UtcNow; + } + + private void AtualizarWatchdogProgressoTrajetoria() + { + bool operacaoEmAndamento = + op?.Sensoriamento?.Operacao?.StatusOperacaoAtual == + StatusOperacao.EmAndamento; + + bool controleAutomatico = op?.Parametros?.ControleAutomatico ?? false; + bool controleManualAcionado = op?.Parametros?.Controle?.ControleManualAcionado == true; + + // Fora da operação automática, o operador tem autoridade para + // reposicionar/rearmar o equipamento. Uma nova entrada no modo + // automático começa com janela limpa. + if (!WatchdogProgressoHabilitado || + !operacaoEmAndamento || + !controleAutomatico || + controleManualAcionado) + { + DesarmarWatchdog( + "watchdog fora do regime automático", + limparParadaSegura: !operacaoEmAndamento || controleManualAcionado + ); + return; + } + + if (WatchdogEstado == EstadoWatchdogTrajetoria.ParadaSegura) + { + ReafirmarParadaSeguraWatchdog(); + return; + } + + if (!_TrajetoriaFixaDefinida || + GPSPosicaoAtual == null || + GPSPosicaoAnterior == null || + PontoAtual == null || + ProximoPonto == null || + ReferenceEquals(PontoAtual, ProximoPonto) || + TrajeotiraConcluida || + StatusAtual == StatusCarroMapa.Parado) + { + DesarmarWatchdog("trajetória sem condição de monitoramento", limparParadaSegura: false); + return; + } + + var controle = op?.Controle; + if (controle == null || controle.EmFreio) + { + DesarmarWatchdog("movimento freado", limparParadaSegura: false); + return; + } + + double velocidadeComandadaMs = + FuncoesMatematicas.CalculaVelocidadeMsPercentual( + controle.PercentualVelocidadeSP + ); + + if (!ValorFinito(velocidadeComandadaMs) || + velocidadeComandadaMs < WatchdogVelocidadeMinimaMs) + { + DesarmarWatchdog("velocidade abaixo do limiar do watchdog", limparParadaSegura: false); + return; + } + + if (!WatchdogNovaAmostraGps()) + return; + + DateTime agoraUtc = DateTime.UtcNow; + + int idxAtual = PontoAtual.idxPonto; + int idxProximo = ProximoPonto.idxPonto; + int idxCorredor = CorredorAtual?.Idx ?? PontoAtual.idxCorredor; + int qtdPontos = _TrajetoriaFixa?.Count ?? 0; + + double distanciaProximo = + GPSUtils.DistanciaEntrePontos( + GPSPosicaoAtual, + ProximoPonto.Posicao + ); + + bool referenciaMudou = + _watchdogInicioJanelaUtc == DateTime.MinValue || + _watchdogIdxPontoAtualRef != idxAtual || + _watchdogIdxProximoPontoRef != idxProximo || + _watchdogIdxCorredorRef != idxCorredor || + _watchdogQtdPontosRef != qtdPontos || + _watchdogStatusRef != StatusAtual; + + if (referenciaMudou) + { + ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria.Normal, + "referência autoritativa avançou ou mudou", + registrarRecuperacao: true + ); + return; + } + + double passoFisico = + GPSUtils.DistanciaEntrePontos( + GPSPosicaoAnterior, + GPSPosicaoAtual + ); + + double distanciaEsperadaAmostra = + velocidadeComandadaMs / + Math.Max(1, GPSService.TaxaAmostragemHz); + + double limiteSaltoGps = + Math.Max( + 0.75, + distanciaEsperadaAmostra * 6.0 + 0.35 + ); + + if (!ValorFinito(passoFisico) || passoFisico < 0) + passoFisico = 0; + + if (passoFisico > limiteSaltoGps) + { + // Um salto GNSS não pode ser interpretado nem como progresso + // nem como ausência de progresso. Reinicia a janela e espera + // novas amostras coerentes. + ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria.Normal, + $"salto GNSS ignorado ({passoFisico:F2}m)", + registrarRecuperacao: false + ); + return; + } + + double pisoMovimentoGps = + Math.Max(0.015, distanciaEsperadaAmostra * 0.15); + + if (passoFisico >= pisoMovimentoGps) + WatchdogDistanciaFisicaJanelaM += passoFisico; + + double anguloAtual = controle.Angulo; + bool anguloAtualValido = + ValorFinito(anguloAtual) && + Math.Abs(anguloAtual) >= WatchdogAnguloMinimoInversaoGraus; + + if (_watchdogUltimoAnguloValido && + anguloAtualValido && + Math.Sign(_watchdogUltimoAnguloComandoGraus) != Math.Sign(anguloAtual)) + { + WatchdogInversoesAnguloJanela++; + } + + if (anguloAtualValido) + { + _watchdogUltimoAnguloComandoGraus = anguloAtual; + _watchdogUltimoAnguloValido = true; + } + + double melhoraRestante = + ValorFinito(_watchdogDistanciaRestanteRefM) && ValorFinito(DistanciaRestante) + ? _watchdogDistanciaRestanteRefM - DistanciaRestante + : 0.0; + + double melhoraProximo = + ValorFinito(_watchdogDistanciaProximoRefM) && ValorFinito(distanciaProximo) + ? _watchdogDistanciaProximoRefM - distanciaProximo + : 0.0; + + bool progressoGeometrico = + melhoraRestante >= WatchdogProgressoRestanteMinimoM || + melhoraProximo >= WatchdogProgressoAlvoMinimoM; + + if (progressoGeometrico) + { + ReiniciarJanelaWatchdog( + EstadoWatchdogTrajetoria.Normal, + $"progresso geométrico confirmado (rest={melhoraRestante:F2}m, alvo={melhoraProximo:F2}m)", + registrarRecuperacao: true + ); + return; + } + + double tempoJanelaS = + Math.Max( + 0.0, + (agoraUtc - _watchdogInicioJanelaUtc).TotalSeconds + ); + + // Caso separado: existe comando de movimento há bastante tempo, + // mas o rover praticamente não saiu do lugar. Isso é mais parecido + // com obstáculo/falha mecânica do que com erro de waypoint. + if (tempoJanelaS >= WatchdogTempoMaximoSemMovimentoS && + WatchdogDistanciaFisicaJanelaM < WatchdogDistanciaMinimaMovimentoM) + { + AcionarParadaSeguraWatchdog( + $"movimento comandado por {tempoJanelaS:F1}s, mas deslocamento físico foi apenas " + + $"{WatchdogDistanciaFisicaJanelaM:F2}m" + ); + return; + } + + double distanciaMinimaSuspeita = + ObterDistanciaMinimaSuspeitaWatchdogM(); + + if (WatchdogEstado == EstadoWatchdogTrajetoria.Normal && + tempoJanelaS >= WatchdogTempoMinimoSuspeitaS && + WatchdogDistanciaFisicaJanelaM >= distanciaMinimaSuspeita) + { + EntrarSuspeitaWatchdog( + $"percorreu {WatchdogDistanciaFisicaJanelaM:F2}m sem reduzir a rota restante nem o próximo alvo" + ); + } + + bool muitasInversoes = + WatchdogInversoesAnguloJanela >= + WatchdogInversoesParaReaquisicao; + + double distanciaMinimaReaquisicao = + distanciaMinimaSuspeita * 1.55; + + bool evidenciaForteReaquisicao = + tempoJanelaS >= WatchdogTempoMinimoReaquisicaoS && + ( + WatchdogDistanciaFisicaJanelaM >= distanciaMinimaReaquisicao || + ( + muitasInversoes && + WatchdogDistanciaFisicaJanelaM >= distanciaMinimaSuspeita * 1.20 + ) + ); + + if (WatchdogEstado == EstadoWatchdogTrajetoria.Suspeita && + evidenciaForteReaquisicao) + { + EntrarReaquisicaoWatchdog( + muitasInversoes + ? "falta de progresso + inversões repetidas do comando direcional" + : "falta de progresso persistente apesar de deslocamento físico" + ); + } + + if (WatchdogEstado != EstadoWatchdogTrajetoria.Reaquisicao) + return; + + LimitarVelocidadeReaquisicaoWatchdog(); + + double tempoReaquisicaoS = + Math.Max( + 0.0, + (agoraUtc - _watchdogInicioReaquisicaoUtc).TotalSeconds + ); + + double distanciaFisicaReaquisicaoM = + Math.Max( + 0.0, + WatchdogDistanciaFisicaJanelaM - + _watchdogDistanciaFisicaInicioReaquisicaoM + ); + + bool excedeuDistanciaReaquisicao = + distanciaFisicaReaquisicaoM >= + WatchdogDistanciaMaximaReaquisicaoM; + + bool excedeuTempoReaquisicao = + tempoReaquisicaoS >= WatchdogTempoMaximoReaquisicaoS && + distanciaFisicaReaquisicaoM >= 0.40; + + if (excedeuDistanciaReaquisicao || excedeuTempoReaquisicao) + { + AcionarParadaSeguraWatchdog( + $"reaquisição não convergiu: tempo={tempoReaquisicaoS:F1}s, " + + $"deslocamento={distanciaFisicaReaquisicaoM:F2}m" + ); + } + } + [JsonProperty] public double PercentualTrajetoria { get; private set; } public void AtualizarPercentualTrajetoria() @@ -1482,6 +2125,12 @@ namespace AgroBase.Models AtualizarTrajetoriaDinamicaCore(); AtualizarDistanciaRestante(); + + // V11 - supervisão de progresso. Executa somente depois de termos + // posição, status, trajetória dinâmica e distância restante todos + // atualizados no mesmo snapshot GNSS do ciclo. + AtualizarWatchdogProgressoTrajetoria(); + AtualizarPercentualTrajetoria(); AtualizarTempoEstimado(); AtualizarTrajetoriaConcluida(); diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index f4f4e1469..bb7358558 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -1084,87 +1084,215 @@ namespace AgroBase.Models return novoPonto; } - public static GPSModel SimularNovaPosicao(TipoMovimentoDirecional tipoMovimento, double anguloControleDeg, double velocidadeMs, double tempoDelta, double anguloAtualDeg, GPSModel pos) - { - double L = DistanciaEntreEixosCm / 100.0; + // V16 - Simulação 4WS unificada com o MPC Python. + // + // Convenção de alto nível: + // anguloControleDeg > 0 => yaw positivo do corpo em qualquer modo. + // Em RodasTraseiras isso implica esterçamento físico traseiro invertido. + // + // Referência cinemática: ponto médio entre os eixos. + // velocidadeMs: módulo da velocidade do centro do rover. + // + // Modelo: + // beta = atan((tan(df) + tan(dr)) / 2) + // kappaGeom = cos(beta) * (tan(df) - tan(dr)) / L + // kappaEf = kappaGeom / (1 + Ku*v²) + // omega = v*kappaEf + // + // Integração analítica com beta e omega constantes no passo. + + public static GPSModel SimularNovaPosicao( + TipoMovimentoDirecional tipoMovimento, + double anguloControleDeg, + double velocidadeMs, + double tempoDelta, + double anguloAtualDeg, + GPSModel pos) + { + double L = Math.Max(0.05, DistanciaEntreEixosCm / 100.0); + + // ------------------------------------------------------------ + // 1) Comando de alto nível -> ângulos FÍSICOS dos dois eixos + // ------------------------------------------------------------ + double dfDeg = 0.0; + double drDeg = 0.0; - // Defina deltas por modo - double dfDeg = 0, drDeg = 0; switch (tipoMovimento) { - case TipoMovimentoDirecional.RodasDianteiras: dfDeg = anguloControleDeg; drDeg = 0; break; - case TipoMovimentoDirecional.RodasTraseiras: dfDeg = 0; drDeg = anguloControleDeg; break; - case TipoMovimentoDirecional.MovimentoArco: dfDeg = anguloControleDeg; drDeg = -anguloControleDeg; break; - case TipoMovimentoDirecional.MovimentoDiagonal: dfDeg = anguloControleDeg; drDeg = anguloControleDeg; break; + case TipoMovimentoDirecional.RodasDianteiras: + dfDeg = anguloControleDeg; + drDeg = 0.0; + break; + + case TipoMovimentoDirecional.RodasTraseiras: + // Importante: + // +angulo de comando significa +yaw do corpo. + // Para obter o mesmo yaw usando apenas o eixo traseiro, + // a roda traseira precisa esterçar fisicamente no sentido oposto. + dfDeg = 0.0; + drDeg = -anguloControleDeg; + break; + + case TipoMovimentoDirecional.MovimentoArco: + dfDeg = anguloControleDeg; + drDeg = -anguloControleDeg; + break; + + case TipoMovimentoDirecional.MovimentoDiagonal: + case TipoMovimentoDirecional.MovimentoLateral: + // Mesma família cinemática: eixos em fase, sem yaw. + dfDeg = anguloControleDeg; + drDeg = anguloControleDeg; + break; + + default: + // Fallback conservador: dianteiras. + dfDeg = anguloControleDeg; + drDeg = 0.0; + break; } - // Curvatura geométrica - double tf = Math.Tan(dfDeg * Math.PI / 180.0); - double tr = Math.Tan(drDeg * Math.PI / 180.0); - if (Math.Abs(tf) < 1e-6) tf = 0; - if (Math.Abs(tr) < 1e-6) tr = 0; + double df = dfDeg * Math.PI / 180.0; + double dr = drDeg * Math.PI / 180.0; - double kappa = 0; // 1/m - if (tipoMovimento == TipoMovimentoDirecional.RodasDianteiras) kappa = tf / L; - else if (tipoMovimento == TipoMovimentoDirecional.RodasTraseiras) kappa = tr / L; - else if (tipoMovimento == TipoMovimentoDirecional.MovimentoArco) kappa = (tf - tr) / L; - else /*Diagonal,Lateral*/ kappa = 0; + double tf = Math.Tan(df); + double tr = Math.Tan(dr); - // Correção dinâmica (opcional) - double Ku = 2.5; // ajuste fino em campo - double kappaEf = kappa / (1 + Ku * velocidadeMs * velocidadeMs); + if (Math.Abs(tf) < 1e-12) tf = 0.0; + if (Math.Abs(tr) < 1e-12) tr = 0.0; - // Integração - double theta = anguloAtualDeg * Math.PI / 180.0; - double omega = velocidadeMs * kappaEf; // rad/s + // ------------------------------------------------------------ + // 2) Cinemática 4WS no ponto médio entre eixos + // ------------------------------------------------------------ + double beta = Math.Atan(0.5 * (tf + tr)); + + // velocidadeMs é o módulo da velocidade do centro. + // Por isso entra cos(beta) na curvatura de yaw por metro percorrido. + double kappaGeom = + Math.Cos(beta) * (tf - tr) / L; + + if (Math.Abs(kappaGeom) < 1e-12) + kappaGeom = 0.0; + + // ------------------------------------------------------------ + // 3) Correção dinâmica + // ------------------------------------------------------------ + // Deve ser o MESMO valor usado em: + // mpc_v16 -> cinematica_4ws_ku + double Ku = 2.5; + + double kappaEf = + kappaGeom / + (1.0 + Ku * velocidadeMs * velocidadeMs); + + double omega = velocidadeMs * kappaEf; // rad/s + + // ------------------------------------------------------------ + // 4) Integração analítica + // ------------------------------------------------------------ + // Convenção: + // theta = heading do corpo, 0=Norte, +90°=Leste. + // A velocidade do centro aponta para theta + beta. + double theta0 = anguloAtualDeg * Math.PI / 180.0; double dtheta = omega * tempoDelta; - theta += dtheta; + double theta1 = theta0 + dtheta; - double heading = theta; - if (new List() { TipoMovimentoDirecional.MovimentoDiagonal, TipoMovimentoDirecional.MovimentoLateral }.Contains(tipoMovimento)) - heading = theta + dfDeg * Math.PI / 180.0; // crab: desloca na direção do steering + double phi0 = theta0 + beta; - // >>> mesma convenção do Python: 0=Norte - double dx = velocidadeMs * tempoDelta * Math.Sin(heading); // Leste(+) - double dy = velocidadeMs * tempoDelta * Math.Cos(heading); // Norte(+) + double dx; + double dy; + + if (Math.Abs(omega) <= 1e-9) + { + double distancia = velocidadeMs * tempoDelta; + + dx = Math.Sin(phi0) * distancia; // Leste(+) + dy = Math.Cos(phi0) * distancia; // Norte(+) + } + else + { + double phi1 = theta1 + beta; + double raioVel = velocidadeMs / omega; + + dx = + raioVel * + (Math.Cos(phi0) - Math.Cos(phi1)); + + dy = + raioVel * + (Math.Sin(phi1) - Math.Sin(phi0)); + } GPSModel ultimaPosicao = GPSService.historicoPosicao.Peek(); - if (pos == null) return ultimaPosicao; + if (pos == null) + return ultimaPosicao; - // geo + // ------------------------------------------------------------ + // 5) Plano local -> geográfico + // ------------------------------------------------------------ double R_earth = GPSUtils.RaioDaTerra; - 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 dLat = + (dy / R_earth) * + 180.0 / Math.PI; + + double cosLat = + Math.Cos(pos.LatitudeAnt * Math.PI / 180.0); + + // Proteção só para evitar divisão numérica absurda. + if (Math.Abs(cosLat) < 1e-9) + cosLat = cosLat >= 0 ? 1e-9 : -1e-9; + + double dLon = + (dx / (R_earth * cosLat)) * + 180.0 / Math.PI; double latitude = pos.LatitudeAnt + dLat; double longitude = pos.LongitudeAnt + dLon; DateTime agora = DateTime.Now; - double agoraMono = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; + 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); + double orientacaoRealDeg = + GPSUtils.NormalizarAngulo( + theta1 * 180.0 / Math.PI + ); + + // Direção instantânea de deslocamento do centro no final do passo. + double orientacaoMovimentoDeg = + GPSUtils.NormalizarAngulo( + (theta1 + beta) * 180.0 / Math.PI + ); GPSModel novaPosicao = new GPSModel { Momento = agora, DataHora = agora, UltimoComandoRespondido = agora, - Lat0 = GPSService.UltimaLeitura.Lat0, - Lon0 = GPSService.UltimaLeitura.Lon0, + + Lat0 = GPSService.UltimaLeitura.Lat0, + Lon0 = GPSService.UltimaLeitura.Lon0, + LatitudeAnt = latitude, LongitudeAnt = longitude, + OrientacaoReal = orientacaoRealDeg, AnguloCarroDefinido = orientacaoRealDeg, OrientacaoMovimento = orientacaoMovimentoDeg, + TipoOrientacao = "A", Distancia = velocidadeMs * tempoDelta, Velocidade = velocidadeMs, + Heartbeat = ultimaPosicao.Heartbeat + 1, + TimestampOri = ultimaPosicao.TimestampOri.Clone(), TimestampPos = ultimaPosicao.TimestampPos.Clone(), }; + // Mantém exatamente o lever-arm já usado no simulador real. var corrigida = GPSService.LeverArm.FixLeverArmLatLon_Fast( novaPosicao.LatitudeAnt, @@ -1172,16 +1300,13 @@ namespace AgroBase.Models 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 = agoraMono; novaPosicao.TimestampPos.valor = agoraMono; - + return novaPosicao; } diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py index 69ab8b3e9..f7cc8285f 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_multispectral.py @@ -576,10 +576,16 @@ class CameraMultispectral: ) try: - if stream is not None and hasattr(stream, "parar"): - stream.parar() - except Exception: - pass + if stream is not None: + fechar_stream = getattr(stream, "fechar", None) + if not callable(fechar_stream): + fechar_stream = getattr(stream, "parar", None) + if callable(fechar_stream): + fechar_stream() + except Exception as e: + self.mostrar_log( + f"[CameraMultispectral] Erro ao fechar stream TCP: {e}" + ) with self._lock: self.ultimo_tensor_multispec = None diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py index 8ad2dfdbb..bee232e2c 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py @@ -223,6 +223,19 @@ class CameraOak: self.rodando = False self.iniciado = False + stream = self.stream + self.stream = None + + try: + if stream is not None: + fechar_stream = getattr(stream, "fechar", None) + if not callable(fechar_stream): + fechar_stream = getattr(stream, "parar", None) + if callable(fechar_stream): + fechar_stream() + except Exception as e: + self.mostrar_log(f"Erro ao fechar stream TCP da câmera: {e}") + try: if self._cache_thread is not None and self._cache_thread.is_alive(): self._cache_thread.join(timeout=1.0) @@ -282,10 +295,6 @@ class CameraOak: "frame_valido": False, } - try: - self.stream = None - except Exception: - pass def _is_erro_fatal_depthai(self, erro): if erro is None: diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py index c827ccdc8..5c0eaa719 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/oak_fcc3_core/oak_fcc3_manager.py @@ -208,6 +208,8 @@ class OakFcc3Manager: "dropped_queue": 0, "last_error": None, "last_loop_ms": 0.0, + "last_packet_age_ms": None, + "_last_frame_seen_perf_by_cam": {}, } def __enter__(self): @@ -1633,6 +1635,15 @@ class OakFcc3Manager: t0_get = self._cap_now_ms() msg = q.get() + + try: + seen = self._capture_thread_stats.setdefault( + "_last_frame_seen_perf_by_cam", {} + ) + seen[cam_id] = time.perf_counter() + except Exception: + pass + if perf is not None: perf["drain_get_msg_ms"] += self._cap_now_ms() - t0_get @@ -2343,6 +2354,7 @@ class OakFcc3Manager: "last_error": None, "last_loop_ms": 0.0, "last_packet_age_ms": None, + "_last_frame_seen_perf_by_cam": {}, } def _start_async_capture_thread(self): @@ -2417,6 +2429,11 @@ class OakFcc3Manager: # Importante: esta thread é a única que mexe nas queues/buffers. self._drain_queues_to_buffers(perf=perf) + if perf is not None: + perf["buffer_lengths_after_drain"] = ( + self._buffer_lengths_snapshot() + ) + # Em RAW_BRUTO + async, o produtor publica apenas descritores # ImgFrame da tripleta sincronizada. O consumidor materializa # somente o pacote latest que realmente pegar. Isso elimina @@ -2432,6 +2449,25 @@ class OakFcc3Manager: ) if synced is None: + # Guarda diagnóstico do motivo pelo qual o sincronizador + # ainda não conseguiu formar um pacote. Isso aparece no + # status usado pelo watchdog do WeedWorker e permite saber + # se uma cabeça específica ficou sem frames ou se houve + # descarte por sincronismo estrito. + if perf is not None: + self._capture_thread_stats["last_wait_reason"] = ( + perf.get("wait_reason") + ) + self._capture_thread_stats["last_buffer_lengths"] = dict( + perf.get("buffer_lengths_after_drain") or {} + ) + self._capture_thread_stats["last_drained_by_cam"] = dict( + perf.get("drained_by_cam") or {} + ) + self._capture_thread_stats["last_queue_has_true_by_cam"] = dict( + perf.get("queue_has_true_by_cam") or {} + ) + # Dorme curto. Pode testar 0.0005 se quiser reduzir latência. time.sleep(0.001) continue @@ -2503,6 +2539,10 @@ class OakFcc3Manager: self._capture_thread_stats["packets"] += 1 self._capture_thread_stats["last_error"] = None + self._capture_thread_stats["last_wait_reason"] = "packet_ready" + self._capture_thread_stats["last_buffer_lengths"] = ( + self._buffer_lengths_snapshot() + ) self._capture_thread_stats["last_loop_ms"] = (time.perf_counter() - t_loop0) * 1000.0 self._capture_cond.notify_all() @@ -2550,6 +2590,20 @@ class OakFcc3Manager: latest_age_ms = (time.perf_counter() - float(self._latest_packet.get("created_perf_counter", 0.0))) * 1000.0 st = dict(getattr(self, "_capture_thread_stats", {}) or {}) + + last_seen_perf = st.pop( + "_last_frame_seen_perf_by_cam", {} + ) or {} + now_perf = time.perf_counter() + camera_frame_age_ms = { + str(cam_id): round( + max(0.0, now_perf - float(ts)) * 1000.0, + 1, + ) + for cam_id, ts in last_seen_perf.items() + if ts is not None + } + st.update({ "enabled": bool(getattr(self, "async_capture_enabled", True)), "mode": str(getattr(self, "async_capture_mode", "latest")), @@ -2559,5 +2613,8 @@ class OakFcc3Manager: "last_consumed_seq": int(getattr(self, "_last_consumed_packet_seq", 0)), "queue_len": int(len(getattr(self, "_packet_queue", []))), "latest_age_ms": latest_age_ms, + "last_packet_age_ms": latest_age_ms, + "buffer_lengths": self._buffer_lengths_snapshot(), + "camera_frame_age_ms": camera_frame_age_ms, }) return st diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/tcp_streamer.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/tcp_streamer.py index 8dd16ce8f..a79f909ea 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/tcp_streamer.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/tcp_streamer.py @@ -1572,6 +1572,10 @@ class CameraTcpStreamer: stream_fps=0.0, ) + def parar(self): + """Alias de compatibilidade para encerramento explícito.""" + self.fechar() + def __del__(self): try: self.fechar() diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py index ee460033f..2ae1143e3 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py @@ -341,6 +341,115 @@ class ControladorMPC: max_value=20.0, ) + # V9 - projeção específica para manobra de cabeceira. + # + # Em uma curva em U a polilinha dobra sobre si mesma. A janela normal de + # 5 m pode conter, ao mesmo tempo, o ramo de saída e o ramo de retorno. + # Escolher simplesmente o segmento euclidianamente mais próximo permite + # que a projeção salte para alguns metros adiante da curva e o pure-pursuit + # passe a cortar a abertura. O C# continua sendo a autoridade de progresso; + # durante Manobrando só deixamos a projeção procurar uma vizinhança curta + # ao redor do próximo ponto autoritativo. + self._janela_projecao_manobra_frente_m = _safe_float( + parametros_mpc.get("janela_projecao_manobra_frente_m", 1.80), + 1.80, min_value=0.80, max_value=3.00, + ) + self._janela_projecao_manobra_tras_m = _safe_float( + parametros_mpc.get("janela_projecao_manobra_tras_m", 0.75), + 0.75, min_value=0.20, max_value=1.50, + ) + + # ------------------------------------------------------------------ + # V11 - SEGUIDOR DUBINS FEED-FORWARD PARA CABECEIRA + # ------------------------------------------------------------------ + # Durante Status=Manobrando a geometria da manobra ja esta escrita na + # centerline (Dubins). Em vez de abrir uma arvore MPC e permitir que o + # horizonte recedente adie a curva (ex.: -25 agora, +30 depois), usamos + # um seguidor deterministico de caminho: + # + # delta = feed-forward(curvatura Dubins) + # + feedback de heading + # + feedback lateral + # + # O modo de movimento fica MovimentoArco durante toda a manobra. Assim + # a curva desenhada passa a ser uma referencia executavel, enquanto os + # termos de feedback continuam corrigindo derrapagem/erro de pose. + self._seguidor_dubins_enabled = bool( + parametros_mpc.get("seguidor_dubins_enabled", True) + ) + self._dubins_ff_lookahead_m = _safe_float( + parametros_mpc.get("dubins_ff_lookahead_m", 0.30), + 0.30, min_value=0.0, max_value=1.50, + ) + self._dubins_curvature_window_m = _safe_float( + parametros_mpc.get("dubins_curvature_window_m", 0.40), + 0.40, min_value=0.15, max_value=1.50, + ) + self._dubins_heading_lookahead_m = _safe_float( + parametros_mpc.get("dubins_heading_lookahead_m", 0.0), + 0.0, min_value=0.0, max_value=1.50, + ) + self._dubins_ganho_heading = _safe_float( + parametros_mpc.get("dubins_ganho_heading", 0.65), + 0.65, min_value=0.0, max_value=2.50, + ) + self._dubins_ganho_lateral = _safe_float( + parametros_mpc.get("dubins_ganho_lateral", 0.28), + 0.28, min_value=0.0, max_value=2.50, + ) + self._dubins_velocidade_offset_lateral = _safe_float( + parametros_mpc.get("dubins_velocidade_offset_lateral", 0.35), + 0.35, min_value=0.05, max_value=2.0, + ) + self._dubins_lateral_deadband_m = _safe_float( + parametros_mpc.get("dubins_lateral_deadband_m", 0.04), + 0.04, min_value=0.0, max_value=0.25, + ) + self._dubins_correcao_heading_max_graus = _safe_float( + parametros_mpc.get("dubins_correcao_heading_max_graus", 8.0), + 8.0, min_value=0.0, max_value=20.0, + ) + self._dubins_correcao_lateral_max_graus = _safe_float( + parametros_mpc.get("dubins_correcao_lateral_max_graus", 7.0), + 7.0, min_value=0.0, max_value=20.0, + ) + self._dubins_preview_m = _safe_float( + parametros_mpc.get("dubins_preview_m", 2.0), + 2.0, min_value=0.50, max_value=5.0, + ) + self._dubins_preview_max_passos = _safe_int( + parametros_mpc.get("dubins_preview_max_passos", 12), + 12, min_value=2, max_value=30, + ) + + # V12 - seguidor deterministico da cabeceira por Pure Pursuit 4WS. + # Diferente da V11, o comando NAO tenta alinhar o rover com a tangente + # local da curva. Ele mira um ponto real a frente sobre a polilinha + # Dubins e calcula a curvatura circular que intercepta esse alvo. + self._dubins_pp_lookahead_base_m = _safe_float( + parametros_mpc.get("dubins_pp_lookahead_base_m", 0.55), + 0.55, min_value=0.20, max_value=2.0, + ) + self._dubins_pp_lookahead_velocidade_s = _safe_float( + parametros_mpc.get("dubins_pp_lookahead_velocidade_s", 0.15), + 0.15, min_value=0.0, max_value=2.0, + ) + self._dubins_pp_lookahead_min_m = _safe_float( + parametros_mpc.get("dubins_pp_lookahead_min_m", 0.40), + 0.40, min_value=0.15, max_value=2.0, + ) + self._dubins_pp_lookahead_max_m = _safe_float( + parametros_mpc.get("dubins_pp_lookahead_max_m", 0.85), + 0.85, min_value=0.20, max_value=3.0, + ) + if self._dubins_pp_lookahead_max_m < self._dubins_pp_lookahead_min_m: + self._dubins_pp_lookahead_max_m = self._dubins_pp_lookahead_min_m + + self._dubins_pp_ganho_curvatura = _safe_float( + parametros_mpc.get("dubins_pp_ganho_curvatura", 1.0), + 1.0, min_value=0.25, max_value=2.0, + ) + # ------------------------------------------------------------------ # PATH TRACKING V2 - referência geométrica da passada # ------------------------------------------------------------------ @@ -380,6 +489,44 @@ class ControladorMPC: parametros_mpc.get("diagonal_saida_heading_graus", 2.50), 2.50, min_value=self._diagonal_entrada_heading_graus, max_value=12.0, ) + + # V6 - prioridade real para manter o rover no centro da passada. + # + # Em V5 a diagonal só podia entrar com heading quase perfeito + # (tipicamente <= 1.25 grau). Em rua curva isso fazia o rover acumular + # 20, 30, 50 cm de cross-track antes de a diagonal ser autorizada. + # + # Agora o limite de heading permitido para a diagonal cresce + # continuamente com o erro lateral. Pequenos desvios continuam + # conservadores; um desvio grande ganha prioridade mesmo com o corpo + # alguns graus fora do heading local. A V7 complementa esta política: + # RodasDianteiras também recebem uma parcela lateral suave, enquanto a + # diagonal preserva a maior autoridade de recentralização. + self._diagonal_heading_lateral_max_graus = _safe_float( + parametros_mpc.get("diagonal_heading_lateral_max_graus", 7.0), + 7.0, min_value=self._diagonal_entrada_heading_graus, max_value=12.0, + ) + self._diagonal_lateral_urgente_m = _safe_float( + parametros_mpc.get("diagonal_lateral_urgente_m", 0.30), + 0.30, min_value=0.10, max_value=1.00, + ) + self._diagonal_histerese_heading_graus = _safe_float( + parametros_mpc.get("diagonal_histerese_heading_graus", 1.25), + 1.25, min_value=0.0, max_value=4.0, + ) + + # V8 - handoff Manobrando -> EntrandoRua. + # + # A diagonal é excelente para remover cross-track quando o corpo já está + # alinhado, mas não deve ser o primeiro movimento logo após um U-turn. + # Durante EntrandoRua exigimos um heading mais limpo antes de autorizar + # crab. Arco continua com a histerese 15/8 graus e RodasDianteiras ocupa + # a faixa intermediária, agora também corrigindo cross-track (V7). + self._entrada_rua_diagonal_heading_max_graus = _safe_float( + parametros_mpc.get("entrada_rua_diagonal_heading_max_graus", 3.0), + 3.0, min_value=0.5, max_value=8.0, + ) + self._lateral_deadband_m = _safe_float( parametros_mpc.get("lateral_deadband_m", 0.05), 0.05, min_value=0.0, max_value=0.30, @@ -392,6 +539,14 @@ class ControladorMPC: parametros_mpc.get("ganho_lateral_dianteira", 0.32), 0.32, min_value=0.0, max_value=3.0, ) + # V7 - RodasDianteiras também participam da correção de cross-track. + # A diagonal continua sendo a correção lateral de maior autoridade, + # porém a dianteira não pode deixar o rover derivar enquanto corrige + # heading. Este limite impede que a parcela lateral domine a orientação. + self._angulo_lateral_dianteira_max_graus = _safe_float( + parametros_mpc.get("angulo_lateral_dianteira_max_graus", 10.0), + 10.0, min_value=1.0, max_value=self.angulo_max_graus, + ) self._ganho_lateral_diagonal = _safe_float( parametros_mpc.get("ganho_lateral_diagonal", 0.85), 0.85, min_value=0.0, max_value=4.0, @@ -412,6 +567,215 @@ class ControladorMPC: parametros_mpc.get("lateral_reta_aquisicao_max_m", 0.80), 0.80, min_value=0.15, max_value=2.0, ) + + # ------------------------------------------------------------------ + # V13 - SUPERVISOR DE TRACKING DA PASSADA + # ------------------------------------------------------------------ + # Filosofia: + # 1) preservar a centralizacao e a tarefa prioritaria; + # 2) MovimentoDiagonal vira o modo de HOLD da passada; + # 3) heading so toma o volante quando realmente saiu da banda; + # 4) Dianteira/Traseira corrigem yaw com feed-forward de curvatura; + # 5) Arco continua reservado para erro de heading grande. + # + # A histerese evita a oscilacao curta Diagonal <-> Dianteira observada + # em ruas curvas: primeiro termina uma tarefa, depois entrega o volante + # para a outra. + self._tracking_center_hold_m = _safe_float( + parametros_mpc.get("tracking_center_hold_m", 0.04), + 0.04, min_value=0.0, max_value=0.30, + ) + self._tracking_center_recenter_m = _safe_float( + parametros_mpc.get("tracking_center_recenter_m", 0.08), + 0.08, min_value=self._tracking_center_hold_m, max_value=0.60, + ) + self._tracking_center_urgent_m = _safe_float( + parametros_mpc.get("tracking_center_urgent_m", 0.15), + 0.15, min_value=self._tracking_center_recenter_m, max_value=1.00, + ) + + self._tracking_yaw_enter_graus = _safe_float( + parametros_mpc.get("tracking_yaw_enter_graus", 3.0), + 3.0, min_value=0.5, max_value=12.0, + ) + self._tracking_yaw_exit_graus = _safe_float( + parametros_mpc.get("tracking_yaw_exit_graus", 0.8), + 0.8, min_value=0.0, max_value=self._tracking_yaw_enter_graus, + ) + self._tracking_diag_heading_safety_graus = _safe_float( + parametros_mpc.get("tracking_diag_heading_safety_graus", 7.0), + 7.0, min_value=self._tracking_yaw_enter_graus, max_value=15.0, + ) + + # Antecipacao suave de ruas curvas. Diferente do Dubins, aqui a + # curvatura e apenas feed-forward; o feedback de heading continua ativo. + self._tracking_ff_lookahead_m = _safe_float( + parametros_mpc.get("tracking_ff_lookahead_m", 1.00), + 1.00, min_value=0.0, max_value=4.0, + ) + self._tracking_ff_window_m = _safe_float( + parametros_mpc.get("tracking_ff_window_m", 1.60), + 1.60, min_value=0.40, max_value=5.0, + ) + self._tracking_ff_gain = _safe_float( + parametros_mpc.get("tracking_ff_gain", 1.0), + 1.0, min_value=0.0, max_value=2.0, + ) + self._tracking_ff_max_graus = _safe_float( + parametros_mpc.get("tracking_ff_max_graus", 8.0), + 8.0, min_value=0.0, max_value=20.0, + ) + + # Mini-predicao Dianteira x Traseira. Se o modelo nao diferenciar os + # dois eixos, o desempate favorece Dianteira, portanto e seguro manter + # ligado mesmo antes de calibrar o efeito da traseira em campo. + self._tracking_rear_enabled = bool( + parametros_mpc.get("tracking_rear_enabled", True) + ) + self._tracking_yaw_preview_s = _safe_float( + parametros_mpc.get("tracking_yaw_preview_s", 0.55), + 0.55, min_value=0.15, max_value=1.50, + ) + self._tracking_yaw_preview_passos = _safe_int( + parametros_mpc.get("tracking_yaw_preview_passos", 3), + 3, min_value=1, max_value=8, + ) + self._tracking_yaw_custo_lateral = _safe_float( + parametros_mpc.get("tracking_yaw_custo_lateral", 3.0), + 3.0, min_value=0.1, max_value=20.0, + ) + self._tracking_yaw_custo_heading = _safe_float( + parametros_mpc.get("tracking_yaw_custo_heading", 1.5), + 1.5, min_value=0.1, max_value=20.0, + ) + self._tracking_yaw_penalidade_troca = _safe_float( + parametros_mpc.get("tracking_yaw_penalidade_troca", 0.08), + 0.08, min_value=0.0, max_value=2.0, + ) + self._tracking_rear_min_gain = _safe_float( + parametros_mpc.get("tracking_rear_min_gain", 0.03), + 0.03, min_value=0.0, max_value=0.50, + ) + + # ------------------------------------------------------------------ + # V15 - seletor de eixo consciente da geometria dianteira/traseira + # ------------------------------------------------------------------ + # A V14 já comparava RodasDianteiras x RodasTraseiras por uma + # micro-predição, porém a integração usada tratava o centro do rover + # praticamente igual nos dois casos. Isso escondia justamente a + # vantagem cinemática da traseira: preservar melhor a frente enquanto + # a cauda varre para alinhar o corpo (e vice-versa para a dianteira). + # + # V15 usa um modelo bicicleta no CENTRO durante essa micro-predição e + # também mede o cross-track dos centros dos dois eixos. O custo prefere + # corrigir yaw sem estragar o eixo que já está bem colocado na passada. + self._tracking_yaw_custo_eixos = _safe_float( + parametros_mpc.get("tracking_yaw_custo_eixos", 1.20), + 1.20, min_value=0.0, max_value=10.0, + ) + self._tracking_yaw_custo_preservar_eixo = _safe_float( + parametros_mpc.get("tracking_yaw_custo_preservar_eixo", 2.00), + 2.00, min_value=0.0, max_value=20.0, + ) + self._tracking_yaw_preservacao_deadband_m = _safe_float( + parametros_mpc.get("tracking_yaw_preservacao_deadband_m", 0.015), + 0.015, min_value=0.0, max_value=0.10, + ) + self._tracking_eixo_yaw_ultimo_log = None + + # ------------------------------------------------------------------ + # V16 - CINEMATICA 4WS UNIFICADA + # ------------------------------------------------------------------ + # Uma única regra cinemática passa a alimentar: + # - micro-predição Dianteira x Traseira; + # - MPC pesado / beam search; + # - preview da manobra Dubins; + # - correção de pose por latência; + # - trajetória curta usada pelo costmap/LUT. + # + # Convenção de comando: + # angulo > 0 = yaw positivo do corpo para TODOS os modos. + # Em RodasTraseiras isso significa que o ângulo físico do eixo traseiro + # é o oposto do comando de alto nível (efeito empilhadeira). + # + # O fator Ku preserva a mesma correção dinâmica usada pelo simulador C#. + self._cinematica_4ws_ku = _safe_float( + parametros_mpc.get("cinematica_4ws_ku", 2.5), + 2.5, min_value=0.0, max_value=20.0, + ) + self._cinematica_entre_eixos_m = _safe_float( + parametros_mpc.get("cinematica_entre_eixos_m", 0.94), + 0.94, min_value=0.20, max_value=4.0, + ) + + # ------------------------------------------------------------------ + # V14 - CURVE TRACK: yaw contínuo quando a própria rua está curvando + # ------------------------------------------------------------------ + # V13 priorizava Diagonal sempre que o cross-track passava da banda. + # Em rua curva isso podia criar um equilíbrio ruim: o crab empurrava + # para o centro enquanto a tangente da rua continuava girando, mas o + # rover não gerava yaw. O erro lateral então ficava quase constante e + # YAW_ALIGN nunca recebia o volante. + # + # V14 separa RETA de CURVA pela curvatura da centerline. Em curva, + # Dianteira/Traseira seguem continuamente: feed-forward da curvatura + + # feedback de heading + feedback lateral. Diagonal fica reservado para + # recuperação lateral realmente grande. + self._tracking_curve_ff_enter_graus = _safe_float( + parametros_mpc.get("tracking_curve_ff_enter_graus", 0.35), + 0.35, min_value=0.05, max_value=5.0, + ) + self._tracking_curve_ff_exit_graus = _safe_float( + parametros_mpc.get("tracking_curve_ff_exit_graus", 0.18), + 0.18, min_value=0.0, max_value=self._tracking_curve_ff_enter_graus, + ) + self._tracking_curve_lateral_gain = _safe_float( + parametros_mpc.get("tracking_curve_lateral_gain", 0.65), + 0.65, min_value=0.0, max_value=3.0, + ) + self._tracking_curve_lateral_max_graus = _safe_float( + parametros_mpc.get("tracking_curve_lateral_max_graus", 9.0), + 9.0, min_value=0.0, max_value=self.angulo_max_graus, + ) + self._tracking_curve_diag_emergency_m = _safe_float( + parametros_mpc.get("tracking_curve_diag_emergency_m", 0.24), + 0.24, min_value=self._tracking_center_recenter_m, max_value=0.80, + ) + self._tracking_curve_diag_release_m = _safe_float( + parametros_mpc.get("tracking_curve_diag_release_m", 0.14), + 0.14, min_value=self._tracking_center_hold_m, + max_value=self._tracking_curve_diag_emergency_m, + ) + + # Estado interno apenas para histerese do recovery lateral em curva. + # Não sai do MPC e não altera o contrato C#/Redis. + self._tracking_curve_diag_recovery_active = False + + # V5.1 - RetornoBase: recuperação geométrica de controle. + # + # O progresso visitado continua 100% autoritativo no C#, mas o ponto + # usado pelo controle não pode ficar cego para sempre caso um waypoint + # discreto seja perdido. Se a projeção local ficar muito distante, o + # MPC pode procurar um segmento FUTURO da própria rota de retorno e + # usá-lo apenas como referência de controle. Nada é marcado visitado + # aqui; o C# faz a confirmação independente. + self._retorno_reacquire_trigger_m = _safe_float( + parametros_mpc.get("retorno_reacquire_trigger_m", 1.00), + 1.00, min_value=self._lateral_reta_aquisicao_max_m, max_value=5.0, + ) + self._retorno_reacquire_window_m = _safe_float( + parametros_mpc.get("retorno_reacquire_window_m", 30.0), + 30.0, min_value=max(5.0, self._janela_projecao_alvo_m), max_value=80.0, + ) + self._retorno_reacquire_max_dist_m = _safe_float( + parametros_mpc.get("retorno_reacquire_max_dist_m", 2.00), + 2.00, min_value=0.50, max_value=5.0, + ) + self._retorno_reacquire_gain_m = _safe_float( + parametros_mpc.get("retorno_reacquire_gain_m", 0.35), + 0.35, min_value=0.05, max_value=2.0, + ) + self._penalidade_troca_tipo = _safe_float( parametros_mpc.get("penalidade_troca_tipo", 0.18), 0.18, min_value=0.0, max_value=3.0, @@ -610,12 +974,18 @@ class ControladorMPC: self._s_nodes = s_nodes self._mg_nodes = mg_nodes - def _projetar_na_trajetoria_local(self, x, y, idx_referencia=None): + def _projetar_na_trajetoria_local( + self, x, y, idx_referencia=None, *, janela_frente_m=None, janela_tras_m=None + ): """Projeta a pose apenas na vizinhanca do progresso corrente. A busca global e ambigua em mapas agricolas: duas passadas paralelas podem estar a apenas 1,5 m. Ancorar a busca no proximo indice da rota impede o erro lateral de trocar silenciosamente para outra rua. + + V9: a janela pode ser reduzida durante manobras em U. Isso evita que o + ramo de retorno da própria curva, geometricamente próximo mas vários + metros adiante em ``s``, roube a projeção do ramo que está sendo percorrido. """ if self._seg_p0 is None or self._seg_p0.shape[0] == 0: return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7 @@ -628,10 +998,21 @@ class ControladorMPC: else: idx = max(0, min(_safe_int(idx_referencia, 0), len(self.pontos_info) - 1)) s_base = float(self._s_nodes[idx]) - s_min = max(0.0, s_base - 1.25) + + janela_tras = ( + 1.25 if janela_tras_m is None + else max(0.0, float(janela_tras_m)) + ) + janela_frente = ( + float(self._janela_projecao_alvo_m) + if janela_frente_m is None + else max(0.25, float(janela_frente_m)) + ) + + s_min = max(0.0, s_base - janela_tras) s_max = min( float(self._s_nodes[-1]), - s_base + float(self._janela_projecao_alvo_m), + s_base + janela_frente, ) i0 = max(0, int(np.searchsorted(self._s_nodes, s_min, side="right")) - 1) @@ -672,6 +1053,101 @@ class ControladorMPC: return e_lat, s_proj, i, proj_i, theta_path, margem + def _projetar_para_controle(self, x, y, idx_referencia, status_carro): + """Projeção usada pelo CONTROLE, sem alterar progresso visitado. + + Em operação normal mantém a janela local autoritativa, que protege + contra ruas agrícolas paralelas. Em RetornoBase existe um fallback + conservador: se a referência local ficou claramente distante, procura + somente à frente do índice C# e só troca se encontrar um segmento da + própria rota substancialmente mais próximo. + """ + if _status_in(status_carro, [StatusCarroMapa.Manobrando]): + # A curva em U pode voltar a menos de 1-2 m do ramo anterior. + # Uma busca de 5 m em s é grande o bastante para enxergar o outro + # lado da ferradura e produzir um atalho. Durante a manobra, mantém + # a projeção amarrada ao progresso autoritativo publicado pelo C#. + local = self._projetar_na_trajetoria_local( + x, y, idx_referencia=idx_referencia, + janela_frente_m=self._janela_projecao_manobra_frente_m, + janela_tras_m=self._janela_projecao_manobra_tras_m, + ) + else: + local = self._projetar_na_trajetoria_local( + x, y, idx_referencia=idx_referencia + ) + + if not _status_in(status_carro, [StatusCarroMapa.RetornandoBase]): + return local + + if self._seg_p0 is None or self._seg_p0.shape[0] == 0: + return local + + e_lat_local, _, _, proj_local, _, _ = local + dist_local = float(np.linalg.norm(np.asarray([x, y], dtype=float) - proj_local)) + + # Perto da rota, a proteção local continua sendo a melhor opção. + if ( + abs(float(e_lat_local)) <= self._retorno_reacquire_trigger_m + or dist_local <= self._retorno_reacquire_trigger_m + ): + return local + + n = len(self.pontos_info) + if n < 2: + return local + + idx = max(0, min(_safe_int(idx_referencia, 0), n - 1)) + total_segmentos = int(self._seg_p0.shape[0]) + s_base = float(self._s_nodes[idx]) + s_min = max(0.0, s_base - 1.25) + s_max = min( + float(self._s_nodes[-1]), + s_base + float(self._retorno_reacquire_window_m), + ) + + i0 = max(0, int(np.searchsorted(self._s_nodes, s_min, side="right")) - 1) + i1 = min( + total_segmentos - 1, + int(np.searchsorted(self._s_nodes, s_max, side="left")), + ) + i0 = min(i0, max(0, idx - 1)) + i1 = max(i1, min(total_segmentos - 1, idx)) + + P = np.asarray([x, y], dtype=float) + p0 = self._seg_p0[i0:i1 + 1] + vetores = self._seg_v[i0:i1 + 1] + comprimentos = self._seg_L[i0:i1 + 1] + l2 = np.maximum(comprimentos ** 2, 1e-12) + + w = P - p0 + t = np.clip(np.sum(w * vetores, axis=1) / l2, 0.0, 1.0) + projecoes = p0 + t[:, None] * vetores + d2 = np.sum((projecoes - P) ** 2, axis=1) + rel = int(np.argmin(d2)) + i = i0 + rel + dist_ampla = float(math.sqrt(max(0.0, float(d2[rel])))) + + # Não troca por uma referência só marginalmente melhor. Isso evita + # saltos em rotas que passam perto de si mesmas. + if ( + dist_ampla > self._retorno_reacquire_max_dist_m + or dist_ampla + self._retorno_reacquire_gain_m >= dist_local + ): + return local + + proj_i = projecoes[rel] + t_i = float(t[rel]) + e_lat = float(np.dot(P - proj_i, self._seg_n_hat[i])) + s_proj = float(self._s_nodes[i] + t_i * self._seg_L[i]) + theta_path = float(np.arctan2(self._seg_t_hat[i, 0], self._seg_t_hat[i, 1])) + + mg0 = self._mg_nodes[i] + mg1 = self._mg_nodes[i + 1] if (i + 1) < self._mg_nodes.size else mg0 + margem = float((1.0 - t_i) * mg0 + t_i * mg1) + + return e_lat, s_proj, i, proj_i, theta_path, margem + def cross_track_error_point(self, x, y, idx_referencia=None): try: """ @@ -710,6 +1186,252 @@ class ControladorMPC: xy = self._seg_p0[i] + t * self._seg_v[i] return (float(xy[0]), float(xy[1])), i + def _heading_segmento_centerline(self, idx_segmento): + """Heading compass-like (0=N, +90=E) de um segmento da centerline.""" + if self._seg_v is None or self._seg_v.shape[0] == 0: + return 0.0 + i = max(0, min(_safe_int(idx_segmento, 0), self._seg_v.shape[0] - 1)) + dx = float(self._seg_v[i][0]) + dy = float(self._seg_v[i][1]) + if math.hypot(dx, dy) <= 1e-9: + return 0.0 + return float(math.atan2(dx, dy)) + + def _curvatura_centerline(self, s_centro): + """Estima curvatura assinada da polilinha em 1/m. + + Na convencao do projeto, heading positivo gira para a direita. Logo: + kappa > 0 -> curva para a direita -> delta de Arco positivo. + kappa < 0 -> curva para a esquerda -> delta de Arco negativo. + """ + if self._s_nodes is None or len(self._s_nodes) < 2: + return 0.0 + + s_max = float(self._s_nodes[-1]) + janela = max(0.15, float(self._dubins_curvature_window_m)) + meio = 0.5 * janela + sc = float(np.clip(s_centro, 0.0, s_max)) + s0 = max(0.0, sc - meio) + s1 = min(s_max, sc + meio) + + # Perto das extremidades desloca a janela para preservar comprimento. + if (s1 - s0) < 0.75 * janela: + if s0 <= 1e-9: + s1 = min(s_max, janela) + elif s1 >= s_max - 1e-9: + s0 = max(0.0, s_max - janela) + + if (s1 - s0) <= 1e-6: + return 0.0 + + _, i0 = self._xy_na_abscissa(s0) + _, i1 = self._xy_na_abscissa(s1) + h0 = self._heading_segmento_centerline(i0) + h1 = self._heading_segmento_centerline(i1) + dh = float(self._wrap_pi(h1 - h0)) + return float(dh / max(s1 - s0, 1e-6)) + + def _referencia_seguidor_dubins(self, x, y, theta, idx_base, velocidade, contexto): + """V12: Pure Pursuit deterministico sobre a geometria Dubins. + + A V11 usava tangente/heading local + curvatura estimada. Em uma + ferradura isso podia aliviar o estercoamento simplesmente porque o + corpo do rover ficou paralelo a tangente daquele instante. Aqui a + referencia e um PONTO fisico adiante na curva. Enquanto esse ponto + estiver para o lado, o Arco continua esterçado. + + Para o centro cinematico do rover, a curvatura circular que intercepta + um alvo a distancia Ld e angulo alpha e: + + kappa = 2*sin(alpha)/Ld + + No Arco 4WS simetrico do projeto: + + kappa = 2*tan(delta)/L + + portanto: + + delta = atan(L*sin(alpha)/Ld) + + Nao ha arvore MPC, nem promessa de "virar depois": o valor calculado + e o comando executado neste ciclo. + """ + e_lat, s_proj, i_seg, proj, theta_local, margem = self._projetar_para_controle( + x, y, idx_base, StatusCarroMapa.Manobrando.value + ) + + s_max = float(self._s_nodes[-1]) if self._s_nodes is not None and len(self._s_nodes) else 0.0 + v = max(0.0, float(velocidade)) + look = ( + float(self._dubins_pp_lookahead_base_m) + + float(self._dubins_pp_lookahead_velocidade_s) * v + ) + look = float(np.clip( + look, + float(self._dubins_pp_lookahead_min_m), + float(self._dubins_pp_lookahead_max_m), + )) + + s_alvo = min(s_max, float(s_proj) + look) + xy_ref, i_ref = self._xy_na_abscissa(s_alvo) + + dx = float(xy_ref[0]) - float(x) + dy = float(xy_ref[1]) - float(y) + ld = max(0.05, math.hypot(dx, dy)) + + # Mesmo helper/convenção angular ja usados pelo MPC legado: bearing + # compass-like em radianos (0=N, +90=E). + bearing = self.gps_handler.calcular_orientacao( + xy_ref, (float(x), float(y), float(theta)) + ) + alpha = float(self._wrap_pi(float(bearing) - float(theta))) + + equipamento = _as_dict(_as_dict(contexto).get("Equipamento", {})) + entre_eixos = _safe_float( + equipamento.get("entre_eixos", 0.94), + 0.94, min_value=0.20, max_value=4.0, + ) + + kappa_pp = (2.0 * math.sin(alpha)) / ld + kappa_cmd = float(self._dubins_pp_ganho_curvatura) * float(kappa_pp) + + # Mapeia a curvatura desejada para o Arco 4WS simetrico. + delta = math.atan(0.5 * entre_eixos * kappa_cmd) + limite = math.radians(float(self.angulo_max_graus)) + delta = float(np.clip(delta, -limite, limite)) + + # Heading da trajetoria fica somente para TELEMETRIA. Ele nao entra + # no comando da V12. Assim nao reduzimos o estercoamento apenas porque + # o rover momentaneamente ficou paralelo a tangente da ferradura. + theta_ref = self._heading_segmento_centerline(i_ref) + erro_heading = float(self._wrap_pi(theta_ref - float(theta))) + + return delta, { + "e_lat": float(e_lat), + "s_proj": float(s_proj), + "i_seg": int(i_seg), + "i_ref": int(i_ref), + "projecao": proj, + "theta_local": float(theta_local), + "theta_ref": float(theta_ref), + "erro_heading": float(erro_heading), + "bearing_alvo": float(bearing), + "alpha": float(alpha), + "lookahead_m": float(max(0.0, s_alvo - s_proj)), + "distancia_alvo_m": float(ld), + "kappa_pp": float(kappa_pp), + "kappa_cmd": float(kappa_cmd), + "delta_ff": float(delta), + "delta_heading": 0.0, + "delta_lateral": 0.0, + "delta_final": float(delta), + "xy_ref": (float(xy_ref[0]), float(xy_ref[1])), + "s_ref": float(s_alvo), + "margem": float(margem), + } + + def _simular_preview_seguidor_dubins(self, x, y, theta, idx_base, velocidade, contexto, dt_ctrl): + """Preview leve usando exatamente o mesmo Pure Pursuit da V12.""" + v = max(float(velocidade), float(getattr(self, "_vmin_planejamento", 0.30))) + dt = max(0.05, min(float(dt_ctrl), 0.50)) + dist_passo = max(0.05, v * dt) + passos = int(math.ceil(float(self._dubins_preview_m) / dist_passo)) + passos = max(2, min(passos, int(self._dubins_preview_max_passos))) + + xs, ys, ths = float(x), float(y), float(theta) + idx_sim = max(0, min(_safe_int(idx_base, 0), len(self.pontos_info) - 1)) + traj = [] + + for _ in range(passos): + ang, dbg = self._referencia_seguidor_dubins( + xs, ys, ths, idx_sim, v, contexto + ) + omega = self._calcular_omega_4ws( + v, ang, TipoMovimentoDirecional.MovimentoArco + ) + xs, ys, ths = self._nova_posicao( + xs, ys, ths, omega, v, + TipoMovimentoDirecional.MovimentoArco, ang, dt=dt + ) + traj.append((xs, ys, ths)) + + # A projeção simulada pode caminhar junto com a centerline sem tocar + # no progresso real/visitados, que continua autoritativo no C#. + idx_sim = max(idx_sim, min(len(self.pontos_info) - 1, int(dbg["i_seg"]) + 1)) + + return traj + + def _usar_seguidor_dubins(self, status_carro, idx_base): + """V12: cabeceira estrutural usa Pure Pursuit dedicado em vez da arvore MPC. + + O C# ja coloca Status=Manobrando quando o ponto atual/proximo pertence + a geometria de manobra. Para o primeiro A/B de campo/simulador usamos + esse contrato diretamente, evitando depender do valor numerico privado + de TipoPontoRua no Python. + """ + if not bool(self._seguidor_dubins_enabled): + return False + return _status_in(status_carro, [StatusCarroMapa.Manobrando]) + + def _comando_seguidor_dubins( + self, contexto, comando_anterior, x, y, theta, velocidade, status_carro, + idx_alvo_correcao, pos_lat, pos_lon, pos_theta, pos_latencia, ponto_alvo_real + ): + angulo_rad, dbg = self._referencia_seguidor_dubins( + x, y, theta, idx_alvo_correcao, velocidade, contexto + ) + + preview_xy = self._simular_preview_seguidor_dubins( + x, y, theta, idx_alvo_correcao, velocidade, contexto, + self.tempo_execucao_local, + ) + simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(preview_xy) or []) + simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta)) + + #mostrar_log( + # "[MPC/V12 DUBINS-PP] " + # f"idx={idx_alvo_correcao} | " + # f"look={dbg['lookahead_m']:.2f}m | " + # f"Ld={dbg['distancia_alvo_m']:.2f}m | " + # f"alpha={math.degrees(dbg['alpha']):+.1f}deg | " + # f"kappa={dbg['kappa_cmd']:+.3f} 1/m | " + # f"cmd={math.degrees(angulo_rad):+.1f} | " + # f"e_lat={dbg['e_lat']:+.2f}m | " + # f"e_head_dbg={math.degrees(dbg['erro_heading']):+.1f}deg" + #) + + return { + "enviar_comando": True, + "parada_necessaria": False, + "erro": False, + "latencia": pos_latencia, + "angulo": float(round(math.degrees(angulo_rad), 2)), + "tipo": TipoMovimentoDirecional.MovimentoArco.value, + "simulacao": simulacao_latlon, + "erro_lateral": float(dbg["e_lat"]), + "erro_orientacao": abs(float(math.degrees(dbg["erro_heading"]))), + "idx_alvo_autoritativo": int(idx_alvo_correcao), + "alvo_direcional_xy": dbg["xy_ref"], + "lookahead_direcional_m": float(dbg["lookahead_m"]), + "tracking_reta": False, + "heading_caminho_lookahead_graus": float(math.degrees(dbg["theta_ref"])), + "heading_caminho_local_graus": float(math.degrees(dbg["theta_local"])), + "heading_lookahead_m": float(dbg["lookahead_m"]), + "erro_heading_caminho_graus": abs(float(math.degrees(dbg["erro_heading"]))), + "debug_custo": {}, + "debug_seguidor_dubins": { + "modo": "pure_pursuit_4ws", + "lookahead_m": float(dbg["lookahead_m"]), + "distancia_alvo_m": float(dbg["distancia_alvo_m"]), + "alpha_graus": float(math.degrees(dbg["alpha"])), + "kappa_pp": float(dbg["kappa_pp"]), + "kappa_cmd": float(dbg["kappa_cmd"]), + "comando_graus": float(math.degrees(dbg["delta_final"])), + }, + "candidatos_testados": 0, + "motivos": [], + } + def _distancia_lookahead(self, status_carro, velocidade): """Look-ahead do alvo de manobra/pure-pursuit. @@ -740,10 +1462,8 @@ class ControladorMPC: return {"xy": (float(x), float(y)), "_idx_base": 0} idx = max(0, min(_safe_int(idx_base, 0), n - 1)) - _, s_proj, _, _, _, margem = self._projetar_na_trajetoria_local( - x, - y, - idx_referencia=idx, + _, s_proj, _, _, _, margem = self._projetar_para_controle( + x, y, idx, status_carro ) lookahead = self._distancia_lookahead(status_carro, velocidade) @@ -864,7 +1584,7 @@ class ControladorMPC: # no fim, a tangente local será usada pelo chamador. return float(min(float(s_desejado), s_lim)) - def _referencia_caminho_lookahead(self, x, y, idx_referencia, velocidade): + def _referencia_caminho_lookahead(self, x, y, idx_referencia, velocidade, status_carro=None): """Retorna heading médio dos próximos metros + cross-track. A orientação é a corda da centerline entre a projeção atual e um ponto @@ -872,8 +1592,8 @@ class ControladorMPC: Nunca aponta do ROBÔ para o alvo, portanto deslocamento lateral não contamina o erro de heading. """ - e_lat, s_proj, i_seg, proj, theta_local, margem = self._projetar_na_trajetoria_local( - x, y, idx_referencia=idx_referencia, + e_lat, s_proj, i_seg, proj, theta_local, margem = self._projetar_para_controle( + x, y, idx_referencia, status_carro ) v = max(0.0, _safe_float(velocidade, 0.0)) @@ -896,8 +1616,13 @@ class ControladorMPC: theta_ref = float(theta_local) look_real = 0.0 + distancia_projecao_m = float( + np.linalg.norm(np.asarray([x, y], dtype=float) - np.asarray(proj, dtype=float)) + ) + return { "e_lat": float(e_lat), + "distancia_projecao_m": distancia_projecao_m, "s_proj": float(s_proj), "i_seg": int(i_seg), "projecao": proj, @@ -910,28 +1635,47 @@ class ControladorMPC: def _erro_heading_caminho(self, theta_robo, theta_ref): return float(self._wrap_pi(float(theta_ref) - float(theta_robo))) - def _tracking_reta_permitido(self, contexto, idx_base, e_lat, erro_heading_rad): + def _tracking_reta_permitido( + self, contexto, idx_base, e_lat, erro_heading_rad, distancia_rota_m=None + ): """ Decide se o path tracking geométrico deve assumir o controle. Regras V5: - 1) CaminhandoRua / EntrandoRua / SaindoRua / Direcionando / - RetornandoBase: - usam a mesma política geométrica universal. O estado operacional - continua útil para velocidade e semântica, mas não deve aprisionar - a direção em uma geometria específica. + 1) CaminhandoRua / EntrandoRua / SaindoRua / Direcionando: + usam a política geométrica universal. - 2) Manobrando em trecho Rua -> Rua do MESMO corredor: + 2) RetornandoBase: + primeiro ADQUIRE a rota com alvo contínuo quando lateral/heading + ainda estão grandes; depois entra na mesma política universal. + + 3) Manobrando em trecho Rua -> Rua do MESMO corredor: é recuperação de trajetória e também pode usar a política universal. - 3) Manobra estrutural real: + 4) Manobra estrutural real: fica fora deste ramo e preserva MovimentoArco obrigatório, adequado à curva de cabeceira/180 graus. """ carro = _as_dict(_as_dict(contexto).get("Carro", {})) status = carro.get("Status", StatusCarroMapa.Parado.value) + # RetornoBase possui uma fase explícita de AQUISIÇÃO. Longe da + # centerline não usamos Diagonal para tentar corrigir vários metros de + # cross-track; o ramo pure-pursuit aponta para um interceptador contínuo + # da rota. Assim que a rota é adquirida, volta ao tracking universal. + if _status_in(status, [StatusCarroMapa.RetornandoBase]): + erro_heading_graus = abs(float(math.degrees(erro_heading_rad))) + distancia_referencia = ( + abs(float(e_lat)) + if distancia_rota_m is None + else abs(float(distancia_rota_m)) + ) + return ( + distancia_referencia <= self._lateral_reta_aquisicao_max_m + and erro_heading_graus <= self._heading_reta_aquisicao_max_graus + ) + if _status_in( status, [ @@ -939,7 +1683,6 @@ class ControladorMPC: StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Direcionando, - StatusCarroMapa.RetornandoBase, ], ): return True @@ -949,17 +1692,791 @@ class ControladorMPC: return False - def _referencia_direcional_reta(self, e_lat, erro_heading_rad, velocidade, tipo_preferido=None): + def _curvatura_tracking_centerline(self, s_proj, idx_base): + """Curvatura suave da passada para feed-forward do supervisor V13. + + Usa uma janela maior que a Dubins porque os pontos normais de rua sao + mais espacados (~0.8 m). A estimativa e limitada ao corredor corrente, + para nunca "enxergar" a cabeceira seguinte como se fosse curvatura da rua. + """ + if self._s_nodes is None or len(self._s_nodes) < 2: + return 0.0 + + s_max = float(self._s_nodes[-1]) + sc = float(np.clip( + float(s_proj) + float(self._tracking_ff_lookahead_m), + 0.0, + s_max, + )) + sc = self._limitar_s_ao_corredor(float(s_proj), sc, idx_base) + + janela = max(0.40, float(self._tracking_ff_window_m)) + meio = 0.5 * janela + s0 = max(0.0, sc - meio) + s1 = min(s_max, sc + meio) + + # Nao atravessa a fronteira do corredor com a janela de curvatura. + s1 = self._limitar_s_ao_corredor(float(s_proj), s1, idx_base) + + if (s1 - s0) < 0.35: + return 0.0 + + _, i0 = self._xy_na_abscissa(s0) + _, i1 = self._xy_na_abscissa(s1) + h0 = self._heading_segmento_centerline(i0) + h1 = self._heading_segmento_centerline(i1) + dh = float(self._wrap_pi(h1 - h0)) + return float(dh / max(s1 - s0, 1e-6)) + + def _feedforward_curva_tracking(self, s_proj, idx_base, contexto): + """Retorna (kappa, delta_ff) da centerline para steering simples. + + Para Dianteira/Traseira usamos a aproximação de bicicleta: + kappa ~= tan(delta) / L + logo: + delta_ff = atan(L * kappa) + + A estimativa de kappa já usa janela suave e é limitada ao corredor + atual por _curvatura_tracking_centerline(). + """ + if s_proj is None or idx_base is None: + return 0.0, 0.0 + + try: + kappa = float( + self._curvatura_tracking_centerline( + float(s_proj), int(idx_base) + ) + ) + + equipamento = _as_dict( + _as_dict(contexto or {}).get("Equipamento", {}) + ) + entre_eixos = _safe_float( + equipamento.get("entre_eixos", 0.94), + 0.94, min_value=0.20, max_value=4.0, + ) + + delta_ff = math.atan( + entre_eixos + * float(self._tracking_ff_gain) + * kappa + ) + lim_ff = math.radians(float(self._tracking_ff_max_graus)) + delta_ff = float(np.clip(delta_ff, -lim_ff, lim_ff)) + return float(kappa), float(delta_ff) + except Exception: + return 0.0, 0.0 + + def _angulo_lateral_curve_tracking(self, e_lat, velocidade): + """Parcela lateral do CURVE_TRACK sem substituir o yaw da curva. + + O objetivo é puxar suavemente o rover ao centro enquanto + Dianteira/Traseira continuam acompanhando a rotação da centerline. + """ + e_lat = float(e_lat) + v = max(0.0, float(velocidade)) + + e_eff_abs = max(0.0, abs(e_lat) - self._tracking_center_hold_m) + if e_eff_abs <= 0.0 or self._tracking_curve_lateral_gain <= 0.0: + return 0.0 + + e_eff = math.copysign(e_eff_abs, e_lat) + delta = math.atan2( + self._tracking_curve_lateral_gain * e_eff, + v + self._velocidade_offset_lateral, + ) + lim = math.radians( + min( + self._tracking_curve_lateral_max_graus, + self.angulo_max_graus, + ) + ) + return float(np.clip(delta, -lim, lim)) + + def _angulo_diagonal_tracking(self, e_lat, velocidade): + """Comando crab para centralizar sem alterar o heading do corpo.""" + e_lat = float(e_lat) + v = max(0.0, float(velocidade)) + + e_eff_abs = max(0.0, abs(e_lat) - self._lateral_deadband_m) + e_eff = math.copysign(e_eff_abs, e_lat) if e_eff_abs > 0.0 else 0.0 + + if e_eff == 0.0: + return 0.0 + + delta = math.atan2( + self._ganho_lateral_diagonal * e_eff, + v + self._velocidade_offset_lateral, + ) + lim = math.radians( + min(self._angulo_diagonal_max_graus, self.angulo_max_graus) + ) + return float(np.clip(delta, -lim, lim)) + + def _entre_eixos_tracking(self, contexto): + equipamento = _as_dict(_as_dict(contexto or {}).get("Equipamento", {})) + fallback = float(getattr(self, "_cinematica_entre_eixos_m", 0.94)) + return _safe_float( + equipamento.get("entre_eixos", fallback), + fallback, min_value=0.20, max_value=4.0, + ) + + def _atualizar_cinematica_contexto(self, contexto): + """Atualiza parâmetros físicos usados por TODAS as previsões do MPC.""" + try: + self._cinematica_entre_eixos_m = self._entre_eixos_tracking(contexto) + except Exception: + pass + + @staticmethod + def _angulos_eixos_4ws(tipo, angulo_comando_rad): + """Mapeia comando de alto nível para ângulos físicos dos dois eixos. + + Convenção V16: + comando positivo = yaw positivo do CORPO em qualquer modo. + + Portanto RodasTraseiras inverte fisicamente o esterçamento traseiro: + +comando -> dr negativo. + + Isso deixa Dianteira e Traseira comparáveis pelo mesmo ângulo ideal, + enquanto preserva a cinemática real de empilhadeira. + """ + tipo = _movimento_from_value(tipo) + a = float(angulo_comando_rad) + + if tipo == TipoMovimentoDirecional.RodasDianteiras: + return a, 0.0 + if tipo == TipoMovimentoDirecional.RodasTraseiras: + return 0.0, -a + if tipo == TipoMovimentoDirecional.MovimentoArco: + return a, -a + if tipo in [ + TipoMovimentoDirecional.MovimentoDiagonal, + TipoMovimentoDirecional.MovimentoLateral, + ]: + return a, a + + return a, 0.0 + + def _cinematica_4ws( + self, + tipo, + angulo_comando_rad, + velocidade, + *, + entre_eixos=None, + aplicar_dinamica=True, + ): + """Modelo cinemático único do centro do rover para direção 4WS. + + Estado de referência = ponto médio entre os eixos. + + Para l_f = l_r = L/2: + beta = atan((tan(df) + tan(dr)) / 2) + kappa_geom = cos(beta) * (tan(df) - tan(dr)) / L + + `velocidade` é tratada como módulo da velocidade do centro do rover. + A direção instantânea de translação é theta + beta. + + Ku é uma correção empírica de subesterço já usada pelo simulador: + kappa_ef = kappa_geom / (1 + Ku*v²) + omega = v*kappa_ef + """ + tipo = _movimento_from_value(tipo) + v = float(velocidade) + L = ( + float(self._cinematica_entre_eixos_m) + if entre_eixos is None + else max(0.20, float(entre_eixos)) + ) + + df, dr = self._angulos_eixos_4ws(tipo, angulo_comando_rad) + tf = math.tan(float(df)) + tr = math.tan(float(dr)) + + if abs(tf) < 1e-12: + tf = 0.0 + if abs(tr) < 1e-12: + tr = 0.0 + + beta = math.atan(0.5 * (tf + tr)) + kappa_geom = math.cos(beta) * (tf - tr) / max(L, 1e-9) + + if abs(kappa_geom) < 1e-12: + kappa_geom = 0.0 + + if aplicar_dinamica: + Ku = max(0.0, float(self._cinematica_4ws_ku)) + kappa_ef = kappa_geom / (1.0 + Ku * v * v) + else: + kappa_ef = kappa_geom + + omega = v * kappa_ef + + return { + "df": float(df), + "dr": float(dr), + "beta": float(beta), + "kappa_geom": float(kappa_geom), + "kappa_ef": float(kappa_ef), + "omega": float(omega), + "entre_eixos": float(L), + } + + def _calcular_omega_4ws(self, velocidade, angulo_rad, tipo): + """Compatível com a assinatura histórica calcular_omega(v, ang, tipo).""" + return float( + self._cinematica_4ws( + tipo, + angulo_rad, + velocidade, + aplicar_dinamica=True, + )["omega"] + ) + + @staticmethod + def _posicoes_eixos_tracking(x, y, theta, entre_eixos): + """Retorna centros dos eixos dianteiro/traseiro a partir do centro. + + Coordenadas do projeto: x=Leste, y=Norte, theta compass-like + (0=Norte, +90=Leste). + """ + metade = 0.5 * max(0.0, float(entre_eixos)) + fx = math.sin(float(theta)) + fy = math.cos(float(theta)) + frente = ( + float(x) + metade * fx, + float(y) + metade * fy, + ) + traseira = ( + float(x) - metade * fx, + float(y) - metade * fy, + ) + return frente, traseira + + def _erro_lateral_ponto_tracking(self, px, py, idx_base, status_carro): + try: + e_lat, *_ = self._projetar_para_controle( + float(px), float(py), int(idx_base), status_carro + ) + return float(e_lat) + except Exception: + return float("inf") + + def _passo_micro_eixo_yaw( + self, + tipo, + angulo_rad, + x, + y, + theta, + velocidade, + dt, + ): + """V16: micro-predição usa EXATAMENTE a mesma cinemática global. + + Não existe mais um modelo especial para o seletor Dianteira/Traseira. + Isso evita o MPC escolher um eixo por uma física e depois avaliar o + candidato pesado por outra. + """ + v = max(0.0, float(velocidade)) + ang = float(angulo_rad) + tipo = _movimento_from_value(tipo) + omega = self._calcular_omega_4ws(v, ang, tipo) + + return self._nova_posicao( + float(x), float(y), float(theta), + omega, v, tipo, ang, + dt=max(1e-4, float(dt)), + ) + + def _avaliar_eixo_yaw_tracking( + self, + tipo, + angulo_rad, + x, + y, + theta, + velocidade, + idx_base, + status_carro, + tipo_prev, + contexto=None, + ): + """V15: micro-predição axle-aware de Dianteira/Traseira. + + Avalia quatro coisas no fim de ~0.5 s: + 1) cross-track do centro; + 2) heading do corpo; + 3) cross-track dos centros dos eixos dianteiro e traseiro; + 4) quanto pioramos o eixo que JÁ estava melhor colocado. + + Assim RodasTraseiras podem vencer quando a frente já está sobre a linha + e o que falta é "trazer a cauda", enquanto RodasDianteiras vencem no + caso espelhado. + """ + v = max(0.12, float(velocidade)) + n = max(1, int(self._tracking_yaw_preview_passos)) + horizonte = max(0.05, float(self._tracking_yaw_preview_s)) + dt = horizonte / n + self._atualizar_cinematica_contexto(contexto) + entre_eixos = self._entre_eixos_tracking(contexto) + + xp, yp, thp = float(x), float(y), float(theta) + + try: + # Estado inicial dos eixos: usado apenas para saber qual deles já + # está "bom" e deve ser preservado durante a correção de yaw. + eixo_f0, eixo_t0 = self._posicoes_eixos_tracking( + xp, yp, thp, entre_eixos + ) + e_front_0 = self._erro_lateral_ponto_tracking( + eixo_f0[0], eixo_f0[1], idx_base, status_carro + ) + e_rear_0 = self._erro_lateral_ponto_tracking( + eixo_t0[0], eixo_t0[1], idx_base, status_carro + ) + + for _ in range(n): + xp, yp, thp = self._passo_micro_eixo_yaw( + tipo, + float(angulo_rad), + xp, yp, thp, + v, dt, + ) + + ref = self._referencia_caminho_lookahead( + xp, yp, idx_base, v, status_carro + ) + e_lat_pred = float(ref["e_lat"]) + e_head_pred = float( + self._erro_heading_caminho(thp, ref["theta_ref"]) + ) + + eixo_fp, eixo_tp = self._posicoes_eixos_tracking( + xp, yp, thp, entre_eixos + ) + e_front_pred = self._erro_lateral_ponto_tracking( + eixo_fp[0], eixo_fp[1], idx_base, status_carro + ) + e_rear_pred = self._erro_lateral_ponto_tracking( + eixo_tp[0], eixo_tp[1], idx_base, status_carro + ) + + lat_den = max(0.04, float(self._tracking_center_recenter_m)) + head_den = max(1.0, float(self._tracking_yaw_enter_graus)) + + lat_norm = abs(e_lat_pred) / lat_den + head_norm = abs(math.degrees(e_head_pred)) / head_den + eixos_norm = 0.5 * ( + abs(e_front_pred) + abs(e_rear_pred) + ) / lat_den + + # Preserva o eixo que já estava melhor posicionado. Só cobra piora + # acima de um deadband pequeno para não reagir a ruído numérico. + if abs(e_front_0) <= abs(e_rear_0): + melhor_nome = "F" + melhor_0 = abs(e_front_0) + melhor_pred = abs(e_front_pred) + else: + melhor_nome = "T" + melhor_0 = abs(e_rear_0) + melhor_pred = abs(e_rear_pred) + + piora_melhor_eixo = max( + 0.0, + melhor_pred + - melhor_0 + - float(self._tracking_yaw_preservacao_deadband_m), + ) + preservar_norm = piora_melhor_eixo / lat_den + + custo = ( + float(self._tracking_yaw_custo_lateral) * lat_norm + + float(self._tracking_yaw_custo_heading) * head_norm + + float(self._tracking_yaw_custo_eixos) * eixos_norm + + float(self._tracking_yaw_custo_preservar_eixo) * preservar_norm + ) + + if ( + tipo_prev in [ + TipoMovimentoDirecional.RodasDianteiras, + TipoMovimentoDirecional.RodasTraseiras, + ] + and tipo != tipo_prev + ): + custo += float(self._tracking_yaw_penalidade_troca) + + return { + "custo": float(custo), + "e_lat": float(e_lat_pred), + "e_head": float(e_head_pred), + "e_front": float(e_front_pred), + "e_rear": float(e_rear_pred), + "e_front_0": float(e_front_0), + "e_rear_0": float(e_rear_0), + "melhor_eixo_0": melhor_nome, + "preservar": float(piora_melhor_eixo), + "angulo": float(angulo_rad), + } + except Exception: + return { + "custo": float("inf"), + "e_lat": float("inf"), + "e_head": math.pi, + "e_front": float("inf"), + "e_rear": float("inf"), + "e_front_0": float("inf"), + "e_rear_0": float("inf"), + "melhor_eixo_0": "?", + "preservar": float("inf"), + "angulo": float(angulo_rad), + } + + def _log_escolha_eixo_v15(self, escolhido, frente_dbg, traseira_dbg): + """Loga somente quando o eixo escolhido muda, evitando spam.""" + try: + nome = _enum_name( + escolhido, TipoMovimentoDirecional, default=str(escolhido) + ) + if self._tracking_eixo_yaw_ultimo_log == nome: + return + self._tracking_eixo_yaw_ultimo_log = nome + + #_log( + # "[MPC/V15 AXIS] " + # f"eixo={nome} | " + # f"Jf={frente_dbg['custo']:.2f} " + # f"Jt={traseira_dbg['custo']:.2f} | " + # f"F0={frente_dbg['e_front_0']:+.3f} " + # f"T0={frente_dbg['e_rear_0']:+.3f} | " + # f"Ff={frente_dbg['e_front']:+.3f} " + # f"Tf={frente_dbg['e_rear']:+.3f} | " + # f"Ft={traseira_dbg['e_front']:+.3f} " + # f"Tt={traseira_dbg['e_rear']:+.3f} | " + # f"preserva={frente_dbg['melhor_eixo_0']}" + #) + except Exception: + pass + + def _escolher_eixo_yaw_tracking( + self, + delta_base, + x, + y, + theta, + velocidade, + idx_base, + status_carro, + tipo_prev, + contexto=None, + ): + """V16: escolhe o eixo usando a MESMA convenção de comando. + + `delta_base` já representa o sentido desejado de yaw do corpo. + RodasTraseiras converte internamente esse comando para dr=-delta, logo + não testamos mais sinais espelhados. Dianteira e Traseira competem com + exatamente o mesmo comando de alto nível. + """ + frente = TipoMovimentoDirecional.RodasDianteiras + tras = TipoMovimentoDirecional.RodasTraseiras + + dbg_f = self._avaliar_eixo_yaw_tracking( + frente, delta_base, + x, y, theta, velocidade, + idx_base, status_carro, tipo_prev, + contexto=contexto, + ) + + if not bool(self._tracking_rear_enabled): + self._log_escolha_eixo_v15(frente, dbg_f, dbg_f) + return frente, float(delta_base) + + dbg_t = self._avaliar_eixo_yaw_tracking( + tras, delta_base, + x, y, theta, velocidade, + idx_base, status_carro, tipo_prev, + contexto=contexto, + ) + + custo_f = float(dbg_f["custo"]) + custo_t = float(dbg_t["custo"]) + margem = float(self._tracking_rear_min_gain) + + escolhido = frente + + # Histerese de eixo: quem já está ativo só perde o volante quando o + # concorrente demonstra vantagem real. + if tipo_prev == tras: + escolhido = ( + frente + if custo_f < custo_t * (1.0 - margem) + else tras + ) + elif tipo_prev == frente: + escolhido = ( + tras + if custo_t < custo_f * (1.0 - margem) + else frente + ) + elif custo_t < custo_f * (1.0 - margem): + escolhido = tras + + self._log_escolha_eixo_v15(escolhido, dbg_f, dbg_t) + return escolhido, float(delta_base) + + def _referencia_supervisor_corredor( + self, + e_lat, + erro_heading_rad, + velocidade, + tipo_preferido, + status_carro, + *, + x=None, + y=None, + theta=None, + idx_base=None, + s_proj=None, + contexto=None, + ): + """V15: supervisor de tracking da passada com CURVE_TRACK axle-aware. + + RETA: + - Diagonal é HOLD nominal e corrige cross-track sem criar yaw; + - Dianteira/Traseira entram quando heading realmente sai da banda; + - Arco continua sendo recuperação grosseira. + + CURVA: + - Dianteira/Traseira acompanham continuamente a curvatura da rua; + - comando = feed-forward(curvatura) + heading + lateral; + - não esperamos acumular heading/cross-track para começar a virar; + - Diagonal só interrompe CURVE_TRACK em erro lateral de emergência. + + Isso evita o deadlock da V13 em que a diagonal tentava centralizar + enquanto a própria centerline continuava girando. + """ + e_lat = float(e_lat) + e_lat_abs = abs(e_lat) + erro_heading_rad = float(erro_heading_rad) + e_head_deg = abs(math.degrees(erro_heading_rad)) + v = max(0.0, float(velocidade)) + tipo_prev = _movimento_from_value(tipo_preferido) + + tipo_yaw_prev = tipo_prev in [ + TipoMovimentoDirecional.RodasDianteiras, + TipoMovimentoDirecional.RodasTraseiras, + ] + + # -------------------------------------------------------------- + # 1) Recuperação grosseira continua sendo Arco. + # -------------------------------------------------------------- + manter_arco = ( + tipo_prev == TipoMovimentoDirecional.MovimentoArco + and e_head_deg > self._tracking_arco_saida_graus + ) + entrar_arco = e_head_deg >= self._tracking_arco_entrada_graus + + if entrar_arco or manter_arco: + self._tracking_curve_diag_recovery_active = False + delta = self._ganho_heading_reta * erro_heading_rad + lim = math.radians(self.angulo_max_graus) + return ( + TipoMovimentoDirecional.MovimentoArco, + float(np.clip(delta, -lim, lim)), + ) + + # -------------------------------------------------------------- + # 2) Detecta CURVA pela geometria da própria centerline. + # -------------------------------------------------------------- + kappa, delta_ff = self._feedforward_curva_tracking( + s_proj, idx_base, contexto + ) + ff_deg_abs = abs(math.degrees(delta_ff)) + + curva_ativa = bool( + ff_deg_abs >= self._tracking_curve_ff_enter_graus + or ( + tipo_yaw_prev + and ff_deg_abs >= self._tracking_curve_ff_exit_graus + ) + ) + + if curva_ativa: + # ---------------------------------------------------------- + # 2A) Recovery lateral de emergência em curva. + # + # Em curva normal NÃO damos prioridade automática ao crab, + # porque isso recriaria o deadlock da V13. Só interrompemos o + # CURVE_TRACK se o rover realmente saiu bastante do centro. + # ---------------------------------------------------------- + if self._tracking_curve_diag_recovery_active: + if ( + e_lat_abs <= self._tracking_curve_diag_release_m + or e_head_deg > self._tracking_diag_heading_safety_graus + ): + self._tracking_curve_diag_recovery_active = False + elif ( + e_lat_abs >= self._tracking_curve_diag_emergency_m + and e_head_deg <= self._tracking_diag_heading_safety_graus + ): + self._tracking_curve_diag_recovery_active = True + + if self._tracking_curve_diag_recovery_active: + return ( + TipoMovimentoDirecional.MovimentoDiagonal, + self._angulo_diagonal_tracking(e_lat, v), + ) + + # ---------------------------------------------------------- + # 2B) CURVE_TRACK: yaw e centro são corrigidos simultaneamente. + # ---------------------------------------------------------- + delta_heading = self._ganho_heading_reta * erro_heading_rad + delta_lateral = self._angulo_lateral_curve_tracking(e_lat, v) + + delta_base = delta_ff + delta_heading + delta_lateral + lim = math.radians(self.angulo_max_graus) + delta_base = float(np.clip(delta_base, -lim, lim)) + + if ( + x is not None + and y is not None + and theta is not None + and idx_base is not None + ): + return self._escolher_eixo_yaw_tracking( + delta_base, + float(x), float(y), float(theta), + v, + int(idx_base), + status_carro, + tipo_prev, + contexto=contexto, + ) + + return ( + TipoMovimentoDirecional.RodasDianteiras, + delta_base, + ) + + # Saiu da região curva: limpa qualquer recovery pendente. + self._tracking_curve_diag_recovery_active = False + + # -------------------------------------------------------------- + # 3) RETA: política V13 centro-primeiro. + # -------------------------------------------------------------- + lateral_centrado = e_lat_abs <= self._tracking_center_hold_m + lateral_recentralizar = e_lat_abs >= self._tracking_center_recenter_m + lateral_urgente = e_lat_abs >= self._tracking_center_urgent_m + heading_seguro_diag = ( + e_head_deg <= self._tracking_diag_heading_safety_graus + ) + + if heading_seguro_diag and lateral_urgente: + return ( + TipoMovimentoDirecional.MovimentoDiagonal, + self._angulo_diagonal_tracking(e_lat, v), + ) + + if ( + heading_seguro_diag + and tipo_prev == TipoMovimentoDirecional.MovimentoDiagonal + and not lateral_centrado + ): + return ( + TipoMovimentoDirecional.MovimentoDiagonal, + self._angulo_diagonal_tracking(e_lat, v), + ) + + if heading_seguro_diag and lateral_recentralizar: + return ( + TipoMovimentoDirecional.MovimentoDiagonal, + self._angulo_diagonal_tracking(e_lat, v), + ) + + # -------------------------------------------------------------- + # 4) YAW_ALIGN em reta. + # -------------------------------------------------------------- + yaw_precisa_entrar = e_head_deg >= self._tracking_yaw_enter_graus + yaw_precisa_manter = ( + tipo_yaw_prev + and e_head_deg > self._tracking_yaw_exit_graus + ) + lateral_seguro_yaw = ( + e_lat_abs <= self._tracking_center_recenter_m + ) + + usar_yaw = bool( + e_head_deg > self._tracking_diag_heading_safety_graus + or ( + lateral_seguro_yaw + and (yaw_precisa_entrar or yaw_precisa_manter) + ) + ) + + if usar_yaw: + delta_heading = self._ganho_heading_reta * erro_heading_rad + # Em reta delta_ff é aproximadamente zero, mas mantemos a pequena + # parcela residual para uma transição suave perto do início da curva. + delta_base = delta_heading + delta_ff + lim = math.radians(self.angulo_max_graus) + delta_base = float(np.clip(delta_base, -lim, lim)) + + if ( + x is not None + and y is not None + and theta is not None + and idx_base is not None + ): + return self._escolher_eixo_yaw_tracking( + delta_base, + float(x), float(y), float(theta), + v, + int(idx_base), + status_carro, + tipo_prev, + contexto=contexto, + ) + + return ( + TipoMovimentoDirecional.RodasDianteiras, + delta_base, + ) + + # -------------------------------------------------------------- + # 5) HOLD / centralização em reta: Diagonal. + # -------------------------------------------------------------- + return ( + TipoMovimentoDirecional.MovimentoDiagonal, + self._angulo_diagonal_tracking(e_lat, v), + ) + + def _referencia_direcional_reta( + self, + e_lat, + erro_heading_rad, + velocidade, + tipo_preferido=None, + status_carro=None, + *, + x=None, + y=None, + theta=None, + idx_base=None, + s_proj=None, + contexto=None, + ): """Escolhe a geometria pelo estado geométrico rover x caminho. - Política universal V5: - - heading muito errado -> MovimentoArco para recuperar orientação com - menor raio; - - heading moderadamente errado -> RodasDianteiras; - - heading praticamente alinhado + cross-track -> MovimentoDiagonal; - - heading alinhado e centralizado -> RodasDianteiras praticamente reta. + V15 em CaminhandoRua: + - reta: Diagonal como HOLD/centralização; + - curva: CURVE_TRACK contínuo com seletor axle-aware Dianteira/Traseira; + - CURVE_TRACK = feed-forward + heading + lateral; + - Diagonal interrompe curva só em recovery lateral de emergência; + - Arco apenas para heading grande. - A decisão usa heading do CAMINHO, nunca bearing rover->waypoint. + Demais estados preservam a política V8/V12 já validada. """ e_lat = float(e_lat) v = max(0.0, float(velocidade)) @@ -967,9 +2484,24 @@ class ControladorMPC: e_head_deg = abs(math.degrees(erro_heading_rad)) tipo_prev = _movimento_from_value(tipo_preferido) - # 1) Erro grande de heading tem prioridade absoluta sobre cross-track. - # A histerese mantém Arco até o erro cair bem abaixo do limiar de - # entrada, evitando caça de geometria em torno de 15 graus. + if _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]): + return self._referencia_supervisor_corredor( + e_lat, + erro_heading_rad, + v, + tipo_preferido, + status_carro, + x=x, + y=y, + theta=theta, + idx_base=idx_base, + s_proj=s_proj, + contexto=contexto, + ) + + # -------------------------------------------------------------- + # POLITICA LEGADA V8/V12 PARA ENTRADA/SAIDA/DIRECIONANDO/RETORNO + # -------------------------------------------------------------- manter_arco = ( tipo_prev == TipoMovimentoDirecional.MovimentoArco and e_head_deg > self._tracking_arco_saida_graus @@ -984,41 +2516,108 @@ class ControladorMPC: float(np.clip(delta, -lim, lim)), ) - # 2) Só usa diagonal quando o corpo já está praticamente paralelo ao - # caminho. Ela corrige deslocamento lateral sem criar yaw desnecessário. - precisa_recentralizar = abs(e_lat) > self._lateral_deadband_m + e_lat_abs = abs(e_lat) + precisa_recentralizar = e_lat_abs > self._lateral_deadband_m + + faixa_lat = max( + 1e-6, + self._diagonal_lateral_urgente_m - self._lateral_deadband_m, + ) + t_lat = float(np.clip( + (e_lat_abs - self._lateral_deadband_m) / faixa_lat, + 0.0, + 1.0, + )) + t_lat = t_lat * t_lat * (3.0 - 2.0 * t_lat) + + limite_heading_diag_entrada = ( + self._diagonal_entrada_heading_graus + + ( + self._diagonal_heading_lateral_max_graus + - self._diagonal_entrada_heading_graus + ) * t_lat + ) + limite_heading_diag_saida = min( + self._diagonal_heading_lateral_max_graus + + self._diagonal_histerese_heading_graus, + max( + self._diagonal_saida_heading_graus, + limite_heading_diag_entrada + + self._diagonal_histerese_heading_graus, + ), + ) + manter_diag = ( tipo_prev == TipoMovimentoDirecional.MovimentoDiagonal - and e_head_deg <= self._diagonal_saida_heading_graus + and precisa_recentralizar + and e_head_deg <= limite_heading_diag_saida ) entrar_diag = ( precisa_recentralizar - and e_head_deg <= self._diagonal_entrada_heading_graus + and e_head_deg <= limite_heading_diag_entrada ) usar_diag = bool(manter_diag or entrar_diag) - if usar_diag: - if abs(e_lat) <= self._lateral_deadband_m: - delta = 0.0 - else: - delta = math.atan2( - self._ganho_lateral_diagonal * e_lat, - v + self._velocidade_offset_lateral, + if _status_in(status_carro, [StatusCarroMapa.EntrandoRua]): + if tipo_prev != TipoMovimentoDirecional.MovimentoDiagonal: + usar_diag = bool( + usar_diag + and e_head_deg <= self._entrada_rua_diagonal_heading_max_graus ) - lim = math.radians(min(self._angulo_diagonal_max_graus, self.angulo_max_graus)) - return TipoMovimentoDirecional.MovimentoDiagonal, float(np.clip(delta, -lim, lim)) - # 3) Faixa intermediária: RodasDianteiras têm UMA responsabilidade, - # corrigir heading. O erro lateral fica reservado para a diagonal quando - # o corpo voltar à janela estreita de alinhamento. + if usar_diag: + return ( + TipoMovimentoDirecional.MovimentoDiagonal, + self._angulo_diagonal_tracking(e_lat, v), + ) + + # Faixa intermediaria: RodasDianteiras fazem tracking combinado, + # preservando exatamente o comportamento V7 para estados de transicao. if e_head_deg <= self._heading_deadband_graus: e_head_eff = 0.0 else: e_head_eff = erro_heading_rad - delta = self._ganho_heading_reta * e_head_eff + delta_heading = self._ganho_heading_reta * e_head_eff + + e_lat_eff_abs = max( + 0.0, abs(e_lat) - self._lateral_deadband_m + ) + e_lat_eff = ( + math.copysign(e_lat_eff_abs, e_lat) + if e_lat_eff_abs > 0.0 + else 0.0 + ) + + if ( + e_lat_eff == 0.0 + or self._ganho_lateral_dianteira <= 0.0 + ): + delta_lateral = 0.0 + else: + delta_lateral = math.atan2( + self._ganho_lateral_dianteira * e_lat_eff, + v + self._velocidade_offset_lateral, + ) + + lim_lat_front = math.radians( + min( + self._angulo_lateral_dianteira_max_graus, + self.angulo_max_graus, + ) + ) + delta_lateral = float(np.clip( + delta_lateral, + -lim_lat_front, + lim_lat_front, + )) + + delta = delta_heading + delta_lateral lim = math.radians(self.angulo_max_graus) - return TipoMovimentoDirecional.RodasDianteiras, float(np.clip(delta, -lim, lim)) + return ( + TipoMovimentoDirecional.RodasDianteiras, + float(np.clip(delta, -lim, lim)), + ) def _estabilizar_angulo_saida( self, @@ -1364,6 +2963,12 @@ class ControladorMPC: self._lut_pronto = True def _build_lut_geom_por_tipo(self, dados, tipo, ang_min=-30.0, ang_max=30.0, passo=1.0, L=1.0, k_r=0.5, crab_in_phase=False, beta_crab_deg=4.0): + """V16: LUT geométrica usando a mesma cinemática 4WS do MPC pesado. + + A LUT continua deliberadamente geométrica (sem Ku), mas agora inclui o + ângulo de deriva cinemático beta do centro. Isso diferencia corretamente + RodasDianteiras de RodasTraseiras já no quick-gate do costmap. + """ C = dados["custo"] W = int(C.shape[1]) escx = np.asarray(dados["row_scale_x_m"], dtype=np.float32) @@ -1371,38 +2976,56 @@ class ControladorMPC: offs = self._lut["offsets"] H = dist.size - ang_grid_deg = np.arange(ang_min, ang_max + 1e-6, passo, dtype=np.float32) # [K] + ang_grid_deg = np.arange( + ang_min, ang_max + 1e-6, passo, dtype=np.float32 + ) ang_grid_rad = np.deg2rad(ang_grid_deg) - modo_diagonal = (tipo in [TipoMovimentoDirecional.MovimentoDiagonal]) - - if crab_in_phase or modo_diagonal: - # κ = 0; deslocamento lateral linear com a inclinação - # DIAGONAL: beta por-ângulo (usa a grade) - # crab “fixo”: mantém compatibilidade usando beta constante - if modo_diagonal: - beta_vec = ang_grid_rad # [K] — varia com a grade - else: - beta_vec = np.full_like(ang_grid_rad, np.deg2rad(beta_crab_deg)) - kappa = np.zeros_like(ang_grid_rad) + if crab_in_phase: + beta_vec = np.full_like( + ang_grid_rad, + np.deg2rad(beta_crab_deg), + dtype=np.float32, + ) + kappa = np.zeros_like(ang_grid_rad, dtype=np.float32) else: - kappa = np.array([self._kappa_from_angle(tipo, a, L=L, k_r=k_r) for a in ang_grid_rad], dtype=np.float32) + geometrias = [ + self._cinematica_4ws( + tipo, + float(a), + 0.0, + entre_eixos=L, + aplicar_dinamica=False, + ) + for a in ang_grid_rad + ] + beta_vec = np.asarray( + [g["beta"] for g in geometrias], dtype=np.float32 + ) + kappa = np.asarray( + [g["kappa_geom"] for g in geometrias], dtype=np.float32 + ) - y = dist[:, None] # [H,1] - k = kappa[None, :] # [1,K] + # `dist` é usado como abscissa curta ao longo do movimento. Para uma + # curvatura constante: + # theta(s) = kappa*s + # direção velocidade = theta + beta + # e portanto o deslocamento lateral é integrado analiticamente. + s = dist[:, None] # [H,1] + b = beta_vec[None, :] # [1,K] + k = kappa[None, :] # [1,K] + ks = k * s - if crab_in_phase or modo_diagonal: - # X(H,K) = Y(H,1) * tan(beta(K)) - x = y * np.tan(beta_vec[None, :]) - else: - ky = k * y - x = np.where( - np.abs(ky) < 1e-4, - 0.5 * k * (y**2), - (np.sign(k) / np.maximum(np.abs(k), 1e-9)) * (1.0 - np.cos(np.abs(ky))) - ).astype(np.float32) + x_reta = s * np.sin(b) + denom = np.where( + np.abs(k) < 1e-9, + 1.0, + k, + ) + x_curva = (np.cos(b) - np.cos(b + ks)) / denom + x = np.where(np.abs(k) < 1e-6, x_reta, x_curva).astype(np.float32) - sx = escx[:, None] # [H,1] + sx = escx[:, None] j_c = np.floor((x + (W * sx) * 0.5) / sx).astype(np.int32) j0 = np.clip(j_c + offs[:, [0]], 0, W - 1).astype(np.int32) @@ -1427,38 +3050,23 @@ class ControladorMPC: return np.clip(idx, 0, K-1) def _kappa_from_angle(self, tipo, a_rad, L, k_r=0.5, crab_in_phase=False, beta_crab=0.0): - """ - Curvatura GEOMÉTRICA usada pela LUT do costmap. + """Curvatura geométrica V16 usada pela LUT do costmap. - Importante: não usa calcular_omega(v=1). O GPSHandler aplica um fator - dinâmico dependente da velocidade (Ku), então omega(1)/1 não representa - uma curvatura geométrica independente da velocidade. A LUT é deliberadamente - geométrica/conservadora; a simulação pesada do MPC usa calcular_omega com - a velocidade real do ciclo. + Usa exatamente o mesmo mapeamento de eixos da cinemática global, porém + sem correção dinâmica Ku, pois a LUT deve continuar geométrica. """ if crab_in_phase: return 0.0 - tipo_enum = _movimento_from_value(tipo) - L_eff = max(float(L), 1e-6) - t = math.tan(float(a_rad)) - - if abs(t) < 1e-9: - return 0.0 - if tipo_enum == TipoMovimentoDirecional.RodasDianteiras: - return t / L_eff - if tipo_enum == TipoMovimentoDirecional.RodasTraseiras: - # Mantém a convenção atualmente usada pelo GPSHandler/C#. - return t / L_eff - if tipo_enum == TipoMovimentoDirecional.MovimentoArco: - # df=+delta, dr=-delta => (tan(df)-tan(dr))/L = 2*tan(delta)/L - return (2.0 * t) / L_eff - if tipo_enum in [ - TipoMovimentoDirecional.MovimentoDiagonal, - TipoMovimentoDirecional.MovimentoLateral, - ]: - return 0.0 - return t / L_eff + return float( + self._cinematica_4ws( + tipo, + float(a_rad), + 0.0, + entre_eixos=max(float(L), 1e-6), + aplicar_dinamica=False, + )["kappa_geom"] + ) def _calcula_erro_posicao(self, x, y, ponto_alvo, d_sat=3.0): try: @@ -1726,6 +3334,7 @@ class ControladorMPC: custo_movimento = { TipoMovimentoDirecional.MovimentoArco: 0.0, TipoMovimentoDirecional.RodasDianteiras: 0.0, + TipoMovimentoDirecional.RodasTraseiras: 0.0, TipoMovimentoDirecional.MovimentoDiagonal: 0.0 } @@ -1745,14 +3354,17 @@ class ControladorMPC: if erro_heading_modo >= self._tracking_arco_entrada_graus: custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.5 + custo_movimento[TipoMovimentoDirecional.RodasTraseiras] += 1.5 custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0 elif erro_heading_modo <= self._diagonal_saida_heading_graus: custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 3.0 custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.0 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 0.25 + custo_movimento[TipoMovimentoDirecional.RodasTraseiras] += 0.25 else: custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 1.5 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 0.0 + custo_movimento[TipoMovimentoDirecional.RodasTraseiras] += 0.0 custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0 elif status == StatusCarroMapa.Manobrando: @@ -1762,6 +3374,7 @@ class ControladorMPC: # Rua -> Rua da V3. custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.5 + custo_movimento[TipoMovimentoDirecional.RodasTraseiras] += 1.5 custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0 elif status in [StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua]: @@ -1786,12 +3399,14 @@ class ControladorMPC: custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += penal * t custo_movimento[TipoMovimentoDirecional.MovimentoArco] += penal * (1.0 - t) - # Diagonal não pertence a esta política de transição. + # Diagonal/Traseira não pertencem a esta política de transição. custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0 + custo_movimento[TipoMovimentoDirecional.RodasTraseiras] += 3.0 else: custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 1.5 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 0.0 + custo_movimento[TipoMovimentoDirecional.RodasTraseiras] += 0.5 custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.5 return (peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, custo_movimento) @@ -1800,6 +3415,7 @@ class ControladorMPC: return (1.2, 2.2, 1.0, 0.05, 1.4, 0.0, { TipoMovimentoDirecional.MovimentoArco: 0.0, TipoMovimentoDirecional.RodasDianteiras: 0.0, + TipoMovimentoDirecional.RodasTraseiras: 0.5, TipoMovimentoDirecional.MovimentoDiagonal: 2.0, }) @@ -1982,6 +3598,10 @@ class ControladorMPC: def _processar_mpc_receding(self, contexto, comando_anterior): contexto = _as_dict(contexto) comando_anterior = _as_dict(comando_anterior) + + # V16: o entre-eixos publicado pelo contexto passa a alimentar uma + # única cinemática compartilhada por TODAS as previsões do MPC. + self._atualizar_cinematica_contexto(contexto) now = time.perf_counter t0 = now() pos_latencia = 0.0 @@ -2034,7 +3654,7 @@ class ControladorMPC: "tipo": comando_anterior.get("tipo"), "angulo": np.radians(comando_anterior.get("angulo", 0.0)), "v": velocidade, - "omega": self.gps_handler.calcular_omega( + "omega": self._calcular_omega_4ws( velocidade, np.radians(comando_anterior.get("angulo", 0.0)), comando_anterior.get("tipo") @@ -2049,7 +3669,7 @@ class ControladorMPC: u_hold=u_hold, cmd_seq=cmd_seq, time_left_ms=lambda: (deadline - now()) * 1000.0, - calc_omega_fn=self.gps_handler.calcular_omega + calc_omega_fn=self._calcular_omega_4ws ) idx_alvo_correcao = self._corrigir_pontos_visitados( x, @@ -2066,6 +3686,23 @@ class ControladorMPC: velocidade, ) + # V12 - cabeceira Dubins: a rota ja esta pronta. Em Manobrando nao + # abrimos a arvore MPC; usamos Pure Pursuit 4WS mirando um ponto + # fisico adiante na propria curva. O heading/tangente fica apenas + # como telemetria e nao alivia o comando antes da hora. + if self._usar_seguidor_dubins(status_carro, idx_alvo_correcao): + return self._comando_seguidor_dubins( + contexto=contexto, + comando_anterior=comando_anterior, + x=x, y=y, theta=theta, + velocidade=velocidade, + status_carro=status_carro, + idx_alvo_correcao=idx_alvo_correcao, + pos_lat=pos_lat, pos_lon=pos_lon, pos_theta=pos_theta, + pos_latencia=pos_latencia, + ponto_alvo_real=ponto_alvo_real, + ) + # -------------------- Planejamento: horizonte ESPACIAL fixo -------------------- S_ALVO = float(self.horizonte) V_FLOOR = float(getattr(self, "_vmin_planejamento", 0.30)) @@ -2337,6 +3974,50 @@ class ControladorMPC: debug_custo[key_debug]["melhor"] = True simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(melhor["trajetoria"]) or []) simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta)) + + # V10 DIAGNOSTICO: mostra exatamente o primeiro comando que + # sai do MPC e os proximos comandos da sequencia vencedora. + # A linha verde exibida pelo simulador vem de melhor["trajetoria"], + # enquanto o comando efetivamente devolvido neste ciclo e + # _comandos[1]. Esse log permite separar: + # (a) MPC adiando a curva para o futuro; + # (b) comando correto no MPC, mas nao aplicado no simulador/C#. + if _status_in( + status_carro, + [ + StatusCarroMapa.Manobrando, + StatusCarroMapa.EntrandoRua, + StatusCarroMapa.SaindoRua, + ], + ): + try: + seq_dbg = [] + for tipo_dbg, ang_dbg in _comandos[1:5]: + tipo_dbg_enum = _movimento_from_value(tipo_dbg) + seq_dbg.append( + f"{tipo_dbg_enum.name}:{math.degrees(float(ang_dbg)):+.1f}" + ) + + dh_green = None + if len(simulacao_latlon) >= 2: + try: + h0 = float(simulacao_latlon[0][2]) + h1 = float(simulacao_latlon[1][2]) + dh_green = (h1 - h0 + 180.0) % 360.0 - 180.0 + except Exception: + dh_green = None + + #mostrar_log( + # "[MPC/V10 EXEC] " + # f"status={status_carro} | " + # f"idx={idx_alvo_correcao} | " + # f"prev={tipo_anterior.name}:{math.degrees(float(angulo_anterior)):+.1f} | " + # f"cmd={tipo_final.name}:{math.degrees(float(angulo_final)):+.1f} | " + # f"seq=[{', '.join(seq_dbg)}] | " + # f"green_dh1={(f'{dh_green:+.2f}' if dh_green is not None else 'n/a')}" + #) + except Exception as e: + mostrar_log(f"[MPC/V10 EXEC] falha ao montar diagnostico: {e}") else: angulo_final = 0.0 tipo_final = TipoMovimentoDirecional.RodasDianteiras @@ -2353,12 +4034,13 @@ class ControladorMPC: ponto_alvo_primario = ponto_alvo_real["xy"] ref_saida = self._referencia_caminho_lookahead( - x, y, idx_alvo_correcao, velocidade + x, y, idx_alvo_correcao, velocidade, status_carro ) erro_lateral = float(ref_saida["e_lat"]) erro_heading_saida = self._erro_heading_caminho(theta, ref_saida["theta_ref"]) tracking_reta_saida = self._tracking_reta_permitido( - contexto, idx_alvo_correcao, erro_lateral, erro_heading_saida + contexto, idx_alvo_correcao, erro_lateral, erro_heading_saida, + distancia_rota_m=ref_saida.get("distancia_projecao_m"), ) if tracking_reta_saida: erro_orientacao = abs(float(np.degrees(erro_heading_saida))) @@ -2505,7 +4187,7 @@ class ControladorMPC: # MANOBRA estrutural: mantém pure-pursuit e Arco obrigatório. # -------------------------------------------------------------- ref_path = self._referencia_caminho_lookahead( - ponto_atual[0], ponto_atual[1], idx_base, velocidade + ponto_atual[0], ponto_atual[1], idx_base, velocidade, status ) e_lat = float(ref_path["e_lat"]) theta_ref = float(ref_path["theta_ref"]) @@ -2513,7 +4195,8 @@ class ControladorMPC: e_ori = abs(math.degrees(erro_heading)) tracking_reta = self._tracking_reta_permitido( - contexto, idx_base, e_lat, erro_heading + contexto, idx_base, e_lat, erro_heading, + distancia_rota_m=ref_path.get("distancia_projecao_m"), ) if tracking_reta: @@ -2522,8 +4205,34 @@ class ControladorMPC: erro_heading, velocidade, tipo_preferido=tipo_preferido, + status_carro=status, + x=ponto_atual[0], + y=ponto_atual[1], + theta=ponto_atual[2], + idx_base=idx_base, + s_proj=ref_path.get("s_proj"), + contexto=contexto, ) - tipos_validos = [tipo_ref] + # V16: quando o supervisor está em correção de yaw/curva, + # Dianteira e Traseira deixam de ser apenas uma escolha da + # micro-predição. O MPC pesado pode avaliar AMBAS com a mesma + # convenção angular e escolher pelo custo real do horizonte. + if ( + _status_in(status, [StatusCarroMapa.CaminhandoRua]) + and bool(self._tracking_rear_enabled) + and tipo_ref in [ + TipoMovimentoDirecional.RodasDianteiras, + TipoMovimentoDirecional.RodasTraseiras, + ] + ): + outro_eixo = ( + TipoMovimentoDirecional.RodasTraseiras + if tipo_ref == TipoMovimentoDirecional.RodasDianteiras + else TipoMovimentoDirecional.RodasDianteiras + ) + tipos_validos = [tipo_ref, outro_eixo] + else: + tipos_validos = [tipo_ref] else: orient = self.gps_handler.calcular_orientacao(ponto_alvo["xy"], ponto_atual) delta_theta = self._wrap_pi(orient - ponto_atual[2]) @@ -3049,14 +4758,14 @@ class ControladorMPC: return [], {} def _simular_trajetoria_curta(self, p_atual, tipo, angulo, velocidade, dist_min): - """ - Trajetória curta com v e ω constantes. - Compatível com o sistema: x += sin(θ)*d, y += cos(θ)*d. - Retorna lista [(x,y,θ)] de n amostras (n = ceil(dist_min / (v*dt))). + """V16: trajetória curta usando a cinemática 4WS global. + + A mesma combinação beta/omega usada pelo MPC pesado é integrada aqui + de forma analítica para v, steering e omega constantes. """ try: dt = float(self.dt) - v = float(velocidade) + v = float(velocidade) if v <= 1e-6 or dist_min <= 1e-6 or dt <= 0.0: return [] @@ -3066,33 +4775,45 @@ class ControladorMPC: return [] n = min(n, int(getattr(self, "_traj_curta_max_steps", 64))) - x0, y0, th0 = float(p_atual[0]), float(p_atual[1]), float(p_atual[2]) - omega = float(self.gps_handler.calcular_omega(v, angulo, tipo)) + x0, y0, th0 = ( + float(p_atual[0]), + float(p_atual[1]), + float(p_atual[2]), + ) - k = np.arange(1, n + 1, dtype=np.float32) - if tipo in [TipoMovimentoDirecional.MovimentoDiagonal, TipoMovimentoDirecional.MovimentoLateral]: - # 4WS em fase: heading do corpo permanece, mas a velocidade - # translacional aponta para theta + angulo. - direcao = th0 + float(angulo) - dx = dist_step * k * math.sin(direcao) - dy = dist_step * k * math.cos(direcao) - th = th0 + np.zeros_like(k, dtype=np.float32) - x = x0 + dx - y = y0 + dy + kin = self._cinematica_4ws( + tipo, + float(angulo), + v, + aplicar_dinamica=True, + ) + beta = float(kin["beta"]) + omega = float(kin["omega"]) + + t = np.arange(1, n + 1, dtype=np.float64) * dt + th = th0 + omega * t + + phi0 = th0 + beta + + if abs(omega) <= 1e-9: + x = x0 + v * t * math.sin(phi0) + y = y0 + v * t * math.cos(phi0) else: - # Mesma integração semi-implícita usada por _nova_posicao e - # pelo simulador C#: primeiro atualiza theta, depois translada. - # O cumsum reproduz exatamente a aplicação passo a passo. - dth = omega * dt - th = th0 + dth * k - dx_step = dist_step * np.sin(th) - dy_step = dist_step * np.cos(th) - x = x0 + np.cumsum(dx_step) - y = y0 + np.cumsum(dy_step) + phi = th + beta + x = x0 + (v / omega) * ( + math.cos(phi0) - np.cos(phi) + ) + y = y0 + (v / omega) * ( + np.sin(phi) - math.sin(phi0) + ) - return list(zip(x.astype(float).tolist(), - y.astype(float).tolist(), - th.astype(float).tolist())) + return list( + zip( + np.asarray(x, dtype=float).tolist(), + np.asarray(y, dtype=float).tolist(), + np.asarray(th, dtype=float).tolist(), + ) + ) except Exception as e: mostrar_log(f"❌ Erro na simulação curta: {e}") return [] @@ -3454,7 +5175,7 @@ class ControladorMPC: custo_visual_worker = 0.0 # omega e sub-stepping - omega_const = self.gps_handler.calcular_omega(v_planejado, angulo_testado, tipo) + omega_const = self._calcular_omega_4ws(v_planejado, angulo_testado, tipo) #passos_local = max(1, int(getattr(self, "passos_horizonte_local", 1))) #dt_total = float(dt_pred) * passos_local @@ -3508,14 +5229,16 @@ class ControladorMPC: # ERROS / CUSTOS # -------------------------- ref_path_sim = self._referencia_caminho_lookahead( - x_sim, y_sim, idx_alvo_sim, v_planejado + x_sim, y_sim, idx_alvo_sim, v_planejado, + contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value), ) e_lat = float(ref_path_sim["e_lat"]) theta_path = float(ref_path_sim["theta_ref"]) margem = float(ref_path_sim["margem"]) erro_heading_sim = self._erro_heading_caminho(theta_sim, theta_path) tracking_reta_sim = self._tracking_reta_permitido( - contexto, idx_alvo_sim, e_lat, erro_heading_sim + contexto, idx_alvo_sim, e_lat, erro_heading_sim, + distancia_rota_m=ref_path_sim.get("distancia_projecao_m"), ) if tracking_reta_sim: @@ -3694,32 +5417,59 @@ class ControladorMPC: return float('inf'), [(0.0, 0.0, 0.0)], False, visitados def _nova_posicao(self, x, y, theta, omega, velocidade, tipo, angulo_rad, dt=None): - dt_use = self.dt if (dt is None) else float(dt) - theta_next = theta + omega * dt_use # orientação do corpo (no diagonal, ω≈0 → mantém) - - # Escolhe a direção do deslocamento: - if tipo in [TipoMovimentoDirecional.MovimentoDiagonal, TipoMovimentoDirecional.MovimentoLateral]: - # 4WS em fase: transladar na direção θ+α, mantendo θ - direcao = theta_next + angulo_rad + """V16: integra UM passo com a cinemática 4WS unificada. + + `omega` permanece na assinatura por compatibilidade com callbacks antigos, + mas quando `tipo` e `angulo_rad` existem a autoridade passa a ser + `_cinematica_4ws()`. Assim micro-preview, MPC pesado, latência e Dubins + usam exatamente a mesma física. + + Integração analítica para beta e omega constantes: + direção da velocidade = theta + beta + theta_dot = omega + """ + dt_use = self.dt if (dt is None) else max(0.0, float(dt)) + v = float(velocidade) + + tipo_enum = None + try: + tipo_enum = _movimento_from_value(tipo) + except Exception: + tipo_enum = None + + if tipo_enum is not None: + kin = self._cinematica_4ws( + tipo_enum, + float(angulo_rad), + v, + aplicar_dinamica=True, + ) + beta = float(kin["beta"]) + omega_use = float(kin["omega"]) else: - # modos "normais": desloca ao longo do eixo do corpo - direcao = theta_next + beta = 0.0 + omega_use = float(omega) - # ► Escolha como interpretar "velocidade" (deixe a A como padrão): - # A) velocidade = módulo do deslocamento (padrão anterior) - distancia_m = velocidade * dt_use + theta0 = float(theta) + theta_next = theta0 + omega_use * dt_use + phi0 = theta0 + beta - # B) (opcional) velocidade = componente longitudinal do corpo: - # mantém avanço "pra frente" igual à velocidade e adiciona drift lateral. - # use no lugar da linha acima se preferir esse comportamento: - #distancia_m = (velocidade * dt_use) / max(np.cos(angulo_rad), 1e-6) + if abs(omega_use) <= 1e-9: + distancia = v * dt_use + dx = math.sin(phi0) * distancia + dy = math.cos(phi0) * distancia + else: + phi1 = theta_next + beta + raio_vel = v / omega_use + dx = raio_vel * (math.cos(phi0) - math.cos(phi1)) + dy = raio_vel * (math.sin(phi1) - math.sin(phi0)) - x_sim = x + np.sin(direcao) * distancia_m - y_sim = y + np.cos(direcao) * distancia_m - theta_sim = theta_next # não soma α aqui! + return ( + float(x) + float(dx), + float(y) + float(dy), + float(theta_next), + ) - return x_sim, y_sim, theta_sim - def _quick_gate_by_lut_tipo(self, angulos_deg, dados_costmap, tipo, max_linhas=3): """ Gate rápido por LUT: diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py index ce16e08d8..8704a93e8 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py @@ -1651,14 +1651,25 @@ class CameraManager: threading.Thread(target=loop, daemon=True).start() def _iniciar_loop_frame_stream(self, freq=2.0): + """ + Alimenta o CameraTcpStreamer sem duplicar a política de FPS. + + O streamer é a única camada responsável por throttle, banda, + backpressure e reconexão. Este produtor apenas oferece o último frame + em frequência superior ao maior FPS operacional. + """ + producer_hz = max(10.0, float(freq or 2.0) * 2.0) + def loop(): while True: t_wall0 = time.time() t_loop0 = time.perf_counter() + stream_on = False try: - if self.camera is None: - time.sleep(0.5) + camera_atual = self.camera + if camera_atual is None: + time.sleep(0.25) continue camera_ctx = ContextoGlobalRedis.get_camera(self.mx_id) or {} @@ -1673,7 +1684,7 @@ class CameraManager: frame = self._get_cached_preview_frame(frame_type, copiar=False) if frame is not None: - self.camera.enviar_frame_tcp(frame) + camera_atual.enviar_frame_tcp(frame) if self.debug_visual and self._visual_disponivel_para_frame(): self._solicitar_preview(TipoFrameCamera.Debug) @@ -1692,15 +1703,14 @@ class CameraManager: finally: lat = time.time() - t_wall0 + hz = producer_hz if stream_on else 2.0 + time.sleep(max(0.0, (1.0 / hz) - lat)) - try: - freq_atual = float(getattr(self.camera.stream, "_op_fps", freq) or freq) - except Exception: - freq_atual = freq - - time.sleep(max(0.0, (1.0 / max(freq_atual, 0.1)) - lat)) - - threading.Thread(target=loop, daemon=True).start() + threading.Thread( + target=loop, + daemon=True, + name="visual-stream-feed", + ).start() def _iniciar_loop_analise_continua(self, freq=8.0): def loop(): diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py index c233ea788..ae7890c29 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py @@ -3231,14 +3231,26 @@ class CameraManager: threading.Thread(target=loop, daemon=True).start() def _iniciar_loop_frame_stream(self, freq=2.0): + """ + Alimenta o CameraTcpStreamer com o último preview disponível. + + Importante: o controle de FPS/banda existe SOMENTE dentro do + CameraTcpStreamer. Este loop roda um pouco mais rápido que o maior + FPS operacional para não criar um segundo gate sincronizado que possa + pular janelas inteiras por jitter de agendamento. + """ + producer_hz = max(10.0, float(freq or 2.0) * 2.0) + def loop(): while True: t0 = time.time() t_loop0 = time.perf_counter() + stream_on = False try: - if self.camera is None: - time.sleep(5.0) + camera_atual = self.camera + if camera_atual is None: + time.sleep(0.25) continue camera_ctx = ContextoGlobalRedis.get_camera(self.mx_id) or {} @@ -3248,9 +3260,11 @@ class CameraManager: frame_type = TipoFrameCamera( camera_ctx.get("frame_type", TipoFrameCamera.Rgb.value) ) - # Stream também é consumidor de cache. Não monta preview. + + # Stream é apenas consumidor do cache pronto. frame = self._get_cached_preview_frame_ref(frame_type) - self.camera.enviar_frame_tcp(frame) + if frame is not None: + camera_atual.enviar_frame_tcp(frame) if self.debug_visual: dbg = self._get_cached_preview_frame_ref(TipoFrameCamera.Debug) @@ -3270,15 +3284,14 @@ class CameraManager: ) lat = time.time() - t0 + hz = producer_hz if stream_on else 2.0 + time.sleep(max(0.0, (1.0 / hz) - lat)) - try: - freq_atual = float(getattr(self.camera.stream, "_op_fps", freq) or freq) - except Exception: - freq_atual = freq - - time.sleep(max(0.0, (1.0 / freq_atual) - lat)) - - threading.Thread(target=loop, daemon=True).start() + threading.Thread( + target=loop, + daemon=True, + name="weed-stream-feed", + ).start() def _iniciar_loop_analise_continua(self, freq=15.0): def loop(): diff --git a/AgroBase/OperationControl/ViewModels/Windows/DockWindowViewModel.cs b/AgroBase/OperationControl/ViewModels/Windows/DockWindowViewModel.cs index 8cf2cd2b5..f1c5d79f2 100644 --- a/AgroBase/OperationControl/ViewModels/Windows/DockWindowViewModel.cs +++ b/AgroBase/OperationControl/ViewModels/Windows/DockWindowViewModel.cs @@ -537,6 +537,7 @@ namespace OperationControl.ViewModels public void AtualizarBarraSuperior(OperacaoParametrosModel rover) { var obj = rover?.DadosLeitura; + if (rover == null || obj == null) return; @@ -545,12 +546,76 @@ namespace OperationControl.ViewModels var eventos = new List(); - double tempoSemResposta = (DateTime.Now - rover.UltimoContato).TotalSeconds; - bool semComunicacao = !AppShell.Mock && tempoSemResposta >= VariaveisControleOperacao.TempoRoverVivo; + // ============================================================ + // Helpers + // ============================================================ + + string NormalizarTexto(string texto) + { + if (string.IsNullOrWhiteSpace(texto)) + return null; + + var partes = texto + .Split(new[] { "\r\n", "\n", "\r" }, StringSplitOptions.RemoveEmptyEntries) + .Select(x => x?.Trim()) + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Distinct() + .ToList(); + + return partes.Count > 0 + ? string.Join(" • ", partes) + : null; + } + + bool EhModuloMandatorio(T_Code modulo) + { + // A telemetria recebida do rover é a fonte principal. + // Só cai para a configuração local caso o snapshot não exista. + var modulosMandatorios = + obj.Operacao?.ModulosMandatorios + ?? rover.ModulosMandatorios; + + return modulosMandatorios?.Any(x => + (x?.Dispositivo ?? T_Code.Vzo) == modulo && + (x?.Utilizar ?? false) && + (x?.Mandatorio ?? false) + ) ?? false; + } + + string MotivoOperacaoAtual() + { + // 1) Motivo autoritativo de NÃO liberação. + var motivo = NormalizarTexto(obj.Operacao?.ImpedimentoErro); + + if (!string.IsNullOrWhiteSpace(motivo)) + return motivo; + + // 2) Motivo do estado/status atual. + motivo = NormalizarTexto(obj.Operacao?.MotivoStatusOperacao); + + if (!string.IsNullOrWhiteSpace(motivo)) + return motivo; + + // 3) Último motivo de parada apenas como fallback. + motivo = NormalizarTexto(obj.Operacao?.UltimoMotivoParada); + + if (!string.IsNullOrWhiteSpace(motivo)) + return motivo; + + return "A operação não foi liberada pelo rover."; + } + + // ============================================================ + // 0) Comunicação + // ============================================================ + + double tempoSemResposta = + (DateTime.Now - rover.UltimoContato).TotalSeconds; + + bool semComunicacao = + !AppShell.Mock && + tempoSemResposta >= VariaveisControleOperacao.TempoRoverVivo; - // ============================ - // 0) Perda de comunicação - // ============================ if (semComunicacao) { eventos.Add(new MotivoTopBarModel @@ -558,63 +623,116 @@ namespace OperationControl.ViewModels Prioridade = 2000, Fonte = "Comunicacao", Titulo = "PERDA DE COMUNICAÇÃO COM O ROVER", - Descricao = $"Sem resposta do rover há {tempoSemResposta:F1} s. Tentando reconectar automaticamente.", + Descricao = + $"Sem resposta do rover há {tempoSemResposta:F1} s. " + + "Tentando reconectar automaticamente.", StatusVisual = StatusModulo.Desconectado }); } - // ============================ - // 0) NCP desconectado - // ============================ - if (obj.ModulosSaude?.All(x => (x?.status ?? StatusModulo.Desconectado) == StatusModulo.Desconectado) ?? false) + // ============================================================ + // 1) NPC / núcleo central + // ============================================================ + + bool todosModulosDesconectados = + obj.ModulosSaude?.Any() == true && + obj.ModulosSaude.All(x => + (x?.status ?? StatusModulo.Desconectado) == + StatusModulo.Desconectado); + + if (todosModulosDesconectados) { eventos.Add(new MotivoTopBarModel { Prioridade = 1100, Fonte = "NCP", Titulo = "NÚCLEO CENTRAL DE PROCESSAMENTO DESCONECTADO", - Descricao = "O núcleo central de processamento foi desconectado, tentando reconectar...", + Descricao = + "O núcleo central de processamento foi desconectado. " + + "Tentando reconectar...", StatusVisual = StatusModulo.Falha }); } - // ============================ - // 1) Emergência - // ============================ + // ============================================================ + // 2) Emergência + // ============================================================ + if (obj.Operacao?.Emergencia == true) { + var motivo = + NormalizarTexto(obj.Operacao.MotivoStatusOperacao) + ?? "Parada de emergência solicitada."; + eventos.Add(new MotivoTopBarModel { Prioridade = 1000, Fonte = "Emergencia", Titulo = "OPERAÇÃO BLOQUEADA POR EMERGÊNCIA", - Descricao = "Parada de emergência solicitada.", + Descricao = motivo, StatusVisual = StatusModulo.Falha }); } - // ============================ - // 2) Pausa - // ============================ + // ============================================================ + // 3) Pausa + // ============================================================ + if (obj.Operacao?.Pausa == true) { + var motivo = + NormalizarTexto(obj.Operacao.MotivoStatusOperacao) + ?? "Pausa operacional solicitada."; + eventos.Add(new MotivoTopBarModel { Prioridade = 900, Fonte = "Pausa", Titulo = "OPERAÇÃO PAUSADA", - Descricao = "Pausa operacional solicitada.", + Descricao = motivo, StatusVisual = StatusModulo.Alerta }); } - // ============================ - // 3) Controle - // ============================ - var motivosControle = obj.Controle?.Motivos? - .Where(x => !string.IsNullOrWhiteSpace(x)) - .Distinct() - .ToList() ?? new List(); + // ============================================================ + // 4) Operação não liberada + // + // Essa é a informação autoritativa vinda do rover. + // Exemplo: + // Sen: Desconectado + // Gps: Falha + // Parâmetro obrigatório ausente + // ============================================================ + + if ( + obj.Operacao != null && + !obj.Operacao.Liberada && + obj.Operacao.Pausa != true && + obj.Operacao.Emergencia != true + ) + { + eventos.Add(new MotivoTopBarModel + { + Prioridade = 850, + Fonte = "Operacao", + Titulo = "OPERAÇÃO BLOQUEADA", + Descricao = MotivoOperacaoAtual(), + StatusVisual = StatusModulo.Falha + }); + } + + // ============================================================ + // 5) Controle + // ============================================================ + + var motivosControle = + obj.Controle?.Motivos? + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Select(NormalizarTexto) + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Distinct() + .ToList() + ?? new List(); foreach (var motivo in motivosControle) { @@ -628,13 +746,18 @@ namespace OperationControl.ViewModels }); } - // ============================ - // 4) Trajetória / autonomia - // ============================ - var motivosTrajetoria = obj.Trajetoria?.AutonomiaCorredor?.Motivos? - .Where(x => !string.IsNullOrWhiteSpace(x)) - .Distinct() - .ToList() ?? new List(); + // ============================================================ + // 6) Trajetória / autonomia + // ============================================================ + + var motivosTrajetoria = + obj.Trajetoria?.AutonomiaCorredor?.Motivos? + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Select(NormalizarTexto) + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Distinct() + .ToList() + ?? new List(); foreach (var motivo in motivosTrajetoria) { @@ -648,110 +771,215 @@ namespace OperationControl.ViewModels }); } - // ============================ - // 5) Módulos com condições operacionais críticas - // ============================ - var modulosCriticos = obj.ModulosSaude? - .Where(mod => - (mod?.condicoes_operacionais?.Any(cond => cond?.severidade > 90) ?? false) || - ( - !new List() { StatusModulo.Operante, StatusModulo.Alerta }.Contains(mod?.status ?? StatusModulo.Desconectado) && - (rover?.ModulosMandatorios?.Any(x => - (x?.Dispositivo ?? T_Code.Vzo) == (mod?.modulo ?? T_Code.Vzo) && - (x?.Utilizar ?? false) && - (x?.Mandatorio ?? false) - ) ?? false) - ) || - ( - (mod?.status ?? StatusModulo.Desconectado) == StatusModulo.Alerta && - (rover?.ModulosMandatorios?.Any(x => - (x?.Dispositivo ?? T_Code.Vzo) == (mod?.modulo ?? T_Code.Vzo) && - (x?.Utilizar ?? false) && - (x?.Mandatorio ?? false) - ) ?? false) - ) - ) - .ToList() ?? new List(); + // ============================================================ + // 7) Estado operacional + // + // Aqui pegamos estados legítimos em que a operação está + // liberada, porém ainda não está executando. + // + // Exemplos: + // Aguardando retomada segura + // Parametrizando + // Finalizando + // ============================================================ + + if ( + obj.Operacao != null && + obj.Operacao.Liberada && + obj.Operacao.Pausa != true && + obj.Operacao.Emergencia != true && + obj.Operacao.Status != StatusOperacao.EmAndamento && + !string.IsNullOrWhiteSpace(obj.Operacao.MotivoStatusOperacao) + ) + { + var motivo = + NormalizarTexto(obj.Operacao.MotivoStatusOperacao); + + eventos.Add(new MotivoTopBarModel + { + Prioridade = 650, + Fonte = "StatusOperacao", + Titulo = + $"OPERAÇÃO {obj.Operacao.Status.ToString().ToUpper()}", + Descricao = motivo, + StatusVisual = + obj.Operacao.Status == StatusOperacao.Erro + ? StatusModulo.Falha + : StatusModulo.Alerta + }); + } + + // ============================================================ + // 8) Módulos críticos + // ============================================================ + + var modulosCriticos = + obj.ModulosSaude? + .Where(mod => + { + if (mod == null) + return false; + + var status = + mod?.status ?? StatusModulo.Desconectado; + + bool temCondicaoSevera = + mod?.condicoes_operacionais?.Any( + cond => cond?.severidade > 90 + ) ?? false; + + bool mandatorio = + EhModuloMandatorio(mod?.modulo ?? T_Code.Vzo); + + bool naoOperante = + !new[] + { + StatusModulo.Operante, + StatusModulo.Alerta + }.Contains(status); + + bool emAlerta = + status == StatusModulo.Alerta; + + return + temCondicaoSevera || + (mandatorio && naoOperante) || + (mandatorio && emAlerta); + }) + .ToList() + ?? new List(); foreach (var modulo in modulosCriticos) { - var nomeModulo = (modulo?.modulo ?? T_Code.Vzo).ToString(); + var codigoModulo = + modulo?.modulo ?? T_Code.Vzo; + + var nomeModulo = + codigoModulo.ToString(); + + var status = + modulo?.status ?? StatusModulo.Desconectado; + + bool mandatorio = + EhModuloMandatorio(codigoModulo); + + bool temCondicaoSevera = + modulo?.condicoes_operacionais?.Any( + cond => cond?.severidade > 90 + ) ?? false; - bool temCondicaoSevera = modulo?.condicoes_operacionais?.Any(cond => cond?.severidade > 90) ?? false; bool moduloMandatorioNaoOperante = - !new List() { StatusModulo.Operante, StatusModulo.Alerta }.Contains(modulo?.status ?? StatusModulo.Desconectado) && - (rover?.ModulosMandatorios?.Any(x => - (x?.Dispositivo ?? T_Code.Vzo) == (modulo?.modulo ?? T_Code.Vzo) && - (x?.Utilizar ?? false) && - (x?.Mandatorio ?? false) - ) ?? false); - bool moduloMandatorioAlerta = - (modulo?.status ?? StatusModulo.Desconectado) == StatusModulo.Alerta && - (rover?.ModulosMandatorios?.Any(x => - (x?.Dispositivo ?? T_Code.Vzo) == (modulo?.modulo ?? T_Code.Vzo) && - (x?.Utilizar ?? false) && - (x?.Mandatorio ?? false) - ) ?? false); + mandatorio && + status != StatusModulo.Operante && + status != StatusModulo.Alerta; + + bool moduloMandatorioAlerta = + mandatorio && + status == StatusModulo.Alerta; + + // -------------------------------------------------------- + // 8.1) Mandatório não operacional + // -------------------------------------------------------- - // Caso 1: módulo mandatório não operante if (moduloMandatorioNaoOperante) { - var statusTexto = (modulo?.status ?? StatusModulo.Desconectado).ToString(); - string motivos = string.Join("; ", modulo?.motivos ?? new List() { "Desconhecido" }); + List motivos = + modulo?.motivos? + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Select(NormalizarTexto) + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Distinct() + .ToList() + ?? new List(); + + string motivoTexto = + motivos.Count > 0 + ? string.Join("; ", motivos) + : "Sem motivo detalhado informado."; eventos.Add(new MotivoTopBarModel { Prioridade = 620, Fonte = "ModuloCritico", Titulo = "MÓDULO MANDATÓRIO NÃO OPERACIONAL", - Descricao = $"{nomeModulo}: módulo mandatório em estado '{statusTexto}': {motivos}", + Descricao = + $"{nomeModulo}: módulo mandatório em estado " + + $"'{status}': {motivoTexto}", StatusVisual = StatusModulo.Falha, - Modulo = modulo?.modulo + Modulo = codigoModulo }); } - // Caso 2: condições operacionais severas + // -------------------------------------------------------- + // 8.2) Condições operacionais severas + // -------------------------------------------------------- + if (temCondicaoSevera) { - var descricoesCriticas = modulo?.condicoes_operacionais? - .Where(cond => cond?.severidade > 90) - .Select(cond => cond?.descricao ?? "") - .Where(desc => !string.IsNullOrWhiteSpace(desc)) - .Distinct() - .ToList() ?? new List(); + var descricoesCriticas = + modulo?.condicoes_operacionais? + .Where(cond => cond?.severidade > 90) + .Select(cond => + NormalizarTexto(cond?.descricao)) + .Where(desc => + !string.IsNullOrWhiteSpace(desc)) + .Distinct() + .ToList() + ?? new List(); - foreach (var desc in descricoesCriticas) + foreach (var descricao in descricoesCriticas) { eventos.Add(new MotivoTopBarModel { Prioridade = 610, Fonte = "ModuloCritico", Titulo = "CONDIÇÕES OPERACIONAIS CRÍTICAS", - Descricao = $"{nomeModulo}: {desc}", + Descricao = + $"{nomeModulo}: {descricao}", StatusVisual = StatusModulo.Alerta, - Modulo = modulo?.modulo + Modulo = codigoModulo }); } } - // Caso 3: módulo mandatório em alerta + // -------------------------------------------------------- + // 8.3) Mandatório em alerta + // -------------------------------------------------------- + if (moduloMandatorioAlerta) { - var statusTexto = (modulo?.status ?? StatusModulo.Desconectado).ToString(); - string motivos = string.Join("; ", modulo?.motivos ?? new List() { "Desconhecido" }); + List motivos = + modulo?.motivos? + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Select(NormalizarTexto) + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Distinct() + .ToList() + ?? new List(); + + string motivoTexto = + motivos.Count > 0 + ? string.Join("; ", motivos) + : "Sem motivo detalhado informado."; eventos.Add(new MotivoTopBarModel { Prioridade = 600, Fonte = "ModuloCritico", Titulo = "MÓDULO MANDATÓRIO EM ALERTA", - Descricao = $"{nomeModulo}: módulo mandatório em estado '{statusTexto}': {motivos}", + Descricao = + $"{nomeModulo}: módulo mandatório em estado " + + $"'{status}': {motivoTexto}", StatusVisual = StatusModulo.Alerta, - Modulo = modulo?.modulo + Modulo = codigoModulo }); } } + // ============================================================ + // 9) Ordenação dos módulos + // ============================================================ + var ordemModulos = new Dictionary { { T_Code.Ipb, 1 }, @@ -763,17 +991,38 @@ namespace OperationControl.ViewModels { T_Code.Sen, 7 } }; - // ============================ - // 6) Remove vazios / repetidos - // ============================ - var eventosOrdenados = eventos - .Where(x => !string.IsNullOrWhiteSpace(x.Titulo) && !string.IsNullOrWhiteSpace(x.Descricao)) - .GroupBy(x => new { x.Fonte, x.Titulo, x.Descricao, x.Prioridade, x.StatusVisual }) - .Select(g => g.First()) - .OrderByDescending(x => x.Prioridade) - .ThenBy(x => x.Modulo.HasValue && ordemModulos.ContainsKey(x.Modulo.Value)? ordemModulos[x.Modulo.Value] : int.MaxValue) - .ThenBy(x => x.Fonte) - .ToList(); + // ============================================================ + // 10) Limpeza / ordenação dos eventos + // ============================================================ + + var eventosOrdenados = + eventos + .Where(x => + !string.IsNullOrWhiteSpace(x.Titulo) && + !string.IsNullOrWhiteSpace(x.Descricao)) + .GroupBy(x => new + { + x.Fonte, + x.Titulo, + x.Descricao, + x.Prioridade, + x.StatusVisual + }) + .Select(g => g.First()) + .OrderByDescending(x => x.Prioridade) + .ThenBy(x => + x.Modulo.HasValue && + ordemModulos.TryGetValue( + x.Modulo.Value, + out var ordem) + ? ordem + : int.MaxValue) + .ThenBy(x => x.Fonte) + .ToList(); + + // ============================================================ + // 11) Estado normal + // ============================================================ if (!eventosOrdenados.Any()) { @@ -782,96 +1031,163 @@ namespace OperationControl.ViewModels Prioridade = 0, Fonte = "Normal", Titulo = "ROVER OPERANDO NORMALMENTE", - Descricao = "Trajetória, controle e operação em condição normal.", - StatusVisual = obj.StatusRover == StatusModulo.Desconectado - ? StatusModulo.Desconectado - : StatusModulo.Operante + Descricao = + "Trajetória, controle e operação em condição normal.", + StatusVisual = + obj.StatusRover == StatusModulo.Desconectado + ? StatusModulo.Desconectado + : StatusModulo.Operante }); } - // ============================ - // 7) Monta visual final - // ============================ - StatusModulo statusVisual; - string linha1; + // ============================================================ + // 12) Montagem visual + // ============================================================ + + var principal = + eventosOrdenados.First(); + + StatusModulo statusVisual = + principal.StatusVisual; + + string linha1 = + principal.Titulo; + string linha2; - string badge; - string resumo; - if (eventosOrdenados.Any()) + // Se perdeu comunicação, não exibimos diagnóstico antigo + // recebido antes da queda junto com a perda de comunicação. + if (principal.Fonte == "Comunicacao") { - var principal = eventosOrdenados.First(); - - statusVisual = principal.StatusVisual; - linha1 = principal.Titulo; - - // Pega até 3 descrições distintas, por prioridade - var descricoes = eventosOrdenados - .Select(x => x.Descricao?.Trim()) - .Where(x => !string.IsNullOrWhiteSpace(x)) - .Distinct() - .Take(3) - .ToList(); - - linha2 = string.Join(" • ", descricoes); - - // Badge curto - badge = principal.Fonte switch - { - "Comunicacao" => "DESCONECTADO", - "Emergencia" => "EMERGÊNCIA", - "Pausa" => "PAUSADO", - "Trajetoria" => "TRAJETÓRIA", - "Controle" => "CONTROLE", - "ModuloCritico" => "ALERTA", - _ => statusVisual.ToString().ToUpper() - }; - - // Resumo curto - resumo = principal.Fonte switch - { - "Comunicacao" => "Sem telemetria", - "Emergencia" => "Parada imediata", - "Pausa" => "Aguardando retomada", - "Trajetoria" => "Operação bloqueada", - "Controle" => "Controle bloqueado", - "ModuloCritico" => "Atenção operacional", - _ => (obj.Operacao?.Status ?? StatusOperacao.NaoIniciado).ToString() - }; + linha2 = principal.Descricao; } else { - statusVisual = obj.StatusRover == StatusModulo.Desconectado - ? StatusModulo.Desconectado - : StatusModulo.Operante; + var descricoes = + eventosOrdenados + .Select(x => x.Descricao?.Trim()) + .Where(x => !string.IsNullOrWhiteSpace(x)) + .Distinct() + .Take(3) + .ToList(); - linha1 = "ROVER OPERANDO NORMALMENTE"; - linha2 = "Trajetória, controle e operação em condição normal."; - badge = statusVisual.ToString().ToUpper(); - resumo = (obj.Operacao?.Status ?? StatusOperacao.NaoIniciado).ToString(); + linha2 = + string.Join(" • ", descricoes); } - _viewOperacaoCenter?.Historico?._vm?.CompararERegistrarMudancasEventos(rover.RoverId, eventosOrdenados); + string badge = + principal.Fonte switch + { + "Comunicacao" => "DESCONECTADO", + "NCP" => "DESCONECTADO", + "Emergencia" => "EMERGÊNCIA", + "Pausa" => "PAUSADO", + "Operacao" => "BLOQUEADO", + "Controle" => "CONTROLE", + "Trajetoria" => "TRAJETÓRIA", + "StatusOperacao" + => obj.Operacao?.Status + .ToString() + .ToUpper() ?? "OPERAÇÃO", + "ModuloCritico" => "ALERTA", + "Normal" => statusVisual + .ToString() + .ToUpper(), + _ => statusVisual + .ToString() + .ToUpper() + }; + + string resumo = + principal.Fonte switch + { + "Comunicacao" => + "Sem telemetria", + + "NCP" => + "Núcleo central desconectado", + + "Emergencia" => + "Parada imediata", + + "Pausa" => + "Aguardando retomada", + + "Operacao" => + "Aguardando liberação", + + "Controle" => + "Controle bloqueado", + + "Trajetoria" => + "Trajetória bloqueada", + + "StatusOperacao" => + NormalizarTexto( + obj.Operacao?.MotivoStatusOperacao) + ?? obj.Operacao?.Status.ToString() + ?? "Estado operacional", + + "ModuloCritico" => + "Atenção operacional", + + "Normal" => + obj.Operacao?.Status.ToString() + ?? StatusOperacao.NaoIniciado.ToString(), + + _ => + obj.Operacao?.Status.ToString() + ?? StatusOperacao.NaoIniciado.ToString() + }; + + // ============================================================ + // 13) Histórico + // ============================================================ + + _viewOperacaoCenter? + .Historico? + ._vm? + .CompararERegistrarMudancasEventos( + rover.RoverId, + eventosOrdenados); + + // ============================================================ + // 14) Segurança visual + // ============================================================ + + if (string.IsNullOrWhiteSpace(linha1)) + linha1 = "ESTADO DO ROVER"; - // Segurança extra: evita linha vazia if (string.IsNullOrWhiteSpace(linha2)) linha2 = "Sem detalhes adicionais."; - // ============================ - // 8) Atualiza UI - // ============================ - System.Windows.Application.Current.Dispatcher.BeginInvoke(new Action(() => - { - _viewOperacaoTop?._vm?.AtualizarStatus( - statusVisual, - rover.RoverId, - obj?.Operacao?.Status ?? StatusOperacao.NaoIniciado, - linha1, - linha2, - badge, - resumo - ); - })); + if (string.IsNullOrWhiteSpace(badge)) + badge = "STATUS"; + + if (string.IsNullOrWhiteSpace(resumo)) + resumo = + obj.Operacao?.Status.ToString() + ?? StatusOperacao.NaoIniciado.ToString(); + + // ============================================================ + // 15) Atualização da UI + // ============================================================ + + System.Windows.Application.Current.Dispatcher.BeginInvoke( + new Action(() => + { + _viewOperacaoTop?._vm?.AtualizarStatus( + statusVisual, + rover.RoverId, + obj.Operacao?.Status + ?? StatusOperacao.NaoIniciado, + linha1, + linha2, + badge, + resumo + ); + }) + ); } public void AtualizarBarraSuperior(StatusModulo status, string titulo, string linha1, string linha2, string status_str, string resumo)