diff --git a/AgroBase/AgroBase/AgroBase.csproj b/AgroBase/AgroBase/AgroBase.csproj index 8ede81463..ee33126dc 100644 --- a/AgroBase/AgroBase/AgroBase.csproj +++ b/AgroBase/AgroBase/AgroBase.csproj @@ -764,9 +764,7 @@ - - - + diff --git a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs index 20f3127e4..aec949648 100644 --- a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs +++ b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs @@ -54,8 +54,25 @@ namespace AgroBase.Forms new SemaphoreSlim(1, 1); private readonly object _estadoSimulacaoLock = new object(); - private readonly Queue historicoAngulosControle = - new Queue(); + + private struct ComandoDirecionalSimulacao + { + public double AnguloCanonicoDeg; + public TipoMovimentoDirecional TipoMovimento; + + public ComandoDirecionalSimulacao( + double anguloCanonicoDeg, + TipoMovimentoDirecional tipoMovimento) + { + AnguloCanonicoDeg = anguloCanonicoDeg; + TipoMovimento = tipoMovimento; + } + } + + private readonly Queue + historicoComandosDirecionais = + new Queue(); + private readonly Queue _trilhaVisual = new Queue(); @@ -88,8 +105,19 @@ namespace AgroBase.Forms private int idxPassoOperacao; private double velocidadeCarroMs; + + // O comando canônico continua existindo para compatibilidade com a IHM, + // telemetria e depuração. A física, porém, passa a ser mantida por eixo. private double _ultimoAnguloControleAplicado; private double _ultimoAnguloControleAlvoAtrasado; + private TipoMovimentoDirecional _ultimoTipoMovimentoAlvoAtrasado = + TipoMovimentoDirecional.RodasDianteiras; + + // Estado FÍSICO real da direção simulada. Estes dois valores são os que + // entram na cinemática 4WS. Em uma troca de modo, cada eixo percorre o + // próprio caminho até o novo alvo, em vez de a geometria trocar instantaneamente. + private double _anguloFisicoDianteiroAplicado; + private double _anguloFisicoTraseiroAplicado; // Modelo simples do atuador direcional: a fila existente representa o // atraso de transporte; este limite representa o tempo físico de giro. @@ -404,18 +432,60 @@ namespace AgroBase.Forms if (atualizarAnguloControle) { - _ultimoAnguloControleAlvoAtrasado = CalcularAnguloControleAtrasado( - Volatile.Read(ref _delayComandoPassos) - ); + ComandoDirecionalSimulacao comandoAtrasado = + CalcularComandoDirecionalAtrasado( + Volatile.Read(ref _delayComandoPassos) + ); + + _ultimoAnguloControleAlvoAtrasado = + comandoAtrasado.AnguloCanonicoDeg; + + _ultimoTipoMovimentoAlvoAtrasado = + comandoAtrasado.TipoMovimento; } - // Diferente da versão anterior, o fim da fila vira um alvo e - // não um salto instantâneo. A pose usa somente o ângulo físico. - anguloControle = AtualizarAnguloControleFisico( + // O comando canônico é convertido em dois alvos FÍSICOS. Cada + // eixo então avança independentemente com a velocidade real do + // atuador. Isso é essencial nas trocas de modo direcional. + double alvoDianteiroDeg; + double alvoTraseiroDeg; + + ConverterComandoParaAngulosFisicos( + _ultimoTipoMovimentoAlvoAtrasado, _ultimoAnguloControleAlvoAtrasado, - tempoPassadoSegundos + out alvoDianteiroDeg, + out alvoTraseiroDeg ); + double anguloDianteiroFisicoDeg; + double anguloTraseiroFisicoDeg; + + AtualizarAngulosFisicosDirecionais( + alvoDianteiroDeg, + alvoTraseiroDeg, + tempoPassadoSegundos, + out anguloDianteiroFisicoDeg, + out anguloTraseiroFisicoDeg + ); + + // Mantém um ângulo canônico representativo apenas para IHM, + // logs e consumidores legados. A pose não usa mais esse valor. + anguloControle = CalcularAnguloCanonicoRepresentativo( + _ultimoTipoMovimentoAlvoAtrasado, + anguloDianteiroFisicoDeg, + anguloTraseiroFisicoDeg + ); + + lock (_estadoSimulacaoLock) + _ultimoAnguloControleAplicado = anguloControle; + + // Publica o estado físico por eixo. O HealthWorker usa estes + // valores quando op.Simulando=true, exatamente como usa a + // telemetria dos MKS no rover real. + operacao.SimulacaoAnguloDianteiroFisico = anguloDianteiroFisicoDeg; + operacao.SimulacaoAnguloTraseiroFisico = anguloTraseiroFisicoDeg; + operacao.SimulacaoAngulosFisicosDirecionaisValidos = true; + if (operacao.DispMvd?.Dados != null) { operacao.DispMvd.Dados.AnguloDirecionalSimuladoCanonico = @@ -433,12 +503,9 @@ namespace AgroBase.Forms ? gps.OrientacaoReal : 0.0; - TipoMovimentoDirecional tipoMovimento = - operacao.Controle.TipoMovimento; - - novaPosicao = VariaveisEquipamento.SimularNovaPosicao( - tipoMovimento, - anguloControle, + novaPosicao = SimularNovaPosicaoComAngulosFisicos( + anguloDianteiroFisicoDeg, + anguloTraseiroFisicoDeg, velocidadeFisica, tempoPassadoSegundos, anguloAtual, @@ -468,7 +535,6 @@ namespace AgroBase.Forms { posicaoAtual = novaPosicao.Clone(); velocidadeCarroMs = velocidadeCalculada; - _ultimoAnguloControleAplicado = anguloControle; if (posicaoCorrigida != null) { @@ -1614,9 +1680,13 @@ namespace AgroBase.Forms passosOperacao = passos ?? new List(); idxPassoOperacao = 0; _trilhaVisual.Clear(); - historicoAngulosControle.Clear(); + historicoComandosDirecionais.Clear(); } + var operacao = Variaveis.OperacaoEmAndamento; + if (operacao != null) + operacao.SimulacaoAngulosFisicosDirecionaisValidos = false; + Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1); } @@ -1630,48 +1700,133 @@ namespace AgroBase.Forms { AtualizarConfiguracaoSimulacaoDaTela(); - double angulo = CalcularAnguloControleAtrasado( - Volatile.Read(ref _delayComandoPassos) + ComandoDirecionalSimulacao comando = + CalcularComandoDirecionalAtrasado( + Volatile.Read(ref _delayComandoPassos) + ); + + double alvoDianteiroDeg; + double alvoTraseiroDeg; + + ConverterComandoParaAngulosFisicos( + comando.TipoMovimento, + comando.AnguloCanonicoDeg, + out alvoDianteiroDeg, + out alvoTraseiroDeg ); lock (_estadoSimulacaoLock) { - _ultimoAnguloControleAlvoAtrasado = angulo; - _ultimoAnguloControleAplicado = angulo; + _ultimoAnguloControleAlvoAtrasado = + comando.AnguloCanonicoDeg; + _ultimoTipoMovimentoAlvoAtrasado = + comando.TipoMovimento; + + // O botão é uma ferramenta de depuração manual. Mantém a + // semântica histórica de aplicar imediatamente o valor atual. + _anguloFisicoDianteiroAplicado = alvoDianteiroDeg; + _anguloFisicoTraseiroAplicado = alvoTraseiroDeg; + _ultimoAnguloControleAplicado = + comando.AnguloCanonicoDeg; } - txtAnguloControle.Text = angulo.ToString("0.00"); + var operacao = Variaveis.OperacaoEmAndamento; + if (operacao != null) + { + operacao.SimulacaoAnguloDianteiroFisico = alvoDianteiroDeg; + operacao.SimulacaoAnguloTraseiroFisico = alvoTraseiroDeg; + operacao.SimulacaoAngulosFisicosDirecionaisValidos = true; + } + + txtAnguloControle.Text = + comando.AnguloCanonicoDeg.ToString("0.00"); } - private double CalcularAnguloControleAtrasado(int errosConsiderar) + private ComandoDirecionalSimulacao CalcularComandoDirecionalAtrasado( + int errosConsiderar) { + var operacao = Variaveis.OperacaoEmAndamento; + double novoAngulo = - Variaveis.OperacaoEmAndamento?.SimulacaoAnguloControle ?? 0.0; + operacao?.SimulacaoAnguloControle ?? 0.0; if (!ValorFinito(novoAngulo)) novoAngulo = 0.0; + TipoMovimentoDirecional novoTipo = + operacao?.SimulacaoTipoMovimentoControle ?? + TipoMovimentoDirecional.RodasDianteiras; + + var novoComando = new ComandoDirecionalSimulacao( + novoAngulo, + novoTipo + ); + lock (_estadoSimulacaoLock) { - historicoAngulosControle.Enqueue(novoAngulo); + historicoComandosDirecionais.Enqueue(novoComando); - while (historicoAngulosControle.Count > 1 && - (historicoAngulosControle.Count > errosConsiderar || + while (historicoComandosDirecionais.Count > 1 && + (historicoComandosDirecionais.Count > errosConsiderar || errosConsiderar == 0)) { - historicoAngulosControle.Dequeue(); + historicoComandosDirecionais.Dequeue(); } - return historicoAngulosControle.Peek(); + return historicoComandosDirecionais.Peek(); } } - private double AtualizarAnguloControleFisico( - double anguloAlvo, - double dtSegundos) + private static void ConverterComandoParaAngulosFisicos( + TipoMovimentoDirecional tipoMovimento, + double anguloCanonicoDeg, + out double anguloDianteiroDeg, + out double anguloTraseiroDeg) { - if (!ValorFinito(anguloAlvo)) - anguloAlvo = 0.0; + anguloDianteiroDeg = 0.0; + anguloTraseiroDeg = 0.0; + + switch (tipoMovimento) + { + case TipoMovimentoDirecional.RodasDianteiras: + anguloDianteiroDeg = anguloCanonicoDeg; + break; + + case TipoMovimentoDirecional.RodasTraseiras: + // Convenção do projeto: comando canônico positivo produz + // yaw positivo. No eixo traseiro isso exige esterço físico oposto. + anguloTraseiroDeg = -anguloCanonicoDeg; + break; + + case TipoMovimentoDirecional.MovimentoArco: + anguloDianteiroDeg = anguloCanonicoDeg; + anguloTraseiroDeg = -anguloCanonicoDeg; + break; + + case TipoMovimentoDirecional.MovimentoDiagonal: + case TipoMovimentoDirecional.MovimentoLateral: + anguloDianteiroDeg = anguloCanonicoDeg; + anguloTraseiroDeg = anguloCanonicoDeg; + break; + + default: + anguloDianteiroDeg = anguloCanonicoDeg; + break; + } + } + + private void AtualizarAngulosFisicosDirecionais( + double alvoDianteiroDeg, + double alvoTraseiroDeg, + double dtSegundos, + out double anguloDianteiroDeg, + out double anguloTraseiroDeg) + { + if (!ValorFinito(alvoDianteiroDeg)) + alvoDianteiroDeg = 0.0; + + if (!ValorFinito(alvoTraseiroDeg)) + alvoTraseiroDeg = 0.0; double passoMaximo = VelocidadeAngularDirecionalGrausSegundo * @@ -1679,13 +1834,231 @@ namespace AgroBase.Forms lock (_estadoSimulacaoLock) { - double erro = anguloAlvo - _ultimoAnguloControleAplicado; - double passo = Math.Max(-passoMaximo, Math.Min(passoMaximo, erro)); - _ultimoAnguloControleAplicado += passo; - return _ultimoAnguloControleAplicado; + _anguloFisicoDianteiroAplicado = AproximarLinear( + _anguloFisicoDianteiroAplicado, + alvoDianteiroDeg, + passoMaximo + ); + + _anguloFisicoTraseiroAplicado = AproximarLinear( + _anguloFisicoTraseiroAplicado, + alvoTraseiroDeg, + passoMaximo + ); + + anguloDianteiroDeg = _anguloFisicoDianteiroAplicado; + anguloTraseiroDeg = _anguloFisicoTraseiroAplicado; } } + private static double AproximarLinear( + double atual, + double alvo, + double passoMaximo) + { + if (passoMaximo <= 0.0) + return atual; + + double erro = alvo - atual; + double passo = Math.Max( + -passoMaximo, + Math.Min(passoMaximo, erro) + ); + + return atual + passo; + } + + private static double CalcularAnguloCanonicoRepresentativo( + TipoMovimentoDirecional tipoMovimento, + double anguloDianteiroDeg, + double anguloTraseiroDeg) + { + switch (tipoMovimento) + { + case TipoMovimentoDirecional.RodasDianteiras: + return anguloDianteiroDeg; + + case TipoMovimentoDirecional.RodasTraseiras: + return -anguloTraseiroDeg; + + case TipoMovimentoDirecional.MovimentoArco: + return 0.5 * + (anguloDianteiroDeg - anguloTraseiroDeg); + + case TipoMovimentoDirecional.MovimentoDiagonal: + case TipoMovimentoDirecional.MovimentoLateral: + return 0.5 * + (anguloDianteiroDeg + anguloTraseiroDeg); + + default: + return anguloDianteiroDeg; + } + } + + private GPSModel SimularNovaPosicaoComAngulosFisicos( + double anguloDianteiroDeg, + double anguloTraseiroDeg, + double velocidadeMs, + double tempoDelta, + double anguloAtualDeg, + GPSModel pos) + { + if (pos == null) + return GPSService.GetSnapshot(); + + double L = Math.Max( + 0.05, + VariaveisEquipamento.DistanciaEntreEixosCm / 100.0 + ); + + double df = anguloDianteiroDeg * Math.PI / 180.0; + double dr = anguloTraseiroDeg * Math.PI / 180.0; + + double tf = Math.Tan(df); + double tr = Math.Tan(dr); + + if (Math.Abs(tf) < 1e-12) + tf = 0.0; + + if (Math.Abs(tr) < 1e-12) + tr = 0.0; + + // Cinemática 4WS no ponto médio entre eixos. Diferente do método + // legado, df/dr chegam aqui como estados FÍSICOS e podem representar + // corretamente uma configuração intermediária durante a troca de modo. + double beta = Math.Atan(0.5 * (tf + tr)); + + double kappaGeom = + Math.Cos(beta) * (tf - tr) / L; + + if (Math.Abs(kappaGeom) < 1e-12) + kappaGeom = 0.0; + + double ku = Math.Max( + 0.0, + VariaveisEquipamento.KuDirecional + ); + + double kappaEf = + kappaGeom / + (1.0 + ku * velocidadeMs * velocidadeMs); + + double omega = velocidadeMs * kappaEf; + + double theta0 = anguloAtualDeg * Math.PI / 180.0; + double dtheta = omega * tempoDelta; + double theta1 = theta0 + dtheta; + double phi0 = theta0 + beta; + + double dx; + double dy; + + if (Math.Abs(omega) <= 1e-9) + { + double distancia = velocidadeMs * tempoDelta; + dx = Math.Sin(phi0) * distancia; + dy = Math.Cos(phi0) * distancia; + } + 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.GetSnapshot() ?? pos; + + double raioTerra = GPSUtils.RaioDaTerra; + + double dLat = + (dy / raioTerra) * + 180.0 / Math.PI; + + double cosLat = + Math.Cos(pos.LatitudeAnt * Math.PI / 180.0); + + if (Math.Abs(cosLat) < 1e-9) + cosLat = cosLat >= 0.0 ? 1e-9 : -1e-9; + + double dLon = + (dx / (raioTerra * 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 orientacaoRealDeg = + GPSUtils.NormalizarAngulo( + theta1 * 180.0 / Math.PI + ); + + double orientacaoMovimentoDeg = + GPSUtils.NormalizarAngulo( + (theta1 + beta) * 180.0 / Math.PI + ); + + var timestampOri = ultimaPosicao.TimestampOri?.Clone(); + var timestampPos = ultimaPosicao.TimestampPos?.Clone(); + + GPSModel novaPosicao = new GPSModel + { + Momento = agora, + DataHora = agora, + UltimoComandoRespondido = agora, + + 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 = timestampOri, + TimestampPos = timestampPos, + }; + + var corrigida = + GPSService.LeverArm.FixLeverArmLatLon_Fast( + novaPosicao.LatitudeAnt, + novaPosicao.LongitudeAnt, + novaPosicao.OrientacaoReal, + Math.Max(1, GPSService.TaxaAmostragemHz) + ); + + novaPosicao.Latitude = corrigida.lat; + novaPosicao.Longitude = corrigida.lon; + + if (novaPosicao.TimestampOri != null) + novaPosicao.TimestampOri.valor = agoraMono; + + if (novaPosicao.TimestampPos != null) + novaPosicao.TimestampPos.valor = agoraMono; + + return novaPosicao; + } + private double ObterUltimoAnguloControle() { lock (_estadoSimulacaoLock) diff --git a/AgroBase/AgroBase/Models/MPCController.cs b/AgroBase/AgroBase/Models/MPCController.cs deleted file mode 100644 index 624bcf90a..000000000 --- a/AgroBase/AgroBase/Models/MPCController.cs +++ /dev/null @@ -1,209 +0,0 @@ -using AgroBase.Models; -using AgroBase.Services; -using Newtonsoft.Json; -using System; -using System.Collections.Generic; -using System.Diagnostics; -using System.Linq; -using System.Threading.Tasks; -using static AgroBase.Models.Enums; - -public class MPCController -{ - private string TopicoRota = "mpc/rota"; - private string TopicoPosicao = "mpc/posicao"; - private string TopicoComando = "mpc/comando"; - private double Horizonte = 4.0; - private double _dt = 0.0; - private DateTime _ultimaAtualizacao = DateTime.MinValue; - public Process pythonProcess; - public readonly object processLock = new object(); - public bool Iniciado = false; - - public MPCComandoModel ComandoAtual = new MPCComandoModel(); - - public MPCController() - { - - } - - public async Task Inicializar() - { - await Variaveis.MqttServiceLocal.AdicionarNovoTopico(TopicoRota); - await Variaveis.MqttServiceLocal.AdicionarNovoTopico(TopicoPosicao); - await Variaveis.MqttServiceLocal.AdicionarNovoTopico(TopicoComando, true, 2, async (mensagem) => - { - if (mensagem.Mensagem == "OK") - { - Iniciado = true; - } - else - { - try - { - ComandoAtual = JsonConvert.DeserializeObject(mensagem.Mensagem); - } - catch (Exception ex) - { - Console.WriteLine("Erro ao deserializar resposta do MPC: " + ex.Message.ToString()); - } - _ultimaAtualizacao = DateTime.Now; - } - }); - - lock (processLock) - { - //pythonProcess = PythonService.RunScript(PythonService.ScriptMPCController, new string[] {}); - } - - bool ScriptIniciado() => Iniciado; - await FuncoesGlobais.AguardarCondicaoAsync(ScriptIniciado); - - return Iniciado; - } - - public async Task EnviarDadosTrajetoria(List _trajetoria) - { - var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; - - var trajetoria = new - { - pontos = _trajetoria - .Select(p => new - { - lat = p.Posicao.Latitude, - lon = p.Posicao.Longitude, - tipo = (int)p.Tipo, - distanciaMargem = p.LarguraCorredor * 0.8 - }) - .ToList(), - horizonte = Horizonte, - angulo_max_graus = pControle.DirAnguloMaximo, - distancia_entre_eixos = (VariaveisEquipamento.DistanciaEntreEixosCm / 100.0), - velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeCErvasPercent), - velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeSErvasPercent) - }; - - await Variaveis.MqttServiceLocal.PublishAsync( - Variaveis.MqttServiceLocal.Topicos.First(x => x.Topico == TopicoRota), - JsonConvert.SerializeObject(trajetoria) - ); - } - - public async Task Compute(double _lat, double _long, double _theta, double _velocidadeMs) - { - var op = Variaveis.OperacaoEmAndamento; - - DateTime EnviadoEm = DateTime.Now; - - double taxaHz = GPSService.TaxaAmostragemHz; // ex: 10 Hz - double tempoIdeal = 1.0 / taxaHz; // = 0.1 s - double tempoMinimo = tempoIdeal * 0.5; // 50% abaixo - double tempoMaximo = tempoIdeal * 2.0; // até 2x acima - _dt = (EnviadoEm - _ultimaAtualizacao).TotalSeconds; - double dtCalc = Math.Max(tempoMinimo, Math.Min(tempoMaximo, _dt)); - - var _Sensoriamento = op.Sensoriamento; - - var dadosEnvio = new - { - lat = _lat, - lon = _long, - theta = _theta, - velocidade = _velocidadeMs, - dt = dtCalc, - comando_anterior = new - { - angulo = op.Controle.Angulo, - tipo = op.Controle.TipoMovimento, - }, - contexto = new - { - StatusCarro = (int)_Sensoriamento.Trajetoria.StatusCarro, - DentroCorredor = _Sensoriamento.Trajetoria.CorredorAtual.Dentro, - ManobrandoEntreRuas = _Sensoriamento.Trajetoria.ManobrandoEntreRuas - } - }; - - await Variaveis.MqttServiceLocal.PublishAsync( - Variaveis.MqttServiceLocal.Topicos.First(x => x.Topico == TopicoPosicao), - JsonConvert.SerializeObject(dadosEnvio) - ); - - bool Respondido() => _ultimaAtualizacao > EnviadoEm; - await FuncoesGlobais.AguardarCondicaoAsync(Respondido, 200, 10); - - return ComandoAtual; - } - - public MPCController Clone() - { - return new MPCController() - { - ComandoAtual = ComandoAtual?.Clone() ?? new MPCComandoModel(), - Horizonte = Horizonte, - Iniciado = Iniciado, - _ultimaAtualizacao = _ultimaAtualizacao, - _dt = _dt - } -; } - - public void Dispose() - { - lock (processLock) - { - if (pythonProcess != null && !pythonProcess.HasExited) - { - pythonProcess?.Kill(); - } - pythonProcess?.Dispose(); - pythonProcess = null; - } - - Iniciado = false; - ComandoAtual = new MPCComandoModel(); - Task.Run(async () => { - Variaveis.MqttServiceLocal.Topicos.Remove(Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == TopicoRota)); - Variaveis.MqttServiceLocal.Topicos.Remove(Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == TopicoPosicao)); - await Variaveis.MqttServiceLocal.UnsubscribeAsync(Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == TopicoComando)); - Variaveis.MqttServiceLocal.Topicos.Remove(Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == TopicoComando)); - }); - } - -} - -public class MPCComandoModel -{ - public TipoMovimentoDirecional tipo { get; set; } = TipoMovimentoDirecional.RodasDianteiras; - public double angulo { get; set; } = 0; - public List simulacao { get; set; } = new List(); - - public MPCComandoModel Clone() - { - return new MPCComandoModel() - { - tipo = tipo, - angulo = angulo, - simulacao = new List(simulacao) - }; - } -} - -public class MPCSimulacaoModel -{ - public double latitude { get; set; } - public double longitude { get; set; } - public double orientacao { get; set; } -} - -public class MPCPesosModel -{ - public double posicao { get; set; } - public double orientacao { get; set; } - public double suavidade_min { get; set; } - public double suavidade_max { get; set; } - public double fator_re { get; set; } - public double ideal { get; set; } - public double lateral_min { get; set; } - public double lateral_max { get; set; } -} diff --git a/AgroBase/AgroBase/Models/MPCControllerAprimorado.cs b/AgroBase/AgroBase/Models/MPCControllerAprimorado.cs deleted file mode 100644 index eb1e042f2..000000000 --- a/AgroBase/AgroBase/Models/MPCControllerAprimorado.cs +++ /dev/null @@ -1,178 +0,0 @@ -using AgroBase.Models; -using AgroBase.Services; -using System; -using System.Collections.Generic; -using System.Linq; - -public class MPCControllerAprimorado -{ - private double distanciaPrevisao = 2.0; - private int horizonte; - private double maxAnguloDirecao; - private double maxAngulo4Rodas; - private double velocidadeMin; - private double velocidadeMax; - - private double pesoOrientacao = 2.5; - private double pesoLateral = 1.5; - private double pesoDistancia = 0.8; - private double pesoSuavidade = 0.5; - private double limiteErroOrientacao = 45.0; - private double limiteErroLateral = 2.5; // metros - - private DateTime ultimaAtualizacao = DateTime.MinValue; - - public MPCControllerAprimorado() - { - var op = Variaveis.OperacaoEmAndamento; - - this.maxAnguloDirecao = op.Parametros?.Controle?.DirAnguloMaximo ?? 30.0; - this.maxAngulo4Rodas = this.maxAnguloDirecao * 0.8; - this.velocidadeMin = op.Parametros?.Controle?.MovVelocidadeCErvasPercent ?? 20.0; - this.velocidadeMax = op.Parametros?.Controle?.MovVelocidadeSErvasPercent ?? 100.0; - } - - public (double anguloControle, Enums.TipoMovimentoDirecional modo) CalcularControle(GPSModel posicalAtual, double velocidadeAtual, List proximaTrajetoria, Enums.TipoMovimentoDirecional[] modosPermitidos) - { - if (proximaTrajetoria == null || proximaTrajetoria.Count < 2) - return (0, Enums.TipoMovimentoDirecional.RodasDianteiras); - - if (proximaTrajetoria.Count > 3) - { - proximaTrajetoria = new List(proximaTrajetoria.Take(3)); - } - - if (ultimaAtualizacao == DateTime.MinValue) - { - ultimaAtualizacao = DateTime.Now.AddMilliseconds(-(1.0 / GPSService.TaxaAmostragemHz)); - } - - double dt = (DateTime.Now - ultimaAtualizacao).TotalSeconds; - double tempoMax = (1.0 / GPSService.TaxaAmostragemHz) * 1.5; - dt = Math.Max(0.5, Math.Min(dt, tempoMax)); - - double velMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(velocidadeAtual); - double passoDist = velMs * dt; - - horizonte = Math.Min(Convert.ToInt32(distanciaPrevisao / passoDist), proximaTrajetoria.Count); - - var candidatos = GerarCandidatos(modosPermitidos); - double melhorCusto = double.MaxValue; - (double angulo, Enums.TipoMovimentoDirecional modo) melhorControle = (0, Enums.TipoMovimentoDirecional.RodasDianteiras); - - foreach (var candidato in candidatos) - { - double custo = SimularTrajetoriaComReavaliacao(posicalAtual, proximaTrajetoria, posicalAtual.AnguloCarroDefinido, candidato.angulo, candidato.modo, velMs, dt); - //Console.WriteLine($"Candidato: {candidato.angulo}, Custo: {custo.ToString()}"); - if (custo < melhorCusto) - { - melhorCusto = custo; - melhorControle = candidato; - } - } - - ultimaAtualizacao = DateTime.Now; - return melhorControle; - } - - private List<(double angulo, Enums.TipoMovimentoDirecional modo)> GerarCandidatos(Enums.TipoMovimentoDirecional[] modos) - { - var candidatos = new List<(double, Enums.TipoMovimentoDirecional)>(); - - foreach (var modo in modos) - { - double maxAng = (modo == Enums.TipoMovimentoDirecional.MovimentoArco || modo == Enums.TipoMovimentoDirecional.MovimentoDiagonal) - ? maxAngulo4Rodas : maxAnguloDirecao; - - for (double ang = -maxAng; ang <= maxAng; ang += 5.0) - { - candidatos.Add((ang, modo)); - } - } - - return candidatos; - } - - private double SimularTrajetoriaComReavaliacao(GPSModel posicaoInicial, List trajeto, double anguloInicial, double anguloControleInicial, Enums.TipoMovimentoDirecional modoInicial, double velocidadeMs, double dt) - { - var op = Variaveis.OperacaoEmAndamento; - - double custoTotal = 0; - GPSModel posAtual = posicaoInicial; - double anguloAtual = anguloInicial; - double anguloControle = anguloControleInicial; - Enums.TipoMovimentoDirecional modo = modoInicial; - - posAtual = VariaveisEquipamento.SimularNovaPosicao(modoInicial, anguloControleInicial, velocidadeMs, dt, anguloAtual, posAtual); - anguloAtual = posAtual.OrientacaoReal; - - for (int i = 0; i < horizonte; i++) - { - double melhorCustoIteracao = double.MaxValue; - - foreach (var candidato in GerarCandidatos(new[] { modo })) - { - var novaPos = VariaveisEquipamento.SimularNovaPosicao(candidato.modo, candidato.angulo, velocidadeMs, dt, anguloAtual, posAtual); - var pontoAlvo = trajeto[Math.Min(i + 1, trajeto.Count - 1)]; - - double orientacaoAlvo = GPSUtils.CalcularOrientacao(posAtual, pontoAlvo); - - double d0 = GPSUtils.DistanciaEntrePontos(posAtual, novaPos); - double a0 = GPSUtils.CalcularOrientacao(posAtual, novaPos); - - double d1 = GPSUtils.DistanciaEntrePontos(posAtual, pontoAlvo); - double a1 = GPSUtils.CalcularOrientacao(posAtual, pontoAlvo); - - double d2 = GPSUtils.DistanciaEntrePontos(novaPos, pontoAlvo); - double a2 = GPSUtils.CalcularOrientacao(novaPos, pontoAlvo); - - double erroDist = d2 - d1; - double erroOri = Math.Abs(GPSUtils.CalcularDiferencaAngulo(novaPos.OrientacaoReal, orientacaoAlvo)); - - (GPSModel ponto, int idx) = GPSUtils.PontoMaisProximoTrechoComIndice(posAtual, trajeto); - int idxNovoPonto = op.Trajetoria.PontoAtual.idxPonto + idx; - PontoTrajetoriaModel pontoAtual = op.Trajetoria._TrajetoriaFixa[idxNovoPonto].Clone(); - - int idxRuaEsquerda = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaEsquerda; - double distEsq = op.Trajetoria.CalcularDistanciaLateral(true, posAtual, idxRuaEsquerda, pontoAtual.Posicao); - - int idxRuaDireita = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaDireita; - double distDir = op.Trajetoria.CalcularDistanciaLateral(false, posAtual, idxRuaDireita, pontoAtual.Posicao); - - double erroLat = Math.Abs(distEsq - distDir); - - double deltaAngulo = Math.Abs(GPSUtils.CalcularDiferencaAngulo(novaPos.OrientacaoReal, anguloAtual)); - - //Console.WriteLine($"Angulo Inicial: {anguloControleInicial}, Passo: {i}, Candidato: {candidato.angulo}, Erro lateral: {erroLat}, Erro orientacao: {erroOri}"); - - if (false && (erroOri > limiteErroOrientacao || erroLat > limiteErroLateral)) - { - continue; // Ignora este candidato - } - - double custo = (erroOri * pesoOrientacao) + (erroLat * pesoLateral) + (erroDist * pesoDistancia) + (deltaAngulo * pesoSuavidade); - - if (custo < melhorCustoIteracao) - { - melhorCustoIteracao = custo; - posAtual = novaPos; - anguloAtual = novaPos.OrientacaoReal; - anguloControle = candidato.angulo; - modo = candidato.modo; - - //Console.WriteLine($"Angulo Inicial: {anguloControleInicial}, Passo: {i}, Candidato: {candidato.angulo}, Custo: {custo}, Erro lateral: {erroLat}, Erro orientacao: {erroOri}, Erro distancia: {erroDist}"); - } - } - - custoTotal += melhorCustoIteracao; - - if (melhorCustoIteracao == double.MaxValue) - { - break; - } - } - - return custoTotal; - } - -} diff --git a/AgroBase/AgroBase/Models/MPCControllerSimple.cs b/AgroBase/AgroBase/Models/MPCControllerSimple.cs deleted file mode 100644 index fea74f6e2..000000000 --- a/AgroBase/AgroBase/Models/MPCControllerSimple.cs +++ /dev/null @@ -1,167 +0,0 @@ -using AgroBase.Models; -using AgroBase.Services; -using System; -using System.Collections.Generic; - -public class MPCControllerSimple -{ - private int horizon; - private double maxSteeringAngle; - private double maxSteeringAngleFourWheels; - private double minVelocity; - private double maxVelocity; - private double accelerationLimit; - - // Pesos da função de custo ajustados - private double distancePredict = 5.0; - private double weightPosition = 1.0; - private double weightAlignment = 0.8; - private double weightSteering = 0.5; - private double weightVelocity = 0.3; - private double weightAcceleration = 1.0; // Penaliza mudanças bruscas de velocidade - private double weightObstacle = 2.5; // Maior penalização para obstáculos - private double weightLaneCentering = 1.2; // Mantém o robô no centro do corredor - - private DateTime _ultimaAtualizacao = DateTime.MinValue; - - public MPCControllerSimple() - { - // Definições baseadas em variáveis globais - this.horizon = Convert.ToInt32(distancePredict / TrajetoriaMapaOperacaoModel.DistanciaEntrePontos); - this.maxSteeringAngle = Variaveis.OperacaoEmAndamento.Parametros.Controle?.DirAnguloMaximo ?? 30.0; - this.maxSteeringAngleFourWheels = this.maxSteeringAngle * 1.0; - this.minVelocity = Variaveis.OperacaoEmAndamento.Parametros.Controle?.MovVelocidadeCErvasPercent ?? 20.0; - this.maxVelocity = Variaveis.OperacaoEmAndamento.Parametros.Controle?.MovVelocidadeSErvasPercent ?? 100.0; - this.accelerationLimit = 1.5; // Limita aceleração a valores realistas - } - - public (double steeringAngle, Enums.TipoMovimentoDirecional mode) ComputeControl( - double currentX, double currentY, double currentTheta, - double targetX, double targetY, double targetTheta, - double currentVelocity, double leftEdge, double rightEdge, List obstacles) - { - if (_ultimaAtualizacao == DateTime.MinValue) - { - _ultimaAtualizacao = DateTime.Now.AddMilliseconds(-(1.0 / GPSService.TaxaAmostragemHz)); - } - - double dt = (DateTime.Now - _ultimaAtualizacao).TotalSeconds; - double maxTime = (1.0 / GPSService.TaxaAmostragemHz) * 1.5; - dt = Math.Max(0.01, Math.Min(dt, maxTime)); - - List<(double steering, Enums.TipoMovimentoDirecional mode)> candidates = GenerateControlCandidates(); - double bestCost = double.MaxValue; - (double steeringAngle, Enums.TipoMovimentoDirecional mode) bestControl = (0, Enums.TipoMovimentoDirecional.RodasDianteiras); - bool insideStreet = Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria.CorredorAtual.Dentro; - - double velocity = FuncoesMatematicas.CalculaVelocidadeMsPercentual(currentVelocity); - double distanciaPasso = dt * velocity; - - this.horizon = Convert.ToInt32(distancePredict / distanciaPasso); - - foreach (var control in candidates) - { - double cost = SimulateTrajectory( - currentX, currentY, currentTheta, - targetX, targetY, targetTheta, - velocity, control.steering, control.mode, - leftEdge, rightEdge, obstacles, dt, insideStreet - ); - - if (cost < bestCost) - { - bestCost = cost; - bestControl = control; - } - } - - _ultimaAtualizacao = DateTime.Now; - return bestControl; - } - - private List<(double steering, Enums.TipoMovimentoDirecional mode)> GenerateControlCandidates() - { - List<(double steering, Enums.TipoMovimentoDirecional mode)> candidates = new List<(double, Enums.TipoMovimentoDirecional)>(); - - List modes = new List() - { - Enums.TipoMovimentoDirecional.RodasDianteiras, - Enums.TipoMovimentoDirecional.RodasTraseiras, - Enums.TipoMovimentoDirecional.MovimentoArco, - }; - - foreach (var mode in modes) - { - double maxAngle = (mode == Enums.TipoMovimentoDirecional.MovimentoArco || mode == Enums.TipoMovimentoDirecional.MovimentoDiagonal) - ? maxSteeringAngleFourWheels - : maxSteeringAngle; - - for (double s = -maxAngle; s <= maxAngle; s += 0.5) - { - candidates.Add((s, mode)); - } - } - - return candidates; - } - - private double SimulateTrajectory( - double lat, double lon, double theta, - double targetLat, double targetLon, double targetTheta, - double currentVelocity, double steering, - Enums.TipoMovimentoDirecional mode, double leftEdge, double rightEdge, - List obstacles, double dt, bool insideStreet) - { - double cost = 0; - - // Converte a posição atual e o alvo para coordenadas métricas - var posicaoAtual = new GPSModel { Latitude = lat, Longitude = lon, OrientacaoReal = theta }; - var posicaoAlvo = new GPSModel { Latitude = targetLat, Longitude = targetLon }; - - // Converte para coordenadas métricas usando o método mais preciso - (double targetX, double targetY) = GPSUtils.ConverterLatLongParaMetros(posicaoAlvo, posicaoAtual); - - double lastSteering = steering; // Para suavizar mudanças bruscas de direção - - // Simula a trajetória ao longo do horizonte de previsão - for (int i = 0; i < horizon; i++) - { - // Simula a próxima posição usando o método SimularNovaPosicao - var novaPosicao = VariaveisEquipamento.SimularNovaPosicao(mode, steering, currentVelocity, dt, theta, posicaoAtual); - - // Atualiza a posição atual e a orientação - posicaoAtual = novaPosicao; - theta = novaPosicao.OrientacaoReal; - - // Converte a nova posição para coordenadas métricas - (double novaX, double novaY) = GPSUtils.ConverterLatLongParaMetros(novaPosicao, posicaoAlvo); - - // Calcula o erro de posição e alinhamento - // Calcula apenas o erro de posição - double distanceError = Math.Sqrt(Math.Pow(targetX - novaX, 2) + Math.Pow(targetY - novaY, 2)); - - // Calcula o erro lateral (distância da esquerda e direita devem ser iguais) - double laneError = insideStreet ? Math.Abs(leftEdge - rightEdge) : 0.0; - - // Penaliza mudanças muito bruscas na direção (para suavizar a oscilação) - double steeringChangePenalty = Math.Pow(Math.Abs(steering - lastSteering), 2) * 10.0; - - // Atualiza o último valor de steering - lastSteering = steering; - - double centralLaneError = Math.Abs((leftEdge + rightEdge) / 2.0 - novaX); - - // Função de custo ajustada - cost += - (distanceError * 10.0) + - (laneError * 20.0) + - (steeringChangePenalty * 15.0) + - (centralLaneError * 5.0); - } - - return cost; - } - - - -} diff --git a/AgroBase/AgroBase/Models/MPCModel.cs b/AgroBase/AgroBase/Models/MPCModel.cs new file mode 100644 index 000000000..dec626ca0 --- /dev/null +++ b/AgroBase/AgroBase/Models/MPCModel.cs @@ -0,0 +1,41 @@ +using System.Collections.Generic; +using static AgroBase.Models.Enums; + +namespace AgroBase.Models +{ + public class MPCComandoModel + { + public TipoMovimentoDirecional tipo { get; set; } = TipoMovimentoDirecional.RodasDianteiras; + public double angulo { get; set; } = 0; + public List simulacao { get; set; } = new List(); + + public MPCComandoModel Clone() + { + return new MPCComandoModel() + { + tipo = tipo, + angulo = angulo, + simulacao = new List(simulacao) + }; + } + } + + public class MPCSimulacaoModel + { + public double latitude { get; set; } + public double longitude { get; set; } + public double orientacao { get; set; } + } + + public class MPCPesosModel + { + public double posicao { get; set; } + public double orientacao { get; set; } + public double suavidade_min { get; set; } + public double suavidade_max { get; set; } + public double fator_re { get; set; } + public double ideal { get; set; } + public double lateral_min { get; set; } + public double lateral_max { get; set; } + } +} diff --git a/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs b/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs index b2994b508..2604f74ec 100644 --- a/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs +++ b/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs @@ -488,6 +488,7 @@ namespace AgroBase.Models.Modules if (Comandar) { op.SimulacaoAnguloControle = op.Controle.Angulo; + op.SimulacaoTipoMovimentoControle = op.Controle.TipoMovimento; //Console.WriteLine($"[{Mod_ID}] Angulo Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle}"); } diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index 0f5a75062..161e5840d 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -339,6 +339,14 @@ namespace AgroBase.Models public bool Treinando { get; set; } = false; public bool Simulando { get; set; } = false; public double SimulacaoAnguloControle { get; set; } = 0; + public TipoMovimentoDirecional SimulacaoTipoMovimentoControle { get; set; } = TipoMovimentoDirecional.RodasDianteiras; + + // Estado físico 4WS publicado pelo simulador para que o MPC comece + // exatamente da mesma geometria que a planta virtual está executando. + public double SimulacaoAnguloDianteiroFisico { get; set; } = 0; + public double SimulacaoAnguloTraseiroFisico { get; set; } = 0; + public bool SimulacaoAngulosFisicosDirecionaisValidos { get; set; } = false; + public double SimulacaoRpmControle { get; set; } = 0; public int TempoIniciarOperacao { get; set; } = 10; @@ -1585,6 +1593,12 @@ namespace AgroBase.Models this.ReiniciarOperacao(false); + SimulacaoAnguloControle = 0; + SimulacaoTipoMovimentoControle = TipoMovimentoDirecional.RodasDianteiras; + SimulacaoAnguloDianteiroFisico = 0; + SimulacaoAnguloTraseiroFisico = 0; + SimulacaoAngulosFisicosDirecionaisValidos = false; + //var Topico = Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == MapasVariaveisModel.TopicoSelecaoRuasMapa); //Topico.Mensagens.Add(new MqttService.MqttTopicosMensagensModel() //{ diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index 973e5aaa1..9b328537d 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -151,6 +151,94 @@ namespace AgroBase.Services.Operadores } } + private static ( + double dianteiro, + double traseiro, + int rodasDianteirasValidas, + int rodasTraseirasValidas, + bool valido, + string fonte + ) ObterAngulosFisicosDirecionais(OperacaoModel op) + { + if (op == null) + return (0.0, 0.0, 0, 0, false, "indisponivel"); + + // No simulador, a autoridade é o estado físico integrado por eixo. + if (op.Simulando) + { + if (op.SimulacaoAngulosFisicosDirecionaisValidos) + { + double df = op.SimulacaoAnguloDianteiroFisico; + double dr = op.SimulacaoAnguloTraseiroFisico; + + bool finitos = + !double.IsNaN(df) && !double.IsInfinity(df) && + !double.IsNaN(dr) && !double.IsInfinity(dr); + + if (finitos) + return (df, dr, 2, 2, true, "simulador"); + } + + // Nunca mistura hardware real/stale com uma operação simulada. + // O Python reconstruirá df/dr pelo estado canônico se necessário. + return (0.0, 0.0, 0, 0, false, "simulador_fallback_canonico"); + } + + // Rover real: EF/ET têm sinal físico espelhado em relação a DF/DT. + // Normalizamos todos para a convenção cinemática do MPC: + // df > 0 e dr > 0 = esterço físico positivo do respectivo eixo. + var dianteiros = new List(); + var traseiros = new List(); + + var modulos = op.DispMvd?.Dados?.Modulos; + if (modulos != null) + { + foreach (var modulo in modulos) + { + var dir = modulo?.DirMotor; + if (dir == null || !dir.Inicializado) + continue; + + double leitura = dir.AnguloLeitura; + if (double.IsNaN(leitura) || double.IsInfinity(leitura)) + continue; + + string id = (modulo.Modulo_ID ?? dir.Mod_ID ?? "") + .Trim() + .ToUpperInvariant(); + + switch (id) + { + case "EF": + dianteiros.Add(-leitura); + break; + case "DF": + dianteiros.Add(leitura); + break; + case "ET": + traseiros.Add(-leitura); + break; + case "DT": + traseiros.Add(leitura); + break; + } + } + } + + bool valido = dianteiros.Count > 0 && traseiros.Count > 0; + double dianteiro = dianteiros.Count > 0 ? dianteiros.Average() : 0.0; + double traseiro = traseiros.Count > 0 ? traseiros.Average() : 0.0; + + return ( + dianteiro, + traseiro, + dianteiros.Count, + traseiros.Count, + valido, + valido ? "telemetria_mks" : "fallback_canonico" + ); + } + public static void AtualizarDadosContexto() { var op = Variaveis.OperacaoEmAndamento; @@ -200,6 +288,8 @@ namespace AgroBase.Services.Operadores op.Simulando ); + var estadoEixosDirecionais = ObterAngulosFisicosDirecionais(op); + var bombaLinha = op?.DispAtu?.Dados?.BombaPressurizadora; double pressaoAlvo = bombaLinha?._PressaoSPControle > 0 ? bombaLinha._PressaoSPControle : op.Parametros?.Controle?.AtuPressaoLinha ?? 0; double pressaoLinha = bombaLinha?._PressaoAtual ?? _Sensoriamento?.Atuador?.PressaoLinha ?? 0; @@ -265,6 +355,16 @@ namespace AgroBase.Services.Operadores angulo_comando = estadoDirecional.AnguloComando, angulo_sp = estadoDirecional.AnguloSP, angulo_fisico = estadoDirecional.AnguloFisico, + + // Estado físico 4WS por eixo. É a fonte de verdade do + // MPC durante transições de Dianteira/Traseira/Arco/Diagonal. + angulo_fisico_dianteiro = estadoEixosDirecionais.dianteiro, + angulo_fisico_traseiro = estadoEixosDirecionais.traseiro, + rodas_dianteiras_validas = estadoEixosDirecionais.rodasDianteirasValidas, + rodas_traseiras_validas = estadoEixosDirecionais.rodasTraseirasValidas, + eixos_fisicos_validos = estadoEixosDirecionais.valido, + fonte_eixos_fisicos = estadoEixosDirecionais.fonte, + dispersao = estadoDirecional.Dispersao, rodas_validas = estadoDirecional.RodasValidas, valido = estadoDirecional.Valido, diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py index 657420ab1..7ea469776 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py @@ -143,7 +143,7 @@ def _comando_mapa_gps_mpc(contexto: dict): parada_necessaria=True, ) - comando_anterior = _ler_comando_anterior_para_mpc(contexto) + comando_anterior = _ler_comando_anterior_para_mpc() inicio = time.perf_counter() comando = mpc.compute_receding(contexto, comando_anterior) @@ -250,12 +250,6 @@ def _normalizar_comando_mpc(comando: dict, contexto: dict): comando.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value) ) - parada_necessaria = _bool(comando.get("parada_necessaria", False)) - if parada_necessaria: - # A direção deve ficar neutra no mesmo ciclo em que o MPC pede stop. - angulo = 0.0 - tipo = TipoMovimentoDirecional.RodasDianteiras.value - debug_custo = comando.get("debug_custo", {}) if not isinstance(debug_custo, dict): debug_custo = {} @@ -267,7 +261,7 @@ def _normalizar_comando_mpc(comando: dict, contexto: dict): return { "comando_definido": True, "enviar_comando": _bool(comando.get("enviar_comando", True)), - "parada_necessaria": parada_necessaria, + "parada_necessaria": _bool(comando.get("parada_necessaria", False)), "erro": _bool(comando.get("erro", False)), "latencia": _float(comando.get("latencia", 0.0), 0.0), "angulo": round(angulo, 2), @@ -515,35 +509,42 @@ def _montar_contexto_direcional(*, operacao, controle, contexto_global, equipame ), }, "Direcional": { - "TipoMovimento": _int( + "TipoMovimento": _normalizar_tipo_movimento( direcional_fisico.get( "tipo_movimento", controle.get( - "tipo_movimento_direcional", - TipoMovimentoDirecional.RodasDianteiras.value, + "tipo_movimento_direcional_ant", + controle.get( + "tipo_movimento_direcional", + TipoMovimentoDirecional.RodasDianteiras.value, + ), ), - ), - TipoMovimentoDirecional.RodasDianteiras.value, + ) ), - "AnguloComando": _float( - direcional_fisico.get("angulo_comando", controle.get("angulo_sp", 0.0)), - 0.0, + "AnguloComando": _float(direcional_fisico.get("angulo_comando", 0.0), 0.0), + "AnguloSP": _float(direcional_fisico.get("angulo_sp", 0.0), 0.0), + "AnguloFisico": _float(direcional_fisico.get("angulo_fisico", 0.0), 0.0), + "AnguloFisicoDianteiro": _float( + direcional_fisico.get("angulo_fisico_dianteiro", 0.0), 0.0 ), - "AnguloSP": _float( - direcional_fisico.get("angulo_sp", controle.get("angulo_sp", 0.0)), - 0.0, + "AnguloFisicoTraseiro": _float( + direcional_fisico.get("angulo_fisico_traseiro", 0.0), 0.0 ), - "AnguloFisico": _float( - direcional_fisico.get("angulo_fisico", controle.get("angulo_sp_ant", 0.0)), - 0.0, + "EixosFisicosValidos": _bool( + direcional_fisico.get("eixos_fisicos_validos", False) + ), + "RodasDianteirasValidas": _int( + direcional_fisico.get("rodas_dianteiras_validas", 0), 0 + ), + "RodasTraseirasValidas": _int( + direcional_fisico.get("rodas_traseiras_validas", 0), 0 ), - "Dispersao": _float(direcional_fisico.get("dispersao", 0.0), 0.0), - "RodasValidas": _int(direcional_fisico.get("rodas_validas", 0), 0), - "Valido": _bool(direcional_fisico.get("valido", False)), "EmTransicao": _bool(direcional_fisico.get("em_transicao", False)), - "TimestampUnixMs": _float( - direcional_fisico.get("timestamp_unix_ms", 0.0), - 0.0, + "FonteEixosFisicos": str( + direcional_fisico.get("fonte_eixos_fisicos", "indisponivel") + ), + "TimestampUnixMs": _int( + direcional_fisico.get("timestamp_unix_ms", 0), 0 ), }, "VisualWorker": visual_worker, @@ -551,7 +552,6 @@ def _montar_contexto_direcional(*, operacao, controle, contexto_global, equipame "Equipamento": { "largura": _float(equipamento.get("largura", 0.85), 0.85), "entre_eixos": _float(equipamento.get("distancia_entre_eixos", 0.92), 0.92), - "ku_direcional": _float(equipamento.get("ku_direcional", 2.65), 2.65), "imu_roll_direita_sinal": _float(equipamento.get("imu_roll_direita_sinal", 1.0), 1.0), "dir_angulo_direita_sinal": _float(equipamento.get("dir_angulo_direita_sinal", 1.0), 1.0), "imu_dir_roll_min_aux": _float(equipamento.get("imu_dir_roll_min_aux", 6.0), 6.0), @@ -1123,25 +1123,6 @@ def _montar_comando_retorno(comando, latencia=-1.0): comando["erro_lateral"] = _float(comando.get("erro_lateral", 0.0), 0.0) comando["erro_orientacao"] = _float(comando.get("erro_orientacao", 0.0), 0.0) - # V17 SAFETY: - # "parada_necessaria" é um contrato forte. Nenhum chamador pode receber - # esse flag junto com um esterçamento residual e continuar aplicando-o. - if comando["parada_necessaria"]: - comando["enviar_comando"] = True - comando["angulo"] = 0.0 - comando["tipo"] = TipoMovimentoDirecional.RodasDianteiras.value - comando["simulacao"] = [] - - debug_stop = comando.get("debug_custo", {}) - if not isinstance(debug_stop, dict): - debug_stop = {} - - debug_stop["parada_necessaria"] = { - "ativo": True, - "acao_direcional": "angulo_zero_rodas_dianteiras", - } - comando["debug_custo"] = debug_stop - if not isinstance(comando.get("simulacao", []), list): comando["simulacao"] = [] @@ -1158,53 +1139,59 @@ def _montar_comando_retorno(comando, latencia=-1.0): return comando -def _ler_comando_anterior_para_mpc(contexto=None): +def _ler_comando_anterior_para_mpc(): controle = _dict(ContextoGlobalRedis.get_controle()) - direcional = _dict(_dict(contexto).get("Direcional", {})) + contexto_global = _dict(ContextoGlobalRedis.get_contexto()) + direcional = _dict(contexto_global.get("Direcional", {})) - telemetria_valida = _bool(direcional.get("Valido", False)) - timestamp_ms = _float(direcional.get("TimestampUnixMs", 0.0), 0.0) - idade_ms = max(0.0, time.time() * 1000.0 - timestamp_ms) if timestamp_ms > 0 else 1e9 - telemetria_valida = telemetria_valida and idade_ms <= 1500.0 - - comando = { - "angulo": _float( - controle.get("angulo_sp_ant", controle.get("angulo_sp", 0.0)), - 0.0, - ), - "tipo": _normalizar_tipo_movimento( + angulo = _float( + controle.get("angulo_sp_ant", controle.get("angulo_sp", 0.0)), + 0.0, + ) + tipo = _normalizar_tipo_movimento( + controle.get( + "tipo_movimento_direcional_ant", controle.get( - "tipo_movimento_direcional_ant", - controle.get( - "tipo_movimento_direcional", - TipoMovimentoDirecional.RodasDianteiras.value, - ), - ) - ), + "tipo_movimento_direcional", + TipoMovimentoDirecional.RodasDianteiras.value, + ), + ) + ) + + return { + "angulo": angulo, + "tipo": tipo, "velocidade": _float( controle.get("velocidade_sp_ant", controle.get("velocidade_sp", 0.0)), 0.0, ), - } - comando["angulo_aplicado"] = ( - _float(direcional.get("AnguloFisico", comando["angulo"]), comando["angulo"]) - if telemetria_valida else comando["angulo"] - ) - comando["angulo_sp_aplicado"] = ( - _float(direcional.get("AnguloSP", comando["angulo"]), comando["angulo"]) - if telemetria_valida else comando["angulo"] - ) - comando["tipo_aplicado"] = ( - _normalizar_tipo_movimento(direcional.get("TipoMovimento", comando["tipo"])) - if telemetria_valida else comando["tipo"] - ) - comando["telemetria_direcional_valida"] = telemetria_valida - comando["telemetria_direcional_idade_ms"] = idade_ms - comando["telemetria_direcional_dispersao"] = _float( - direcional.get("Dispersao", 0.0), 0.0 - ) - return comando + # Estado realmente aplicado. Mantemos os campos canônicos para + # compatibilidade e acrescentamos df/dr físicos como autoridade. + # O SP/tipo aplicado vêm do ControleAnterior, atualizado somente quando + # o scheduler C# realmente libera o comando. O bloco Direcional abaixo + # fornece a POSIÇÃO física medida, não substitui a autoridade do scheduler. + "angulo_sp_aplicado": angulo, + "tipo_aplicado": tipo, + "angulo_aplicado": _float( + direcional.get("angulo_fisico", angulo), angulo + ), + "angulo_dianteiro_aplicado": _float( + direcional.get("angulo_fisico_dianteiro", 0.0), 0.0 + ), + "angulo_traseiro_aplicado": _float( + direcional.get("angulo_fisico_traseiro", 0.0), 0.0 + ), + "eixos_fisicos_validos": _bool( + direcional.get("eixos_fisicos_validos", False) + ), + "direcional_em_transicao": _bool( + direcional.get("em_transicao", False) + ), + "fonte_eixos_fisicos": str( + direcional.get("fonte_eixos_fisicos", "indisponivel") + ), + } # ============================================================ 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 00bf46847..66911832c 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 @@ -1530,17 +1530,26 @@ class ControladorMPC: 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 = [] + df_estado, dr_estado, _ = self._estado_eixos_fisicos_rad( + contexto=contexto, + tipo_fallback=TipoMovimentoDirecional.MovimentoArco, + angulo_fallback_rad=0.0, + ) 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 + df_alvo, dr_alvo = self._angulos_eixos_4ws( + TipoMovimentoDirecional.MovimentoArco, ang ) + df_estado = self._avancar_atuador_direcional(df_estado, df_alvo, dt) + dr_estado = self._avancar_atuador_direcional(dr_estado, dr_alvo, dt) + kin = self._cinematica_4ws_eixos(df_estado, dr_estado, v) xs, ys, ths = self._nova_posicao( - xs, ys, ths, omega, v, - TipoMovimentoDirecional.MovimentoArco, ang, dt=dt + xs, ys, ths, kin["omega"], v, + TipoMovimentoDirecional.MovimentoArco, ang, dt=dt, + df_rad=df_estado, dr_rad=dr_estado, ) traj.append((xs, ys, ths)) @@ -2304,31 +2313,21 @@ class ControladorMPC: return a, 0.0 - def _cinematica_4ws( + def _cinematica_4ws_eixos( self, - tipo, - angulo_comando_rad, + angulo_dianteiro_rad, + angulo_traseiro_rad, velocidade, *, entre_eixos=None, aplicar_dinamica=True, ): - """Modelo cinemático único do centro do rover para direção 4WS. + """Cinemática 4WS a partir dos ângulos FÍSICOS atuais dos eixos. - 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 + Este é o núcleo físico usado durante transições de modo. `df` e `dr` + podem estar entre duas geometrias nominais, exatamente como acontece + enquanto os MKS ainda estão girando. """ - tipo = _movimento_from_value(tipo) v = float(velocidade) L = ( float(self._cinematica_entre_eixos_m) @@ -2336,9 +2335,10 @@ class ControladorMPC: 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)) + df = float(angulo_dianteiro_rad) + dr = float(angulo_traseiro_rad) + tf = math.tan(df) + tr = math.tan(dr) if abs(tf) < 1e-12: tf = 0.0 @@ -2360,8 +2360,8 @@ class ControladorMPC: omega = v * kappa_ef return { - "df": float(df), - "dr": float(dr), + "df": df, + "dr": dr, "beta": float(beta), "kappa_geom": float(kappa_geom), "kappa_ef": float(kappa_ef), @@ -2369,6 +2369,27 @@ class ControladorMPC: "entre_eixos": float(L), } + def _cinematica_4ws( + self, + tipo, + angulo_comando_rad, + velocidade, + *, + entre_eixos=None, + aplicar_dinamica=True, + ): + """Cinemática nominal de um comando de alto nível. + + Mantida para compatibilidade. Converte `tipo + ângulo` nos alvos físicos + dos dois eixos e delega para o núcleo `_cinematica_4ws_eixos`. + """ + df, dr = self._angulos_eixos_4ws(tipo, angulo_comando_rad) + return self._cinematica_4ws_eixos( + df, dr, velocidade, + entre_eixos=entre_eixos, + aplicar_dinamica=aplicar_dinamica, + ) + def _calcular_omega_4ws(self, velocidade, angulo_rad, tipo): """Compatível com a assinatura histórica calcular_omega(v, ang, tipo).""" return float( @@ -2381,12 +2402,83 @@ class ControladorMPC: ) def _avancar_atuador_direcional(self, angulo_atual, angulo_alvo, dt): - """Aplica limite de velocidade ao estado físico da direção (radianos).""" + """Aplica limite de velocidade a UM eixo direcional (radianos).""" atual = float(angulo_atual) alvo = float(angulo_alvo) passo_max = np.radians(self._atuador_dir_taxa_graus_s) * max(0.0, float(dt)) return float(atual + np.clip(alvo - atual, -passo_max, passo_max)) + def _estado_eixos_fisicos_rad( + self, + contexto=None, + comando_anterior=None, + tipo_fallback=TipoMovimentoDirecional.RodasDianteiras, + angulo_fallback_rad=0.0, + ): + """Resolve o estado físico atual (df, dr) em radianos. + + Prioridade: + 1) estado interno já propagado pelo beam/latência; + 2) telemetria física por eixo entregue pelo C#/simulador; + 3) reconstrução conservadora a partir do estado canônico legado. + """ + cmd = _as_dict(comando_anterior) + + try: + df = float(cmd.get("_df_aplicado_rad")) + dr = float(cmd.get("_dr_aplicado_rad")) + if math.isfinite(df) and math.isfinite(dr): + return df, dr, True + except Exception: + pass + + if bool(cmd.get("eixos_fisicos_validos", False)): + try: + df = math.radians(float(cmd.get("angulo_dianteiro_aplicado"))) + dr = math.radians(float(cmd.get("angulo_traseiro_aplicado"))) + if math.isfinite(df) and math.isfinite(dr): + return df, dr, True + except Exception: + pass + + direcional = _as_dict(_as_dict(contexto).get("Direcional", {})) + if bool(direcional.get("EixosFisicosValidos", False)): + try: + df = math.radians(float(direcional.get("AnguloFisicoDianteiro", 0.0))) + dr = math.radians(float(direcional.get("AnguloFisicoTraseiro", 0.0))) + if math.isfinite(df) and math.isfinite(dr): + return df, dr, True + except Exception: + pass + + tipo = cmd.get("tipo_aplicado", cmd.get("tipo", None)) + if tipo is None: + tipo = direcional.get("TipoMovimento", tipo_fallback) + tipo = _movimento_from_value(tipo, _movimento_from_value(tipo_fallback)) + + # Em comando_anterior os ângulos públicos são graus. O fallback explícito + # já chega em radianos e evita ambiguidade nos estados internos do beam. + angulo_rad = float(angulo_fallback_rad) + if cmd: + try: + angulo_deg = cmd.get( + "angulo_aplicado", + cmd.get("angulo", math.degrees(angulo_rad)), + ) + angulo_rad = math.radians(float(angulo_deg)) + except Exception: + pass + elif direcional: + try: + angulo_rad = math.radians(float( + direcional.get("AnguloFisico", math.degrees(angulo_rad)) + )) + except Exception: + pass + + df, dr = self._angulos_eixos_4ws(tipo, angulo_rad) + return float(df), float(dr), False + @staticmethod def _posicoes_eixos_tracking(x, y, theta, entre_eixos): """Retorna centros dos eixos dianteiro/traseiro a partir do centro. @@ -2425,23 +2517,28 @@ class ControladorMPC: theta, velocidade, dt, + df_estado, + dr_estado, ): - """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. - """ + """Micro-predição axle-aware com dinâmica física dos dois eixos.""" v = max(0.0, float(velocidade)) ang = float(angulo_rad) tipo = _movimento_from_value(tipo) - omega = self._calcular_omega_4ws(v, ang, tipo) + dt_use = max(1e-4, float(dt)) - return self._nova_posicao( + df_alvo, dr_alvo = self._angulos_eixos_4ws(tipo, ang) + df_estado = self._avancar_atuador_direcional(df_estado, df_alvo, dt_use) + dr_estado = self._avancar_atuador_direcional(dr_estado, dr_alvo, dt_use) + kin = self._cinematica_4ws_eixos(df_estado, dr_estado, v) + + xp, yp, thp = self._nova_posicao( float(x), float(y), float(theta), - omega, v, tipo, ang, - dt=max(1e-4, float(dt)), + kin["omega"], v, tipo, ang, + dt=dt_use, + df_rad=df_estado, + dr_rad=dr_estado, ) + return xp, yp, thp, float(df_estado), float(dr_estado) def _avaliar_eixo_yaw_tracking( self, @@ -2476,6 +2573,11 @@ class ControladorMPC: entre_eixos = self._entre_eixos_tracking(contexto) xp, yp, thp = float(x), float(y), float(theta) + df_estado, dr_estado, _ = self._estado_eixos_fisicos_rad( + contexto=contexto, + tipo_fallback=tipo_prev, + angulo_fallback_rad=0.0, + ) try: # Estado inicial dos eixos: usado apenas para saber qual deles já @@ -2491,11 +2593,12 @@ class ControladorMPC: ) for _ in range(n): - xp, yp, thp = self._passo_micro_eixo_yaw( + xp, yp, thp, df_estado, dr_estado = self._passo_micro_eixo_yaw( tipo, float(angulo_rad), xp, yp, thp, v, dt, + df_estado, dr_estado, ) ref = self._referencia_caminho_lookahead( @@ -3992,24 +4095,24 @@ class ControladorMPC: def corrigir_pose_por_latencia( self, - x, y, theta, visitados, - latency_s, # atraso a compensar (s) - dt, # dt nominal do seu controle - nova_posicao_fn, # self._nova_posicao - u_hold, # {'v': m/s, 'omega': rad/s} (ou forneça 'tipo' e 'angulo' + calc_omega) - cmd_seq=None, # opcional: [(dur_s, u_dict), ...] que cobre latency_s + x, y, theta, visitados, + latency_s, + dt, + nova_posicao_fn, + u_hold, + cmd_seq=None, time_left_ms=lambda: 1e9, - calc_omega_fn=None # opcional: fn(tipo, angulo_rad, v) -> omega + calc_omega_fn=None, ): - """ - Propaga (x, y, theta) por 'latency_s' usando 'nova_posicao_fn' que depende de self.dt. - Retorna (x, y, theta) estimados no presente. - """ + """Propaga pose + estados físicos df/dr até o instante presente.""" latency_s = max(0.0, float(latency_s)) - if latency_s == 0.0: - return x, y, theta, visitados - # --- constrói sequência efetiva de comandos --- + df_estado = float(u_hold.get("df_inicial", 0.0)) + dr_estado = float(u_hold.get("dr_inicial", 0.0)) + + if latency_s == 0.0: + return x, y, theta, visitados, df_estado, dr_estado + if not cmd_seq: cmd_seq = [(latency_s, u_hold)] else: @@ -4017,66 +4120,55 @@ class ControladorMPC: if total < latency_s: cmd_seq = list(cmd_seq) + [(latency_s - total, cmd_seq[-1][1])] - # --- adaptador para usar dt_eff com fn que usa self.dt --- - def aplicar_passo_dt(xk, yk, thetak, u, dt_eff): - # garantir que temos omega e v - v = u.get('v', 0.0) - if 'omega' in u: - omega = u['omega'] - else: - if calc_omega_fn is not None and 'tipo' in u and 'angulo' in u: - omega = calc_omega_fn(v, u['angulo'], u['tipo']) - else: - omega = 0.0 # fallback: reta - old_dt = self.dt - try: - self.dt = dt_eff - return nova_posicao_fn(xk, yk, thetak, omega, v, u.get('tipo'), u.get('angulo', 0.0)) - finally: - self.dt = old_dt - - # --- integra por tempo, quebrando cada trecho em subpassos ~dt --- - angulo_estado = float( - u_hold.get('angulo_inicial', u_hold.get('angulo', 0.0)) - ) t_rem = latency_s for dur_s, u in cmd_seq: if t_rem <= 0.0: break - seg = min(dur_s, t_rem) + + seg = min(max(0.0, float(dur_s)), t_rem) if seg <= 0.0: continue - n = max(1, int(round(seg / dt))) + n = max(1, int(round(seg / max(float(dt), 1e-4)))) dt_eff = seg / n + if "df_alvo" in u and "dr_alvo" in u: + df_alvo = float(u["df_alvo"]) + dr_alvo = float(u["dr_alvo"]) + else: + df_alvo, dr_alvo = self._angulos_eixos_4ws( + u.get("tipo", TipoMovimentoDirecional.RodasDianteiras), + u.get("angulo", 0.0), + ) + for _ in range(n): if time_left_ms() <= 0.0: - return x, y, theta, visitados - angulo_estado = self._avancar_atuador_direcional( - angulo_estado, - u.get('angulo', angulo_estado), - dt_eff, + return x, y, theta, visitados, df_estado, dr_estado + + df_estado = self._avancar_atuador_direcional( + df_estado, df_alvo, dt_eff ) - u_aplicado = dict(u) - u_aplicado['angulo'] = angulo_estado - # omega precisa acompanhar o ângulo físico, não o alvo antigo. - u_aplicado.pop('omega', None) - x, y, theta = aplicar_passo_dt( - x, y, theta, u_aplicado, dt_eff + dr_estado = self._avancar_atuador_direcional( + dr_estado, dr_alvo, dt_eff ) - idx_alvo = self._corrigir_pontos_visitados( - x, - y, - visitados, - theta=theta, + + v = float(u.get("v", 0.0)) + kin = self._cinematica_4ws_eixos(df_estado, dr_estado, v) + x, y, theta = nova_posicao_fn( + x, y, theta, + kin["omega"], v, + u.get("tipo"), u.get("angulo", 0.0), + dt=dt_eff, + df_rad=df_estado, + dr_rad=dr_estado, + ) + self._corrigir_pontos_visitados( + x, y, visitados, theta=theta ) t_rem -= seg - # normaliza theta se quiser - # theta = (theta + np.pi) % (2*np.pi) - np.pi - return x, y, theta, visitados + return x, y, theta, visitados, float(df_estado), float(dr_estado) def _wrap_pi(self, a): @@ -4317,23 +4409,38 @@ class ControladorMPC: #self.passos_horizonte_local = max(1, math.floor(1 / self.tempo_execucao_local)) # -------------------- Correção por latência (vida real) -------------------- - # Mantém correção com 'self.dt' e 'velocidade' REAIS - u_hold = { - "tipo": comando_anterior.get( - "tipo_aplicado", comando_anterior.get("tipo") - ), - "angulo": np.radians( - comando_anterior.get( - "angulo_sp_aplicado", - comando_anterior.get("angulo", 0.0), - ) - ), - "angulo_inicial": np.radians( + # Estado inicial agora é 4WS físico: frente e traseira independentes. + tipo_aplicado = comando_anterior.get( + "tipo_aplicado", comando_anterior.get("tipo") + ) + angulo_sp_aplicado_rad = np.radians( + comando_anterior.get( + "angulo_sp_aplicado", + comando_anterior.get("angulo", 0.0), + ) + ) + df_inicial, dr_inicial, eixos_reais_validos = self._estado_eixos_fisicos_rad( + contexto=contexto, + comando_anterior=comando_anterior, + tipo_fallback=tipo_aplicado, + angulo_fallback_rad=np.radians( comando_anterior.get( "angulo_aplicado", comando_anterior.get("angulo", 0.0), ) ), + ) + df_alvo, dr_alvo = self._angulos_eixos_4ws( + tipo_aplicado, angulo_sp_aplicado_rad + ) + + u_hold = { + "tipo": tipo_aplicado, + "angulo": angulo_sp_aplicado_rad, + "df_inicial": df_inicial, + "dr_inicial": dr_inicial, + "df_alvo": df_alvo, + "dr_alvo": dr_alvo, "v": velocidade, } cmd_seq = None @@ -4341,7 +4448,10 @@ class ControladorMPC: pos_latencia + self.tempo_execucao_local, LAT_MAX, ) - x, y, theta, self.visitados_execucao = self.corrigir_pose_por_latencia( + ( + x, y, theta, self.visitados_execucao, + df_aplicado_estimado, dr_aplicado_estimado, + ) = self.corrigir_pose_por_latencia( x, y, theta, self.visitados_execucao, latency_s=latencia_compensada_s, dt=self.dt, @@ -4349,21 +4459,14 @@ class ControladorMPC: u_hold=u_hold, cmd_seq=cmd_seq, time_left_ms=lambda: (deadline - now()) * 1000.0, - calc_omega_fn=self._calcular_omega_4ws + calc_omega_fn=self._calcular_omega_4ws, ) - # A pose acima já foi trazida até o presente. O estado inicial do - # beam search precisa avançar pelo mesmo intervalo para não voltar - # ao ângulo físico antigo recebido junto com a posição GNSS. - angulo_aplicado_estimado = self._avancar_atuador_direcional( - u_hold["angulo_inicial"], - u_hold["angulo"], - latencia_compensada_s, - ) + # A pose e os dois eixos foram trazidos para o mesmo instante. comando_anterior = dict(comando_anterior) - comando_anterior["angulo_aplicado"] = float( - np.degrees(angulo_aplicado_estimado) - ) + comando_anterior["_df_aplicado_rad"] = float(df_aplicado_estimado) + comando_anterior["_dr_aplicado_rad"] = float(dr_aplicado_estimado) + comando_anterior["eixos_fisicos_validos"] = bool(eixos_reais_validos) idx_alvo_correcao = self._corrigir_pontos_visitados( x, y, @@ -4557,16 +4660,18 @@ class ControladorMPC: angulo_final, tipo_final = comando_parado() parada_necessaria = True else: + df_beam_inicial, dr_beam_inicial, _ = self._estado_eixos_fisicos_rad( + contexto=contexto, + comando_anterior=comando_anterior, + tipo_fallback=tipo_anterior, + angulo_fallback_rad=angulo_anterior, + ) candidatos_ativos = [{ "x": x, "y": y, "theta": theta, "custo": 0.0, "comandos": [(tipo_anterior, angulo_anterior)], - "angulo_atuador": np.radians( - comando_anterior.get( - "angulo_aplicado", - comando_anterior.get("angulo", 0.0), - ) - ), + "angulo_dianteiro_atuador": float(df_beam_inicial), + "angulo_traseiro_atuador": float(dr_beam_inicial), "trajetoria": [], "visitados": self.visitados_execucao, "inicial": True @@ -4593,9 +4698,11 @@ class ControladorMPC: cmd_anterior_local = { "tipo": candidato["comandos"][-1][0], "angulo": candidato["comandos"][-1][1], - "angulo_aplicado": candidato.get( - "angulo_atuador", - candidato["comandos"][-1][1], + "_df_aplicado_rad": float( + candidato["angulo_dianteiro_atuador"] + ), + "_dr_aplicado_rad": float( + candidato["angulo_traseiro_atuador"] ), } @@ -4636,6 +4743,8 @@ class ControladorMPC: 15.0, dados_costmap=dados_costmap_ciclo, tipo_preferido=cmd_anterior_local.get("tipo"), + df_inicial_rad=cmd_anterior_local.get("_df_aplicado_rad"), + dr_inicial_rad=cmd_anterior_local.get("_dr_aplicado_rad"), ) if ms_left() <= 0 or exp_budget <= 0: @@ -4689,15 +4798,18 @@ class ControladorMPC: t_c_ini = now() try: # >>> CHANGED: passa dt_pred e v_sim para a simulação de um PASSO - custo, sim, valido, visitados_sim, _debug_custo, angulo_atuador_final = self._simular_passo( + ( + custo, sim, valido, visitados_sim, _debug_custo, + df_atuador_final, dr_atuador_final, + ) = self._simular_passo( x_atual, y_atual, theta_atual, tipo_k, ang_k, custos_candidatos, cmd_anterior_local, contexto, visitados, - dt_pred=dt_pred, # <-- NOVO - v_planejado=v_sim, # <-- NOVO - passo=passo + dt_pred=dt_pred, + v_planejado=v_sim, + passo=passo, ) if passo == 0: debug_custo[f"{tipo_k.value}_{np.degrees(ang_k):.2f}"] = _debug_custo @@ -4707,7 +4819,8 @@ class ControladorMPC: "x": x_f, "y": y_f, "theta": theta_f, "custo": candidato["custo"] + float(custo), "comandos": candidato["comandos"] + [(tipo_k, ang_k)], - "angulo_atuador": angulo_atuador_final, + "angulo_dianteiro_atuador": df_atuador_final, + "angulo_traseiro_atuador": dr_atuador_final, "trajetoria": candidato["trajetoria"] + sim, "visitados": visitados_sim, "inicial": False @@ -4965,6 +5078,8 @@ class ControladorMPC: margem_ms, dados_costmap=None, tipo_preferido=None, + df_inicial_rad=None, + dr_inicial_rad=None, ): """ Gera candidatos de (tipo, ângulo) respeitando o deadline do ciclo: @@ -5277,7 +5392,9 @@ class ControladorMPC: margem_ms=float(margem_ms), permitidos=permitidos, e_lat=e_lat, - e_ori=e_ori + e_ori=e_ori, + df_inicial_rad=df_inicial_rad, + dr_inicial_rad=dr_inicial_rad, ) pares_final = self._uniformizar_pares_candidatos( @@ -5355,7 +5472,9 @@ class ControladorMPC: margem_ms: float = 20.0, permitidos=None, e_lat=0.0, - e_ori=0.0 + e_ori=0.0, + df_inicial_rad=None, + dr_inicial_rad=None, ): """ Seleciona e avalia candidatos (tipo, ângulo) contra a matriz de custo. @@ -5509,7 +5628,11 @@ class ControladorMPC: t_c_ini = now() try: - traj = self._simular_trajetoria_curta(p_atual, tipo, ang_rad, velocidade, distancia_sim_m) + traj = self._simular_trajetoria_curta( + p_atual, tipo, ang_rad, velocidade, distancia_sim_m, + df_inicial_rad=df_inicial_rad, + dr_inicial_rad=dr_inicial_rad, + ) custo, valido = self._avaliar_trajetoria_matriz_custo(tipo, ang_rad, traj, p_ref, dados_costmap) #print(f"avaliando {tipo} {np.degrees(ang_rad):.2f} - custo: {custo}, valido: {valido}") if valido: @@ -5569,65 +5692,62 @@ class ControladorMPC: mostrar_log(f"❌ Erro ao filtrar candidatos por matriz: {e}") return [], {} - def _simular_trajetoria_curta(self, p_atual, tipo, angulo, velocidade, dist_min): - """V16: trajetória curta usando a cinemática 4WS global. + def _simular_trajetoria_curta( + self, + p_atual, + tipo, + angulo, + velocidade, + dist_min, + *, + df_inicial_rad=None, + dr_inicial_rad=None, + ): + """Trajetória curta com slew físico independente de df/dr. - A mesma combinação beta/omega usada pelo MPC pesado é integrada aqui - de forma analítica para v, steering e omega constantes. + Quando o chamador fornece o estado atual dos eixos, uma troca de modo + só ganha a nova geometria conforme cada eixo realmente consegue girar. """ try: - dt = float(self.dt) + dt = max(1e-4, float(self.dt)) v = float(velocidade) - if v <= 1e-6 or dist_min <= 1e-6 or dt <= 0.0: + if abs(v) <= 1e-6 or dist_min <= 1e-6: return [] - dist_step = v * dt - n = int(math.ceil(dist_min / dist_step)) + dist_step = abs(v) * dt + n = int(math.ceil(dist_min / max(dist_step, 1e-9))) if n <= 0: 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]), - ) + x, y, th = map(float, p_atual[:3]) + df_alvo, dr_alvo = self._angulos_eixos_4ws(tipo, float(angulo)) - 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) + if df_inicial_rad is None or dr_inicial_rad is None: + # Compatibilidade para chamadas de debug sem estado físico. + df_estado, dr_estado = float(df_alvo), float(dr_alvo) else: - phi = th + beta - x = x0 + (v / omega) * ( - math.cos(phi0) - np.cos(phi) - ) - y = y0 + (v / omega) * ( - np.sin(phi) - math.sin(phi0) - ) + df_estado = float(df_inicial_rad) + dr_estado = float(dr_inicial_rad) - return list( - zip( - np.asarray(x, dtype=float).tolist(), - np.asarray(y, dtype=float).tolist(), - np.asarray(th, dtype=float).tolist(), + traj = [] + for _ in range(n): + df_estado = self._avancar_atuador_direcional( + df_estado, df_alvo, dt ) - ) + dr_estado = self._avancar_atuador_direcional( + dr_estado, dr_alvo, dt + ) + kin = self._cinematica_4ws_eixos(df_estado, dr_estado, v) + x, y, th = self._nova_posicao( + x, y, th, kin["omega"], v, tipo, float(angulo), + dt=dt, df_rad=df_estado, dr_rad=dr_estado, + ) + traj.append((x, y, th)) + + return traj except Exception as e: - mostrar_log(f"❌ Erro na simulação curta: {e}") + mostrar_log(f"Erro ao simular trajetória curta 4WS física: {e}") return [] def _avaliar_trajetoria_matriz_custo(self, tipo, ang, trajetoria, p_ref, dados_costmap): @@ -5952,11 +6072,19 @@ class ControladorMPC: self.x_ant, self.y_ant = 0.0, 0.0 simulacoes = [] - prev_ang = float(comando_anterior.get("angulo", 0.0)) - prev_tipo = comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value) - angulo_atuador = float( - comando_anterior.get("angulo_aplicado", prev_ang) + prev_ang = float(comando_anterior.get("angulo", 0.0)) + prev_tipo = comando_anterior.get( + "tipo", TipoMovimentoDirecional.RodasDianteiras.value ) + df_atuador, dr_atuador, _ = self._estado_eixos_fisicos_rad( + contexto=contexto, + comando_anterior=comando_anterior, + tipo_fallback=prev_tipo, + angulo_fallback_rad=prev_ang, + ) + df_inicial_passo = float(df_atuador) + dr_inicial_passo = float(dr_atuador) + df_alvo, dr_alvo = self._angulos_eixos_4ws(tipo, angulo_testado) pontos_visitados = visitados.copy() # chave para custo heurístico vindo do VisualWorker (por ângulo/tipo) @@ -6012,22 +6140,23 @@ class ControladorMPC: side_mem_local = 0 for n_sub in range(n_subs): - angulo_atuador = self._avancar_atuador_direcional( - angulo_atuador, - angulo_testado, - sub_dt, + df_atuador = self._avancar_atuador_direcional( + df_atuador, df_alvo, sub_dt ) - omega_aplicado = self._calcular_omega_4ws( - v_planejado, - angulo_atuador, - tipo, + dr_atuador = self._avancar_atuador_direcional( + dr_atuador, dr_alvo, sub_dt ) - # IMPORTANTE: _nova_posicao precisa aceitar dt opcional + kin_aplicada = self._cinematica_4ws_eixos( + df_atuador, dr_atuador, v_planejado + ) + omega_aplicado = float(kin_aplicada["omega"]) x_sim, y_sim, theta_sim = self._nova_posicao( x_sim, y_sim, theta_sim, omega_aplicado, v_planejado, - tipo, angulo_atuador, - dt=sub_dt + tipo, angulo_testado, + dt=sub_dt, + df_rad=df_atuador, + dr_rad=dr_atuador, ) simulacoes.append((x_sim, y_sim, theta_sim)) @@ -6234,22 +6363,42 @@ class ControladorMPC: #print(custo_visual_worker) debug_custo["atuador_direcional"] = { - "angulo_inicial_deg": float(np.degrees( - comando_anterior.get("angulo_aplicado", prev_ang) - )), - "angulo_alvo_deg": float(np.degrees(angulo_testado)), - "angulo_final_deg": float(np.degrees(angulo_atuador)), + "comando_alvo_deg": float(np.degrees(angulo_testado)), + "df_inicial_deg": float(np.degrees(df_inicial_passo)), + "dr_inicial_deg": float(np.degrees(dr_inicial_passo)), + "df_alvo_deg": float(np.degrees(df_alvo)), + "dr_alvo_deg": float(np.degrees(dr_alvo)), + "df_final_deg": float(np.degrees(df_atuador)), + "dr_final_deg": float(np.degrees(dr_atuador)), "taxa_graus_s": float(self._atuador_dir_taxa_graus_s), } - return float(custo_total), simulacoes, True, pontos_visitados, debug_custo, angulo_atuador - - except Exception as e: - mostrar_log(f"Erro ao simular passo para {tipo.name} | angulo: {np.degrees(angulo_testado):.2f}: {e}") - return float('inf'), [(0.0, 0.0, 0.0)], False, visitados, {}, float( - comando_anterior.get("angulo_aplicado", comando_anterior.get("angulo", 0.0)) + return ( + float(custo_total), simulacoes, True, pontos_visitados, + debug_custo, float(df_atuador), float(dr_atuador), ) - def _nova_posicao(self, x, y, theta, omega, velocidade, tipo, angulo_rad, dt=None): + except Exception as e: + mostrar_log( + f"Erro ao simular passo para {tipo.name} | " + f"angulo: {np.degrees(angulo_testado):.2f}: {e}" + ) + df_fallback, dr_fallback, _ = self._estado_eixos_fisicos_rad( + contexto=contexto, + comando_anterior=comando_anterior, + tipo_fallback=comando_anterior.get( + "tipo", TipoMovimentoDirecional.RodasDianteiras.value + ), + angulo_fallback_rad=float(comando_anterior.get("angulo", 0.0)), + ) + return ( + float('inf'), [(0.0, 0.0, 0.0)], False, visitados, {}, + float(df_fallback), float(dr_fallback), + ) + + def _nova_posicao( + self, x, y, theta, omega, velocidade, tipo, angulo_rad, dt=None, + df_rad=None, dr_rad=None, + ): """V16: integra UM passo com a cinemática 4WS unificada. `omega` permanece na assinatura por compatibilidade com callbacks antigos, @@ -6270,7 +6419,13 @@ class ControladorMPC: except Exception: tipo_enum = None - if tipo_enum is not None: + if df_rad is not None and dr_rad is not None: + kin = self._cinematica_4ws_eixos( + float(df_rad), float(dr_rad), v, aplicar_dinamica=True + ) + beta = float(kin["beta"]) + omega_use = float(kin["omega"]) + elif tipo_enum is not None: kin = self._cinematica_4ws( tipo_enum, float(angulo_rad),