Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'produto' of http://zendioninc.com.br:3000/Zendion/agrobot_base into produto

This commit is contained in:
Diego Freitas 2026-09-16 15:39:38 -03:00
parent 8e4a0b4dc8 295057ca67
commit deff4dc2db
36 changed files with 30543 additions and 904 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

@ -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

2
AgroBase/AgroBase/Forms/IHM/Operacao/Parametros/ucParametrosMapa.cs
View File

@ -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;

2
AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoNavegacao.cs
View File

@ -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
{

2
AgroBase/AgroBase/Forms/IHM/Operacao/ucOperacaoParametros.cs
View File

@ -197,7 +197,7 @@ namespace AgroBase.Forms.IHM.Operacao
if (mapaAtual != null)
{
novaOperacao.Mapa = mapaAtual;
await mapaAtual.LimparParametrizacaoMapaAsync();
await mapaAtual.LimparParametrizacaoMapaAsync(novaOperacao);
}
/*

122
AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.Designer.cs generated
View File

@ -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;
}
}

157
AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs
View File

@ -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;
}
}
}

2
AgroBase/AgroBase/Forms/Simuladores/frmTreinamentoIA.cs
View File

@ -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();
}

24
AgroBase/AgroBase/Models/MapasModel.cs
View File

@ -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([]);");

13
AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs
View File

@ -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();

72
AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs
View File

@ -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;
}

4
AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs
View File

@ -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 =

54
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py
View File

@ -419,6 +419,7 @@ class CameraOak:
t0 = time.time()
frame = pkt.getCvFrame()
dur = time.time() - t0
frame_ts_host = time.time()
resultado = {
"erro": None,
@ -450,7 +451,7 @@ class CameraOak:
self.perf.tick(
"camera_rgb",
latencia_ms=dur * 1000.0,
frame_ts_host=self._rgb_cache_ts,
frame_ts_host=frame_ts_host,
frame_ts_device=pkt_ts_device,
seq=pkt_seq,
seq_delta=seq_delta,
@ -461,7 +462,7 @@ class CameraOak:
with self._cache_lock:
self._rgb_cache = frame
self._rgb_cache_ts = time.time()
self._rgb_cache_ts = frame_ts_host
self._rgb_cache_resultado = resultado
self.timestamp_ultimo_frame_rgb = self._rgb_cache_ts
self._rgb_seq = pkt_seq
@ -478,6 +479,7 @@ class CameraOak:
t0 = time.time()
frame = pkt.getFrame()
dur = time.time() - t0
frame_ts_host = time.time()
resultado = {
"erro": None,
@ -509,7 +511,7 @@ class CameraOak:
self.perf.tick(
"camera_depth",
latencia_ms=dur * 1000.0,
frame_ts_host=self._depth_cache_ts,
frame_ts_host=frame_ts_host,
frame_ts_device=pkt_ts_device,
seq=pkt_seq,
seq_delta=seq_delta,
@ -520,7 +522,7 @@ class CameraOak:
with self._cache_lock:
self._depth_cache = frame
self._depth_cache_ts = time.time()
self._depth_cache_ts = frame_ts_host
self._depth_cache_resultado = resultado
self.timestamp_ultimo_frame_depth = self._depth_cache_ts
self.ultimo_resultado_depth = resultado
@ -1062,6 +1064,50 @@ class CameraOak:
self._definir_heartbeat()
return frame, resultado
def requisitar_frame_rgb_ref(self):
"""
Retorna referência READ-ONLY ao latest frame RGB cacheado.
Diferente de requisitar_frame_rgb(), não copia ~6 MB por frame 1080p.
O cache da câmera apenas substitui a referência por um ndarray novo; não
modifica o ndarray anterior. Portanto o consumidor pode manter a referência
durante resize/inferência sem segurar o lock.
REGRA: o consumidor não deve escrever no ndarray retornado.
"""
with self._cache_lock:
resultado = dict(self._rgb_cache_resultado)
erro = resultado.get("erro") if self._is_erro_fatal_depthai(resultado.get("erro")) else None
frame = self._rgb_cache
if erro:
self._falha_fatal_depthai(erro)
return None, resultado
self._definir_heartbeat()
return frame, resultado
def requisitar_frame_depth_ref(self):
"""Mesmo contrato read-only do RGB, aplicado ao depth latest-frame."""
if not self.tem_depth:
return None, {
"erro": "Camera nao possui sensor de profundidade",
"duracao": 0,
"frame_valido": False,
}
with self._cache_lock:
resultado = dict(self._depth_cache_resultado)
erro = resultado.get("erro") if self._is_erro_fatal_depthai(resultado.get("erro")) else None
frame = self._depth_cache
if erro:
self._falha_fatal_depthai(erro)
return None, resultado
self._definir_heartbeat()
return frame, resultado
def requisitar_frame_depth(self):
if not self.tem_depth:
return None, {

587
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py
View File

@ -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,12 +146,43 @@ 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"]
debug["status_carro"] = status_carro.name
debug["erro_orientacao"] = erro_angular
debug["erro_orientacao_fonte"] = estado.get(
"erro_orientacao_fonte",
"nao_informada"
)
debug["erros_orientacao"] = {
"selecionado": round(float(erro_angular), 4),
"combinado": round(
float(estado.get("erro_angular_combinado", erro_angular)),
4
),
"caminho": round(
float(estado.get("erro_angular_caminho", erro_angular)),
4
),
"proximo_ponto": round(
float(estado.get("erro_angular_proximo_ponto", erro_angular)),
4
),
}
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")
@ -133,10 +205,45 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
resetar_filtro = False
reduzir_para_pulverizar = False
reduzir_por_weed_indisponivel = False
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
@ -151,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 [
@ -158,26 +267,64 @@ 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(
f"{status_carro.name}: velocidade pela geometria do caminho/alvo"
)
debug["motivos"].append("direcionando/retornando com redução por erro angular")
elif status_carro == StatusCarroMapa.CaminhandoRua:
info_ervas = _ler_weed_worker()
ervas_no_radar = (
pulverizador_automatico and
info_ervas["atualizado"] and
info_ervas["ervas_no_radar"]
)
# O WeedWorker é a autoridade para dizer se há ervas no radar.
#
# Contrato atual:
# DadosWeedWorker.analise = envelope
# envelope.analise = dados_visuais reais
#
# _ler_weed_worker() normaliza esse contrato e também aceita o
# formato legado (campos diretamente em DadosWeedWorker.analise).
#
# Se o campo booleano não existir em uma versão legada, usamos
# ema_global apenas como fallback.
if info_ervas["campo_ervas_no_radar_presente"]:
ervas_detectadas = bool(info_ervas["ervas_no_radar"])
else:
ervas_detectadas = (
info_ervas["percentual_ervas"] >= ervas_min_percent
)
reduzir_para_pulverizar = bool(ervas_no_radar)
if pulverizador_automatico:
if info_ervas["atualizado"]:
reduzir_para_pulverizar = bool(ervas_detectadas)
else:
# Falha conservadora de qualidade de aplicação:
# percepção desconhecida NÃO significa "sem ervas".
# Mantém a velocidade de pulverização até a análise voltar,
# sem parar a operação por si só.
reduzir_por_weed_indisponivel = True
limitar_velocidade_por_weed = bool(
reduzir_para_pulverizar
or reduzir_por_weed_indisponivel
)
reduzir_para_fim_corredor = (
pontos_fim_corredor >= 0 and
@ -185,20 +332,57 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
)
debug["weed"] = info_ervas
debug["weed"]["pulverizador_automatico"] = bool(
pulverizador_automatico
)
debug["weed"]["ervas_detectadas_efetivo"] = bool(
ervas_detectadas
)
debug["weed"]["limitar_velocidade"] = bool(
limitar_velocidade_por_weed
)
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"
)
if reduzir_para_pulverizar:
velocidade_sp = min(velocidade_livre, vel_com_ervas)
debug["motivos"].append("ervas no radar: reduzindo para pulverizar")
debug["motivos"].append(
"ervas no radar: reduzindo para pulverizar"
)
elif reduzir_por_weed_indisponivel:
velocidade_sp = min(velocidade_livre, vel_com_ervas)
debug["motivos"].append(
"WeedWorker ausente/desatualizado: "
"mantendo velocidade conservadora de pulverização"
)
elif reduzir_para_fim_corredor:
velocidade_sp = min(velocidade_livre, vel_com_ervas)
@ -217,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)
@ -284,7 +478,8 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
if usar_imu_movimento:
vel_imu, imu_resetar, imu_stop, imu_debug = _aplicar_limitador_imu(
velocidade_atual=velocidade_sp
velocidade_atual=velocidade_sp,
vel_min_equip=vel_min_equip
)
debug["imu"] = imu_debug
@ -329,7 +524,7 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
# Aceleração normal é filtrada para não dar tranco.
reducao_seguranca = bool(
reduzir_para_pulverizar
limitar_velocidade_por_weed
or reduzir_para_fim_corredor
or reduzir_por_curva
or reduzir_por_ipb
@ -351,23 +546,59 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
debug=debug,
)
# Correção crítica:
# havendo erva no radar, a saída nunca pode superar a mínima definida.
if reduzir_para_pulverizar:
velocidade_filtrada = min(
velocidade_filtrada,
vel_com_ervas
)
# 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)
# Sincroniza o estado da rampa com a saída realmente aplicada.
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"ervas no radar: teto rígido em {vel_com_ervas:.1f}%"
f"{motivo_curva}: teto geométrico em {teto_curva_pct:.1f}%"
)
# Teto rígido do WeedWorker:
#
# - ervas detectadas: nunca ultrapassa vel_com_ervas;
# - WeedWorker desatualizado com pulverizador automático ligado:
# também não assume que a rua está livre.
#
# Esse clamp acontece DEPOIS da rampa para não existir overshoot
# transitório acima da velocidade de aplicação.
if limitar_velocidade_por_weed:
velocidade_filtrada = min(
velocidade_filtrada,
vel_com_ervas
)
# Sincroniza o estado da rampa com a saída realmente aplicada.
# Assim, quando a condição desaparecer, a aceleração volta a
# acontecer pela rampa normal em vez de saltar para o SP antigo.
try:
filtro_vel._ultimo_sp_campo = velocidade_filtrada
filtro_vel._ultimo_ts_campo = time.monotonic()
except Exception:
pass
if reduzir_para_pulverizar:
motivo_teto = "ervas no radar"
else:
motivo_teto = "WeedWorker indisponível/desatualizado"
debug["limitadores"].append(
f"{motivo_teto}: teto rígido em {vel_com_ervas:.1f}%"
)
velocidade_filtrada = round(
@ -415,6 +646,33 @@ def _resolver_estado_operacional(
contexto,
erro_orientacao
):
"""
Resolve o estado operacional e escolhe a referência angular usada
EXCLUSIVAMENTE para a política de velocidade.
Política alinhada ao MPC Path Tracking V2:
- CaminhandoRua:
usa erro_angular_caminho.
O que importa para liberar velocidade é o corpo do rover estar
alinhado com a orientação local da passada. Deslocamento lateral
pode ser corrigido pelo MovimentoDiagonal sem penalizar a velocidade
como se o rover estivesse "torto".
- Direcionando / RetornandoBase:
usa erro_angular combinado.
Fora da passada, a aquisição geométrica do alvo continua relevante.
- EntrandoRua / SaindoRua / Manobrando:
a velocidade base é vel_curva, portanto o erro selecionado não
altera a velocidade nesses estados. Mantemos o combinado para
diagnóstico/coerência.
- erro_orientacao explícito:
continua tendo prioridade para preservar compatibilidade com
chamadas externas/legadas.
"""
if op_modo in [ModoOperacao.MapaGPS, ModoOperacao.RetornoBase]:
trajetoria = _dict(contexto.get("Trajetoria", {}))
@ -424,6 +682,10 @@ def _resolver_estado_operacional(
"motivo": "Trajetoria ausente no contexto",
"status_carro": StatusCarroMapa.Parado,
"erro_orientacao": 0.0,
"erro_orientacao_fonte": "trajetoria_ausente",
"erro_angular_combinado": 0.0,
"erro_angular_caminho": 0.0,
"erro_angular_proximo_ponto": 0.0,
"pontos_fim_corredor": -1,
}
@ -433,33 +695,86 @@ def _resolver_estado_operacional(
StatusCarroMapa.Parado
)
if erro_orientacao is None:
erro_orientacao = _float(trajetoria.get("erro_angular", 0.0), 0.0)
erro_combinado = _float(
trajetoria.get("erro_angular", 0.0),
0.0
)
erro_caminho = _float(
trajetoria.get(
"erro_angular_caminho",
erro_combinado
),
erro_combinado
)
erro_proximo_ponto = _float(
trajetoria.get(
"erro_angular_proximo_ponto",
erro_combinado
),
erro_combinado
)
if erro_orientacao is not None:
erro_selecionado = _float(
erro_orientacao,
erro_combinado
)
fonte_erro = "override_explicito"
elif status_carro == StatusCarroMapa.CaminhandoRua:
# Na passada, o heading local da rua é a referência correta.
# O bearing para waypoint não deve reduzir velocidade só porque
# o MPC está corrigindo offset lateral em MovimentoDiagonal.
erro_selecionado = erro_caminho
fonte_erro = "erro_angular_caminho"
else:
# Aquisição, retorno e demais estados mantêm a referência
# combinada que incorpora a geometria do alvo.
erro_selecionado = erro_combinado
fonte_erro = "erro_angular_combinado"
corredor = _dict(trajetoria.get("CorredorAtual", {}))
pontos_fim_corredor = _int(corredor.get("pontos_restantes", -1), -1)
pontos_fim_corredor = _int(
corredor.get("pontos_restantes", -1),
-1
)
return {
"valido": True,
"motivo": "",
"status_carro": status_carro,
"erro_orientacao": _float(erro_orientacao, 0.0),
"erro_orientacao": float(erro_selecionado),
"erro_orientacao_fonte": fonte_erro,
"erro_angular_combinado": float(erro_combinado),
"erro_angular_caminho": float(erro_caminho),
"erro_angular_proximo_ponto": float(erro_proximo_ponto),
"pontos_fim_corredor": pontos_fim_corredor,
}
if op_modo == ModoOperacao.MapeamentoVisual:
dados_vw = _dict(ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}))
segmentacao = _dict(dados_vw.get("segmentacao", {}))
dados_vw = _dict(
ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
)
segmentacao = _dict(
dados_vw.get("segmentacao", {})
)
seg_ts = _float(
segmentacao.get(
"ts",
dados_vw.get("ts_segmentacao", dados_vw.get("momento", 0.0))
dados_vw.get(
"ts_segmentacao",
dados_vw.get("momento", 0.0)
)
),
0.0
)
seg_atualizada, idade_ms = _timestamp_atualizado(seg_ts, max_idade_s=1.0)
seg_atualizada, idade_ms = _timestamp_atualizado(
seg_ts,
max_idade_s=1.0
)
if not segmentacao or not seg_atualizada:
motivo = "segmentacao visual ausente/desatualizada"
@ -472,31 +787,61 @@ def _resolver_estado_operacional(
"motivo": motivo,
"status_carro": StatusCarroMapa.Parado,
"erro_orientacao": 0.0,
"erro_orientacao_fonte": "segmentacao_invalida",
"erro_angular_combinado": 0.0,
"erro_angular_caminho": 0.0,
"erro_angular_proximo_ponto": 0.0,
"pontos_fim_corredor": -1,
}
status_carro = _enum_or(
StatusCarroMapa,
segmentacao.get("status_corredor", StatusCarroMapa.Parado.value),
segmentacao.get(
"status_corredor",
StatusCarroMapa.Parado.value
),
StatusCarroMapa.Parado
)
if erro_orientacao is None:
erro_orientacao = _float(segmentacao.get("erro_angular", 0.0), 0.0)
erro_visual = _float(
segmentacao.get("erro_angular", 0.0),
0.0
)
if erro_orientacao is not None:
erro_selecionado = _float(
erro_orientacao,
erro_visual
)
fonte_erro = "override_explicito"
else:
erro_selecionado = erro_visual
fonte_erro = "segmentacao_visual"
return {
"valido": True,
"motivo": "",
"status_carro": status_carro,
"erro_orientacao": _float(erro_orientacao, 0.0),
"erro_orientacao": float(erro_selecionado),
"erro_orientacao_fonte": fonte_erro,
"erro_angular_combinado": float(erro_visual),
"erro_angular_caminho": float(erro_visual),
"erro_angular_proximo_ponto": float(erro_visual),
"pontos_fim_corredor": -1,
}
return {
"valido": False,
"motivo": f"Modo de operação sem estratégia de movimento: {op_modo.name}",
"motivo": (
"Modo de operação sem estratégia de movimento: "
f"{op_modo.name}"
),
"status_carro": StatusCarroMapa.Parado,
"erro_orientacao": 0.0,
"erro_orientacao_fonte": "modo_sem_estrategia",
"erro_angular_combinado": 0.0,
"erro_angular_caminho": 0.0,
"erro_angular_proximo_ponto": 0.0,
"pontos_fim_corredor": -1,
}
@ -506,34 +851,119 @@ def _resolver_estado_operacional(
# ============================================================
def _ler_weed_worker():
try:
dados = _dict(ContextoGlobalRedis.get(CtxKey.DadosWeedWorker, {}))
analise = _dict(dados.get("analise", {}))
"""
Lê e normaliza o contrato do WeedWorker.
Contrato atual publicado pelo CameraManager:
DadosWeedWorker = {
"ts_publicacao": ...,
"ts_analise": ...,
"analise": {
"ts_analise": ...,
"fps_model": ...,
"fps_inferencia": ...,
"infer_ms": ...,
"infer_gpu_ms": ...,
"analise": {
"ervas_no_radar": ...,
"estatisticas": {
"erva": {
"ema_global": ...
}
},
...
}
}
}
Também aceita o formato legado, no qual ervas_no_radar e estatisticas
ficam diretamente em DadosWeedWorker["analise"].
"""
try:
dados = _dict(
ContextoGlobalRedis.get(CtxKey.DadosWeedWorker, {})
)
envelope = _dict(dados.get("analise", {}))
# Contrato atual possui envelope["analise"].
# No contrato legado, o próprio envelope já é a análise.
analise_interna = _dict(envelope.get("analise", {}))
if analise_interna:
analise = analise_interna
esquema = "nested_v2"
else:
analise = envelope
esquema = "flat_legacy"
# Preferência de timestamp:
# 1) timestamp do envelope da análise atual;
# 2) timestamp dentro da análise legada;
# 3) timestamp raiz do DadosWeedWorker.
ts = _float(
analise.get(
"ts",
analise.get(
"timestamp",
dados.get("ts_analise", dados.get("momento", 0.0))
envelope.get(
"ts_analise",
envelope.get(
"ts",
envelope.get(
"timestamp",
analise.get(
"ts",
analise.get(
"timestamp",
dados.get(
"ts_analise",
dados.get(
"ts_publicacao",
dados.get("momento", 0.0)
)
)
)
)
)
)
),
0.0
)
atualizado, idade_ms = _timestamp_atualizado(ts, max_idade_s=1.5)
atualizado, idade_ms = _timestamp_atualizado(
ts,
max_idade_s=1.5
)
estatisticas = _dict(analise.get("estatisticas", {}))
erva = _dict(estatisticas.get("erva", {}))
estatisticas = _dict(
analise.get("estatisticas", {})
)
erva = _dict(
estatisticas.get("erva", {})
)
ervas_no_radar = _bool(analise.get("ervas_no_radar", False))
percentual_ervas = _float(erva.get("ema_global", 0.0), 0.0)
campo_ervas_presente = "ervas_no_radar" in analise
ervas_no_radar = _bool(
analise.get("ervas_no_radar", False)
)
percentual_ervas = _float(
erva.get(
"ema_global",
analise.get("percentual_ervas_no_radar", 0.0)
),
0.0
)
return {
"atualizado": atualizado,
"atualizado": bool(atualizado),
"idade_ms": idade_ms,
"ervas_no_radar": ervas_no_radar,
"ervas_no_radar": bool(ervas_no_radar),
"campo_ervas_no_radar_presente": bool(
campo_ervas_presente
),
"percentual_ervas": percentual_ervas,
"timestamp_analise": ts,
"esquema": esquema,
}
except Exception as e:
@ -541,7 +971,10 @@ def _ler_weed_worker():
"atualizado": False,
"idade_ms": None,
"ervas_no_radar": False,
"campo_ervas_no_radar_presente": False,
"percentual_ervas": 0.0,
"timestamp_analise": 0.0,
"esquema": "erro",
"erro": str(e),
}
@ -643,7 +1076,7 @@ def _aplicar_limitador_visual(
# Limitador IMU
# ============================================================
def _aplicar_limitador_imu(*, velocidade_atual):
def _aplicar_limitador_imu(*, velocidade_atual, vel_min_equip):
debug = {
"habilitado": True,
"aplicado": False,
@ -693,7 +1126,41 @@ def _aplicar_limitador_imu(*, velocidade_atual):
if risk_level_num <= 0:
return velocidade_atual, False, False, debug
vel_nova = float(velocidade_atual) * velocidade_factor
velocidade_atual = max(0.0, float(velocidade_atual))
vel_min_equip = max(0.0, float(vel_min_equip))
# Attention/risk são REDUÇÕES, não ordens de parada.
#
# Se a IMU, sozinha, levar o SP abaixo da velocidade mínima útil do
# equipamento, limita no piso operacional em vez de transformar um
# nível attention/risk em parada.
#
# O min(..., velocidade_atual) é proposital: a IMU nunca pode elevar
# uma velocidade que já tenha sido reduzida por outro limitador
# (Visual Worker, IPB, fim de corredor, pulverização etc.).
vel_calculada = velocidade_atual * velocidade_factor
if velocidade_atual >= vel_min_equip and vel_calculada > 0.0:
vel_nova = min(
velocidade_atual,
max(vel_min_equip, vel_calculada)
)
else:
# Se outro limitador já entregou algo abaixo do piso, a IMU não
# "ressuscita" a velocidade. A regra geral posterior decide se
# esse valor deve virar parada.
vel_nova = min(velocidade_atual, vel_calculada)
debug["velocidade_entrada"] = round(velocidade_atual, 3)
debug["velocidade_calculada"] = round(vel_calculada, 3)
debug["velocidade_saida"] = round(vel_nova, 3)
debug["vel_min_equip"] = round(vel_min_equip, 3)
debug["piso_operacional_aplicado"] = bool(
velocidade_atual >= vel_min_equip
and 0.0 < vel_calculada < vel_min_equip
and abs(vel_nova - vel_min_equip) < 1e-9
)
resetar_filtro = False
return vel_nova, resetar_filtro, False, debug

981
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py
View File

File diff suppressed because it is too large Load Diff

803
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py
View File

File diff suppressed because it is too large Load Diff

186
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py
View File

@ -50,7 +50,7 @@ _CONFIG_LOCK = threading.Lock()
# ============================================================
# CONFIG V1 - VISUAL WORKER
# CONFIG V2 - VISUAL WORKER / ONNX FIELD CONTRACT
# ============================================================
@ -117,12 +117,39 @@ VISUAL_DEFAULT_CONFIG = {
"debug_visual": False,
"posproc_intervalo_min_s": 5.0,
# Publica logs de performance no console.
"debug_perf": False,
# Tamanho dos frames de preview/debug enviados ao C#.
"preview_size": (1280, 720),
# Preview producer lateral: só trabalha quando há consumidor.
"preview_fps": 2.0,
"preview_request_timeout_s": 0.35,
"preview_cache_fresh_s": 0.75,
# Replay/debug usa JPEG leve. PosProcessamento continua em PNG/RGB original.
"replay_jpeg_quality": 70,
# Telemetria técnica. Tudo que custa payload/log extra fica atrás deste bloco.
"telemetry": {
# Publica performance_visual no Redis.
"enabled": True,
# Imprime resumo periódico no console.
"console_performance": False,
# Abre o timing da inferência em crop/resize/cor/session/decode.
"runner_detailed_timing": True,
# Inclui o vetor completo de 8 probabilidades do status no debug.
# Desligado por padrão porque top1/top2/margin/entropy já bastam no campo.
"include_status_probs": False,
# Anexa contrato/runtime do ONNX em performance_visual.
"publish_runtime_info": True,
# Loga contrato/provider/cache na inicialização.
"log_runtime_startup": True,
},
# ============================================================
# 2) Frequências dos loops
@ -144,100 +171,81 @@ VISUAL_DEFAULT_CONFIG = {
# ============================================================
# 3) Modelo ONNX/TensorRT - segmentação de ruas/corredor
# 3) Runtime oficial do modelo de corredor - ONNX FIELD V2
# ============================================================
# Runtime oficial da v1.
"runtime_backend": "onnx",
"onnx_provider": "tensorrt",
# O caminho do ONNX vem do Redis/equipamento: path_ia_model_ruas_seg.
# Resolução, ROI, mean/std, layout, nomes de I/O e classes NÃO ficam aqui.
# Tudo isso é lido da metadata embutida no próprio ONNX.
"model_runtime": {
"provider": "tensorrt",
# Resolução esperada pelo ONNX: [W, H].
"ia_resolution": [1024, 576],
# Se True: TensorRT -> CUDA -> CPU, sempre expondo degradação na telemetria.
# Se False: falha a inicialização quando o provider solicitado não estiver disponível.
"allow_provider_fallback": True,
# ROI vertical da segmentação.
# 0.0 + 1.0 = frame inteiro.
"ia_roi_begin": 0.0,
"ia_roi_size": 1.0,
# CameraOak configura ColorCamera.ColorOrder.RGB. O ONNX também exige RGB,
# então o hot path evita conversão de cor. Se a fonte mudar no futuro,
# BGR continua suportado explicitamente pelo runner.
"camera_frame_color": "RGB",
# Contrato validado do modelo modelseg-2_0.onnx:
# entrada: float32 [1, 3, 576, 1024], RGB 0..1, NCHW
# saída 1: semantic_logits
# saída 2: label_probs
"onnx_has_preprocess": False,
"onnx_input_layout": "nchw_float",
"onnx_input_scale": 1.0 / 255.0,
"onnx_seg_output_format": "logits",
# TensorRT. O cache real vira <root>/<sha-do-onnx>/ automaticamente.
"trt_fp16_enable": True,
"trt_engine_cache_enable": True,
"trt_engine_cache_root": "./trt_cache_visual_worker",
"trt_max_workspace_size": None,
# Nomes das entradas/saídas.
# input_name None deixa o runner detectar automaticamente.
"onnx_input_name": None,
"onnx_seg_output_name": "semantic_logits",
"onnx_aux_output_name": "label_probs",
"onnx_aux_output_format": "probs",
# ONNX Runtime.
"ort_intra_op_num_threads": 1,
"ort_inter_op_num_threads": 1,
# Auxiliar do status de corredor.
# True = retorna label_id, label_name, label_conf.
# False = inclui também vetor de probabilidades.
"use_compact_aux": True,
# Se existir .contract.json, valida o SHA256 do ONNX no startup.
"verify_sidecar_sha256": True,
# Logs internos do runner ONNX.
"debug_timing": False,
"debug_session": True,
# Checagem de IDs únicos a cada inferência. Útil em laboratório, desnecessária no campo.
"validate_outputs_each_inference": False,
},
# ============================================================
# 4) TensorRT
# 4) Análise geométrica + resolução temporal do status
# ============================================================
"trt_fp16_enable": True,
"trt_engine_cache_enable": True,
"trt_engine_cache_path": "./trt_cache_visual_worker",
# Deixe None salvo se não quiser fixar workspace.
"trt_max_workspace_size": None,
# Threads do ONNX Runtime.
# Para TensorRT/CUDA, 1 costuma ser suficiente e evita ruído.
"onnx_intra_op_num_threads": 1,
"onnx_inter_op_num_threads": 1,
# ============================================================
# 5) SegmentacaoManager v1
# ============================================================
# Este bloco controla apenas a análise da máscara:
# pred_ids + label_probs -> dados_visuais.
# IDs semânticos e nomes de status vêm do contrato ONNX.
"segmentacao": {
# Inclui debug textual no payload de segmentação.
# Não gera imagem.
"include_debug": False,
# Inclui timing interno do SegmentacaoManager.
"debug_timing": False,
# ID das classes no modelo de segmentação.
"id_nao_navegavel": 0,
"id_navegavel": 1,
# Frações verticais usadas para estimar centro/ângulo do corredor.
# 0.0 = topo, 1.0 = base.
# Frações verticais para centro/largura/ângulo. 0=topo, 1=base.
"scanline_fracs": (0.96, 0.86, 0.74, 0.62, 0.50, 0.38, 0.26),
# A região próxima ao robô está na base da imagem.
"near_is_bottom": True,
# Suavização das saídas usadas pelo controle.
# Suavização geométrica.
"ema_alpha_ang": 0.25,
"ema_alpha_lat": 0.25,
"ema_alpha_conf": 0.20,
# Histerese temporal do status do corredor.
"status_window_s": 1.5,
# Status v2: voto temporal ponderado + troca rápida quando a cabeça está muito segura.
"status_window_s": 0.90,
"status_decay_tau_s": 0.35,
"status_expected_fps": 10.0,
# Confiança da cabeça auxiliar ONNX.
# >= accept: modelo manda.
# >= soft: modelo ajuda quando heurística está indefinida.
"model_conf_accept": 0.70,
"model_conf_soft": 0.45,
# Modelo manda quando confiança E margem são boas.
"model_conf_accept": 0.72,
"model_margin_accept": 0.15,
# Zona intermediária: só ajuda quando concorda com heurística ou ela está indefinida.
"model_conf_soft": 0.55,
"model_margin_soft": 0.06,
# Transições muito fortes podem furar a inércia após N frames consecutivos.
"model_conf_fast": 0.90,
"model_margin_fast": 0.30,
"fast_switch_min_frames": 2,
# Pesos base do voto temporal.
"status_weight_model": 1.35,
"status_weight_model_soft": 1.00,
"status_weight_heuristic": 0.75,
"status_weight_agreement_bonus": 0.40,
# Grid leve interna para centro/fallback do corredor.
"corridor_grid_rows": 6,
@ -250,7 +258,7 @@ VISUAL_DEFAULT_CONFIG = {
"prefer_prev_weight": 0.65,
"prefer_width_weight": 1.00,
# Heurística de status quando o modelo auxiliar não está confiante.
# Heurística fica como fallback/segundo sensor até termos evidência de campo para removê-la.
"thr_parado_global": 0.30,
"thr_direcionando_global": 0.82,
"thr_caminhando_score": 0.48,
@ -260,7 +268,7 @@ VISUAL_DEFAULT_CONFIG = {
# ============================================================
# 6) Grid de confiança/custo
# 5) Grid de confiança/custo
# ============================================================
"grid": {
# Formato global da grid: (cols, rows).
@ -408,7 +416,7 @@ VISUAL_DEFAULT_CONFIG = {
# ============================================================
# 7) GPU Priority Controller
# 6) GPU Priority Controller
# ============================================================
# Controla FPS do Visual Worker conforme saúde do Weed Worker.
"gpu_priority": {
@ -510,24 +518,16 @@ DET_DEFAULT_CONFIG = {
def aplicar_overrides_redis_seg(cfg: dict) -> dict:
equipamento = ContextoGlobalRedis.get_equipamento()
# Modelo ONNX oficial de ruas/corredor.
cfg["ia_onnx_path"] = equipamento.get("path_ia_model_ruas_seg")
# Labelmap da segmentação.
cfg["ia_labelmap_path"] = equipamento.get("path_ia_labelmap_ruas_seg")
# Único artefato de IA exigido pelo Visual Worker v2.
# Classes, normalização, ROI e I/O vivem dentro do contrato do ONNX.
cfg["model_path"] = equipamento.get("path_ia_model_ruas_seg")
return cfg
def normalizar_config_runtime_seg(cfg: dict) -> dict:
# Alias único interno, para logs e validações.
cfg["onnx_model_path"] = cfg.get("ia_onnx_path")
if not cfg.get("ia_onnx_path"):
mostrar_log("[WARN] path do modelo ONNX de ruas não definido no Redis/equipamento.")
if not cfg.get("ia_labelmap_path"):
mostrar_log("[WARN] path do labelmap de ruas não definido no Redis/equipamento.")
if not cfg.get("model_path"):
mostrar_log("[WARN] path do ONNX de corredor não definido no Redis/equipamento.")
# Garante tuplas onde as dataclasses esperam tuplas.
cfg["preview_size"] = tuple(cfg.get("preview_size", (1280, 720)))
@ -556,10 +556,6 @@ def normalizar_config_runtime_seg(cfg: dict) -> dict:
if "veto_labels" in det and not isinstance(det["veto_labels"], set):
det["veto_labels"] = set(det["veto_labels"])
fuser = grid.get("fuser", {}) or {}
if "y_range_m" in fuser:
fuser["y_range_m"] = tuple(fuser["y_range_m"])
cfg["grid"] = grid
segmentacao = cfg.get("segmentacao", {}) or {}
@ -615,6 +611,8 @@ def load_seg_config():
# Cópia profunda simples dos blocos aninhados.
# Evita compartilhar dict interno entre chamadas.
cfg["telemetry"] = dict(VISUAL_DEFAULT_CONFIG["telemetry"])
cfg["model_runtime"] = dict(VISUAL_DEFAULT_CONFIG["model_runtime"])
cfg["segmentacao"] = dict(VISUAL_DEFAULT_CONFIG["segmentacao"])
cfg["grid"] = {
"grid_shape": VISUAL_DEFAULT_CONFIG["grid"]["grid_shape"],

23
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/main.py
View File

@ -20,8 +20,13 @@ def main():
# Importar e configurar OpenCV antes dos módulos do Visual Worker,
# pois eles podem carregar OpenCV internamente.
import cv2
cv2.setNumThreads(2)
opencv_threads = max(1, int(os.environ.get("VISUAL_OPENCV_THREADS", "4")))
cv2.setNumThreads(opencv_threads)
cv2.ocl.setUseOpenCL(False)
print(
f"[visual][PERF] OpenCV threads requested={opencv_threads} "
f"effective={cv2.getNumThreads()} preview_cache=True"
)
from shared.enums import VisualWorkerCommandType, TipoFrameCamera
from shared.utils import encode_image_base64
@ -53,6 +58,7 @@ def main():
time.sleep(0.1)
def redis_callback(dados):
acao = None
try:
acao = VisualWorkerCommandType(dados.get("cmd", 0))
@ -60,14 +66,14 @@ def main():
mx_id = dados.get("params")
if mx_id is not None:
iniciar_camera_manager(mx_id)
elif acao == VisualWorkerCommandType.CalibrarProfundidade:
n_frames = dados.get("params", 50)
get_camera_manager().gerar_grid_ref(n_frames)
elif acao == VisualWorkerCommandType.AtualizarSaudeCamera:
get_camera_manager().atualizar_saude_camera()
elif acao == VisualWorkerCommandType.GetCameraFrame:
tipo = TipoFrameCamera(dados.get("params", TipoFrameCamera.Rgb.value))
frame = get_camera_manager().get_selected_frame(tipo)
frame = get_camera_manager().get_selected_frame(
tipo,
aguardar_atualizacao=True,
)
if frame is not None:
base64_img = encode_image_base64(frame)
if base64_img is not None:
@ -82,14 +88,11 @@ def main():
pasta = dados.get("params", {}).get("caminho", "frames_salvos")
tipos = dados.get("params", {}).get("tipos", [])
get_camera_manager().salvar_frames(tipos, nome, pasta)
elif acao == VisualWorkerCommandType.EnviarImagemMock:
caminho = dados.get("params", "")
get_camera_manager().segmentacao_manager.use_mock = caminho != ""
get_camera_manager().segmentacao_manager.img_mock = caminho
else:
mostrar_log(f"⚠️ Comando desconhecido: {acao.name}")
except Exception as e:
mostrar_log(f"Erro ao processar comando {acao.name}: {e}")
acao_nome = getattr(acao, "name", str(dados.get("cmd", "desconhecido")))
mostrar_log(f"Erro ao processar comando {acao_nome}: {e}")
def inicializar():
mostrar_log("🚀 Iniciando Visual Worker...")

267
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py
View File

@ -2,7 +2,6 @@ from __future__ import annotations
import time
from dataclasses import dataclass, field
from enum import IntEnum
from collections import deque
from typing import Any, Dict, List, Optional, Sequence, Tuple
@ -12,23 +11,19 @@ import numpy as np
from shared.enums import StatusCarroMapa
class ClassesSegmentacao(IntEnum):
NAONAVEGAVEL = 0
NAVEGAVEL = 1
@dataclass
class SegmentacaoConfig:
"""
Configuração v1 do analisador semântico do Visual Worker.
Configuração v2 do analisador semântico do Visual Worker.
Esta classe controla apenas análise de máscara.
Não controla ONNX, câmera, render, preview, stream nem Redis.
"""
# IDs esperados na máscara de segmentação.
id_nao_navegavel: int = int(ClassesSegmentacao.NAONAVEGAVEL)
id_navegavel: int = int(ClassesSegmentacao.NAVEGAVEL)
# IDs são OBRIGATORIAMENTE injetados do contrato ONNX v2.
# -1 faz o componente falhar fechado se alguém tentar inicializá-lo isolado.
id_nao_navegavel: int = -1
id_navegavel: int = -1
# Scanlines usadas para estimar centro, largura e ângulo do corredor.
# Frações verticais do frame: 0.0 = topo, 1.0 = base.
@ -42,13 +37,27 @@ class SegmentacaoConfig:
ema_alpha_lat: float = 0.25
ema_alpha_conf: float = 0.20
# Histórico para estabilizar status do corredor.
status_window_s: float = 1.5
# Histórico ponderado para estabilizar status do corredor.
status_window_s: float = 0.90
status_decay_tau_s: float = 0.35
status_expected_fps: float = 10.0
# Prioridade do status vindo da cabeça auxiliar ONNX.
model_conf_accept: float = 0.70
model_conf_soft: float = 0.45
# Cabeça de status ONNX v2: confiança + margem top1-top2.
model_conf_accept: float = 0.72
model_margin_accept: float = 0.15
model_conf_soft: float = 0.55
model_margin_soft: float = 0.06
# Troca rápida para transições muito seguras.
model_conf_fast: float = 0.90
model_margin_fast: float = 0.30
fast_switch_min_frames: int = 2
# Pesos do voto temporal.
status_weight_model: float = 1.35
status_weight_model_soft: float = 1.00
status_weight_heuristic: float = 0.75
status_weight_agreement_bonus: float = 0.40
# Grid leve usada para pontuar corredor e fallback de centro.
corridor_grid_rows: int = 6
@ -70,15 +79,16 @@ class SegmentacaoConfig:
# Debug leve: inclui métricas extras no retorno, sem criar imagens.
include_debug: bool = True
include_model_probs: bool = False
debug_timing: bool = False
class SegmentacaoManager:
"""
Analisador de segmentação semântica v1.
Analisador de segmentação semântica v2.
Responsabilidade única:
pred_ids + aux_result -> dados_visuais
pred_ids + status_result -> dados_visuais
Não renderiza.
Não colore máscara.
@ -94,6 +104,14 @@ class SegmentacaoManager:
):
self.config = self._normalizar_config(config)
if int(self.config.id_navegavel) < 0 or int(self.config.id_nao_navegavel) < 0:
raise RuntimeError(
"IDs semânticos não foram injetados pelo contrato ONNX v2: "
f"nav={self.config.id_navegavel} non_nav={self.config.id_nao_navegavel}"
)
if int(self.config.id_navegavel) == int(self.config.id_nao_navegavel):
raise RuntimeError("IDs navegável/não-navegável são iguais no contrato.")
# Mantidos como metadados úteis, mas não usados no caminho quente.
self.color_map = color_map
self.classes = classes
@ -105,6 +123,8 @@ class SegmentacaoManager:
self._status_hist = deque(maxlen=max_len)
self._status_final_hist = deque(maxlen=2)
self._strong_candidate: Optional[StatusCarroMapa] = None
self._strong_candidate_count: int = 0
self._ema_ang: Optional[float] = None
self._ema_lat: Optional[float] = None
@ -118,22 +138,22 @@ class SegmentacaoManager:
# API principal
# ---------------------------------------------------------------------
def segmentar(self, predictions: np.ndarray, aux_result: Optional[Dict[str, Any]] = None):
def segmentar(self, predictions: np.ndarray, status_result: Optional[Dict[str, Any]] = None):
"""
Compatibilidade nominal para o CameraManager atual.
Para a v1, o método preferido é analisar().
No contrato v2, o método preferido é analisar().
Retorna:
resultado, erro
"""
try:
resultado = self.analisar(predictions, aux_result=aux_result)
resultado = self.analisar(predictions, status_result=status_result)
return resultado, None
except Exception as e:
self._ultimo_erro = f"Erro na análise de segmentação: {e}"
return None, self._ultimo_erro
def analisar(self, predictions: np.ndarray, aux_result: Optional[Dict[str, Any]] = None) -> Dict[str, Any]:
def analisar(self, predictions: np.ndarray, status_result: Optional[Dict[str, Any]] = None) -> Dict[str, Any]:
t0 = time.perf_counter()
pred = self._validar_predictions(predictions)
@ -161,7 +181,7 @@ class SegmentacaoManager:
pred=pred,
mask_nav=mask_nav,
corridor_score=corridor_score,
aux_result=aux_result,
status_result=status_result,
)
t_status = time.perf_counter()
@ -182,10 +202,26 @@ class SegmentacaoManager:
"status_corredor_anterior": int(status_before.value),
"status_corredor_anterior_nome": str(status_before.name),
# Telemetria compacta da segunda cabeça. Não depende do debug pesado.
"status_fonte": str(status_debug.get("origem_status", "desconhecida")),
"status_modelo_id": (status_debug.get("modelo") or {}).get("label_id"),
"status_modelo_nome": (status_debug.get("modelo") or {}).get("status_nome"),
"status_modelo_conf": (status_debug.get("modelo") or {}).get("conf"),
"status_modelo_margin": (status_debug.get("modelo") or {}).get("margin"),
"status_modelo_entropy": (status_debug.get("modelo") or {}).get("entropy"),
"status_modelo_second_nome": (status_debug.get("modelo") or {}).get("second_name"),
"status_modelo_second_conf": (status_debug.get("modelo") or {}).get("second_conf"),
"status_fast_switch": bool(status_debug.get("fast_switch", False)),
"centros_corredor": centros,
"larguras_px": larguras_px,
}
if self.config.include_model_probs:
probs = (status_debug.get("modelo") or {}).get("probs")
if probs is not None:
dados_visuais["status_modelo_probs"] = probs
if self.config.include_debug:
dados_visuais["status_corredor_debug"] = status_debug
dados_visuais["debug_corredor"] = {
@ -670,29 +706,100 @@ class SegmentacaoManager:
pred: np.ndarray,
mask_nav: np.ndarray,
corridor_score: float,
aux_result: Optional[Dict[str, Any]],
status_result: Optional[Dict[str, Any]],
):
model_status, model_conf, model_debug = self._status_from_aux(aux_result)
model_status, model_conf, model_margin, model_debug = self._status_from_model(
status_result
)
heur_status, heur_debug = self._status_heuristico(pred, mask_nav, corridor_score)
heur_status, heur_debug = self._status_heuristico(
pred, mask_nav, corridor_score
)
origem = "heuristica"
status_now = heur_status
weight = float(self.config.status_weight_heuristic)
if model_status is not None and model_conf >= self.config.model_conf_accept:
model_fast = (
model_status is not None
and model_conf >= self.config.model_conf_fast
and model_margin >= self.config.model_margin_fast
)
model_accept = (
model_status is not None
and model_conf >= self.config.model_conf_accept
and model_margin >= self.config.model_margin_accept
)
model_soft = (
model_status is not None
and model_conf >= self.config.model_conf_soft
and model_margin >= self.config.model_margin_soft
)
if model_accept:
status_now = model_status
origem = "modelo"
elif model_status is not None and model_conf >= self.config.model_conf_soft:
# Modelo com média confiança só vence quando a heurística está indefinida.
origem = "modelo_fast" if model_fast else "modelo"
weight = float(self.config.status_weight_model)
# Confiança e margem entram suavemente no peso, sem explodir a janela.
weight *= 0.75 + 0.25 * float(np.clip(model_conf, 0.0, 1.0))
weight *= 0.85 + 0.15 * float(np.clip(model_margin, 0.0, 1.0))
if heur_status == model_status:
weight += float(self.config.status_weight_agreement_bonus)
origem += "_agree"
elif model_soft:
if heur_status == StatusCarroMapa.Indefinido:
status_now = model_status
origem = "modelo_soft"
weight = float(self.config.status_weight_model_soft)
elif heur_status == model_status:
status_now = model_status
origem = "modelo_soft_agree"
weight = (
float(self.config.status_weight_model_soft)
+ float(self.config.status_weight_agreement_bonus)
)
else:
status_now = heur_status
origem = "heuristica_com_modelo_soft"
origem = "heuristica_modelo_soft_discorda"
now = self._now()
self._status_hist.append((status_now, now, float(weight), origem))
# Transições realmente fortes não precisam aguardar toda a janela temporal.
fast_switch = False
if model_fast and model_status is not None:
if self._strong_candidate == model_status:
self._strong_candidate_count += 1
else:
self._strong_candidate = model_status
self._strong_candidate_count = 1
if self._strong_candidate_count >= max(1, int(self.config.fast_switch_min_frames)):
status_final = model_status
fast_switch = True
# Re-semeia a janela com o estado forte para não voltar no frame seguinte.
self._status_hist.clear()
self._status_hist.append((
status_final,
now,
float(self.config.status_weight_model)
+ float(self.config.status_weight_agreement_bonus),
"fast_switch_seed",
))
else:
status_final, weighted_scores = self._weighted_status()
else:
self._strong_candidate = None
self._strong_candidate_count = 0
status_final, weighted_scores = self._weighted_status()
if fast_switch:
weighted_scores = {status_final.name: 1.0}
self._status_hist.append((status_now, self._now()))
status_final = self._majority_status()
self._status_final_hist.append(status_final)
status_before = (
@ -705,34 +812,64 @@ class SegmentacaoManager:
"origem_status": origem,
"modelo": model_debug,
"heuristica": heur_debug,
"status_now": status_now.name,
"status_final": status_final.name,
"candidate_weight": round(float(weight), 4),
"weighted_scores": weighted_scores,
"fast_switch": bool(fast_switch),
"fast_candidate": (
self._strong_candidate.name
if self._strong_candidate is not None
else None
),
"fast_candidate_count": int(self._strong_candidate_count),
}
return status_now, status_final, status_before, debug
def _status_from_aux(self, aux_result: Optional[Dict[str, Any]]):
if not aux_result or aux_result.get("type") != "label":
return None, 0.0, {
def _status_from_model(self, status_result: Optional[Dict[str, Any]]):
if not status_result:
return None, 0.0, 0.0, {
"disponivel": False,
"motivo": "sem_aux_result",
"motivo": "sem_status_result",
}
try:
label_id = int(aux_result.get("label_id", StatusCarroMapa.Indefinido.value))
conf = float(aux_result.get("label_conf", 0.0))
status = StatusCarroMapa(label_id)
label_name = str(status_result.get("label_name", "")).strip()
conf = float(status_result.get("confidence", 0.0))
margin = float(status_result.get("margin", 0.0))
entropy = float(status_result.get("entropy", 1.0))
return status, conf, {
if not label_name:
raise ValueError("label_name vazio")
# Mapeamento oficial é por NOME. A ordem dos IDs do treino pode mudar
# sem embaralhar a enum do controle.
status = StatusCarroMapa[label_name]
debug = {
"disponivel": True,
"status": int(status.value),
"status_nome": str(status.name),
"label_name": aux_result.get("label_name"),
"label_id": int(status_result.get("label_id", -1)),
"label_name": label_name,
"conf": round(conf, 4),
"margin": round(margin, 4),
"entropy": round(entropy, 4),
"second_id": int(status_result.get("second_id", -1)),
"second_name": status_result.get("second_name"),
"second_conf": round(float(status_result.get("second_confidence", 0.0)), 4),
}
except Exception as e:
return None, 0.0, {
if "probs" in status_result:
debug["probs"] = list(status_result["probs"])
return status, conf, margin, debug
except Exception as exc:
return None, 0.0, 0.0, {
"disponivel": False,
"motivo": f"aux_invalido: {e}",
"motivo": f"status_modelo_invalido: {exc}",
}
def _status_heuristico(
@ -827,33 +964,39 @@ class SegmentacaoManager:
def _now() -> float:
return time.monotonic()
def _majority_status(self, janela_s: Optional[float] = None) -> StatusCarroMapa:
J = self.config.status_window_s if janela_s is None else float(janela_s)
def _weighted_status(self) -> Tuple[StatusCarroMapa, Dict[str, float]]:
window_s = max(0.05, float(self.config.status_window_s))
tau_s = max(0.05, float(self.config.status_decay_tau_s))
now = self._now()
while self._status_hist and (now - self._status_hist[0][1] > J):
while self._status_hist and (now - self._status_hist[0][1] > window_s):
self._status_hist.popleft()
if not self._status_hist:
return StatusCarroMapa.Direcionando
return StatusCarroMapa.Direcionando, {}
counts: Dict[StatusCarroMapa, int] = {}
scores: Dict[StatusCarroMapa, float] = {}
latest_ts: Dict[StatusCarroMapa, float] = {}
for status, _t in self._status_hist:
counts[status] = counts.get(status, 0) + 1
top = max(counts.values())
tied = [status for status, count in counts.items() if count == top]
for status, ts, base_weight, _source in self._status_hist:
age = max(0.0, now - float(ts))
decay = float(np.exp(-age / tau_s))
score = max(0.01, float(base_weight)) * decay
scores[status] = scores.get(status, 0.0) + score
latest_ts[status] = max(latest_ts.get(status, 0.0), float(ts))
top_score = max(scores.values())
tied = [s for s, score in scores.items() if abs(score - top_score) <= 1e-9]
if len(tied) == 1:
return tied[0]
winner = tied[0]
else:
winner = max(tied, key=lambda st: latest_ts.get(st, 0.0))
# Desempate: status mais recente.
for status, _t in reversed(self._status_hist):
if status in tied:
return status
return StatusCarroMapa.Direcionando
debug_scores = {
status.name: round(float(score), 5)
for status, score in sorted(scores.items(), key=lambda item: item[1], reverse=True)
}
return winner, debug_scores
# ---------------------------------------------------------------------
# Suavização
@ -879,6 +1022,8 @@ class SegmentacaoManager:
def reset_estado_temporal(self):
self._status_hist.clear()
self._status_final_hist.clear()
self._strong_candidate = None
self._strong_candidate_count = 0
self._ema_ang = None
self._ema_lat = None

17
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_debug_renderer.py
View File

@ -57,9 +57,12 @@ class VisualDebugRenderer:
return self.build_detection_overlay(rgb_frame, detections)
if frame_type == TipoFrameCamera.Debug:
overlay = self.build_overlay(rgb_frame, pred_ids, alpha=alpha)
# Debug prefere costmap. Só monta overlay como fallback, evitando
# construir duas imagens completas para descartar uma delas.
grid = self.build_costmap_debug(rgb_frame, snapshot)
return grid if grid is not None else overlay
if grid is not None:
return grid
return self.build_overlay(rgb_frame, pred_ids, alpha=alpha)
return None
@ -67,12 +70,12 @@ class VisualDebugRenderer:
if rgb_frame is None:
return None
img = rgb_frame.copy()
if rgb_frame.shape[1::-1] != self.preview_size:
# cv2.resize já cria um novo buffer; copiar antes seria tráfego de
# memória inútil no caminho de preview.
return cv2.resize(rgb_frame, self.preview_size, interpolation=cv2.INTER_AREA)
if img.shape[1::-1] != self.preview_size:
img = cv2.resize(img, self.preview_size, interpolation=cv2.INTER_AREA)
return img
return rgb_frame.copy()
def build_segmentation_preview(self, pred_ids):
if pred_ids is None or self.color_map is None:

18
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_grid_builder.py
View File

@ -6,8 +6,6 @@ from typing import Any, Dict, Optional, Sequence, Tuple
import cv2
import numpy as np
from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao
@dataclass
class GridGeometryConfig:
@ -41,6 +39,10 @@ class DetectionGridConfig:
@dataclass
class GridConfidenceConfig:
# IDs vêm OBRIGATORIAMENTE do contrato ONNX e são injetados pelo CameraManager.
id_nao_navegavel: int = -1
id_navegavel: int = -1
valid_mm: Tuple[int, int] = (300, 10000)
min_valid_frac: float = 0.30
conf_params: Tuple[float, float] = (0.30, 0.80)
@ -144,6 +146,14 @@ class VisualGridBuilder:
def __init__(self, config: Optional[GridConfidenceConfig | Dict[str, Any]] = None):
self.config = self._normalizar_config(config)
if int(self.config.id_navegavel) < 0 or int(self.config.id_nao_navegavel) < 0:
raise RuntimeError(
"VisualGridBuilder exige IDs do contrato ONNX v2: "
f"nav={self.config.id_navegavel} non_nav={self.config.id_nao_navegavel}"
)
if int(self.config.id_navegavel) == int(self.config.id_nao_navegavel):
raise RuntimeError("Grid recebeu IDs navegável/não-navegável iguais.")
@staticmethod
def _normalizar_config(config):
if config is None:
@ -236,8 +246,8 @@ class VisualGridBuilder:
if n == 0:
continue
n_nav = np.count_nonzero(seg_block == ClassesSegmentacao.NAVEGAVEL.value)
n_naonav = np.count_nonzero(seg_block == ClassesSegmentacao.NAONAVEGAVEL.value)
n_nav = np.count_nonzero(seg_block == cfg.id_navegavel)
n_naonav = np.count_nonzero(seg_block == cfg.id_nao_navegavel)
pct_navegavel[j, i] = n_nav / n
pct_nao_navegavel[j, i] = n_naonav / n

135
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/utils.py
View File

@ -1,144 +1,31 @@
import cv2
import numpy as np
import cupy as cp
import math
from scipy import stats
from shared.enums import T_Code
def gerar_heatmap(depth_frame, dist_max_mm=3000.0):
mask_valid = depth_frame <= dist_max_mm
depth_normalized = np.zeros(depth_frame.shape, dtype=np.uint8)
depth_normalized[mask_valid] = (255 - ((depth_frame[mask_valid] / dist_max_mm) * 255)).astype(np.uint8)
depth_normalized[mask_valid] = (
255 - ((depth_frame[mask_valid] / dist_max_mm) * 255)
).astype(np.uint8)
heatmap = cv2.applyColorMap(depth_normalized, cv2.COLORMAP_JET)
heatmap[~mask_valid] = [0, 0, 0]
return heatmap
def calcular_threshold_anomalias(velocidade_ms=None, threshold_base=0.15, fator_sensibilidade=100):
"""
Calcula o threshold dinamicamente baseado na velocidade do robô.
- velocidade_ms: Velocidade em metros por segundo (m/s).
"""
try:
if velocidade_ms is None:
from shared.contexto_global_redis import ContextoGlobalRedis
velocidade_ms = ContextoGlobalRedis.get_contexto().get("Gerais", {}).get("velocidade_ms", 0.0)
threshold = threshold_base + (velocidade_ms * fator_sensibilidade)
return round(threshold, 3)
except Exception as e:
print(f"❌ Erro ao calcular thresold de anomalias: {e}")
return threshold_base
def obter_regiao_solo(depth_frame, percentual_altura):
depth_frame = cp.asarray(depth_frame) # 🔥 Garante que está na GPU
altura, largura = depth_frame.shape
linhas_solo = int((percentual_altura / 100) * altura)
y_inicio = altura - linhas_solo
depth_solo = depth_frame[y_inicio:altura, :] # 🔥 Slice direto na GPU
return depth_solo, y_inicio, linhas_solo, altura, largura
def calcular_referencia(frames, metodo="moda"):
stack = np.stack(frames)
if metodo == "moda":
ref = stats.mode(stack, axis=0, keepdims=True)[0][0]
elif metodo == "media":
ref = np.mean(stack, axis=0)
elif metodo == "mediana":
ref = np.median(stack, axis=0)
else:
raise ValueError("Método inválido. Use 'moda', 'media' ou 'mediana'.")
return ref
def calcular_inclinacao_solo(perfil, fov_h):
"""
Calcula o ângulo de inclinação do solo em graus.
:param perfil: Lista de profundidades dos setores [z1, z2, ..., zn]
:param largura_total_metros: Largura total em metros do campo de visão
:return: Ângulo de inclinação em graus
"""
if len(perfil) < 2:
return 0 # Não dá pra calcular
distancia_media = np.mean(perfil)
largura = 2 * math.tan(fov_h / 2) * distancia_media
delta_z = perfil[-1] - perfil[0]
delta_x = largura
angulo_rad = math.atan2(delta_z, delta_x)
angulo_graus = math.degrees(angulo_rad)
return round(angulo_graus, 3)
def calcular_profundidades_setores(depth_solo, n_setores, largura, distancia_max_mm):
depth_solo = cp.asarray(depth_solo) # 🔥 Garante que está na GPU
largura_setor = largura // n_setores
profundidades = []
for i in range(n_setores):
x_inicio = i * largura_setor
x_fim = (i + 1) * largura_setor if (i < n_setores - 1) else largura
setor = depth_solo[:, x_inicio:x_fim]
# 🔥 Filtra profundidade válida
setor_valido = setor[(setor > 0) & (setor < distancia_max_mm)]
z = cp.median(setor_valido).item() if setor_valido.size > 0 else 0 # 🔥 .item() p/ float
profundidades.append(z)
return cp.asnumpy(cp.array(profundidades)), largura_setor
def histogram2d_gpu(x, y, bins, range):
try:
if x.size == 0 or y.size == 0:
return cp.zeros(bins, dtype=cp.int32)
x_bins, y_bins = bins
(x_min, x_max), (y_min, y_max) = range
x_idx = cp.floor((x - x_min) / (x_max - x_min) * x_bins).astype(cp.int32)
y_idx = cp.floor((y - y_min) / (y_max - y_min) * y_bins).astype(cp.int32)
mask = (x_idx >= 0) & (x_idx < x_bins) & (y_idx >= 0) & (y_idx < y_bins)
x_idx = x_idx[mask]
y_idx = y_idx[mask]
if x_idx.size == 0:
return cp.zeros((x_bins, y_bins), dtype=cp.int32)
linear_idx = x_idx * y_bins + y_idx
if linear_idx.size == 0:
return cp.zeros((x_bins, y_bins), dtype=cp.int32)
hist_flat = cp.bincount(linear_idx, minlength=x_bins * y_bins)
hist = hist_flat.reshape((x_bins, y_bins))
return hist
except Exception as e:
print(f"Erro ao gerar histograma 2d: {e}")
return cp.zeros((x_bins, y_bins), dtype=cp.int32)
def converter_valores_numpy(obj):
if isinstance(obj, dict):
return {k: converter_valores_numpy(v) for k, v in obj.items()}
elif isinstance(obj, list):
if isinstance(obj, list):
return [converter_valores_numpy(v) for v in obj]
elif isinstance(obj, tuple):
if isinstance(obj, tuple):
return tuple(converter_valores_numpy(v) for v in obj)
elif isinstance(obj, (np.integer, np.int32, np.int64)):
if isinstance(obj, (np.integer, np.int32, np.int64)):
return int(obj)
elif isinstance(obj, (np.floating, np.float32, np.float64)):
if isinstance(obj, (np.floating, np.float32, np.float64)):
return float(obj)
elif isinstance(obj, (np.bool_)):
if isinstance(obj, np.bool_):
return bool(obj)
elif isinstance(obj, np.ndarray):
if isinstance(obj, np.ndarray):
return obj.tolist()
else:
return obj
return obj

312
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/camera_manager.py
View File

@ -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)

2
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py
View File

@ -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.

85
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/weed_detector.py
View File

@ -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)

851
Python/OAK/datasets/oak-d/_0_capture_corridor.py Normal file
View File

@ -0,0 +1,851 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Capture OAK-D Lite - Agri Corridor Dataset
==========================================
Captura RGB para o dataset da camera frontal.
Contrato desta etapa:
- roda de dentro da pasta oak-d/
- captura o mesmo dominio de video usado no runtime:
OAK-D Lite -> ColorCamera 1080p -> cam.video -> host
- salva SOMENTE a imagem RGB original em PNG, sem resize e sem normalizacao
- destino padrao:
dataset/brutas/
- resize existe apenas para PREVIEW da interface e nunca toca a imagem salva
Teclas:
SPACE / S : salvar frame atual
A : ligar/desligar auto-save
E : alternar exposicao AUTO/MANUAL
+ / = : aumentar ISO no modo manual
- : diminuir ISO no modo manual
M : aumentar exposicao no modo manual
N : diminuir exposicao no modo manual
F : alternar foco AUTO/MANUAL
] : aumentar foco manual
[ : diminuir foco manual
V : alternar fullscreen
Q / ESC : sair
Exemplo:
python _0_capture.py
Auto-save a cada 1 segundo:
python _0_capture.py --auto --interval 1.0
Outra pasta:
python _0_capture.py --out dataset/brutas
"""
from __future__ import annotations
import argparse
import time
from concurrent.futures import ThreadPoolExecutor
from datetime import datetime
from pathlib import Path
from typing import Optional, Tuple
import cv2
import depthai as dai
import numpy as np
CAPTURE_W = 1920
CAPTURE_H = 1080
CAPTURE_FPS = 30.0
DEFAULT_OUT = Path("dataset") / "brutas"
DEFAULT_EXPOSURE_US = 6000
DEFAULT_ISO = 400
DEFAULT_FOCUS = 130
EXPOSURE_MIN_US = 100
EXPOSURE_MAX_US = 30000
EXPOSURE_STEP_US = 500
ISO_MIN = 100
ISO_MAX = 1600
ISO_STEP = 50
FOCUS_MIN = 0
FOCUS_MAX = 255
FOCUS_STEP = 5
WINDOW_NAME = "OAK-D Lite | Dataset Corredor Agricola"
CAPTURE_SESSION_ID = datetime.now().strftime("%Y%m%d_%H%M%S")
def timestamp_name() -> str:
"""
Nome novo:
img_sYYYYMMDD_HHMMSS__YYYYMMDD_HHMMSS_micro.png
O primeiro timestamp identifica a SESSAO de captura (uma execução do script).
O segundo identifica o frame.
Isso permite ao split manter uma mesma sessão inteira em train OU val.
"""
frame_ts = datetime.now().strftime("%Y%m%d_%H%M%S_%f")
return f"img_s{CAPTURE_SESSION_ID}__{frame_ts}"
def save_png(path: Path, frame_bgr: np.ndarray) -> Tuple[bool, str]:
path.parent.mkdir(parents=True, exist_ok=True)
ok = cv2.imwrite(
str(path),
frame_bgr,
[cv2.IMWRITE_PNG_COMPRESSION, 3],
)
return bool(ok), str(path)
def fit_inside(image: np.ndarray, max_w: int, max_h: int) -> np.ndarray:
h, w = image.shape[:2]
if w <= 0 or h <= 0:
return image
scale = min(
float(max_w) / float(w),
float(max_h) / float(h),
)
scale = max(scale, 1e-6)
new_w = max(1, int(round(w * scale)))
new_h = max(1, int(round(h * scale)))
interp = cv2.INTER_AREA if scale < 1.0 else cv2.INTER_LINEAR
return cv2.resize(image, (new_w, new_h), interpolation=interp)
def live_quality(frame_bgr: np.ndarray) -> dict:
"""
QA apenas visual.
Nunca rejeita nem altera a imagem salva.
"""
small = fit_inside(frame_bgr, 640, 360)
gray = cv2.cvtColor(small, cv2.COLOR_BGR2GRAY)
mean = float(gray.mean())
dark_pct = float((gray <= 5).mean() * 100.0)
bright_pct = float((gray >= 250).mean() * 100.0)
sharpness = float(cv2.Laplacian(gray, cv2.CV_64F).var())
warnings = []
if mean < 35:
warnings.append("MUITO ESCURA")
elif mean > 225:
warnings.append("MUITO CLARA")
if dark_pct > 20.0:
warnings.append("CLIP PRETO")
if bright_pct > 8.0:
warnings.append("CLIP BRANCO")
if sharpness < 35.0:
warnings.append("FOCO/BLUR?")
return {
"mean": mean,
"dark_pct": dark_pct,
"bright_pct": bright_pct,
"sharpness": sharpness,
"warnings": warnings,
}
def put_line(
canvas: np.ndarray,
text: str,
xy: Tuple[int, int],
scale: float = 0.65,
thickness: int = 1,
color=(235, 235, 235),
):
cv2.putText(
canvas,
text,
xy,
cv2.FONT_HERSHEY_SIMPLEX,
scale,
color,
thickness,
cv2.LINE_AA,
)
class CameraControls:
def __init__(
self,
queue,
exposure_us: int,
iso: int,
focus: int,
):
self.queue = queue
self.exposure_auto = True
self.focus_auto = True
self.exposure_us = int(exposure_us)
self.iso = int(iso)
self.focus = int(focus)
def send_initial_auto(self):
ctrl = dai.CameraControl()
try:
ctrl.setAutoExposureEnable()
except Exception:
pass
try:
ctrl.setAutoWhiteBalanceLock(False)
except Exception:
pass
try:
ctrl.setAutoFocusMode(
dai.CameraControl.AutoFocusMode.CONTINUOUS_VIDEO
)
ctrl.setAutoFocusTrigger()
except Exception:
pass
self.queue.send(ctrl)
def set_exposure_auto(self):
ctrl = dai.CameraControl()
try:
ctrl.setAutoExposureEnable()
except Exception as exc:
print(f"[WARN] Nao consegui ativar AE: {exc}")
return
self.queue.send(ctrl)
self.exposure_auto = True
print("[CAM] Exposicao: AUTO")
def set_exposure_manual(self):
self.exposure_us = int(
np.clip(
self.exposure_us,
EXPOSURE_MIN_US,
EXPOSURE_MAX_US,
)
)
self.iso = int(
np.clip(
self.iso,
ISO_MIN,
ISO_MAX,
)
)
ctrl = dai.CameraControl()
ctrl.setManualExposure(
self.exposure_us,
self.iso,
)
self.queue.send(ctrl)
self.exposure_auto = False
print(
f"[CAM] Exposicao: MANUAL | "
f"{self.exposure_us} us | ISO {self.iso}"
)
def toggle_exposure(self):
if self.exposure_auto:
self.set_exposure_manual()
else:
self.set_exposure_auto()
def adjust_exposure(self, delta_us: int):
if self.exposure_auto:
print("[CAM] Ajuste de exposicao ignorado: pressione E para modo MANUAL.")
return
self.exposure_us += int(delta_us)
self.set_exposure_manual()
def adjust_iso(self, delta_iso: int):
if self.exposure_auto:
print("[CAM] Ajuste de ISO ignorado: pressione E para modo MANUAL.")
return
self.iso += int(delta_iso)
self.set_exposure_manual()
def set_focus_auto(self):
ctrl = dai.CameraControl()
try:
ctrl.setAutoFocusMode(
dai.CameraControl.AutoFocusMode.CONTINUOUS_VIDEO
)
ctrl.setAutoFocusTrigger()
except Exception as exc:
print(f"[WARN] Autofocus nao disponivel: {exc}")
return
self.queue.send(ctrl)
self.focus_auto = True
print("[CAM] Foco: AUTO CONTINUOUS")
def set_focus_manual(self):
self.focus = int(
np.clip(
self.focus,
FOCUS_MIN,
FOCUS_MAX,
)
)
ctrl = dai.CameraControl()
try:
ctrl.setManualFocus(self.focus)
except Exception as exc:
print(f"[WARN] Foco manual nao disponivel: {exc}")
return
self.queue.send(ctrl)
self.focus_auto = False
print(f"[CAM] Foco: MANUAL | lens={self.focus}")
def toggle_focus(self):
if self.focus_auto:
self.set_focus_manual()
else:
self.set_focus_auto()
def adjust_focus(self, delta: int):
if self.focus_auto:
print("[CAM] Ajuste de foco ignorado: pressione F para modo MANUAL.")
return
self.focus += int(delta)
self.set_focus_manual()
def create_pipeline(fps: float) -> dai.Pipeline:
pipeline = dai.Pipeline()
cam_rgb = pipeline.createColorCamera()
cam_rgb.setBoardSocket(dai.CameraBoardSocket.CAM_A)
cam_rgb.setResolution(
dai.ColorCameraProperties.SensorResolution.THE_1080_P
)
cam_rgb.setFps(float(fps))
cam_rgb.setInterleaved(False)
# Mesmo contrato do runtime atual.
cam_rgb.setColorOrder(
dai.ColorCameraProperties.ColorOrder.RGB
)
xout = pipeline.createXLinkOut()
xout.setStreamName("rgb")
# IMPORTANTE: VIDEO, nao preview.
cam_rgb.video.link(xout.input)
control_in = pipeline.createXLinkIn()
control_in.setStreamName("control")
control_in.out.link(cam_rgb.inputControl)
return pipeline
def build_canvas(
frame_bgr: np.ndarray,
last_saved: Optional[np.ndarray],
*,
controls: CameraControls,
auto_save: bool,
interval_s: float,
saved_count: int,
out_dir: Path,
fps_view: float,
quality: dict,
) -> np.ndarray:
canvas_w = 1600
canvas_h = 900
canvas = np.zeros(
(canvas_h, canvas_w, 3),
dtype=np.uint8,
)
live = fit_inside(frame_bgr, 1150, 780)
lh, lw = live.shape[:2]
live_x = 20
live_y = 60
canvas[
live_y:live_y + lh,
live_x:live_x + lw,
] = live
panel_x = 1200
put_line(
canvas,
"OAK-D LITE / CORREDOR",
(panel_x, 60),
scale=0.78,
thickness=2,
)
put_line(
canvas,
"Fonte: VIDEO 1920x1080",
(panel_x, 100),
)
put_line(
canvas,
f"View FPS: {fps_view:.1f}",
(panel_x, 130),
)
put_line(
canvas,
f"Salvas: {saved_count}",
(panel_x, 160),
)
exp_text = (
"AUTO"
if controls.exposure_auto
else f"MANUAL {controls.exposure_us}us ISO{controls.iso}"
)
focus_text = (
"AUTO"
if controls.focus_auto
else f"MANUAL {controls.focus}"
)
put_line(
canvas,
f"Exposure: {exp_text}",
(panel_x, 205),
)
put_line(
canvas,
f"Focus: {focus_text}",
(panel_x, 235),
)
auto_text = (
f"ON ({interval_s:.2f}s)"
if auto_save
else "OFF"
)
put_line(
canvas,
f"Auto-save: {auto_text}",
(panel_x, 265),
)
put_line(
canvas,
f"Luma mean: {quality['mean']:.1f}",
(panel_x, 315),
)
put_line(
canvas,
f"Dark clip: {quality['dark_pct']:.1f}%",
(panel_x, 345),
)
put_line(
canvas,
f"White clip: {quality['bright_pct']:.1f}%",
(panel_x, 375),
)
put_line(
canvas,
f"Sharpness: {quality['sharpness']:.1f}",
(panel_x, 405),
)
if quality["warnings"]:
put_line(
canvas,
"QA: " + " | ".join(quality["warnings"]),
(panel_x, 440),
scale=0.55,
thickness=2,
color=(0, 180, 255),
)
else:
put_line(
canvas,
"QA: OK",
(panel_x, 440),
scale=0.62,
thickness=2,
color=(80, 230, 80),
)
put_line(canvas, "SPACE/S salvar", (panel_x, 510))
put_line(canvas, "A auto-save", (panel_x, 540))
put_line(canvas, "E auto/manual exp", (panel_x, 570))
put_line(canvas, "+/- ISO manual", (panel_x, 600))
put_line(canvas, "M/N exposicao manual", (panel_x, 630))
put_line(canvas, "F auto/manual foco", (panel_x, 660))
put_line(canvas, "[/] foco manual", (panel_x, 690))
put_line(canvas, "V fullscreen", (panel_x, 720))
put_line(canvas, "Q/ESC sair", (panel_x, 750))
if last_saved is not None:
thumb = fit_inside(last_saved, 360, 100)
th, tw = thumb.shape[:2]
tx = panel_x
ty = 780
if ty + th <= canvas_h and tx + tw <= canvas_w:
canvas[
ty:ty + th,
tx:tx + tw,
] = thumb
put_line(
canvas,
f"Saida: {out_dir}",
(20, 30),
scale=0.62,
color=(200, 220, 255),
)
return canvas
def main():
parser = argparse.ArgumentParser(
description=(
"Captura PNG lossless da OAK-D Lite para dataset "
"de corredor agricola."
)
)
parser.add_argument(
"--out",
default=str(DEFAULT_OUT),
help="Fila de imagens brutas (somente PNGs).",
)
parser.add_argument(
"--fps",
type=float,
default=CAPTURE_FPS,
help="FPS da camera.",
)
parser.add_argument(
"--auto",
action="store_true",
help="Inicia com auto-save ligado.",
)
parser.add_argument(
"--interval",
type=float,
default=1.0,
help="Intervalo do auto-save em segundos.",
)
parser.add_argument(
"--exposure_us",
type=int,
default=DEFAULT_EXPOSURE_US,
help="Exposicao inicial quando entrar em modo manual.",
)
parser.add_argument(
"--iso",
type=int,
default=DEFAULT_ISO,
help="ISO inicial quando entrar em modo manual.",
)
parser.add_argument(
"--focus",
type=int,
default=DEFAULT_FOCUS,
help="Lens position inicial quando entrar em foco manual.",
)
args = parser.parse_args()
out_dir = Path(args.out)
out_dir.mkdir(parents=True, exist_ok=True)
saved_count = len(list(out_dir.glob("*.png")))
print("=" * 72)
print("OAK-D Lite | Captura Dataset Corredor Agricola")
print("=" * 72)
print(f"Saida : {out_dir.resolve()}")
print("Formato : PNG lossless")
print(f"Resolucao : {CAPTURE_W}x{CAPTURE_H}")
print("Fonte : ColorCamera.video")
print(f"FPS camera : {args.fps}")
print(f"Ja existem : {saved_count} PNGs")
print(f"Sessao : {CAPTURE_SESSION_ID}")
print("=" * 72)
print("SPACE/S salvar | A auto-save | E exposure | F foco | Q sair")
pipeline = create_pipeline(args.fps)
saver = ThreadPoolExecutor(
max_workers=1,
thread_name_prefix="png_saver",
)
pending = []
last_saved: Optional[np.ndarray] = None
auto_save = bool(args.auto)
interval_s = max(0.10, float(args.interval))
last_auto_save_t = 0.0
fps_view = 0.0
fps_frames = 0
fps_t0 = time.perf_counter()
fullscreen = False
try:
with dai.Device(pipeline) as device:
control_queue = device.getInputQueue("control")
rgb_queue = device.getOutputQueue(
"rgb",
maxSize=2,
blocking=False,
)
controls = CameraControls(
control_queue,
exposure_us=args.exposure_us,
iso=args.iso,
focus=args.focus,
)
controls.send_initial_auto()
cv2.namedWindow(
WINDOW_NAME,
cv2.WINDOW_NORMAL,
)
cv2.resizeWindow(
WINDOW_NAME,
1600,
900,
)
while True:
packet = rgb_queue.get()
frame = packet.getCvFrame()
if (
frame is None
or frame.ndim != 3
or frame.shape[2] != 3
):
print(
f"[WARN] Frame invalido: "
f"{getattr(frame, 'shape', None)}"
)
continue
h, w = frame.shape[:2]
if (w, h) != (CAPTURE_W, CAPTURE_H):
print(
f"[WARN] Frame veio {w}x{h}, "
f"esperado {CAPTURE_W}x{CAPTURE_H}. "
"Nao sera redimensionado silenciosamente."
)
now_perf = time.perf_counter()
fps_frames += 1
dt_fps = now_perf - fps_t0
if dt_fps >= 1.0:
fps_view = fps_frames / dt_fps
fps_frames = 0
fps_t0 = now_perf
quality = live_quality(frame)
def request_save(current_frame: np.ndarray):
nonlocal saved_count, last_saved
name = timestamp_name() + ".png"
path = out_dir / name
snapshot = current_frame.copy()
future = saver.submit(
save_png,
path,
snapshot,
)
pending.append((future, path))
last_saved = snapshot
saved_count += 1
print(
f"[CAPTURE] #{saved_count} -> {path}"
)
if auto_save:
if (
last_auto_save_t <= 0.0
or now_perf - last_auto_save_t >= interval_s
):
request_save(frame)
last_auto_save_t = now_perf
still_pending = []
for future, path in pending:
if not future.done():
still_pending.append((future, path))
continue
try:
ok, saved_path = future.result()
except Exception as exc:
print(
f"[ERRO] Falha salvando {path}: {exc}"
)
continue
if not ok:
print(
f"[ERRO] cv2.imwrite retornou False: "
f"{saved_path}"
)
pending = still_pending
canvas = build_canvas(
frame,
last_saved,
controls=controls,
auto_save=auto_save,
interval_s=interval_s,
saved_count=saved_count,
out_dir=out_dir,
fps_view=fps_view,
quality=quality,
)
cv2.imshow(
WINDOW_NAME,
canvas,
)
key = cv2.waitKey(1) & 0xFF
if key in (ord("q"), 27):
print("[INFO] Encerrando captura...")
break
if key in (ord("s"), ord(" ")):
request_save(frame)
elif key == ord("a"):
auto_save = not auto_save
last_auto_save_t = 0.0
print(
f"[CAPTURE] Auto-save "
f"{'ON' if auto_save else 'OFF'} "
f"| interval={interval_s:.2f}s"
)
elif key == ord("e"):
controls.toggle_exposure()
elif key in (ord("+"), ord("=")):
controls.adjust_iso(ISO_STEP)
elif key == ord("-"):
controls.adjust_iso(-ISO_STEP)
elif key == ord("m"):
controls.adjust_exposure(EXPOSURE_STEP_US)
elif key == ord("n"):
controls.adjust_exposure(-EXPOSURE_STEP_US)
elif key == ord("f"):
controls.toggle_focus()
elif key == ord("]"):
controls.adjust_focus(FOCUS_STEP)
elif key == ord("["):
controls.adjust_focus(-FOCUS_STEP)
elif key == ord("v"):
fullscreen = not fullscreen
cv2.setWindowProperty(
WINDOW_NAME,
cv2.WND_PROP_FULLSCREEN,
(
cv2.WINDOW_FULLSCREEN
if fullscreen
else cv2.WINDOW_NORMAL
),
)
finally:
cv2.destroyAllWindows()
print(
f"[INFO] Aguardando {len(pending)} PNG(s) "
"que ja foram solicitados..."
)
saver.shutdown(wait=True)
failures = 0
for future, path in pending:
try:
ok, _ = future.result()
if not ok:
failures += 1
except Exception as exc:
failures += 1
print(f"[ERRO] {path}: {exc}")
print("=" * 72)
print("Captura encerrada.")
print(f"Total contabilizado : {saved_count}")
print(f"Falhas finais : {failures}")
print(f"Saida : {out_dir.resolve()}")
print("=" * 72)
if __name__ == "__main__":
main()

2376
Python/OAK/datasets/oak-d/_1_annotate_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

1938
Python/OAK/datasets/oak-d/_2_normalize_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

1682
Python/OAK/datasets/oak-d/_3_split_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

4198
Python/OAK/datasets/oak-d/_4_train_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

4052
Python/OAK/datasets/oak-d/_5_test_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

3285
Python/OAK/datasets/oak-d/_6_export_corridor_onnx.py Normal file
View File

File diff suppressed because it is too large Load Diff

3589
Python/OAK/datasets/oak-d/_7_benchmark_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

5125
Python/OAK/datasets/oak-d/audit/review_corridor_dataset_v2.py Normal file
View File

File diff suppressed because it is too large Load Diff

185
Python/OAK/datasets/oak-d/config.json Normal file
View File

@ -0,0 +1,185 @@
{
"camera": "oak-d",
"modelo": "segformer_b0",
"model_name": "corredores_v2",
"main_class_name": "navegavel",
"model_to_use": "geral",
"ckpt_test": "best_operational",
"raw_size": [1920, 1080],
"resolucao": [1024, 576],
"roi_inicio": 0.0,
"roi_tamanho": 1.0,
"channels": 3,
"backbone": "nvidia/mit-b0",
"label_classes": [
"Parado",
"EntrandoRua",
"CaminhandoRua",
"SaindoRua",
"Manobrando",
"Direcionando",
"RetornandoBase",
"Indefinido"
],
"corridor_training": {
"data": {
"strict_labels": true,
"strict_pipeline_contract": true,
"preflight_max_samples": 0
},
"augmentation": {
"enabled": true,
"horizontal_flip_p": 0.50,
"affine_p": 0.60,
"rotate_deg": 4.0,
"scale_min": 0.92,
"scale_max": 1.08,
"translate_frac": 0.03,
"global_gain_p": 0.45,
"global_gain_min": 0.82,
"global_gain_max": 1.18,
"contrast_p": 0.35,
"contrast_min": 0.85,
"contrast_max": 1.15,
"gamma_p": 0.30,
"gamma_min": 0.82,
"gamma_max": 1.18,
"rgb_gain_p": 0.25,
"rgb_gain_min": 0.94,
"rgb_gain_max": 1.06,
"shadow_p": 0.35,
"shadow_strength_min": 0.12,
"shadow_strength_max": 0.42,
"noise_p": 0.18,
"noise_sigma_min": 0.003,
"noise_sigma_max": 0.018,
"blur_p": 0.12,
"blur_kernel": 3,
"occlusion_p": 0.00,
"occlusion_min_frac": 0.04,
"occlusion_max_frac": 0.15,
"ramp_epochs": 6
},
"sampler": {
"mode": "joint",
"joint_power": 0.30,
"status_power": 0.12,
"group_power": 0.10,
"max_weight_ratio": 5.0,
"samples_per_epoch": 0
},
"class_weighting": {
"seg_method": "log_inverse",
"seg_log_offset": 1.02,
"seg_min_weight": 0.35,
"seg_max_weight": 3.0,
"seg_max_samples": 1200,
"status_enabled": true,
"status_power": 0.30,
"status_min_weight": 0.50,
"status_max_weight": 3.0
},
"loss": {
"seg_ce_weight": 0.65,
"seg_dice_weight": 0.35,
"boundary_boost": 0.15,
"unsafe_nav_weight": 0.06,
"status_weight": 0.40,
"status_ramp_epochs": 8,
"status_label_smoothing": 0.03
},
"status_head": {
"pool_h": 3,
"pool_w": 4,
"hidden": 256,
"dropout": 0.20,
"detach_seg_summary": true,
"seg_summary": "probabilities"
},
"optimizer": {
"encoder_lr_mult": 0.50,
"decoder_lr_mult": 1.00,
"status_lr_mult": 2.00,
"freeze_encoder_epochs": 2,
"no_decay_bias": true,
"no_decay_norm": true,
"betas": [0.9, 0.999],
"eps": 1e-8
},
"scheduler": {
"mode": "poly",
"warmup_ratio": 0.05,
"warmup_start_factor": 0.10,
"poly_power": 1.0,
"min_lr_ratio": 0.02
},
"optimization": {
"grad_clip_norm": 1.0,
"matmul_precision": "high",
"cudnn_benchmark": true,
"persistent_workers": true,
"prefetch_factor": 2
},
"score": {
"nav_iou": 0.40,
"nav_f1": 0.20,
"status_macro_f1": 0.20,
"safety": 0.20
},
"operational_gate": {
"enabled": true,
"min_epoch": 5,
"min_nav_iou": 0.70,
"min_nav_f1": 0.80,
"min_non_nav_iou": 0.60,
"min_status_macro_f1": 0.60,
"max_unsafe_nav_rate": 0.03,
"group_unsafe": {
"enabled": true,
"min_non_nav_pixels": 5000,
"max_rate": 0.05,
"max_rate_by_group": {
"naonavegavel": 0.04,
"navegavel_naonavegavel": 0.05
}
},
"status_recall": {
"enabled": false,
"min_support": 5,
"minimums": {}
}
},
"checkpoint": {
"early_stop_patience": 18,
"early_stop_min_delta": 0.0003
}
}
}

148
Python/OAK/datasets/oak-d/roi_seg_dataset.py Normal file
View File

@ -0,0 +1,148 @@
# -*- coding: utf-8 -*-
from PIL import Image
import glob
import os
import cv2
import numpy as np
import torch
from torch.utils.data import Dataset
from utils import carregar_labelmap_completo, compute_roi_indices, resize_keep_width
IMG_EXTS = (".jpg", ".jpeg", ".png")
MSK_EXTS = (".png", ".jpg", ".jpeg") # preferimos .png se existir
def _is_dir(p): return os.path.isdir(p)
def _is_file(p): return os.path.isfile(p)
def _list_groups(group_root):
if not _is_dir(group_root): return []
out = []
for g in sorted(os.listdir(group_root)):
gdir = os.path.join(group_root, g)
if not _is_dir(gdir):
continue
if _is_dir(os.path.join(gdir, "images")) and _is_dir(os.path.join(gdir, "masks")):
out.append(g)
return out
def _mask_for_base(msk_dir, base):
"""Encontra a máscara que casa com o base, priorizando .png."""
best = None
for ext in MSK_EXTS:
cand = os.path.join(msk_dir, base + ext)
if _is_file(cand):
if best is None: best = cand
# mantém .png se aparecer depois
if os.path.splitext(cand)[1].lower() == ".png":
return cand
return best
def _collect_pairs_legacy(root):
"""root/{images,masks}"""
img_dir = os.path.join(root, "images")
msk_dir = os.path.join(root, "masks")
imgs = []
msks = []
for p in sorted(glob.glob(os.path.join(img_dir, "*"))):
base, ext = os.path.splitext(os.path.basename(p))
if ext.lower() not in IMG_EXTS:
continue
m = _mask_for_base(msk_dir, base)
if m:
imgs.append(p)
msks.append(m)
return imgs, msks
def _collect_pairs_grouped(root):
"""Suporta:
- root/group/<g>/{images,masks}
- root/<g>/{images,masks} (quando root já é '.../group')
"""
# case A: root tem subpasta 'group'
group_root = os.path.join(root, "group")
if not _is_dir(group_root):
# case B: root JÁ É a pasta 'group'
group_root = root
groups = _list_groups(group_root)
imgs, msks = [], []
for g in groups:
img_dir = os.path.join(group_root, g, "images")
msk_dir = os.path.join(group_root, g, "masks")
for p in sorted(glob.glob(os.path.join(img_dir, "*"))):
base, ext = os.path.splitext(os.path.basename(p))
if ext.lower() not in IMG_EXTS:
continue
m = _mask_for_base(msk_dir, base)
if m:
imgs.append(p)
msks.append(m)
return imgs, msks
class ROISegDataset(Dataset):
"""
Compatível com o dataset original, mas agora aceita:
- root = '.../split/train' (com 'group' dentro)
- root = '.../split/train/group'
- root = '.../split/train/<grupo>' (ainda funciona via legado se tiver images/masks)
- root legado = '.../split/train' com 'images' e 'masks' diretamente
"""
def __init__(self, root, out_dir, zona_inicio, faixa_atuacao, input_w=384, min_input_h=96, labelmap_path="labelmap.txt",
mean=(0.485, 0.456, 0.406), std=(0.229, 0.224, 0.225)):
self.out_dir = out_dir
self.zona_inicio = zona_inicio
self.faixa_atuacao = faixa_atuacao
self.input_w = input_w
self.min_input_h = min_input_h
self.mean = np.array(mean, dtype=np.float32).reshape(1, 1, 3)
self.std = np.array(std, dtype=np.float32).reshape(1, 1, 3)
# tenta agrupar; se não encontrar, cai pro legado
imgs, msks = _collect_pairs_grouped(root)
if not imgs:
imgs, msks = _collect_pairs_legacy(root)
assert len(imgs) == len(msks) and len(imgs) > 0, f"Nenhuma imagem/máscara encontrada em {root}"
self.img_paths = imgs
self.msk_paths = msks
# labelmap
_, _, self.classes, self.ignore_rgb = carregar_labelmap_completo(labelmap_path)
# tipicamente ignore_rgb é (255,255,255)
self.ignore_id = int(self.ignore_rgb[0]) if isinstance(self.ignore_rgb, (list, tuple)) else int(self.ignore_rgb)
def __len__(self):
return len(self.img_paths)
def __getitem__(self, idx):
img_rgb = np.array(Image.open(self.img_paths[idx]).convert("RGB"))
# Máscara como escala de cinza (IDs já foram normalizados na etapa de normalize)
msk_grayscale = np.array(Image.open(self.msk_paths[idx]).convert("L"))
H, W = img_rgb.shape[:2]
y_fim, y_inicio = compute_roi_indices(H, self.zona_inicio, self.faixa_atuacao)
img_roi = img_rgb[y_fim:y_inicio, 0:W]
msk_roi = msk_grayscale[y_fim:y_inicio, 0:W]
img_in = resize_keep_width(img_roi, self.input_w, self.min_input_h, cv2.INTER_AREA)
msk_ids = resize_keep_width(msk_roi, self.input_w, self.min_input_h, cv2.INTER_NEAREST)
valores_validos = list(range(len(self.classes))) + [255]
msk_ids[np.isin(msk_ids, valores_validos, invert=True)] = self.ignore_id
if np.all(msk_ids == self.ignore_id):
raise ValueError(f"Máscara {self.msk_paths[idx]} está só com valor de ignore ({self.ignore_id})")
img_f = img_in.astype(np.float32) / 255.0
img_f = (img_f - self.mean) / self.std
img_chw = np.transpose(img_f, (2, 0, 1))
if idx == 0:
os.makedirs(self.out_dir, exist_ok=True)
cv2.imwrite(os.path.join(self.out_dir, "debug_roi_input.png"), cv2.cvtColor(img_roi, cv2.COLOR_RGB2BGR))
cv2.imwrite(os.path.join(self.out_dir, "debug_roi_mask.png"), msk_roi)
return torch.from_numpy(img_chw).float(), torch.from_numpy(msk_ids.astype(np.int64))

144
Python/OAK/datasets/oak-d/utils.py Normal file
View File

@ -0,0 +1,144 @@
import cv2
import numpy as np
# ----------------------------
# Helpers LABELMAP
# ----------------------------
def carregar_labelmap_completo(caminho):
cor_para_id = {}
id_para_nome = {}
cores_rgb = []
with open(caminho, 'r') as arquivo:
idx = 0
for linha in arquivo:
if linha.startswith("#") or not linha.strip():
continue
partes = linha.strip().split(':')
if len(partes) >= 2:
nome_classe, cor_rgb_str = partes[0], partes[1]
r, g, b = map(int, cor_rgb_str.split(','))
cor_rgb = (r, g, b)
if nome_classe.lower() == "ignore":
ignore_rgb = cor_rgb
continue # NÃO adiciona ignore no LUT de classes
cor_para_id[cor_rgb] = idx
cores_rgb.append(cor_rgb)
id_para_nome[idx] = nome_classe
idx += 1
#print(f"Mapa: {cor_para_id}")
#print(f"Colormap RGB: {cores_rgb}")
#print(f"Classes: {id_para_nome}")
#print(f"Ignore RGB: {ignore_rgb}")
return cor_para_id, cores_rgb, id_para_nome, ignore_rgb
def converter_mask_rgb_para_ids(img_rgb, mapa_rgb, ignore_id):
# Cria um mapa 256^3 para IDs (usa int32 para indexar)
lut = np.full((256**3,), ignore_id, dtype=np.uint8)
for cor, classe_id in mapa_rgb.items():
r, g, b = cor
lut[(r << 16) + (g << 8) + b] = classe_id
# Converte RGB para índice único
flat_idx = (img_rgb[:,:,0].astype(np.int32) << 16) + \
(img_rgb[:,:,1].astype(np.int32) << 8) + \
img_rgb[:,:,2].astype(np.int32)
# Aplica LUT vetorizada
return lut[flat_idx]
def converter_mask_ids_para_rgb(mask_ids: np.ndarray, colormap_rgb: list, ignore_id: int = 255) -> np.ndarray:
# Criar lookup table (256 cores possíveis)
lut = np.zeros((256, 3), dtype=np.uint8)
for i, color in enumerate(colormap_rgb):
lut[i] = color
lut[ignore_id] = (255, 255, 255)
# Aplicar LUT direto (vetorizado)
return lut[mask_ids]
def converter_mask_ids_para_bgr(mask_ids: np.ndarray, colormap_rgb: list, ignore_id: int = 255) -> np.ndarray:
"""
Converte máscara de IDs para imagem BGR (uint8),
pronta para uso com OpenCV.
"""
lut = np.zeros((256, 3), dtype=np.uint8)
for i, (r, g, b) in enumerate(colormap_rgb):
lut[i] = (b, g, r) # RGB -> BGR
lut[ignore_id] = (255, 255, 255) # branco em BGR = RGB
return lut[mask_ids]
def desenhar_legenda_vertical(colormap_rgb, classes, largura=200):
"""
Retorna uma imagem com a legenda das classes (cor + nome)
"""
nomes_classes = [classes[i] for i in range(len(classes))]
altura_por_classe = 30
altura_total = altura_por_classe * len(colormap_rgb)
legenda = np.ones((altura_total, largura, 3), dtype=np.uint8) * 255
for idx, (rgb, nome) in enumerate(zip(colormap_rgb, nomes_classes)):
y = idx * altura_por_classe
color = tuple(int(c) for c in rgb)
cv2.rectangle(legenda, (10, y + 5), (30, y + 25), color, -1)
cv2.putText(legenda, nome, (40, y + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 1, cv2.LINE_AA)
return legenda
def desenhar_legenda_horizontal(colormap_rgb, classes, altura=30, largura_por_classe=120):
"""
Retorna uma imagem com a legenda das classes (cor + nome), em uma única linha horizontal
"""
nomes_classes = [classes[i] for i in range(len(classes))]
largura_total = largura_por_classe * len(colormap_rgb)
legenda = np.ones((altura, largura_total, 3), dtype=np.uint8) * 255 # faixa branca
for idx, (rgb, nome) in enumerate(zip(colormap_rgb, nomes_classes)):
x = idx * largura_por_classe
color = tuple(int(c) for c in rgb)
# Retângulo colorido
cv2.rectangle(legenda, (x + 10, 5), (x + 30, 25), color, -1)
# Texto da classe
cv2.putText(legenda, nome, (x + 35, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 1, cv2.LINE_AA)
return legenda
def _infer_ignore_id(ignore_rgb, default_id=255):
import numpy as _np
if isinstance(ignore_rgb, (list, tuple)):
if len(ignore_rgb) == 1 and isinstance(ignore_rgb[0], (int, _np.integer)):
return int(ignore_rgb[0])
if len(ignore_rgb) == 3:
return default_id
if isinstance(ignore_rgb, (int, _np.integer)):
return int(ignore_rgb)
return default_id
# ----------------------------
# Helpers ROI
# ----------------------------
def compute_roi_indices(H: int, zona_inicio: float, faixa_atuacao: float):
y_inicio = int((1.0 - zona_inicio) * H)
y_fim = int((1.0 - (zona_inicio + faixa_atuacao)) * H)
y_fim = max(0, min(H, y_fim))
y_inicio = max(0, min(H, y_inicio))
if y_fim >= y_inicio:
y_fim = max(0, y_inicio - 1)
return y_fim, y_inicio
def resize_keep_width(img: np.ndarray, new_w: int, min_h: int, interpolation: int) -> np.ndarray:
h, w = img.shape[:2]
new_h = int(round(new_w * (h / w)))
if min_h is not None and new_h < min_h:
new_h = min_h
return cv2.resize(img, (new_w, new_h), interpolation=interpolation)