diff --git a/.gitignore b/.gitignore index ae935a783..eccb91e00 100644 --- a/.gitignore +++ b/.gitignore @@ -93,3 +93,7 @@ AgroBase/AgroBase/bin/x64/Debug/Python/venv/ /Exemplos /Python/raspi/cam_2/imx296_pi/ /Python/raspi/cam_3/imx296_pi/ +Python/OAK/datasets/oak-fcc-3/audit/ +/Python/OAK/datasets/oak-fcc-3/benchmarks_async +/Python/OAK/datasets/oak-d/benchmark_corridor +/AgroBase/livox_visual_debugger/x64/Debug diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs index de1e42264..1012f467e 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs @@ -105,7 +105,7 @@ namespace AgroBase.Forms.IHM.Operacao.Parametros _mapa.TipoMapa = op.Parametros.TipoMapa; - await _mapa.DefinirRuasSelecionadasAsync(ruasSelecionadas, false); + await _mapa.DefinirRuasSelecionadasAsync(op, ruasSelecionadas, false); lblMapaPlaceholder.Visible = false; diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs index 3e9188490..4c36e710d 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs @@ -169,7 +169,7 @@ namespace AgroBase.Forms.IHM.Operacao mapa.TipoMapa = op.Parametros?.TipoMapa ?? Enums.TipoMapaOperacao.Indefinido; - await mapa.DefinirRuasSelecionadasAsync(ruasSelecionadas, false); + await mapa.DefinirRuasSelecionadasAsync(op, ruasSelecionadas, false); } else { diff --git a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs index 8b8186250..7bbaf041a 100644 --- a/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs +++ b/AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs @@ -197,7 +197,7 @@ namespace AgroBase.Forms.IHM.Operacao if (mapaAtual != null) { novaOperacao.Mapa = mapaAtual; - await mapaAtual.LimparParametrizacaoMapaAsync(); + await mapaAtual.LimparParametrizacaoMapaAsync(novaOperacao); } /* diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.Designer.cs b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.Designer.cs index f36dac0c9..602aa38c2 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.Designer.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.Designer.cs @@ -367,6 +367,11 @@ this.tabOrientacao = new System.Windows.Forms.TabPage(); this.pnlInclinacao = new System.Windows.Forms.Panel(); this.pnl3D = new System.Windows.Forms.Panel(); + this.pnlOrientacaoDados = new System.Windows.Forms.Panel(); + this.txtImuLateral = new System.Windows.Forms.TextBox(); + this.txtImuFrontal = new System.Windows.Forms.TextBox(); + this.label147 = new System.Windows.Forms.Label(); + this.label146 = new System.Windows.Forms.Label(); this.tabSonar = new System.Windows.Forms.TabPage(); this.chbSonarDeteccoes = new System.Windows.Forms.CheckBox(); this.tabControl3 = new System.Windows.Forms.TabControl(); @@ -531,11 +536,7 @@ this.label36 = new System.Windows.Forms.Label(); this.txtMomento = new System.Windows.Forms.TextBox(); this.btnPlay = new System.Windows.Forms.Button(); - this.pnlOrientacaoDados = new System.Windows.Forms.Panel(); - this.label146 = new System.Windows.Forms.Label(); - this.label147 = new System.Windows.Forms.Label(); - this.txtImuFrontal = new System.Windows.Forms.TextBox(); - this.txtImuLateral = new System.Windows.Forms.TextBox(); + this.btnCarregarMapa = new System.Windows.Forms.Button(); ((System.ComponentModel.ISupportInitialize)(this.trbMomento)).BeginInit(); this.panel1.SuspendLayout(); this.tabControl4.SuspendLayout(); @@ -590,6 +591,7 @@ ((System.ComponentModel.ISupportInitialize)(this.picRobo)).BeginInit(); this.tabOrientacao.SuspendLayout(); this.pnl3D.SuspendLayout(); + this.pnlOrientacaoDados.SuspendLayout(); this.tabSonar.SuspendLayout(); this.tabControl3.SuspendLayout(); this.tabSonarRGB.SuspendLayout(); @@ -616,7 +618,6 @@ this.panel6.SuspendLayout(); this.pnlInformacoesOperacao.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.picCameraCaminho)).BeginInit(); - this.pnlOrientacaoDados.SuspendLayout(); this.SuspendLayout(); // // trbMomento @@ -4131,6 +4132,51 @@ this.pnl3D.Size = new System.Drawing.Size(486, 512); this.pnl3D.TabIndex = 1; // + // pnlOrientacaoDados + // + this.pnlOrientacaoDados.Controls.Add(this.txtImuLateral); + this.pnlOrientacaoDados.Controls.Add(this.txtImuFrontal); + this.pnlOrientacaoDados.Controls.Add(this.label147); + this.pnlOrientacaoDados.Controls.Add(this.label146); + this.pnlOrientacaoDados.Location = new System.Drawing.Point(1, 362); + this.pnlOrientacaoDados.Name = "pnlOrientacaoDados"; + this.pnlOrientacaoDados.Size = new System.Drawing.Size(150, 150); + this.pnlOrientacaoDados.TabIndex = 0; + // + // txtImuLateral + // + this.txtImuLateral.Location = new System.Drawing.Point(6, 87); + this.txtImuLateral.Name = "txtImuLateral"; + this.txtImuLateral.Size = new System.Drawing.Size(100, 20); + this.txtImuLateral.TabIndex = 3; + this.txtImuLateral.TextAlign = System.Windows.Forms.HorizontalAlignment.Right; + // + // txtImuFrontal + // + this.txtImuFrontal.Location = new System.Drawing.Point(6, 32); + this.txtImuFrontal.Name = "txtImuFrontal"; + this.txtImuFrontal.Size = new System.Drawing.Size(100, 20); + this.txtImuFrontal.TabIndex = 1; + this.txtImuFrontal.TextAlign = System.Windows.Forms.HorizontalAlignment.Right; + // + // label147 + // + this.label147.AutoSize = true; + this.label147.Location = new System.Drawing.Point(3, 71); + this.label147.Name = "label147"; + this.label147.Size = new System.Drawing.Size(39, 13); + this.label147.TabIndex = 2; + this.label147.Text = "Lateral"; + // + // label146 + // + this.label146.AutoSize = true; + this.label146.Location = new System.Drawing.Point(3, 16); + this.label146.Name = "label146"; + this.label146.Size = new System.Drawing.Size(39, 13); + this.label146.TabIndex = 1; + this.label146.Text = "Frontal"; + // // tabSonar // this.tabSonar.Controls.Add(this.chbSonarDeteccoes); @@ -5875,10 +5921,10 @@ // // btnCarregarOperacao // - this.btnCarregarOperacao.Location = new System.Drawing.Point(1180, 587); + this.btnCarregarOperacao.Location = new System.Drawing.Point(1138, 587); this.btnCarregarOperacao.Margin = new System.Windows.Forms.Padding(2); this.btnCarregarOperacao.Name = "btnCarregarOperacao"; - this.btnCarregarOperacao.Size = new System.Drawing.Size(128, 28); + this.btnCarregarOperacao.Size = new System.Drawing.Size(115, 28); this.btnCarregarOperacao.TabIndex = 4; this.btnCarregarOperacao.Text = "Carregar Opereação"; this.btnCarregarOperacao.UseVisualStyleBackColor = true; @@ -5907,7 +5953,7 @@ // // btnPlay // - this.btnPlay.Location = new System.Drawing.Point(1120, 587); + this.btnPlay.Location = new System.Drawing.Point(1078, 587); this.btnPlay.Margin = new System.Windows.Forms.Padding(2); this.btnPlay.Name = "btnPlay"; this.btnPlay.Size = new System.Drawing.Size(56, 28); @@ -5916,56 +5962,23 @@ this.btnPlay.UseVisualStyleBackColor = true; this.btnPlay.Click += new System.EventHandler(this.btnPlay_Click); // - // pnlOrientacaoDados + // btnCarregarMapa // - this.pnlOrientacaoDados.Controls.Add(this.txtImuLateral); - this.pnlOrientacaoDados.Controls.Add(this.txtImuFrontal); - this.pnlOrientacaoDados.Controls.Add(this.label147); - this.pnlOrientacaoDados.Controls.Add(this.label146); - this.pnlOrientacaoDados.Location = new System.Drawing.Point(1, 362); - this.pnlOrientacaoDados.Name = "pnlOrientacaoDados"; - this.pnlOrientacaoDados.Size = new System.Drawing.Size(150, 150); - this.pnlOrientacaoDados.TabIndex = 0; - // - // label146 - // - this.label146.AutoSize = true; - this.label146.Location = new System.Drawing.Point(3, 16); - this.label146.Name = "label146"; - this.label146.Size = new System.Drawing.Size(39, 13); - this.label146.TabIndex = 1; - this.label146.Text = "Frontal"; - // - // label147 - // - this.label147.AutoSize = true; - this.label147.Location = new System.Drawing.Point(3, 71); - this.label147.Name = "label147"; - this.label147.Size = new System.Drawing.Size(39, 13); - this.label147.TabIndex = 2; - this.label147.Text = "Lateral"; - // - // txtImuFrontal - // - this.txtImuFrontal.Location = new System.Drawing.Point(6, 32); - this.txtImuFrontal.Name = "txtImuFrontal"; - this.txtImuFrontal.Size = new System.Drawing.Size(100, 20); - this.txtImuFrontal.TabIndex = 1; - this.txtImuFrontal.TextAlign = System.Windows.Forms.HorizontalAlignment.Right; - // - // txtImuLateral - // - this.txtImuLateral.Location = new System.Drawing.Point(6, 87); - this.txtImuLateral.Name = "txtImuLateral"; - this.txtImuLateral.Size = new System.Drawing.Size(100, 20); - this.txtImuLateral.TabIndex = 3; - this.txtImuLateral.TextAlign = System.Windows.Forms.HorizontalAlignment.Right; + this.btnCarregarMapa.Location = new System.Drawing.Point(1257, 587); + this.btnCarregarMapa.Margin = new System.Windows.Forms.Padding(2); + this.btnCarregarMapa.Name = "btnCarregarMapa"; + this.btnCarregarMapa.Size = new System.Drawing.Size(77, 28); + this.btnCarregarMapa.TabIndex = 96; + this.btnCarregarMapa.Text = "Mapa"; + this.btnCarregarMapa.UseVisualStyleBackColor = true; + this.btnCarregarMapa.Click += new System.EventHandler(this.btnCarregarMapa_Click); // // frmResultadosOperacao // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.ClientSize = new System.Drawing.Size(1345, 632); + this.Controls.Add(this.btnCarregarMapa); this.Controls.Add(this.btnPlay); this.Controls.Add(this.txtMomento); this.Controls.Add(this.label36); @@ -6061,6 +6074,8 @@ ((System.ComponentModel.ISupportInitialize)(this.picRobo)).EndInit(); this.tabOrientacao.ResumeLayout(false); this.pnl3D.ResumeLayout(false); + this.pnlOrientacaoDados.ResumeLayout(false); + this.pnlOrientacaoDados.PerformLayout(); this.tabSonar.ResumeLayout(false); this.tabSonar.PerformLayout(); this.tabControl3.ResumeLayout(false); @@ -6096,8 +6111,6 @@ this.pnlInformacoesOperacao.ResumeLayout(false); this.pnlInformacoesOperacao.PerformLayout(); ((System.ComponentModel.ISupportInitialize)(this.picCameraCaminho)).EndInit(); - this.pnlOrientacaoDados.ResumeLayout(false); - this.pnlOrientacaoDados.PerformLayout(); this.ResumeLayout(false); this.PerformLayout(); @@ -6588,5 +6601,6 @@ private System.Windows.Forms.TextBox txtImuFrontal; private System.Windows.Forms.Label label147; private System.Windows.Forms.Label label146; + private System.Windows.Forms.Button btnCarregarMapa; } } \ No newline at end of file diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs index 9be4a4842..bd4445d75 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs @@ -1,5 +1,6 @@ using AgroBase.Models; using AgroBase.Models.Modules; +using AgroBase.Models.Operacoes; using AgroBase.Models.Operadores; using AgroBase.Properties; using AgroBase.Services; @@ -12,6 +13,7 @@ using System.Drawing.Drawing2D; using System.Globalization; using System.IO; using System.Linq; +using System.Text; using System.Threading.Tasks; using System.Windows.Forms; @@ -53,9 +55,14 @@ namespace AgroBase.Forms.Operacoes DateTime MomentoAtual = DateTime.Now; AsyncTaskTimerModel tmrPlay; + + private List> _ruasMapaReplay = new List>(); + private List _trajetoriaFixaReplay = new List(); + MapasModel Mapa = new MapasModel(); - private Visualizador3DService visualizador3D; + //TrajetoriaMapaOperacaoModel Trajetoria = new TrajetoriaMapaOperacaoModel(new List>()); private MapaDinamicoModel MapaDinamico; + private Visualizador3DService visualizador3D; private int idxMomentoAtual { get @@ -165,12 +172,46 @@ namespace AgroBase.Forms.Operacoes LogsOperacao = JsonConvert.DeserializeObject(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, data.operacao)).ToList(); LogsGPS = JsonConvert.DeserializeObject(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, Enums.T_Code.Gps.ToString())).ToList(); - GPSModel pos = GPSService.UltimaLeitura.Clone(); - GPSService.UltimaLeitura = LogsGPS.FirstOrDefault(); - bool sucesso = op.CarregarParametrizacaoOperacao(null, null, ofd.FileName.Replace("data.lgop", "operacao.opr")); - bool condicaoOperacaoCarregada() => op.Trajetoria?.CorredorAtual != null; - bool resultado = await FuncoesGlobais.AguardarCondicaoAsync(condicaoOperacaoCarregada, 15000, 200); - GPSService.UltimaLeitura = pos; + + string caminhoOpr = Path.Combine( + Path.GetDirectoryName(ofd.FileName), + "operacao.opr" + ); + + OperacaoParametrosModel parametrosOperacao = null; + + if (File.Exists(caminhoOpr)) + { + parametrosOperacao = + JsonConvert.DeserializeObject( + File.ReadAllText(caminhoOpr, Encoding.UTF8) + ); + + if (parametrosOperacao != null) + { + GPSModel posAnterior = + GPSService.UltimaLeitura?.Clone(); + + try + { + // Importantíssimo: + // a trajetória será projetada tomando como referência + // a posição inicial real daquela operação. + GPSService.UltimaLeitura = + LogsGPS.FirstOrDefault()?.Clone(); + + Variaveis.OperacaoEmAndamento = OperacaoModel.CarregarParamerosOperacaoBase( + parametrosOperacao + ); + } + finally + { + if (posAnterior != null) + GPSService.UltimaLeitura = posAnterior; + } + } + } + LogsControle = JsonConvert.DeserializeObject(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, "controle")).ToList(); LogsCooler = JsonConvert.DeserializeObject(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, "cooler")).ToList(); @@ -239,7 +280,7 @@ namespace AgroBase.Forms.Operacoes }; MapasService.CriarArquivoMapa(new List>() { LogsGPS.ToList() }, 1517, data.id); Mapa.btnCarregar_Click(sender, e, Path.Combine(Variaveis.CaminhoSistema, Variaveis.CaminhoMapasConvertidos + data.id + ".json")); - + trbMomento.Maximum = LogsOperacao.Count - 1; trbMomento.Minimum = 0; trbMomento.Value = 0; @@ -1309,28 +1350,60 @@ namespace AgroBase.Forms.Operacoes pnlOrientacaoGPS.Invalidate(); - var _trajaetoria = LogsGPS.Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) - { - Posicao = x - }) - .ToList(); - - var LogOpe = LogsOperacao[idxMomentoAtual]; var LogSnr = LogsVisualWorker[idxMomentoAtual]; var LogTrj = LogsTrajetoria[idxMomentoAtual]; var LogCtrl = LogsControle[idxMomentoAtual]; + + var trajetoriaFixa = + (_trajetoriaFixaReplay?.Count ?? 0) > 0 + ? _trajetoriaFixaReplay + : Variaveis.OperacaoEmAndamento + ?.Trajetoria + ?._TrajetoriaFixa + ?? new List(); + + var trajetoriaDinamica = + MontarTrajetoriaDinamicaReplay( + Log, + LogTrj + ); + + var ruasMapa = + (_ruasMapaReplay?.Count ?? 0) > 0 + ? _ruasMapaReplay + : Variaveis.OperacaoEmAndamento + ?.Mapa + ?.TrajetoriaMapa + ?? new List>(); + + var rastroRover = + LogsGPS + .Take(idxMomentoAtual + 1) + .ToList(); + + + Variaveis.OperacaoEmAndamento.Trajetoria.LoopAtualizaDados(); MapaDinamico.AtualizarDados( - //(LogSnr?.Iniciado ?? false) && LogSnr.Resumo.DirecaoDesvio != Enums.Direcao.Frente ? LogSnr.Resumo.ObstaculoCritico : null, - null, + null, (float)LogTrj.AnguloCaminho, (float)Log.AnguloCarroDefinido, Log, - _trajaetoria, - Variaveis.OperacaoEmAndamento.Trajetoria?._TrajetoriaFixa, - new List(LogsGPS.Take(LogsGPS.IndexOf(Log) + 1)), - Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa, + + // Roxo: trajetória ainda restante naquele instante + trajetoriaDinamica, + + // Verde/azul: trajetória fixa completa projetada + trajetoriaFixa, + + // Laranja: caminho realmente percorrido até aquele instante + rastroRover, + + // Azul/preto: linhas originais do mapa + ruasMapa, + + // Verde tracejado: simulação MPC daquele frame LogCtrl.SimulacaoMPC ); } @@ -2334,6 +2407,48 @@ namespace AgroBase.Forms.Operacoes } } } + + private async void btnCarregarMapa_Click(object sender, EventArgs e) + { + + } + + private List MontarTrajetoriaDinamicaReplay(GPSModel posicaoAtual, OperacaoSensoriamentoLogTrajetoriaModel logTrajetoria) + { + var resultado = new List(); + + if (posicaoAtual != null) + { + resultado.Add( + new PontoTrajetoriaModel(Enums.TipoPontoRua.PosicaoRobo) + { + Posicao = posicaoAtual, + LarguraCorredor = 1.0, + idxCorredor = logTrajetoria?.CorredorAtual?.Idx ?? 0, + Visitado = true + } + ); + } + + if (_trajetoriaFixaReplay == null || + _trajetoriaFixaReplay.Count == 0) + { + return resultado; + } + + int idxProximo = + logTrajetoria?.ProximoPonto?.idxPonto ?? 0; + + resultado.AddRange( + _trajetoriaFixaReplay + .Where(x => + x != null && + x.idxPonto >= idxProximo) + ); + + return resultado; + } + } } diff --git a/AgroBase/AgroBase/Forms/Simuladores/frmTreinamentoIA.cs b/AgroBase/AgroBase/Forms/Simuladores/frmTreinamentoIA.cs index d34104928..9d220bddc 100644 --- a/AgroBase/AgroBase/Forms/Simuladores/frmTreinamentoIA.cs +++ b/AgroBase/AgroBase/Forms/Simuladores/frmTreinamentoIA.cs @@ -88,7 +88,7 @@ namespace AgroBase.Forms txtAnguloGPS.Text = anguloInicial.ToString("0.00"); txtDistanciaGPS.Text = distanciaInicial.ToString("0.00"); Variaveis.OperacaoEmAndamento.GPSTrajetoria = new System.Collections.Generic.List(); - Variaveis.OperacaoEmAndamento.Trajetoria = new TrajetoriaMapaOperacaoModel(Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa); + Variaveis.OperacaoEmAndamento.Trajetoria = new TrajetoriaMapaOperacaoModel(Variaveis.OperacaoEmAndamento, Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa); Variaveis.OperacaoEmAndamento.Trajetoria.ProjetarTrajetoriaFixa(); _robotState = new TreinamentoIAModel(); } diff --git a/AgroBase/AgroBase/Models/MapasModel.cs b/AgroBase/AgroBase/Models/MapasModel.cs index ae9cb907a..8b83102db 100644 --- a/AgroBase/AgroBase/Models/MapasModel.cs +++ b/AgroBase/AgroBase/Models/MapasModel.cs @@ -344,7 +344,7 @@ namespace AgroBase.Models _mapaEnviado = true; } - public void DefinirRuasSelecionadas(IEnumerable ruas, bool popularTrajetoria = false) + public void DefinirRuasSelecionadas(OperacaoModel op, IEnumerable ruas, bool popularTrajetoria = false) { RuasPercorrer = ruas == null @@ -361,12 +361,12 @@ namespace AgroBase.Models } if (popularTrajetoria) - PopularTrajetoriaMapa(); + PopularTrajetoriaMapa(op); } - public async Task DefinirRuasSelecionadasAsync(IEnumerable ruas, bool popularTrajetoria = false) + public async Task DefinirRuasSelecionadasAsync(OperacaoModel op, IEnumerable ruas, bool popularTrajetoria = false) { - DefinirRuasSelecionadas(ruas, popularTrajetoria); + DefinirRuasSelecionadas(op, ruas, popularTrajetoria); if (!_paginaCarregada || browser?.CoreWebView2 == null) { @@ -378,11 +378,11 @@ namespace AgroBase.Models await browser.CoreWebView2.ExecuteScriptAsync("definirSelecaoRuas(" + ruasJson + ");"); } - public Task DefinirRuasSelecionadasAsync(IEnumerable ruas) + public Task DefinirRuasSelecionadasAsync(OperacaoModel op, IEnumerable ruas) { IEnumerable convertidas = ruas == null ? null : ruas.Select(x => x.ToString()); - return DefinirRuasSelecionadasAsync(convertidas); + return DefinirRuasSelecionadasAsync(op, convertidas); } private bool PodeSelecionarRuas() @@ -424,7 +424,7 @@ namespace AgroBase.Models .Distinct() .ToList(); - PopularTrajetoriaMapa(); + PopularTrajetoriaMapa(Variaveis.OperacaoEmAndamento); RuasSelecionadasAlteradas?.Invoke(this, EventArgs.Empty); } catch (Exception ex) @@ -433,7 +433,7 @@ namespace AgroBase.Models } } - public void PopularTrajetoriaMapa(MapaFeatureCollectionModel dados = null) + public void PopularTrajetoriaMapa(OperacaoModel op, MapaFeatureCollectionModel dados = null) { if (dados != null) mapaService.DefinirMapa(dados, mapaService.NomeArquivos ?? "Mapa"); @@ -512,14 +512,12 @@ namespace AgroBase.Models ruasMontadas.Add(rua); } - OperacaoModel op = Variaveis.OperacaoEmAndamento; - if (op == null) throw new InvalidOperationException("Operação indisponível para instalar a trajetória."); var trajetoriaAnterior = op.Trajetoria; var mapaAnterior = TrajetoriaMapa; - var novaTrajetoria = new TrajetoriaMapaOperacaoModel(ruasMontadas); + var novaTrajetoria = new TrajetoriaMapaOperacaoModel(op, ruasMontadas); try { @@ -647,9 +645,9 @@ namespace AgroBase.Models pnlMapa = null; } - public async Task LimparParametrizacaoMapaAsync() + public async Task LimparParametrizacaoMapaAsync(OperacaoModel op) { - DefinirRuasSelecionadas(new List(), false); + DefinirRuasSelecionadas(op, new List(), false); if (_paginaCarregada && browser?.CoreWebView2 != null) { await browser.CoreWebView2.ExecuteScriptAsync("definirSelecaoRuas([]);"); diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index 8686b9426..9e6c6ebee 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -476,7 +476,6 @@ namespace AgroBase.Models Simulando = false, GPSTrajetoria = new List(), Mapa = new MapasModel(), - Trajetoria = new TrajetoriaMapaOperacaoModel(new List>()), ControleAnterior = new OperacaoControleModel(), Parametros = new OperacaoParametrosModel() { @@ -499,10 +498,12 @@ namespace AgroBase.Models } } }; + op.Trajetoria = new TrajetoriaMapaOperacaoModel(op, new List>()); op.Sensoriamento = new OperacaoSensoriamentoConjuntoModel(op) { Operacao = new OperacaoSensoriamentoLogModel() { + Modo = Modo, OperacaoIniciada = false, DataInicio = DateTime.MinValue, DataFim = DateTime.MinValue @@ -559,7 +560,7 @@ namespace AgroBase.Models AtuPercentualErvasBicoOff = 2, AtuPercentualErvasBicoOn = 1, AtuPercentualInicioPulverizacao = 85, - AtuPressaoLinha = 18, + AtuPressaoLinha = 22, AtuAgitadorModo = ModoAgitadorCalda.SemAgitacao, AtuModoControle = ModoControleBomba.PID, AtuModeloCabeca = CabecasModelo.Target, @@ -666,14 +667,14 @@ namespace AgroBase.Models DirVelocidadeMovimento = 80, DirTipoMovimento = TiposControladorDirecional.MPC, - DistanciaManobra = 3.0, + DistanciaManobra = 2.2, AtuAlturaAreaPulverizacao = 10, AtuDuracaoAtuacao = 0, AtuPercentualErvasBicoOff = 2, AtuPercentualErvasBicoOn = 1, AtuPercentualInicioPulverizacao = 85, - AtuPressaoLinha = 18, + AtuPressaoLinha = 22, AtuAgitadorModo = ModoAgitadorCalda.Continuo, AtuModoControle = ModoControleBomba.PID, AtuModeloCabeca = CabecasModelo.Target, @@ -1312,9 +1313,9 @@ namespace AgroBase.Models if (parametros.Mapa != null) { novaOp.Mapa = new MapasModel(); - novaOp.Mapa.DefinirRuasSelecionadas(ruasSelecionadas); + novaOp.Mapa.DefinirRuasSelecionadas(novaOp, ruasSelecionadas); novaOp.Mapa.TipoMapa = novaOp.Parametros.TipoMapa; - novaOp.Mapa.PopularTrajetoriaMapa(parametros.Mapa); + novaOp.Mapa.PopularTrajetoriaMapa(novaOp, parametros.Mapa); } novaOp?.DispAtu?.Dados?.PrepararAtuadorParaOperacao(); diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index 1b9d1c39e..a539806c2 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -16,8 +16,9 @@ namespace AgroBase.Models { public class TrajetoriaMapaOperacaoModel { - public TrajetoriaMapaOperacaoModel(List> RuasMapa) + public TrajetoriaMapaOperacaoModel(OperacaoModel _op, List> RuasMapa) { + op = _op; RuasPlantacao = ClonarRuasMapa(RuasMapa); AutonomiaCorredor = new AutonomiaCorredorModel(); } @@ -47,12 +48,15 @@ namespace AgroBase.Models private const double ToleranciaCoordenada = 1e-12; private const double DistanciaMaximaSaltoMapaM = 25.0; + [JsonIgnore] + private readonly OperacaoModel op; + #region PARAMETROS public double AnguloAberturaCurva { get; set; } = 25; // Angulo usado para deslocar o ponto de curva public double DistanciaProjecaoRua => (VariaveisEquipamento.DistanciaEntreEixosCm / 100.0 / 2.0) + - (Variaveis.OperacaoEmAndamento?.Parametros?.Controle?.DistanciaManobra ?? 3.0); // Distancia para projetar o primeiro ponto para fora do corredor + (op?.Parametros?.Controle?.DistanciaManobra ?? 3.0); // Distancia para projetar o primeiro ponto para fora do corredor public static double DistanciaEntrePontos { get; set; } = 0.8; // Distancia entre os pontos dentro do corredor public static double DistanciaEntrePontosCurva { get; set; } = 0.25; // Distancia entre os pontos durante a curva entre corredores private double DistanciaManobraEntreRuas { get; set; } = 3.0; // Distancia máxima para gerar a curva de conexão entre os corredores @@ -318,7 +322,7 @@ namespace AgroBase.Models public static double DistanciaMaximaEntreLeituras { get; private set; } public void AtualizarDistanciaMaximaEntreLeituras() { - double percentualVelocidade = Variaveis.OperacaoEmAndamento?.Controle?.PercentualVelocidadeSP ?? 0; + double percentualVelocidade = op?.Controle?.PercentualVelocidadeSP ?? 0; double velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(percentualVelocidade); double distanciaMaxima = velocidadeCarroMs / Math.Max(1, GPSService.TaxaAmostragemHz); @@ -673,7 +677,7 @@ namespace AgroBase.Models if (CorredorAtual != null) { bool operacaoEmAndamento = - Variaveis.OperacaoEmAndamento?.Sensoriamento?.Operacao?.StatusOperacaoAtual == + op?.Sensoriamento?.Operacao?.StatusOperacaoAtual == StatusOperacao.EmAndamento; if ((RetornandoBase && CorredorAtual.Dentro) || (!RetornandoBase && operacaoEmAndamento)) @@ -784,8 +788,6 @@ namespace AgroBase.Models public string TempoEstimadoRestante { get; private set; } public void AtualizarTempoEstimado(double? velocidadeSemErvasMs = null, double? velocidadeComErvasMs = null) { - var op = Variaveis.OperacaoEmAndamento; - if (op?.Sensoriamento == null || op?.Parametros?.Controle == null) { TempoEstimadoOperacao = "00:00:00"; @@ -871,8 +873,6 @@ namespace AgroBase.Models if (CorredorAtual.Idx > 0) return; if (!CorredorAtual.Dentro) return; - var op = Variaveis.OperacaoEmAndamento; - if (op?.Sensoriamento == null) return; @@ -952,7 +952,7 @@ namespace AgroBase.Models { MarcarVisitadoAte(_TrajetoriaFixa.Count - 1); - Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog( + op.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Operante, 100, @@ -976,7 +976,6 @@ namespace AgroBase.Models } private void AtualizarErroCombinado() { - var op = Variaveis.OperacaoEmAndamento; var gps = GPSPosicaoAtual; if (op?.Parametros?.Controle == null || gps == null || PontoAtual?.Posicao == null || ProximoPonto?.Posicao == null) @@ -1139,6 +1138,10 @@ namespace AgroBase.Models for (int i = idxInicial; i <= idxFinal; i++) { + if (PontoAtual.idxPonto == _TrajetoriaFixa[i].idxPonto) + { + + } _TrajetoriaFixa[i].AtualizarPropriedades( _gpsAtualCiclo, _gpsAnteriorCiclo, @@ -1261,7 +1264,7 @@ namespace AgroBase.Models if (!confirmou) return; - Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog( + op.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Operante, 100, @@ -1468,7 +1471,7 @@ namespace AgroBase.Models private bool VerificaInicioOperacaoMeioRua(bool considerarStatus) { - if (_TrajetoriaFixaDefinida && !VerificacaoInicialMeioRuaConcluida && ((considerarStatus && new List() { StatusOperacao.Aguardando, StatusOperacao.EmAndamento }.Contains(Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual)) || !considerarStatus)) + if (_TrajetoriaFixaDefinida && !VerificacaoInicialMeioRuaConcluida && ((considerarStatus && new List() { StatusOperacao.Aguardando, StatusOperacao.EmAndamento }.Contains(op.Sensoriamento.Operacao.StatusOperacaoAtual)) || !considerarStatus)) { var _posicaoAtual = GPSPosicaoAtual; if (_posicaoAtual == null) @@ -1487,7 +1490,7 @@ namespace AgroBase.Models if (!double.IsNaN(distanciaUltimoPonto) && !double.IsInfinity(distanciaUltimoPonto) && distanciaUltimoPonto < 5.0) { msg = $"Início no meio da rua ignorado: robô está a {distanciaUltimoPonto:0.00}m do fim. Nova operação começará do zero."; - Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, msg); + op?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, msg); Variaveis.MostrarLog($"[TRJ] {msg}"); return false; @@ -1504,7 +1507,7 @@ namespace AgroBase.Models ) { msg = "Inicio no meio da rua rejeitado: geometria e trajetoria discordam sobre o corredor atual."; - Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Falha, 0, msg); + op?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Falha, 0, msg); Variaveis.MostrarLog("[TRJ] " + msg); throw new InvalidOperationException(msg); } @@ -1517,14 +1520,14 @@ namespace AgroBase.Models CorredorAtual?.AtualizarDados(); msg = $"Início no meio da rua confirmado: idx={idxPontoMaisProximo}/{_TrajetoriaFixa.Count - 1}, marcados={idxPontoMaisProximo + 1}/{_TrajetoriaFixa.Count}."; - Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Alerta, 100, msg); + op?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Alerta, 100, msg); Variaveis.MostrarLog($"[TRJ] {msg}"); return true; } msg = $"Início no meio da rua ignorado: robô está fora do corredor. Nova operação começará do zero."; - Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, msg); + op?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, msg); Variaveis.MostrarLog($"[TRJ] {msg}"); } @@ -1534,7 +1537,6 @@ namespace AgroBase.Models private void VerificaFimCorredorSegmentacao() { - var op = Variaveis.OperacaoEmAndamento; double limiteFim = op?.Parametros?.Controle?.AnteciparManobraCorredorM ?? 0; if ( @@ -1972,7 +1974,7 @@ namespace AgroBase.Models catch (Exception ex) { Variaveis.MostrarLog("[TRJ] Falha no ciclo da trajetoria: " + ex.Message); - Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog( + op?.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Falha, 0, @@ -2005,8 +2007,6 @@ namespace AgroBase.Models private void LoopAtualizaDadosCore() { - var op = Variaveis.OperacaoEmAndamento; - if (!(op?.Parametros?.ControleAutomatico ?? false)) return; @@ -2023,8 +2023,6 @@ namespace AgroBase.Models private void AtualizarDadosControleRedis() { - var op = Variaveis.OperacaoEmAndamento; - if (op == null || !op.Sensoriamento.Operacao.OperacaoIniciada) { return; @@ -2270,7 +2268,7 @@ namespace AgroBase.Models DefinirPontoAtual(); DefinirProximoPonto(); - Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog( + op.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Operante, 100, @@ -3203,7 +3201,7 @@ namespace AgroBase.Models List> ruasParaProjetar; - if (Variaveis.OperacaoEmAndamento?.Mapa?.TipoMapa == TipoMapaOperacao.Corredores) + if (op?.Mapa?.TipoMapa == TipoMapaOperacao.Corredores) { throw new NotSupportedException( "O tipo de mapa Corredores ainda nao possui geracao validada para campo. " + @@ -3679,7 +3677,7 @@ namespace AgroBase.Models private void ProjetarTrajetoriaFixaCore(GPSModel posicaoRobo) { - if (Variaveis.OperacaoEmAndamento?.Sensoriamento?.Operacao?.Modo != ModoOperacao.MapaGPS) + if (op?.Parametros?.Modo != ModoOperacao.MapaGPS) return; List _trajetoriaFixa = new List(); @@ -3693,7 +3691,7 @@ namespace AgroBase.Models catch (Exception ex) { Variaveis.MostrarLog($"[TRJ] Trajetória rejeitada: {ex.Message}"); - Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Falha, 0, $"Trajetória rejeitada: {ex.Message}"); + op?.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Falha, 0, $"Trajetória rejeitada: {ex.Message}"); throw; } @@ -3810,7 +3808,8 @@ namespace AgroBase.Models Corredores[idx] = CorredorAtual; double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).FirstOrDefault(), CorredorAtual.FirstOrDefault()); - GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), DistanciaProjecaoRua * 0.8, anguloProjetar1); + double percentualAcrescimoPontoAberturaCurvaPrimeiroPonto = 0.7; + GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurvaPrimeiroPonto, anguloProjetar1); var ultimoPontoTrajetoria = _trajetoriaFixa.LastOrDefault(); @@ -3823,7 +3822,7 @@ namespace AgroBase.Models Direcao = direcaoAtual, Orientacao = GPSUtils.CalcularOrientacao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto), Visitado = false, - LarguraCorredor = larguraCorredorMenor + LarguraCorredor = LarguraCorredorPadrao // larguraCorredorMenor }; _trajetoriaFixa.Add(PontoInicial); @@ -3849,8 +3848,8 @@ namespace AgroBase.Models _trajetoriaFixa.Add(PontoTrajetoria); } - double percentualAcrescimoPontoAberturaCurva = 0.8; - double distancia_projetar = !ultimoCorredor ? (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva) : DistanciaProjecaoRua; + double percentualAcrescimoPontoAberturaCurvaUltimoPonto = 0.8; + double distancia_projetar = !ultimoCorredor ? (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurvaUltimoPonto) : DistanciaProjecaoRua; GPSModel ultimoPontoCorredorAtual = CorredorAtual.Last(); double anguloProjetar2 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(CorredorAtual.Count() - 2).FirstOrDefault(), ultimoPontoCorredorAtual); @@ -3882,7 +3881,7 @@ namespace AgroBase.Models Direcao = direcaoAtual, Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, UltimoPonto), Visitado = false, - LarguraCorredor = larguraCorredorMenor // ultimoCorredor ? larguraCorredorMenor : (larguraCorredorMenor * 0.8) + LarguraCorredor = LarguraCorredorPadrao * 1.1 // larguraCorredorMenor // ultimoCorredor ? larguraCorredorMenor : (larguraCorredorMenor * 0.8) }; _trajetoriaFixa.Add(PontoFinal); @@ -4376,7 +4375,6 @@ namespace AgroBase.Models double distanciaTotalAnterior = DistanciaTotal; double distanciaPercorridaAnterior = DistanciaPercorrida; var autonomiaAnterior = AutonomiaCorredor?.Clone(); - var op = Variaveis.OperacaoEmAndamento; var parametrosAnteriores = op?.Parametros; bool simulandoAnterior = op?.Simulando ?? false; bool? operacaoIniciadaAnterior = op?.Sensoriamento?.Operacao?.OperacaoIniciada; @@ -4407,7 +4405,7 @@ namespace AgroBase.Models } Variaveis.MostrarLog("[RETORNO] Trajetoria rejeitada: " + ex.Message); - Variaveis.OperacaoEmAndamento?.Sensoriamento?.InserirLog( + op?.Sensoriamento?.InserirLog( T_Code.Trj, StatusModulo.Falha, 0, @@ -4649,7 +4647,7 @@ namespace AgroBase.Models AtualizarTrajetoriaDinamicaCore(); AtualizarDistanciaRestante(); - Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, $"[RETORNO] Trajetória aplicada ({motivo}) - Pontos: {traj.Count}"); + op.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, $"[RETORNO] Trajetória aplicada ({motivo}) - Pontos: {traj.Count}"); } private List CortarFinalDaRota(List pontos, double distanciaAntesDoFimM) @@ -4689,8 +4687,6 @@ namespace AgroBase.Models private void AtualizarDadosOperacaoParaRetorno(List waypointsManual) { - var op = Variaveis.OperacaoEmAndamento; - if (op == null) throw new InvalidOperationException("Operacao indisponivel ao iniciar retorno."); @@ -4784,7 +4780,7 @@ namespace AgroBase.Models */ if (!ReferenceEquals( op, - Variaveis.OperacaoEmAndamento)) + op)) { Variaveis.MostrarLog( "[RETORNO/MAPSYNC] Operação mudou durante sincronização." @@ -5971,7 +5967,7 @@ namespace AgroBase.Models */ limiteDistancia = Math.Min(limiteDistancia, 0.80); - if (NaMargem && DistanciaAtual <= limiteDistancia) + if (NaMargem) // && DistanciaAtual <= limiteDistancia { Visitado = true; } diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index 14b8bac52..b5da57be5 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -324,7 +324,7 @@ namespace AgroBase.Services.Operadores { percent_vel_max = pControle.MovVelocidadeSErvasPercent, percent_vel_min = pControle.MovVelocidadeCErvasPercent, - percent_vel_curva = 20.0, + percent_vel_curva = 15.0, rampa_partida_pct = VariaveisEquipamento.PercentualVelMin, rampa_aceleracao_pct_s = 12.0, rampa_desaceleracao_pct_s = 18.0, @@ -548,7 +548,7 @@ namespace AgroBase.Services.Operadores $"[MAPSYNC] Tentativa={tentativa} | " + $"cmd={commandId} | " + $"rev={mapRevision} | " + - $"pontos={op.Trajetoria._TrajetoriaFixa.Count}" + $"pontos={op.Trajetoria._TrajetoriaFixa?.Count}" ); bool atualizado = diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py index e531d713f..9486e6031 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py @@ -1,6 +1,9 @@ import time -from shared.enums import ModoOperacao, StatusModulo, StatusCarroMapa, T_Code +from shared.enums import ( + ModoOperacao, StatusModulo, StatusCarroMapa, T_Code, + TipoMovimentoDirecional, +) from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey from manager_worker.config import mostrar_log from manager_worker.filtros import FiltroVelocidade @@ -67,6 +70,44 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) abs(_float(dir_cfg.get("angulo_max", 30.0), 30.0)) ) + # V5 - velocidade coerente com a política geométrica do MPC. + # Até ~5° o carro pode manter velocidade de cruzeiro. Entre 5° e 15° + # reduz progressivamente; a partir de 15° (faixa de MovimentoArco) usa + # velocidade de curva. Os parâmetros podem ser publicados em Mov sem + # exigir mudança imediata do contrato C#. + erro_heading_curva_forte_graus = _clamp( + _float( + mov_cfg.get("erro_heading_curva_forte_graus", 15.0), + 15.0, + ), + 3.0, + ang_max, + ) + erro_heading_curva_deadband_graus = _clamp( + _float( + mov_cfg.get("erro_heading_curva_deadband_graus", 5.0), + 5.0, + ), + 0.0, + max(0.0, erro_heading_curva_forte_graus - 0.5), + ) + + # Movimento/ângulo atualmente aplicados. Como o Processador calcula MOV + # antes de DIR, isto funciona como memória segura do ciclo anterior. O + # erro geométrico abaixo continua sendo o gatilho primário do ciclo atual. + tipo_direcional_atual = _enum_or( + TipoMovimentoDirecional, + controle.get( + "tipo_movimento_direcional", + TipoMovimentoDirecional.RodasDianteiras.value, + ), + TipoMovimentoDirecional.RodasDianteiras, + ) + angulo_direcional_atual = abs(_float( + controle.get("angulo_sp", 0.0), + 0.0, + )) + pontos_min_fim_corredor = max( 0, _int(mov_cfg.get("pontos_fim_corredor_reduzir", 3), 3) @@ -105,6 +146,12 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) status_carro = estado["status_carro"] erro_angular = estado["erro_orientacao"] + erro_angular_caminho = float( + estado.get("erro_angular_caminho", erro_angular) + ) + erro_angular_combinado = float( + estado.get("erro_angular_combinado", erro_angular) + ) pontos_fim_corredor = estado["pontos_fim_corredor"] estado_valido = estado["valido"] @@ -130,6 +177,12 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) ), } debug["pontos_fim_corredor"] = pontos_fim_corredor + debug["geometria_curva"] = { + "erro_heading_curva_forte_graus": round(erro_heading_curva_forte_graus, 3), + "erro_heading_curva_deadband_graus": round(erro_heading_curva_deadband_graus, 3), + "tipo_direcional_atual": tipo_direcional_atual.name, + "angulo_direcional_atual": round(angulo_direcional_atual, 3), + } if not estado_valido: motivo = estado.get("motivo", "Estado operacional inválido para movimento") @@ -156,8 +209,41 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) limitar_velocidade_por_weed = False reduzir_para_fim_corredor = False reduzir_por_curva = False + teto_rigido_curva = False + teto_curva_pct = None reduzir_por_ipb = False + # Erro geométrico que conversa com o seletor do MPC. Em tracking de + # caminho e no retorno usamos o heading da centerline, evitando que um + # simples offset lateral pareça uma curva. Em Direcionando preservamos + # o erro combinado porque ali a aquisição do alvo ainda é relevante. + if status_carro in [ + StatusCarroMapa.CaminhandoRua, + StatusCarroMapa.Direcionando, + StatusCarroMapa.RetornandoBase, + ]: + # Mesma referência geométrica do MPC V5: heading da centerline. + # Assim, rover paralelo porém deslocado não é confundido com curva. + erro_curva_graus = abs(erro_angular_caminho) + fonte_curva = "erro_angular_caminho" + else: + erro_curva_graus = abs(erro_angular_combinado) + fonte_curva = "erro_angular_combinado" + + arco_ja_aplicado = ( + tipo_direcional_atual == TipoMovimentoDirecional.MovimentoArco + ) + curva_forte_geometrica = ( + erro_curva_graus >= erro_heading_curva_forte_graus + ) + + debug["geometria_curva"].update({ + "erro_usado_graus": round(float(erro_curva_graus), 3), + "fonte_erro": fonte_curva, + "arco_ja_aplicado": bool(arco_ja_aplicado), + "curva_forte_geometrica": bool(curva_forte_geometrica), + }) + if status_carro == StatusCarroMapa.Parado: velocidade_sp = 0.0 hard_stop = True @@ -172,6 +258,8 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) ]: velocidade_sp = vel_curva reduzir_por_curva = True + teto_rigido_curva = True + teto_curva_pct = vel_curva debug["motivos"].append("movimento em curva/manobra") elif status_carro in [ @@ -179,16 +267,27 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) StatusCarroMapa.RetornandoBase ]: velocidade_sp = calcular_velocidade_relativa( - vel_min=vel_com_ervas, + vel_min=vel_curva, vel_max=vel_sem_ervas, - ang_max=ang_max, - erro_orientacao=erro_angular, - k=0.85, + ang_max=erro_heading_curva_forte_graus, + erro_orientacao=erro_curva_graus, + k=1.0, curva=0.80, - dead=10.0 + dead=erro_heading_curva_deadband_graus ) + reduzir_por_curva = bool( + erro_curva_graus > erro_heading_curva_deadband_graus + or arco_ja_aplicado + ) + teto_rigido_curva = bool(reduzir_por_curva) + if teto_rigido_curva: + teto_curva_pct = ( + vel_curva + if (curva_forte_geometrica or arco_ja_aplicado) + else velocidade_sp + ) debug["motivos"].append( - "direcionando/retornando: velocidade por erro angular combinado" + f"{status_carro.name}: velocidade pela geometria do caminho/alvo" ) elif status_carro == StatusCarroMapa.CaminhandoRua: @@ -244,15 +343,30 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) ) velocidade_livre = calcular_velocidade_relativa( - vel_min=vel_com_ervas, + vel_min=vel_curva, vel_max=vel_sem_ervas, - ang_max=ang_max, - erro_orientacao=erro_angular, + ang_max=erro_heading_curva_forte_graus, + erro_orientacao=erro_curva_graus, k=1.0, - curva=0.70, - dead=5.0 + curva=0.80, + dead=erro_heading_curva_deadband_graus ) + # Mesmo que o C# publique CaminhandoRua durante RetornoBase, uma + # curva forte da centerline não pode ser tratada como reta rápida. + # O heading do caminho é a mesma referência usada pelo MPC V5. + reduzir_por_curva = bool( + erro_curva_graus > erro_heading_curva_deadband_graus + or arco_ja_aplicado + ) + teto_rigido_curva = bool(reduzir_por_curva) + if teto_rigido_curva: + teto_curva_pct = ( + vel_curva + if (curva_forte_geometrica or arco_ja_aplicado) + else velocidade_livre + ) + debug["motivos"].append( "caminhando rua: velocidade por erro angular do caminho" ) @@ -287,6 +401,16 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) f"status_carro não tratado com segurança: {status_carro.name}" ) + debug["geometria_curva"].update({ + "reduzir_por_curva": bool(reduzir_por_curva), + "teto_rigido_curva": bool(teto_rigido_curva), + "teto_curva_pct": ( + None if teto_curva_pct is None + else round(float(teto_curva_pct), 3) + ), + "vel_curva_pct": round(float(vel_curva), 3), + }) + # Se já decidiu parar, não faz limitador tentar reviver velocidade. if hard_stop: _resetar_rampa_velocidade(filtro_vel) @@ -422,6 +546,29 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None) debug=debug, ) + # Teto rígido de curva/Arco. A rampa de segurança continua suavizando + # reduções moderadas, mas uma geometria já classificada como curva forte + # não pode passar um ciclo em velocidade de cruzeiro enquanto o MPC + # coloca o rover em MovimentoArco. + if teto_rigido_curva and teto_curva_pct is not None: + teto_curva_pct = _clamp(teto_curva_pct, vel_min_equip, vel_sem_ervas) + velocidade_filtrada = min(velocidade_filtrada, teto_curva_pct) + + try: + filtro_vel._ultimo_sp_campo = velocidade_filtrada + filtro_vel._ultimo_ts_campo = time.monotonic() + except Exception: + pass + + motivo_curva = ( + "curva forte/Arco" + if (curva_forte_geometrica or arco_ja_aplicado) + else "curva moderada" + ) + debug["limitadores"].append( + f"{motivo_curva}: teto geométrico em {teto_curva_pct:.1f}%" + ) + # Teto rígido do WeedWorker: # # - ervas detectadas: nunca ultrapassa vel_com_ervas; 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 bd24d5bec..ee460033f 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 @@ -430,6 +430,65 @@ class ControladorMPC: 8.0, min_value=1.0, max_value=self._direcionando_arco_entrada_graus, ) + # -------------------------------------------------------------- + # V5 - Política geométrica universal do path tracking. + # + # A lógica vale tanto para a passada agrícola quanto para a rota geral + # de RetornoBase: + # - heading muito errado -> MovimentoArco; + # - heading moderadamente errado -> RodasDianteiras; + # - heading praticamente alinhado + cross-track -> MovimentoDiagonal. + # + # A histerese 15°/8° evita ficar trocando Arco <-> Dianteira na + # fronteira. Os defaults herdam a política já validada em Direcionando. + # Manobras estruturais reais continuam com Arco obrigatório. + # -------------------------------------------------------------- + self._tracking_arco_entrada_graus = _safe_float( + parametros_mpc.get( + "tracking_arco_entrada_graus", + self._direcionando_arco_entrada_graus, + ), + self._direcionando_arco_entrada_graus, + min_value=3.0, + max_value=60.0, + ) + self._tracking_arco_saida_graus = _safe_float( + parametros_mpc.get( + "tracking_arco_saida_graus", + self._direcionando_arco_saida_graus, + ), + self._direcionando_arco_saida_graus, + min_value=1.0, + max_value=self._tracking_arco_entrada_graus, + ) + + # -------------------------------------------------------------- + # V4 - Entrada/saída de rua: Arco + RodasDianteiras competem. + # + # A preferência usa o HEADING DO CAMINHO, e não o bearing até o + # alvo. Assim, quando o corpo já está alinhado com a próxima rua, + # RodasDianteiras ganham vantagem e terminam a aquisição de forma + # suave. Arco continua favorecido quando o heading ainda está muito + # errado. + # + # Entre os dois limiares a preferência varia continuamente; a + # penalidade normal de troca de tipo fornece histerese adicional. + # -------------------------------------------------------------- + self._transicao_rua_dianteira_heading_graus = _safe_float( + parametros_mpc.get("transicao_rua_dianteira_heading_graus", 7.0), + 7.0, min_value=0.5, max_value=30.0, + ) + self._transicao_rua_arco_heading_graus = _safe_float( + parametros_mpc.get("transicao_rua_arco_heading_graus", 20.0), + 20.0, + min_value=self._transicao_rua_dianteira_heading_graus + 0.5, + max_value=60.0, + ) + self._transicao_rua_penalidade_modo = _safe_float( + parametros_mpc.get("transicao_rua_penalidade_modo", 1.0), + 1.0, min_value=0.0, max_value=5.0, + ) + # Amortecimento aplicado ao tracking de passada. MovimentoArco em # manobra preserva autoridade; RodasDianteiras/Diagonal são limitados. # MPC preserva toda a autoridade angular necessaria para a manobra. @@ -744,6 +803,46 @@ class ControladorMPC: return True return False + def _segmento_rua_puro_mesmo_corredor(self, idx_base): + """ + True somente quando a referência local está realmente em um trecho + operacional Rua -> Rua do MESMO corredor. + + Diferente de _segmento_operacional_mesmo_corredor(), aqui não aceitamos + ponto estrutural isolado. Isso é importante para distinguir: + + - recuperação dentro/ao lado de uma passada reta; + - entrada, saída, ligação ou cabeceira real. + + Na recuperação de uma passada, MovimentoArco é indesejado: ele dobra a + autoridade de yaw e pode transformar um desvio lateral em uma inversão + brusca de heading. + """ + n = len(self.pontos_info) + if n < 2: + return False + + idx = max(0, min(_safe_int(idx_base, 0), n - 1)) + + pares = [] + if idx + 1 < n: + pares.append((idx, idx + 1)) + if idx - 1 >= 0: + pares.append((idx - 1, idx)) + + for a, b in pares: + ca, cb = self._idx_corredor(a), self._idx_corredor(b) + + if ca < 0 or cb < 0 or ca != cb: + continue + + if self._ponto_estrutural(a) or self._ponto_estrutural(b): + continue + + return True + + return False + def _limitar_s_ao_corredor(self, s_proj, s_desejado, idx_base): """Não deixa o heading look-ahead enxergar a rua seguinte pela cabeceira.""" if self._s_nodes is None or len(self._s_nodes) == 0: @@ -812,42 +911,81 @@ class ControladorMPC: return float(self._wrap_pi(float(theta_ref) - float(theta_robo))) def _tracking_reta_permitido(self, contexto, idx_base, e_lat, erro_heading_rad): - """Decide se a política de passada pode assumir o controle. + """ + Decide se o path tracking geométrico deve assumir o controle. - CaminhandoRua sempre usa path tracking. Entrando/Manobrando só migra - cedo para a política de reta quando a referência ainda pertence ao - mesmo corredor e a pose já está perto o bastante da passada. Isso - evita o caso real em que o rover inicia alinhado mas recebe Arco por - burocracia de estado. + 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. + + 2) 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: + 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) - if _status_in(status, [StatusCarroMapa.CaminhandoRua]): + + if _status_in( + status, + [ + StatusCarroMapa.CaminhandoRua, + StatusCarroMapa.EntrandoRua, + StatusCarroMapa.SaindoRua, + StatusCarroMapa.Direcionando, + StatusCarroMapa.RetornandoBase, + ], + ): return True - if not _status_in(status, [StatusCarroMapa.EntrandoRua, StatusCarroMapa.Manobrando]): - return False - if not self._segmento_operacional_mesmo_corredor(idx_base): - return False + if _status_in(status, [StatusCarroMapa.Manobrando]): + return self._segmento_rua_puro_mesmo_corredor(idx_base) - return bool( - abs(float(e_lat)) <= self._lateral_reta_aquisicao_max_m - and abs(math.degrees(float(erro_heading_rad))) <= self._heading_reta_aquisicao_max_graus - ) + return False def _referencia_direcional_reta(self, e_lat, erro_heading_rad, velocidade, tipo_preferido=None): - """Escolhe modo e centro angular da busca para uma passada. + """Escolhe a geometria pelo estado geométrico rover x caminho. - - heading fora do alinhamento -> RodasDianteiras e corrige heading; - - heading alinhado -> MovimentoDiagonal para remover cross-track sem - girar o corpo; - - histerese estreita impede ficar 5-10 graus torto em diagonal. + 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. + + A decisão usa heading do CAMINHO, nunca bearing rover->waypoint. """ e_lat = float(e_lat) v = max(0.0, float(velocidade)) - e_head_deg = abs(math.degrees(float(erro_heading_rad))) + erro_heading_rad = float(erro_heading_rad) + 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. + 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: + 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) 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 manter_diag = ( tipo_prev == TipoMovimentoDirecional.MovimentoDiagonal @@ -870,21 +1008,15 @@ class ControladorMPC: lim = math.radians(min(self._angulo_diagonal_max_graus, self.angulo_max_graus)) return TipoMovimentoDirecional.MovimentoDiagonal, float(np.clip(delta, -lim, lim)) - # Fora da janela diagonal, prioridade absoluta é voltar a ficar - # paralelo. A parcela lateral perde força conforme o erro de heading - # cresce, para não transformar a correção em perseguição oscilante. - if abs(math.degrees(erro_heading_rad)) <= self._heading_deadband_graus: + # 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 e_head_deg <= self._heading_deadband_graus: e_head_eff = 0.0 else: - e_head_eff = float(erro_heading_rad) + e_head_eff = erro_heading_rad - fator_lat = 1.0 - min(1.0, e_head_deg / max(3.0, self._diagonal_saida_heading_graus * 3.0)) - lat_eff = 0.0 if abs(e_lat) <= self._lateral_deadband_m else e_lat - delta_lat = math.atan2( - self._ganho_lateral_dianteira * lat_eff, - v + self._velocidade_offset_lateral, - ) * fator_lat - delta = self._ganho_heading_reta * e_head_eff + delta_lat + delta = self._ganho_heading_reta * e_head_eff lim = math.radians(self.angulo_max_graus) return TipoMovimentoDirecional.RodasDianteiras, float(np.clip(delta, -lim, lim)) @@ -917,21 +1049,51 @@ class ControladorMPC: )) dt = _safe_float(dt_controle, 0.5, min_value=0.05, max_value=1.5) - tau = float(self._filtro_angulo_reta_tau_s) - alpha = 1.0 if tau <= 1e-6 else dt / (tau + dt) - filtrado = anterior + alpha * (desejado - anterior) - - max_delta = np.radians(self._taxa_angulo_reta_graus_s) * dt - filtrado = anterior + float(np.clip( - filtrado - anterior, - -max_delta, - max_delta, - )) - dead = np.radians(self._deadband_angulo_reta_graus) + + # Guarda de fase: + # + # Um filtro/rate-limit não pode continuar esterçando para a direita + # quando o MPC já pediu correção significativa para a esquerda, ou + # vice-versa. Nos logs reais isso chegou a persistir por vários ciclos + # e o rover atravessou a centerline antes de o comando cruzar zero. + # + # Na inversão significativa fazemos uma passagem deliberada por zero. + # É mais suave mecanicamente do que saltar direto de +A para -B e, + # principalmente, jamais aplica correção no sentido sabidamente errado. + inversao_significativa = bool( + abs(desejado) > dead + and abs(anterior) > dead + and (desejado * anterior) < 0.0 + ) + + if inversao_significativa: + filtrado = 0.0 + else: + tau = float(self._filtro_angulo_reta_tau_s) + alpha = 1.0 if tau <= 1e-6 else dt / (tau + dt) + filtrado = anterior + alpha * (desejado - anterior) + + max_delta = np.radians(self._taxa_angulo_reta_graus_s) * dt + filtrado = anterior + float(np.clip( + filtrado - anterior, + -max_delta, + max_delta, + )) + if abs(desejado) <= dead and abs(anterior) <= 2.0 * dead: filtrado = 0.0 + # Última barreira: mesmo diante de qualquer alteração futura no filtro, + # uma saída não nula nunca pode permanecer no hemisfério oposto ao + # comando desejado. + if ( + abs(desejado) > dead + and abs(filtrado) > dead + and (desejado * filtrado) < 0.0 + ): + filtrado = 0.0 + return float(np.clip( filtrado, -np.radians(self.angulo_max_graus), @@ -1476,7 +1638,7 @@ class ControladorMPC: (theta_trecho - theta_robo + math.pi) % (2.0 * math.pi) - math.pi ) - erro_maximo = math.radians(35.0 if estrutural else 45.0) + erro_maximo = math.radians(40.0 if estrutural else 50.0) # 35.0 e 45.0 ponto_ficou_para_tras = ( math.isfinite(avanco) @@ -1495,7 +1657,14 @@ class ControladorMPC: return self._proximo_nao_visitado(pontos_visitados) - def _calcular_pesos_movimento(self, contexto, erro_ori_graus, erro_lat_m, tracking_reta=False): + def _calcular_pesos_movimento( + self, + contexto, + erro_ori_graus, + erro_lat_m, + tracking_reta=False, + erro_heading_caminho_graus=None, + ): # helpers simples def _clamp(x, lo, hi): return lo if x < lo else hi if x > hi else x @@ -1564,20 +1733,62 @@ class ControladorMPC: erro_lat_abs = abs(float(erro_lat_m)) if tracking_reta: - # Na passada Arco é indesejado. Diagonal é o modo de - # recentralização quando heading está alinhado; dianteira - # recupera heading quando ele sai da janela estreita. - custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 3.0 - if erro_ori_abs <= self._diagonal_saida_heading_graus: + # V5: os pesos acompanham a mesma política geométrica usada na + # geração de candidatos. Embora normalmente apenas um tipo seja + # gerado por ciclo, manter o custo coerente evita preferências + # contraditórias nas simulações do horizonte. + if erro_heading_caminho_graus is None: + erro_heading_modo = erro_ori_abs + else: + erro_heading_modo = abs(float(erro_heading_caminho_graus)) + + if erro_heading_modo >= self._tracking_arco_entrada_graus: + custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 + custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 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 else: - custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0 + custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 1.5 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 0.0 - elif status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]: + custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0 + + elif status == StatusCarroMapa.Manobrando: + # Cabeceira/ligação estrutural real: preserva a autoridade do + # Arco. Na geração de candidatos V4 ele continua sendo o único + # tipo permitido neste estado quando não estamos em recuperação + # Rua -> Rua da V3. custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 - custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.0 - custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 2.0 + custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.5 + custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0 + + elif status in [StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua]: + # V4: usa o heading do CAMINHO para decidir qual geometria + # merece preferência. O bearing/erro combinado continua nos + # demais custos da manobra, mas não decide sozinho o modo. + if erro_heading_caminho_graus is None: + erro_heading_modo = erro_ori_abs + else: + erro_heading_modo = abs(float(erro_heading_caminho_graus)) + + e_lo = float(self._transicao_rua_dianteira_heading_graus) + e_hi = float(self._transicao_rua_arco_heading_graus) + + t = (erro_heading_modo - e_lo) / max(e_hi - e_lo, 1e-6) + t = _smoothstep01(_clamp(t, 0.0, 1.0)) + + penal = float(self._transicao_rua_penalidade_modo) + + # t=0: alinhado -> dianteira preferida + # t=1: desalinhado -> arco preferido + 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. + custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0 + else: custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 1.5 custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 0.0 @@ -2289,8 +2500,9 @@ class ControladorMPC: idx_base = _safe_int(ponto_alvo.get("_idx_base", 0), 0) # -------------------------------------------------------------- - # RETA: heading do CAMINHO + cross-track, sem mirar ponto. - # MANOBRA: mantém pure-pursuit para contornar cabeceira/ligação. + # PATH TRACKING: heading do CAMINHO + cross-track, sem mirar ponto. + # A geometria é escolhida por erro de heading/cross-track (V5). + # 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 @@ -2324,17 +2536,33 @@ class ControladorMPC: tipo_preferido_enum = _movimento_from_value(tipo_preferido) erro_heading_alvo_deg = abs(float(math.degrees(delta_theta))) - if _status_in( - status, - [ - StatusCarroMapa.EntrandoRua, - StatusCarroMapa.SaindoRua, - StatusCarroMapa.Manobrando, - ], - ): - # Manobras estruturais continuam com Arco obrigatório. + if _status_in(status, [StatusCarroMapa.Manobrando]): + # Manobra estrutural real continua com Arco obrigatório. + # A recuperação Rua -> Rua da V3 entra antes em + # tracking_reta=True, portanto não cai neste ramo. tipos_validos = [TipoMovimentoDirecional.MovimentoArco] + elif _status_in( + status, + [StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua], + ): + # V4: entrada/saída são estados de TRANSIÇÃO. Arco e + # RodasDianteiras são avaliados pelo mesmo MPC. + # + # Colocar o tipo anterior primeiro só estabiliza a divisão + # do orçamento quando sobra um candidato ímpar. A escolha + # final continua sendo pelo custo acumulado. + if tipo_preferido_enum == TipoMovimentoDirecional.RodasDianteiras: + tipos_validos = [ + TipoMovimentoDirecional.RodasDianteiras, + TipoMovimentoDirecional.MovimentoArco, + ] + else: + tipos_validos = [ + TipoMovimentoDirecional.MovimentoArco, + TipoMovimentoDirecional.RodasDianteiras, + ] + elif _status_in(status, [StatusCarroMapa.Direcionando]): # Fora da rua, um heading muito errado deve ser corrigido # com Arco, que possui raio de giro menor. Quando o rover @@ -2354,6 +2582,22 @@ class ControladorMPC: else: tipos_validos = [TipoMovimentoDirecional.RodasDianteiras] + elif _status_in(status, [StatusCarroMapa.RetornandoBase]): + # Guarda defensiva. Em V5 RetornandoBase deve entrar no + # path tracking acima; se algum contexto incompleto impedir + # isso, ainda assim nunca volta ao antigo fallback Front-only. + manter_arco = ( + tipo_preferido_enum == TipoMovimentoDirecional.MovimentoArco + and erro_heading_alvo_deg > self._tracking_arco_saida_graus + ) + if ( + erro_heading_alvo_deg >= self._tracking_arco_entrada_graus + or manter_arco + ): + tipos_validos = [TipoMovimentoDirecional.MovimentoArco] + else: + tipos_validos = [TipoMovimentoDirecional.RodasDianteiras] + else: tipos_validos = [TipoMovimentoDirecional.RodasDianteiras] @@ -3292,7 +3536,13 @@ class ControladorMPC: ) pesos = self._calcular_pesos_movimento( - contexto, erro_ori_sim, e_lat, tracking_reta=tracking_reta_sim + contexto, + erro_ori_sim, + e_lat, + tracking_reta=tracking_reta_sim, + erro_heading_caminho_graus=abs( + float(np.degrees(erro_heading_sim)) + ), ) (peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py index af8206101..cf9f8f9d7 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py @@ -134,7 +134,7 @@ VISUAL_DEFAULT_CONFIG = { "enabled": True, # Imprime resumo periódico no console. - "console_performance": True, + "console_performance": False, # Abre o timing da inferência em crop/resize/cor/session/decode. "runner_detailed_timing": True, 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 83de8c489..c233ea788 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 @@ -28,7 +28,7 @@ from visual_worker.utils import converter_valores_numpy from shared.perf_monitor import VisualPerfMonitor -CAMERA_MANAGER_VERSION = "production_v1_2026_09_12_runtime_priority_v3" +CAMERA_MANAGER_VERSION = "production_v1_2026_09_15_debug_geometry_sync_v4" PRODUCT_ASSEMBLY_SCHEMA = "multispec_module_params_assembly_v1" PRODUCT_MODULE_SCHEMA = "multispec_module_params_v3" @@ -2044,6 +2044,135 @@ class CameraManager: return True + @staticmethod + def _clamp01_debug(value, default=0.0): + try: + return max(0.0, min(1.0, float(value))) + except Exception: + return max(0.0, min(1.0, float(default))) + + def _resolver_geometria_debug_bicos(self, config, analise_debug=None): + """ + Resolve a geometria que o Debug deve desenhar. + + Autoridade, nesta ordem: + 1) estatísticas produzidas pelo WeedDetector na última decisão real; + 2) o próprio helper do WeedDetector, quando disponível; + 3) fallback local com o MESMO contrato: 0=topo, 1=pé. + + Assim o preview não mantém uma segunda implementação da lógica de atuação. + """ + cfg = config or {} + analise_debug = analise_debug or {} + + dados_visuais = analise_debug.get("dados_visuais", {}) or {} + estatisticas = dados_visuais.get("estatisticas", {}) or {} + estat_bicos = estatisticas.get("bicos", {}) or {} + estat_atuacao = estat_bicos.get("atuacao", {}) or {} + + ordem_invertida = bool( + estat_bicos.get( + "ordem_bicos_invertida", + cfg.get("inverter_ordem_bicos", False), + ) + ) + sentido_vertical_invertido = bool( + estat_bicos.get( + "sentido_vertical_invertido", + estat_atuacao.get( + "sentido_vertical_invertido", + cfg.get("inverter_sentido_vertical_bicos", False), + ), + ) + ) + + zona_ini = estat_atuacao.get("zona_atuacao_inicio_frac") + zona_fim = estat_atuacao.get("zona_atuacao_fim_frac") + eval_ini = estat_atuacao.get("zona_eval_inicio_frac") + eval_fim = estat_atuacao.get("zona_eval_fim_frac") + + # Se ainda não existe uma decisão real (startup, reset etc.), pede ao + # detector a mesma função usada pelo controle. Evita duplicar semântica. + if zona_ini is None or zona_fim is None: + detector = self.weed_detector + helper = getattr(detector, "_calcular_zona_atuacao_frac_from_config", None) + if callable(helper): + try: + zona_ini, zona_fim = helper(cfg) + except Exception: + zona_ini = None + zona_fim = None + + # Último fallback, mantido fail-soft para o Debug. Esta é a convenção + # oficial atual do WeedDetector: faixa é medida de cima para baixo. + if zona_ini is None or zona_fim is None: + faixa = self._clamp01_debug(cfg.get("faixa_atuacao_bicos", 0.70), 0.70) + area = self._clamp01_debug(cfg.get("area_atuacao_bicos", 0.10), 0.10) + area = max(0.001, area) + + zona_ini = faixa + zona_fim = min(1.0, faixa + area) + + if sentido_vertical_invertido: + zona_ini, zona_fim = 1.0 - zona_fim, 1.0 - zona_ini + + zona_ini = self._clamp01_debug(zona_ini, 0.0) + zona_fim = self._clamp01_debug(zona_fim, zona_ini) + if zona_fim < zona_ini: + zona_ini, zona_fim = zona_fim, zona_ini + + # Sem compensação de latência disponível, a zona avaliada coincide com + # a zona física. Quando o detector informar outra zona, mostramos ambas. + if eval_ini is None or eval_fim is None: + eval_ini, eval_fim = zona_ini, zona_fim + + eval_ini = self._clamp01_debug(eval_ini, zona_ini) + eval_fim = self._clamp01_debug(eval_fim, zona_fim) + if eval_fim < eval_ini: + eval_ini, eval_fim = eval_fim, eval_ini + + pulverizacao = dados_visuais.get("pulverizacao", {}) or {} + + return { + "zona_inicio_frac": float(zona_ini), + "zona_fim_frac": float(zona_fim), + "zona_eval_inicio_frac": float(eval_ini), + "zona_eval_fim_frac": float(eval_fim), + "ordem_bicos_invertida": bool(ordem_invertida), + "sentido_vertical_invertido": bool(sentido_vertical_invertido), + "pulverizacao": pulverizacao, + "fonte_detector": bool(estat_atuacao), + } + + @staticmethod + def _frac_vertical_para_pixels(inicio_frac, fim_frac, altura): + if altura <= 1: + return 0, 0 + + inicio = max(0.0, min(1.0, float(inicio_frac))) + fim = max(0.0, min(1.0, float(fim_frac))) + if fim < inicio: + inicio, fim = fim, inicio + + y0 = int(round(inicio * (altura - 1))) + y1 = int(round(fim * (altura - 1))) + y0 = max(0, min(altura - 1, y0)) + y1 = max(0, min(altura - 1, y1)) + return min(y0, y1), max(y0, y1) + + @staticmethod + def _intervalo_visual_bico(indice_bico, qtd_bicos, largura, ordem_invertida): + """Mapeia índice FÍSICO do bico para a coluna correspondente da imagem.""" + qtd = max(1, int(qtd_bicos)) + idx = int(indice_bico) + idx_visual = (qtd - 1 - idx) if ordem_invertida else idx + + x0 = int(round(idx_visual * largura / float(qtd))) + x1 = int(round((idx_visual + 1) * largura / float(qtd))) - 1 + x0 = max(0, min(largura - 1, x0)) + x1 = max(x0, min(largura - 1, x1)) + return x0, x1 + def get_debug_frame(self, mostrar=False, overlay_bgr=None): overlay = overlay_bgr if overlay_bgr is not None else self._ultimo_preview_overlay @@ -2057,12 +2186,21 @@ class CameraManager: "fps_loop": self._ultimo_loop_analise_fps, } + # A detecção publica controle final + análise sob _vida_lock. Tiramos o + # snapshot sob o mesmo lock para o Debug não misturar estados de ciclos + # diferentes enquanto a thread de detecção está atualizando os caches. + with self._vida_lock: + atuacao_bicos = dict(self._ultimo_controle or {}) + analise_debug = dict(self._ultima_analise or {}) + config_debug = dict(self.seg_config or {}) + # Não publica diretamente no cache: o preview producer faz a # publicação atômica depois de reduzir/comprimir o bundle inteiro. return self._montar_debug_overlay( overlay_bgr=overlay, - atuacao_bicos=self._ultimo_controle or {}, - config=self.seg_config or {}, + atuacao_bicos=atuacao_bicos, + config=config_debug, + analise_debug=analise_debug, metricas_perf=metricas_perf, mostrar=mostrar, ) @@ -2072,6 +2210,7 @@ class CameraManager: overlay_bgr, atuacao_bicos, config, + analise_debug=None, metricas_perf=None, mostrar=False, ): @@ -2080,6 +2219,10 @@ class CameraManager: return None metricas_perf = metricas_perf or {} + geometria = self._resolver_geometria_debug_bicos( + config=config, + analise_debug=analise_debug, + ) W, H = self._dbg_shape @@ -2104,25 +2247,68 @@ class CameraManager: self._dbg_img[...] = overlay_bgr qtd_bicos = int(config.get("qtd_bicos", self.qtd_bicos or 1) or 1) - zona_inicio = float(config.get("faixa_atuacao_bicos", 0.7)) - faixa_atuacao = float(config.get("area_atuacao_bicos", 0.1)) + qtd_bicos = max(1, qtd_bicos) - y_inicio = int((1.0 - zona_inicio) * H) - y_fim = int((1.0 - (zona_inicio + faixa_atuacao)) * H) - y0, y1 = min(y_inicio, y_fim), max(y_inicio, y_fim) + y0, y1 = self._frac_vertical_para_pixels( + geometria["zona_inicio_frac"], + geometria["zona_fim_frac"], + H, + ) + ey0, ey1 = self._frac_vertical_para_pixels( + geometria["zona_eval_inicio_frac"], + geometria["zona_eval_fim_frac"], + H, + ) + # ------------------------------------------------------------ + # Zona FÍSICA dos bicos. É a faixa configurada pelo operador. + # ------------------------------------------------------------ self._dbg_layer.fill(0) cv2.rectangle( self._dbg_layer, (0, y0), - (W, y1), + (W - 1, y1), (220, 220, 100), thickness=-1, ) - cv2.addWeighted(self._dbg_layer, 0.18, self._dbg_img, 0.82, 0, dst=self._dbg_img) - cv2.rectangle(self._dbg_img, (0, y0), (W, y1), (180, 180, 80), 2) + cv2.addWeighted( + self._dbg_layer, + 0.18, + self._dbg_img, + 0.82, + 0, + dst=self._dbg_img, + ) + cv2.rectangle( + self._dbg_img, + (0, y0), + (W - 1, y1), + (180, 180, 80), + 2, + ) + + # Zona REALMENTE avaliada pelo detector quando há antecipação por + # latência/velocidade. Se coincidir com a zona física, não duplica. + eval_diferente = abs(ey0 - y0) > 1 or abs(ey1 - y1) > 1 + if eval_diferente: + cv2.rectangle( + self._dbg_img, + (0, ey0), + (W - 1, ey1), + (255, 255, 0), + 2, + ) + cv2.putText( + self._dbg_img, + "EVAL", + (max(5, W - 60), max(16, min(H - 5, ey0 + 16))), + cv2.FONT_HERSHEY_SIMPLEX, + 0.45, + (255, 255, 0), + 1, + cv2.LINE_AA, + ) - largura_bico = W / float(max(qtd_bicos, 1)) cores = [ (0, 255, 0), (255, 0, 0), @@ -2134,33 +2320,64 @@ class CameraManager: (0, 0, 255), ] + ordem_invertida = bool(geometria["ordem_bicos_invertida"]) + + # ------------------------------------------------------------ + # Estado ON/OFF FINAL, já depois do gate de pulverização. + # Cada índice é o bico FÍSICO. A posição visual respeita + # inverter_ordem_bicos exatamente como o WeedDetector. + # ------------------------------------------------------------ self._dbg_layer.fill(0) - for i in range(qtd_bicos): - x0 = int(i * largura_bico) - x1 = int((i + 1) * largura_bico) + x0, x1 = self._intervalo_visual_bico( + i, + qtd_bicos, + W, + ordem_invertida, + ) cor = cores[i % len(cores)] - if atuacao_bicos.get(i, False): - cv2.rectangle(self._dbg_layer, (x0, y0), (x1, y1), cor, thickness=-1) + if bool(atuacao_bicos.get(i, False)): + cv2.rectangle( + self._dbg_layer, + (x0, y0), + (x1, y1), + cor, + thickness=-1, + ) - cv2.addWeighted(self._dbg_layer, 0.15, self._dbg_img, 0.85, 0, dst=self._dbg_img) + cv2.addWeighted( + self._dbg_layer, + 0.15, + self._dbg_img, + 0.85, + 0, + dst=self._dbg_img, + ) for i in range(qtd_bicos): - x0 = int(i * largura_bico) - x1 = int((i + 1) * largura_bico) + x0, x1 = self._intervalo_visual_bico( + i, + qtd_bicos, + W, + ordem_invertida, + ) cor = cores[i % len(cores)] - status = "ON" if atuacao_bicos.get(i, False) else "OFF" + status = "ON" if bool(atuacao_bicos.get(i, False)) else "OFF" cv2.rectangle(self._dbg_img, (x0, y0), (x1, y1), cor, 1) + + texto = f"Bico {i} {status}" + text_y = max(18, min(H - 6, y0 + 20)) cv2.putText( self._dbg_img, - f"Bico {i} {status}", - (x0 + 5, min(H - 5, y1 + 20)), + texto, + (x0 + 4, text_y), cv2.FONT_HERSHEY_SIMPLEX, - 0.55, + 0.48, cor, - 2, + 1, + cv2.LINE_AA, ) fps_infer = float(metricas_perf.get("fps_infer") or 0.0) @@ -2188,6 +2405,51 @@ class CameraManager: 2, ) + zona_txt = ( + f"Zona={geometria['zona_inicio_frac'] * 100:.0f}-" + f"{geometria['zona_fim_frac'] * 100:.0f}%" + ) + if eval_diferente: + zona_txt += ( + f" | Eval={geometria['zona_eval_inicio_frac'] * 100:.0f}-" + f"{geometria['zona_eval_fim_frac'] * 100:.0f}%" + ) + + zona_txt += ( + f" | V={'INV' if geometria['sentido_vertical_invertido'] else 'NORMAL'}" + f" H={'INV' if ordem_invertida else 'NORMAL'}" + ) + + cv2.putText( + self._dbg_img, + zona_txt, + (10, 88), + cv2.FONT_HERSHEY_SIMPLEX, + 0.48, + (230, 230, 230), + 1, + cv2.LINE_AA, + ) + + pulverizacao = geometria.get("pulverizacao", {}) or {} + if pulverizacao: + permitida = bool(pulverizacao.get("permitida", False)) + motivo = str(pulverizacao.get("motivo", "") or "") + gate_txt = f"Pulv: {'ON' if permitida else 'OFF'}" + if motivo and motivo != "ok": + gate_txt += f" ({motivo[:48]})" + + cv2.putText( + self._dbg_img, + gate_txt, + (10, 108), + cv2.FONT_HERSHEY_SIMPLEX, + 0.45, + (200, 255, 200) if permitida else (180, 180, 255), + 1, + cv2.LINE_AA, + ) + if mostrar: cv2.imshow("Debug Weed Worker", self._dbg_img) cv2.waitKey(1) diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py index cfc4661a0..b76c82bc8 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py @@ -170,8 +170,8 @@ WEED_DEFAULT_CONFIG = { # 1) Observabilidade / salvamento # ======================================================== "debug_visual": False, - "debug_perf": True, - "detector_debug_perf": True, + "debug_perf": False, + "detector_debug_perf": False, # RAW científico para pós-processamento. "posproc_intervalo_min_s": 5.0, @@ -256,7 +256,7 @@ WEED_DEFAULT_CONFIG = { # True no rover atual: # a aproximação física do alvo ocorre do pé para o topo da imagem. - "inverter_sentido_vertical_bicos": True, + "inverter_sentido_vertical_bicos": False, # Zona física de atuação no frame. # Ambos podem ser sobrescritos pela configuração da operação. diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py index 166718d68..f5c48bbed 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py @@ -25,11 +25,14 @@ class WeedDetector: - Desloca a memória pela distância real percorrida pelo rover. - Atua quando a evidência chega na zona física de atuação. - Convenção espacial: - - cell index 0 = região mais próxima da linha dos bicos. - - cell index maior = região mais distante, vista antes pela câmera. - - Conforme o rover anda, a memória se desloca de índices maiores - para índices menores. + Convenção espacial oficial: + - eixo vertical sempre usa coordenada de imagem: 0.0 = topo, 1.0 = pé. + - cell index 0 = topo da imagem. + - cell index N-1 = pé da imagem. + - inverter_sentido_vertical_bicos=False (rover atual): + alvo caminha topo -> pé; faixa=0.85, area=0.15 atua em 85%..100%. + - inverter_sentido_vertical_bicos=True: + alvo caminha pé -> topo; a mesma faixa é espelhada para 0%..15%. Este arquivo NÃO: - gera imagem; @@ -41,6 +44,7 @@ class WeedDetector: CONTRATO_OFICIAL = "target_binary" TARGET_ID = 1 + VERSION = "weed_detector_v2_2026_09_15_vertical_contract_fix" def __init__(self, config: Optional[dict] = None): # O CameraManager já mantém um snapshot de configuração. Quando ele é @@ -76,8 +80,14 @@ class WeedDetector: self.config.get("inverter_ordem_bicos", False) ) + # Convenção oficial: + # False = fluxo normal topo -> pé (rover atual). + # True = fluxo invertido pé -> topo. + # + # IMPORTANTE: esta flag altera tanto o sentido da memória espacial + # quanto a posição vertical da zona física de atuação. self.inverter_sentido_vertical_bicos = bool( - self.config.get("inverter_sentido_vertical_bicos", True) + self.config.get("inverter_sentido_vertical_bicos", False) ) self.detector_debug_perf = bool( @@ -168,11 +178,14 @@ class WeedDetector: 0.0 = topo da imagem 1.0 = pé da imagem - Normal: + Normal (inverter_sentido_vertical_bicos=False): faixa=0.90, area=0.10 -> 90% até 100%, embaixo. - Com inverter_sentido_vertical_bicos=True: + Invertido (inverter_sentido_vertical_bicos=True): faixa=0.90, area=0.10 -> 0% até 10%, em cima. + + Portanto, para o rover atual, em que o alvo se aproxima do topo para + o pé da imagem, a configuração correta é False. """ faixa = float(cfg.get("faixa_atuacao_bicos", 0.70) or 0.70) @@ -918,11 +931,12 @@ class WeedDetector: k_shift = max(0.0, k_shift) # A zona configurada representa a posição física fixa dos bicos. - # Com faixa=0,85, área=0,10 e inversão vertical, 85%..95% da imagem - # original vira 5%..15% na convenção interna. Essa zona NÃO muda de - # parado até a velocidade de referência. Somente a parcela de - # velocidade ACIMA da referência antecipa a avaliação para compensar - # o tempo de resposta do sistema. + # Exemplo no rover atual (sentido normal / False): + # faixa=0,85, área=0,15 -> 85%..100%, no pé da imagem. + # No sentido invertido / True a mesma faixa é espelhada para 0%..15%. + # Essa zona NÃO muda de parado até a velocidade de referência. Somente + # a parcela de velocidade ACIMA da referência antecipa a avaliação para + # compensar o tempo de resposta do sistema. vel_referencia = max( 0.0, float(cfg.get("velocidade_referencia_atuacao_mps", 0.65) or 0.65), @@ -1282,19 +1296,46 @@ class WeedDetector: vel_norm: float, cfg: dict, ): - # Convenção legada: - # zona_inicio e zona_altura são frações medidas a partir da parte inferior da imagem. - y_inicio = int((1.0 - zona_inicio) * h) - y_fim = int((1.0 - (zona_inicio + zona_altura)) * h) + """ + ROI do fallback legado usando a MESMA convenção do caminho oficial. - y_top = min(y_inicio, y_fim) - y_bot = max(y_inicio, y_fim) + Coordenada vertical: + 0.0 = topo + 1.0 = pé + False: alvo topo -> pé; 0.85 + 0.15 => faixa inferior. + True : alvo pé -> topo; a mesma faixa é espelhada para o topo. + + Os argumentos zona_inicio/zona_altura são mantidos na assinatura por + compatibilidade, mas o cálculo usa os valores de cfg para garantir que + exista uma única fonte de verdade. + """ + _ = zona_inicio, zona_altura + + zona_ini_frac, zona_fim_frac = self._calcular_zona_atuacao_frac_from_config(cfg) + + y_top = int(np.floor(zona_ini_frac * h)) + y_bot = int(np.ceil(zona_fim_frac * h)) + + y_top = max(0, min(h, y_top)) + y_bot = max(y_top, min(h, y_bot)) + + # Antecipação legada em pixels. A avaliação deve se deslocar para a + # região que o alvo ocupa ANTES de chegar aos bicos: + # fluxo topo -> pé : antecipa para cima (subtrai y) + # fluxo pé -> topo : antecipa para baixo (soma y) roi_shift_per_v = float(cfg.get("k_roi_shift_px_per_vnorm", 24.0)) - shift = int(roi_shift_per_v * self._clamp01(vel_norm)) + shift = int(max(0.0, roi_shift_per_v) * self._clamp01(vel_norm)) - y_top = max(0, y_top - shift) - y_bot = max(y_top, min(h, y_bot - shift)) + if getattr(self, "inverter_sentido_vertical_bicos", False): + y_top = min(h, y_top + shift) + y_bot = min(h, y_bot + shift) + else: + y_top = max(0, y_top - shift) + y_bot = max(0, y_bot - shift) + + if y_bot < y_top: + y_top, y_bot = y_bot, y_top return int(y_top), int(y_bot), int(shift)