ajustes mpc e trajetória, manobra mais suave
This commit is contained in:
Diego Freitas
parent
679f344e39
commit
295057ca67
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -197,7 +197,7 @@ namespace AgroBase.Forms.IHM.Operacao
|
|||
if (mapaAtual != null)
|
||||
{
|
||||
novaOperacao.Mapa = mapaAtual;
|
||||
await mapaAtual.LimparParametrizacaoMapaAsync();
|
||||
await mapaAtual.LimparParametrizacaoMapaAsync(novaOperacao);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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<List<GPSModel>> _ruasMapaReplay = new List<List<GPSModel>>();
|
||||
private List<PontoTrajetoriaModel> _trajetoriaFixaReplay = new List<PontoTrajetoriaModel>();
|
||||
|
||||
MapasModel Mapa = new MapasModel();
|
||||
private Visualizador3DService visualizador3D;
|
||||
//TrajetoriaMapaOperacaoModel Trajetoria = new TrajetoriaMapaOperacaoModel(new List<List<GPSModel>>());
|
||||
private MapaDinamicoModel MapaDinamico;
|
||||
private Visualizador3DService visualizador3D;
|
||||
private int idxMomentoAtual
|
||||
{
|
||||
get
|
||||
|
|
@ -165,12 +172,46 @@ namespace AgroBase.Forms.Operacoes
|
|||
LogsOperacao = JsonConvert.DeserializeObject<OperacaoSensoriamentoLogModel[]>(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, data.operacao)).ToList();
|
||||
LogsGPS = JsonConvert.DeserializeObject<GPSModel[]>(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<OperacaoParametrosModel>(
|
||||
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<OperacaoControleModel[]>(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, "controle")).ToList();
|
||||
LogsCooler = JsonConvert.DeserializeObject<CoolerControlDataModel[]>(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, "cooler")).ToList();
|
||||
|
|
@ -239,7 +280,7 @@ namespace AgroBase.Forms.Operacoes
|
|||
};
|
||||
MapasService.CriarArquivoMapa(new List<List<GPSModel>>() { 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<PontoTrajetoriaModel>();
|
||||
|
||||
var trajetoriaDinamica =
|
||||
MontarTrajetoriaDinamicaReplay(
|
||||
Log,
|
||||
LogTrj
|
||||
);
|
||||
|
||||
var ruasMapa =
|
||||
(_ruasMapaReplay?.Count ?? 0) > 0
|
||||
? _ruasMapaReplay
|
||||
: Variaveis.OperacaoEmAndamento
|
||||
?.Mapa
|
||||
?.TrajetoriaMapa
|
||||
?? new List<List<GPSModel>>();
|
||||
|
||||
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<GPSModel>(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<PontoTrajetoriaModel> MontarTrajetoriaDinamicaReplay(GPSModel posicaoAtual, OperacaoSensoriamentoLogTrajetoriaModel logTrajetoria)
|
||||
{
|
||||
var resultado = new List<PontoTrajetoriaModel>();
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<GPSModel>();
|
||||
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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -344,7 +344,7 @@ namespace AgroBase.Models
|
|||
_mapaEnviado = true;
|
||||
}
|
||||
|
||||
public void DefinirRuasSelecionadas(IEnumerable<string> ruas, bool popularTrajetoria = false)
|
||||
public void DefinirRuasSelecionadas(OperacaoModel op, IEnumerable<string> ruas, bool popularTrajetoria = false)
|
||||
{
|
||||
RuasPercorrer =
|
||||
ruas == null
|
||||
|
|
@ -361,12 +361,12 @@ namespace AgroBase.Models
|
|||
}
|
||||
|
||||
if (popularTrajetoria)
|
||||
PopularTrajetoriaMapa();
|
||||
PopularTrajetoriaMapa(op);
|
||||
}
|
||||
|
||||
public async Task DefinirRuasSelecionadasAsync(IEnumerable<string> ruas, bool popularTrajetoria = false)
|
||||
public async Task DefinirRuasSelecionadasAsync(OperacaoModel op, IEnumerable<string> 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<int> ruas)
|
||||
public Task DefinirRuasSelecionadasAsync(OperacaoModel op, IEnumerable<int> ruas)
|
||||
{
|
||||
IEnumerable<string> 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<string>(), false);
|
||||
DefinirRuasSelecionadas(op, new List<string>(), false);
|
||||
if (_paginaCarregada && browser?.CoreWebView2 != null)
|
||||
{
|
||||
await browser.CoreWebView2.ExecuteScriptAsync("definirSelecaoRuas([]);");
|
||||
|
|
|
|||
|
|
@ -476,7 +476,6 @@ namespace AgroBase.Models
|
|||
Simulando = false,
|
||||
GPSTrajetoria = new List<GPSModel>(),
|
||||
Mapa = new MapasModel(),
|
||||
Trajetoria = new TrajetoriaMapaOperacaoModel(new List<List<GPSModel>>()),
|
||||
ControleAnterior = new OperacaoControleModel(),
|
||||
Parametros = new OperacaoParametrosModel()
|
||||
{
|
||||
|
|
@ -499,10 +498,12 @@ namespace AgroBase.Models
|
|||
}
|
||||
}
|
||||
};
|
||||
op.Trajetoria = new TrajetoriaMapaOperacaoModel(op, new List<List<GPSModel>>());
|
||||
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();
|
||||
|
|
|
|||
|
|
@ -16,8 +16,9 @@ namespace AgroBase.Models
|
|||
{
|
||||
public class TrajetoriaMapaOperacaoModel
|
||||
{
|
||||
public TrajetoriaMapaOperacaoModel(List<List<GPSModel>> RuasMapa)
|
||||
public TrajetoriaMapaOperacaoModel(OperacaoModel _op, List<List<GPSModel>> 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>() { StatusOperacao.Aguardando, StatusOperacao.EmAndamento }.Contains(Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual)) || !considerarStatus))
|
||||
if (_TrajetoriaFixaDefinida && !VerificacaoInicialMeioRuaConcluida && ((considerarStatus && new List<StatusOperacao>() { 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<List<GPSModel>> 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<PontoTrajetoriaModel> _trajetoriaFixa = new List<PontoTrajetoriaModel>();
|
||||
|
|
@ -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<GPSModel> CortarFinalDaRota(List<GPSModel> pontos, double distanciaAntesDoFimM)
|
||||
|
|
@ -4689,8 +4687,6 @@ namespace AgroBase.Models
|
|||
|
||||
private void AtualizarDadosOperacaoParaRetorno(List<GPSModel> 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 =
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue