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

Ajustes de logica do MPC e definicao de comandos

This commit is contained in:
Diego Freitas 2026-02-26 16:57:22 -03:00
parent ffeeafcca7
commit 89e5f1ae41
57 changed files with 1567 additions and 1025 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
AgroBase/AgroBase/Forms/IHM/frmDialogoMapa.cs
View File

@ -251,7 +251,7 @@ namespace AgroBase.Forms.IHM
ReqFrameCam = true;
try
{
var frame = await WeedWorkerService.GetCameraFrame(CameraFrameType.Rgb);
var frame = await WeedWorkerService.GetCameraFrame(TipoFrameCamera.Rgb);
AtualizarImagemPainel(pnlCameraSolo0, frame?.image());
}
finally
@ -265,10 +265,10 @@ namespace AgroBase.Forms.IHM
ReqFrameSnr = true;
try
{
var frameRgb = await VisualWorkerService.GetCameraFrame(CameraFrameType.Rgb);
var frameRgb = await VisualWorkerService.GetCameraFrame(TipoFrameCamera.Rgb);
AtualizarImagemPainel(pnlCameraCaminho, frameRgb?.image());
var frameSeg = await VisualWorkerService.GetCameraFrame(CameraFrameType.Segmentacao);
var frameSeg = await VisualWorkerService.GetCameraFrame(TipoFrameCamera.Segmentacao);
AtualizarImagemPainel(pnlCameraCaminhoSeg, frameSeg?.image());
}
finally
@ -336,7 +336,7 @@ namespace AgroBase.Forms.IHM
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
cmbModoControle.SelectedIndex = (int)_Controle.TipoMovimento;
chbSonarAtivado.Checked = pControle.SonarAtivado;
chbFrenagemAutomatica.Checked = pControle.FrenagemAutomatica;
chbFrenagemAutomatica.Checked = pControle.FrenagemAutomaticaAoParar;
nudVelSemErvas.Value = (int)pControle.MovVelocidadeSErvasPercent;
nudVelComErvas.Value = (int)pControle.MovVelocidadeCErvasPercent;
chbPulverizadorAutomatico.Checked = pControle.PulverizadorAutomatico;
@ -870,7 +870,7 @@ namespace AgroBase.Forms.IHM
if (res == DialogResult.Yes)
{
pControle.SonarAtivado = chbSonarAtivado.Checked;
pControle.FrenagemAutomatica = chbFrenagemAutomatica.Checked;
pControle.FrenagemAutomaticaAoParar = chbFrenagemAutomatica.Checked;
pControle.MovVelocidadeSErvasPercent = (int)FuncoesMatematicas.CalculaRPMVelocidade(FuncoesMatematicas.CalculaVelocidadeMsPercentual((int)nudVelSemErvas.Value));
pControle.MovVelocidadeCErvasPercent = (int)FuncoesMatematicas.CalculaRPMVelocidade(FuncoesMatematicas.CalculaVelocidadeMsPercentual((int)nudVelComErvas.Value));
pControle.PulverizadorAutomatico = chbPulverizadorAutomatico.Checked;
@ -894,7 +894,7 @@ namespace AgroBase.Forms.IHM
else
{
chbSonarAtivado.Checked = pControle.SonarAtivado;
chbFrenagemAutomatica.Checked = pControle.FrenagemAutomatica;
chbFrenagemAutomatica.Checked = pControle.FrenagemAutomaticaAoParar;
nudVelSemErvas.Value = (int)pControle.MovVelocidadeSErvasPercent;
nudVelComErvas.Value = (int)pControle.MovVelocidadeCErvasPercent;
chbPulverizadorAutomatico.Checked = pControle.PulverizadorAutomatico;

4
AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs
View File

@ -198,7 +198,7 @@ namespace AgroBase.Forms.Operacoes
ReqFrameCam = true;
try
{
var frame = await WeedWorkerService.GetCameraFrame(CameraFrameType.Debug);
var frame = await WeedWorkerService.GetCameraFrame(TipoFrameCamera.Debug);
Panel pnl = FuncoesGlobais.FindControlRecursive<Panel>(flwCamerasSolo, "pnlCamSolo_" + Variaveis.OperacaoEmAndamento.DispSen.Dados.CamerasSolo[0].Name);
AtualizarImagemPainel(pnl, frame?.image());
}
@ -213,7 +213,7 @@ namespace AgroBase.Forms.Operacoes
ReqFrameSnr = true;
try
{
var frameRgb = await VisualWorkerService.GetCameraFrame(CameraFrameType.Debug);
var frameRgb = await VisualWorkerService.GetCameraFrame(TipoFrameCamera.Overlay);
AtualizarImagemPainel(pnlCameraRua, frameRgb?.image());
//var frameSeg = await VisualWorkerService.GetCameraFrame(CameraFrameType.Segmentacao);

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

@ -1591,12 +1591,12 @@ namespace AgroBase.Forms.Operacoes
txtCamCaminhoErroLateral.Text = (Log.OperadorVisual?.Analises?.segmentacao?.erro_lateral_pct ?? 0).ToString("0.00");
txtCamCaminhoStatusCarro.Text = (Log.OperadorVisual?.Analises?.segmentacao?.status_corredor ?? Enums.StatusCarroMapa.Parado).ToString();
txtTempoEntrandoRua.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.EntrandoRua).Value.ToString("0.00") + " s";
txtTempoCaminhandoRua.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.CaminhandoRua).Value.ToString("0.00") + " s";
txtTempoSaindoRua.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.SaindoRua).Value.ToString("0.00") + " s";
txtTempoParado.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.Parado).Value.ToString("0.00") + " s";
txtTempoDirecionando.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.Direcionando).Value.ToString("0.00") + " s";
txtTempoManobrando.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.Manobrando).Value.ToString("0.00") + " s";
//txtTempoEntrandoRua.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.EntrandoRua).Value.ToString("0.00") + " s";
//txtTempoCaminhandoRua.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.CaminhandoRua).Value.ToString("0.00") + " s";
//txtTempoSaindoRua.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.SaindoRua).Value.ToString("0.00") + " s";
//txtTempoParado.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.Parado).Value.ToString("0.00") + " s";
//txtTempoDirecionando.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.Direcionando).Value.ToString("0.00") + " s";
//txtTempoManobrando.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.Manobrando).Value.ToString("0.00") + " s";
pnlCaminho.BackgroundImage = CarregarImagemCamera(pathImagensCaminho, "_segmentacao");
pnlCaminho.BackgroundImageLayout = ImageLayout.Zoom;

100
AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.Designer.cs generated
View File

@ -30,6 +30,9 @@
{
System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(frmSimulacaoMapaGPS));
this.pnlOpcoes = new System.Windows.Forms.Panel();
this.btnLiberarCorredor = new System.Windows.Forms.Button();
this.btnFixarBase = new System.Windows.Forms.Button();
this.btnRetornarBase = new System.Windows.Forms.Button();
this.lblDelayPosicao = new System.Windows.Forms.Label();
this.txtDelayPosicao = new System.Windows.Forms.TextBox();
this.lblCarregado = new System.Windows.Forms.Label();
@ -105,9 +108,6 @@
this.lblTempoRestante = new System.Windows.Forms.Label();
this.lblPercentualRua = new System.Windows.Forms.Label();
this.lblPercentualOperacao = new System.Windows.Forms.Label();
this.btnRetornarBase = new System.Windows.Forms.Button();
this.btnFixarBase = new System.Windows.Forms.Button();
this.btnLiberarCorredor = new System.Windows.Forms.Button();
this.pnlOpcoes.SuspendLayout();
this.gpbSonar.SuspendLayout();
this.gpbGPS.SuspendLayout();
@ -151,6 +151,39 @@
this.pnlOpcoes.Size = new System.Drawing.Size(1379, 100);
this.pnlOpcoes.TabIndex = 0;
//
// btnLiberarCorredor
//
this.btnLiberarCorredor.Location = new System.Drawing.Point(388, 75);
this.btnLiberarCorredor.Margin = new System.Windows.Forms.Padding(2);
this.btnLiberarCorredor.Name = "btnLiberarCorredor";
this.btnLiberarCorredor.Size = new System.Drawing.Size(55, 20);
this.btnLiberarCorredor.TabIndex = 48;
this.btnLiberarCorredor.Text = "Liberar";
this.btnLiberarCorredor.UseVisualStyleBackColor = true;
this.btnLiberarCorredor.Click += new System.EventHandler(this.btnLiberarCorredor_Click);
//
// btnFixarBase
//
this.btnFixarBase.Location = new System.Drawing.Point(388, 13);
this.btnFixarBase.Margin = new System.Windows.Forms.Padding(2);
this.btnFixarBase.Name = "btnFixarBase";
this.btnFixarBase.Size = new System.Drawing.Size(55, 27);
this.btnFixarBase.TabIndex = 47;
this.btnFixarBase.Text = "F Base";
this.btnFixarBase.UseVisualStyleBackColor = true;
this.btnFixarBase.Click += new System.EventHandler(this.btnFixarBase_Click);
//
// btnRetornarBase
//
this.btnRetornarBase.Location = new System.Drawing.Point(388, 44);
this.btnRetornarBase.Margin = new System.Windows.Forms.Padding(2);
this.btnRetornarBase.Name = "btnRetornarBase";
this.btnRetornarBase.Size = new System.Drawing.Size(55, 27);
this.btnRetornarBase.TabIndex = 46;
this.btnRetornarBase.Text = "R Base";
this.btnRetornarBase.UseVisualStyleBackColor = true;
this.btnRetornarBase.Click += new System.EventHandler(this.btnRetornarBase_Click);
//
// lblDelayPosicao
//
this.lblDelayPosicao.AutoSize = true;
@ -692,7 +725,7 @@
this.lblTempoOperacao.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblTempoOperacao.AutoSize = true;
this.lblTempoOperacao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblTempoOperacao.Location = new System.Drawing.Point(232, 460);
this.lblTempoOperacao.Location = new System.Drawing.Point(232, 457);
this.lblTempoOperacao.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblTempoOperacao.Name = "lblTempoOperacao";
this.lblTempoOperacao.Size = new System.Drawing.Size(148, 13);
@ -704,7 +737,7 @@
this.lblStatusOperacao.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblStatusOperacao.AutoSize = true;
this.lblStatusOperacao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblStatusOperacao.Location = new System.Drawing.Point(232, 443);
this.lblStatusOperacao.Location = new System.Drawing.Point(232, 437);
this.lblStatusOperacao.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblStatusOperacao.Name = "lblStatusOperacao";
this.lblStatusOperacao.Size = new System.Drawing.Size(133, 13);
@ -728,7 +761,7 @@
this.lblDistanciaFinal.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDistanciaFinal.AutoSize = true;
this.lblDistanciaFinal.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblDistanciaFinal.Location = new System.Drawing.Point(10, 460);
this.lblDistanciaFinal.Location = new System.Drawing.Point(10, 457);
this.lblDistanciaFinal.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistanciaFinal.Name = "lblDistanciaFinal";
this.lblDistanciaFinal.Size = new System.Drawing.Size(135, 13);
@ -740,7 +773,7 @@
this.lblDistanciaOperacao.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDistanciaOperacao.AutoSize = true;
this.lblDistanciaOperacao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblDistanciaOperacao.Location = new System.Drawing.Point(10, 443);
this.lblDistanciaOperacao.Location = new System.Drawing.Point(10, 437);
this.lblDistanciaOperacao.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistanciaOperacao.Name = "lblDistanciaOperacao";
this.lblDistanciaOperacao.Size = new System.Drawing.Size(189, 13);
@ -752,7 +785,7 @@
this.lblDirecao.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDirecao.AutoSize = true;
this.lblDirecao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblDirecao.Location = new System.Drawing.Point(390, 443);
this.lblDirecao.Location = new System.Drawing.Point(390, 437);
this.lblDirecao.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDirecao.Name = "lblDirecao";
this.lblDirecao.Size = new System.Drawing.Size(84, 13);
@ -764,7 +797,7 @@
this.lblStatus.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblStatus.AutoSize = true;
this.lblStatus.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblStatus.Location = new System.Drawing.Point(390, 418);
this.lblStatus.Location = new System.Drawing.Point(390, 417);
this.lblStatus.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblStatus.Name = "lblStatus";
this.lblStatus.Size = new System.Drawing.Size(96, 13);
@ -879,7 +912,7 @@
this.lblRua.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblRua.AutoSize = true;
this.lblRua.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblRua.Location = new System.Drawing.Point(11, 398);
this.lblRua.Location = new System.Drawing.Point(11, 397);
this.lblRua.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblRua.Name = "lblRua";
this.lblRua.Size = new System.Drawing.Size(74, 13);
@ -891,7 +924,7 @@
this.lblMargem.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblMargem.AutoSize = true;
this.lblMargem.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblMargem.Location = new System.Drawing.Point(99, 398);
this.lblMargem.Location = new System.Drawing.Point(99, 397);
this.lblMargem.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblMargem.Name = "lblMargem";
this.lblMargem.Size = new System.Drawing.Size(71, 13);
@ -903,7 +936,7 @@
this.lblProximoPonto.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblProximoPonto.AutoSize = true;
this.lblProximoPonto.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblProximoPonto.Location = new System.Drawing.Point(183, 398);
this.lblProximoPonto.Location = new System.Drawing.Point(183, 397);
this.lblProximoPonto.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblProximoPonto.Name = "lblProximoPonto";
this.lblProximoPonto.Size = new System.Drawing.Size(87, 13);
@ -915,7 +948,7 @@
this.lblAproximando.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblAproximando.AutoSize = true;
this.lblAproximando.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblAproximando.Location = new System.Drawing.Point(285, 398);
this.lblAproximando.Location = new System.Drawing.Point(285, 397);
this.lblAproximando.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblAproximando.Name = "lblAproximando";
this.lblAproximando.Size = new System.Drawing.Size(68, 13);
@ -927,7 +960,7 @@
this.lblDistProx.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDistProx.AutoSize = true;
this.lblDistProx.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblDistProx.Location = new System.Drawing.Point(11, 418);
this.lblDistProx.Location = new System.Drawing.Point(11, 417);
this.lblDistProx.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistProx.Name = "lblDistProx";
this.lblDistProx.Size = new System.Drawing.Size(160, 13);
@ -939,7 +972,7 @@
this.lblDistAnt.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDistAnt.AutoSize = true;
this.lblDistAnt.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblDistAnt.Location = new System.Drawing.Point(183, 418);
this.lblDistAnt.Location = new System.Drawing.Point(183, 417);
this.lblDistAnt.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistAnt.Name = "lblDistAnt";
this.lblDistAnt.Size = new System.Drawing.Size(159, 13);
@ -951,7 +984,7 @@
this.lblTipoMovimento.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblTipoMovimento.AutoSize = true;
this.lblTipoMovimento.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblTipoMovimento.Location = new System.Drawing.Point(390, 398);
this.lblTipoMovimento.Location = new System.Drawing.Point(390, 397);
this.lblTipoMovimento.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblTipoMovimento.Name = "lblTipoMovimento";
this.lblTipoMovimento.Size = new System.Drawing.Size(75, 13);
@ -975,7 +1008,7 @@
this.lblPercentualRua.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblPercentualRua.AutoSize = true;
this.lblPercentualRua.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblPercentualRua.Location = new System.Drawing.Point(390, 460);
this.lblPercentualRua.Location = new System.Drawing.Point(390, 457);
this.lblPercentualRua.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblPercentualRua.Name = "lblPercentualRua";
this.lblPercentualRua.Size = new System.Drawing.Size(62, 13);
@ -994,39 +1027,6 @@
this.lblPercentualOperacao.TabIndex = 42;
this.lblPercentualOperacao.Text = "Operação: 0,00%";
//
// btnRetornarBase
//
this.btnRetornarBase.Location = new System.Drawing.Point(388, 44);
this.btnRetornarBase.Margin = new System.Windows.Forms.Padding(2);
this.btnRetornarBase.Name = "btnRetornarBase";
this.btnRetornarBase.Size = new System.Drawing.Size(55, 27);
this.btnRetornarBase.TabIndex = 46;
this.btnRetornarBase.Text = "R Base";
this.btnRetornarBase.UseVisualStyleBackColor = true;
this.btnRetornarBase.Click += new System.EventHandler(this.btnRetornarBase_Click);
//
// btnFixarBase
//
this.btnFixarBase.Location = new System.Drawing.Point(388, 13);
this.btnFixarBase.Margin = new System.Windows.Forms.Padding(2);
this.btnFixarBase.Name = "btnFixarBase";
this.btnFixarBase.Size = new System.Drawing.Size(55, 27);
this.btnFixarBase.TabIndex = 47;
this.btnFixarBase.Text = "F Base";
this.btnFixarBase.UseVisualStyleBackColor = true;
this.btnFixarBase.Click += new System.EventHandler(this.btnFixarBase_Click);
//
// btnLiberarCorredor
//
this.btnLiberarCorredor.Location = new System.Drawing.Point(388, 75);
this.btnLiberarCorredor.Margin = new System.Windows.Forms.Padding(2);
this.btnLiberarCorredor.Name = "btnLiberarCorredor";
this.btnLiberarCorredor.Size = new System.Drawing.Size(55, 20);
this.btnLiberarCorredor.TabIndex = 48;
this.btnLiberarCorredor.Text = "Liberar";
this.btnLiberarCorredor.UseVisualStyleBackColor = true;
this.btnLiberarCorredor.Click += new System.EventHandler(this.btnLiberarCorredor_Click);
//
// frmSimulacaoMapaGPS
//
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);

36
AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs
View File

@ -125,7 +125,8 @@ namespace AgroBase.Forms
return;
}
lblStatusOperacao.Text = "Operação: " + Enum.GetName(typeof(StatusOperacao), _Sensoriamento.StatusOperacao);
lblStatusOperacao.Text = $"Operação: {_Sensoriamento.StatusOperacao}";
if (_Sensoriamento.StatusOperacao == StatusOperacao.Aguardando) lblStatusOperacao.Text += $" ({_Sensoriamento.TempoAguardandoSeg:0})";
lblTipoMovimento.Text = (_Sensoriamento.Controle?.TipoMovimento ?? TipoMovimentoDirecional.Diagnostico).ToString();
if (_Trajetoria == null)
@ -136,21 +137,21 @@ namespace AgroBase.Forms
if (_Sensoriamento.Trajetoria == null) return;
lblStatus.Text = "Carro: " + Enum.GetName(typeof(StatusCarroMapa), _Sensoriamento.Trajetoria.StatusCarro);
lblDirecao.Text = "Direção: " + Enum.GetName(typeof(DirecaoCarroRua), _Sensoriamento.Trajetoria.DirecaoCaminho);
lblDistanciaOperacao.Text = "Distância Operação: " + _Sensoriamento.Trajetoria.DistanciaPercorrida.ToString("0.00") + " m de " + _Sensoriamento.Trajetoria.DistanciaTotal.ToString("0.00") + " m";
lblDistanciaFinal.Text = "Distância Restante: " + _Sensoriamento.Trajetoria.DistanciaRestante.ToString("0.00") + " m";
lblDistanciaLateral.Text = "Esq: " + _Sensoriamento.Trajetoria.DistanciaEsquerda.ToString("0.00") + " m - Dir: " + _Sensoriamento.Trajetoria.DistanciaDireita.ToString("0.00") + " m " + Variaveis.OperacaoEmAndamento.Controle.ErroLateral.ToString("0.00") + " m";
lblRua.Text = "Corredor: " + (_Sensoriamento.Trajetoria.CorredorAtual.Dentro ? "Dentro" : "Fora");
lblMargem.Text = "Margem: " + (_Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não");
lblProximoPonto.Text = "Próximo Ponto: " + (_Sensoriamento.Trajetoria.ProximoPonto.idxPonto);
lblStatus.Text = $"Carro: {_Sensoriamento.Trajetoria.StatusCarro} ({_Sensoriamento.OperadorVisual.Analises.segmentacao.status_corredor})";
lblDirecao.Text = $"Direção: {_Sensoriamento.Trajetoria.DirecaoCaminho}";
lblDistanciaOperacao.Text = $"Distância Operação: {_Sensoriamento.Trajetoria.DistanciaPercorrida:F2} m de {_Sensoriamento.Trajetoria.DistanciaTotal:F2} m";
lblDistanciaFinal.Text = $"Distância Restante: {_Sensoriamento.Trajetoria.DistanciaRestante:F2} m";
lblDistanciaLateral.Text = $"Esq: {_Sensoriamento.Trajetoria.DistanciaEsquerda:F2} m - Dir: {_Sensoriamento.Trajetoria.DistanciaDireita:F2} m {Variaveis.OperacaoEmAndamento.Controle.ErroLateral:F2} m";
lblRua.Text = $"Corredor: " + (_Sensoriamento.Trajetoria.CorredorAtual.Dentro ? "Dentro" : "Fora");
lblMargem.Text = $"Margem: " + (_Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não");
lblProximoPonto.Text = $"Próximo Ponto: {_Sensoriamento.Trajetoria.ProximoPonto.idxPonto}";
lblAproximando.Text = (_Sensoriamento.Trajetoria.ProximoPonto.Aproximando ? "Aproximando" : "Afastando");
lblDistProx.Text = "Distância Próximo Ponto: " + _Sensoriamento.Trajetoria.ProximoPonto.DistanciaTrajeto.ToString("0.00") + " m";
lblDistAnt.Text = "Distância Ponto Anterior: " + _Sensoriamento.Trajetoria.PontoMaisProximo.DistanciaTrajeto.ToString("0.00") + " m";
lblTempoRestante.Text = "Tempo Restante: " + _Sensoriamento.Trajetoria.TempoEstimadoRestante;
lblTempoOperacao.Text = "Tempo: " + _Sensoriamento.TempoDecorrido.ToString("HH:mm:ss") + " de " + _Sensoriamento.Trajetoria.TempoEstimadoOperacao;
lblPercentualRua.Text = "Rua: " + _Sensoriamento.Trajetoria.CorredorAtual.Progresso.ToString("0.00") + "%";
lblPercentualOperacao.Text = "Operação: " + _Sensoriamento.Trajetoria.ProgressoTrajeto.ToString("0.00") + "%";
lblDistProx.Text = $"Distância Próximo Ponto: {_Sensoriamento.Trajetoria.ProximoPonto.DistanciaTrajeto:F2} m";
lblDistAnt.Text = $"Distância Ponto Anterior: {_Sensoriamento.Trajetoria.PontoMaisProximo.DistanciaTrajeto:F2} m";
lblTempoRestante.Text = $"Tempo Restante: {_Sensoriamento.Trajetoria.TempoEstimadoRestante}";
lblTempoOperacao.Text = $"Tempo: " + _Sensoriamento.TempoDecorrido.ToString("HH:mm:ss") + $" de {_Sensoriamento.Trajetoria.TempoEstimadoOperacao}";
lblPercentualRua.Text = $"Rua: {_Sensoriamento.Trajetoria.CorredorAtual.Progresso:F2}%";
lblPercentualOperacao.Text = $"Operação: {_Sensoriamento.Trajetoria.ProgressoTrajeto:F2}%";
pnlOrientacaoTrajeto.Invalidate();
@ -544,6 +545,11 @@ namespace AgroBase.Forms
private void btnRetornarBase_Click(object sender, EventArgs e)
{
Variaveis.OperacaoEmAndamento.Trajetoria.IniciarRetornoBase(VariaveisOperacao.PosicaoBase);
if (!Variaveis.OperacaoEmAndamento.Iniciado)
{
Variaveis.OperacaoEmAndamento.Iniciado = true;
Variaveis.OperacaoEmAndamento.Simulando = true;
}
}
private void btnFixarBase_Click(object sender, EventArgs e)

3
AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapeamentoVisual.cs
View File

@ -14,6 +14,7 @@ using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using static AgroBase.Models.Enums;
namespace AgroBase.Forms.Simuladores
{
@ -68,7 +69,7 @@ namespace AgroBase.Forms.Simuladores
{
Task.Run(async () =>
{
OAKCameraFrameModel frame = await VisualWorkerService.GetCameraFrame(CameraFrameType.Segmentacao);
OAKCameraFrameModel frame = await VisualWorkerService.GetCameraFrame(TipoFrameCamera.Segmentacao);
var dadosSeg = Variaveis.OperacaoEmAndamento.Sensoriamento.OperadorVisual.Analises.segmentacao;
DesenharCorredor(frame?.image(), dadosSeg.centros_corredor, dadosSeg.width, dadosSeg.height, 80);

37
AgroBase/AgroBase/Forms/frmOperarioSonar.cs
View File

@ -9,6 +9,7 @@ using System.Linq;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Windows.Forms.DataVisualization.Charting;
using static AgroBase.Models.Enums;
namespace AgroBase.Forms
{
@ -79,51 +80,51 @@ namespace AgroBase.Forms
private async void btnLeituraRGB_Click(object sender, EventArgs e)
{
EnviarComandoReqFrame(CameraFrameType.Rgb);
EnviarComandoReqFrame(TipoFrameCamera.Rgb);
}
private void btnLeituraHeatmap_Click(object sender, EventArgs e)
{
EnviarComandoReqFrame(CameraFrameType.Heatmap);
EnviarComandoReqFrame(TipoFrameCamera.Heatmap);
}
private void btnLeituraRadar2D_Click(object sender, EventArgs e)
{
EnviarComandoReqFrame(CameraFrameType.RadarTopDown);
//EnviarComandoReqFrame(TipoFrameCamera.RadarTopDown);
}
private void btnLeituraSegmentacaoSemantica_Click(object sender, EventArgs e)
{
EnviarComandoReqFrame(CameraFrameType.Segmentacao);
EnviarComandoReqFrame(TipoFrameCamera.Segmentacao);
}
private async void EnviarComandoReqFrame(CameraFrameType tipo)
private async void EnviarComandoReqFrame(TipoFrameCamera tipo)
{
OAKCameraFrameModel frame = await VisualWorkerService.GetCameraFrame(tipo);
PictureBox pic =
tipo == CameraFrameType.Rgb ? picRGB :
tipo == CameraFrameType.Heatmap ? picHeatmap :
tipo == CameraFrameType.RadarTopDown ? picRadar2D :
tipo == CameraFrameType.Segmentacao ? picSegmentacaoSemantica :
tipo == TipoFrameCamera.Rgb ? picRGB :
tipo == TipoFrameCamera.Heatmap ? picHeatmap :
//tipo == TipoFrameCamera.RadarTopDown ? picRadar2D :
tipo == TipoFrameCamera.Segmentacao ? picSegmentacaoSemantica :
new PictureBox();
Label lbl =
tipo == CameraFrameType.Rgb ? lblLeituraRGB :
tipo == CameraFrameType.Heatmap ? lblLeituraHeatmap :
tipo == CameraFrameType.RadarTopDown ? lblLeituraRadar2D :
tipo == CameraFrameType.Segmentacao ? lblLeituraSegmentacaoSemantica :
tipo == TipoFrameCamera.Rgb ? lblLeituraRGB :
tipo == TipoFrameCamera.Heatmap ? lblLeituraHeatmap :
//tipo == TipoFrameCamera.RadarTopDown ? lblLeituraRadar2D :
tipo == TipoFrameCamera.Segmentacao ? lblLeituraSegmentacaoSemantica :
new Label();
pic.Image?.Dispose();
try { pic.Image = frame?.image(); } catch { }
lbl.Text = $"Leitura em: " + frame?.timestamp.ToString("dd/MM/yyyy HH:mm:ss");
if (tipo == CameraFrameType.RadarTopDown && VisualWorkerService.DadosLeitura.Analises != null)
/*if (tipo == TipoFrameCamera.RadarTopDown && VisualWorkerService.DadosLeitura.Analises != null)
{
//pic.Image = VariaveisOperacao.Operadores.VisualWorker.DadosLeitura.Leitura.obj.radar_2d.PlotarAnalsie(frame.image);
pic.Image = VariaveisOperacao.Operadores.VisualWorker.DadosLeitura.Leitura.obj.radar_2d.PlotarAnalsie(frame.image);
//PlotarGraficoPerfilCorredor(VisualWorkerService.DadosLeitura.Analises.corredor_prof);
}
PlotarGraficoPerfilCorredor(VisualWorkerService.DadosLeitura.Analises.corredor_prof);
}*/
}
private void btnReqStatusDispositivo_Click(object sender, EventArgs e)
@ -152,7 +153,7 @@ namespace AgroBase.Forms
var analise = VisualWorkerService.DadosLeitura.Analises;
lblLeituraDebug.Text = $"Leitura em: " + (analise?.timestamp ?? DateTime.MinValue).ToString("dd/MM/yyyy HH:mm:ss");
OAKCameraFrameModel frame = VisualWorkerService.DadosLeitura.CameraFrames[CameraFrameType.Heatmap];
OAKCameraFrameModel frame = VisualWorkerService.DadosLeitura.CameraFrames[TipoFrameCamera.Heatmap];
picDebug.Image?.Dispose();
picDebug.Image = analise.PlotarAnalise(frame);

1
AgroBase/AgroBase/Models/Enums.cs
View File

@ -370,6 +370,7 @@
MatrizCusto = 5,
Deteccoes = 6,
Corredor = 7,
Raw4 = 8,
}
}

12
AgroBase/AgroBase/Models/Modules/AtuadorModel.cs
View File

@ -260,7 +260,7 @@ namespace AgroBase.Models.Modules
_EnderecoCAN_Rx = 0x12,
_TaxaAmostragem = 1000,
RequisitarDados = true,
// 71 ~ 73
// 71 ~ 77
BicosPulverizadores = new List<AtuadorBicoModel>(),
// 51 ~ 51
BombasPressurizadoras = new List<AtuadorBombaModel>()
@ -649,10 +649,10 @@ namespace AgroBase.Models.Modules
}
},
},
// 91 ~ 93
// 91 ~ 97
Servos = new List<SensorServoModel>()
{
new SensorServoModel()
/*new SensorServoModel()
{
ID = "SRVB1",
ID_Num = 91,
@ -744,7 +744,7 @@ namespace AgroBase.Models.Modules
analise_health = true
},
}
},
},*/
},
};
for (int i = 0; i < Variaveis.OperacaoEmAndamento.Parametros.QtdBicos; i++)
@ -754,12 +754,12 @@ namespace AgroBase.Models.Modules
{
ID = "B" + posicao.ToString("00"),
ID_Num = 70 + posicao,
Servo_ID_Num = 90 + posicao,
Servo_ID_Num = Atuador.Servos.FirstOrDefault(x => (x.ID_Num - 20) == 70 + posicao)?.ID_Num ?? -1,
Componente = S_Code.sBIC,
Posicao = posicao,
Inicializado = false,
AnguloControle = 0,
AnguloAbertura = 110,
AnguloAbertura = 30,
Comandar = true,
Mandatorio = true,
ComandoAtuar = false,

4
AgroBase/AgroBase/Models/Modules/SensoriamentoModel.cs
View File

@ -2055,8 +2055,8 @@ namespace AgroBase.Models.Modules
loRaService.Configurado = false;
loRaService.ParametrosGet.UltimaLeitura = DateTime.MinValue;
loRaService.MensagensRecebidasAvulsas = new List<LoRaMensagemModel>();
VisualWorkerService.DadosLeitura.CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>();
WeedWorkerService.DadosLeitura.CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>();
VisualWorkerService.DadosLeitura.CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
WeedWorkerService.DadosLeitura.CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
ReiniciarLeituras(Dados);
}

6
AgroBase/AgroBase/Models/OAKCameraModel.cs
View File

@ -36,7 +36,7 @@ namespace AgroBase.Models
public double DistanciaMinimaVisao { get; set; } = 500;
public double PercentualAlturaSolo { get; set; } = 30;
public Dictionary<CameraFrameType, OAKCameraFrameModel> frames
public Dictionary<TipoFrameCamera, OAKCameraFrameModel> frames
{
get
{
@ -47,13 +47,13 @@ namespace AgroBase.Models
case T_Code.Cam:
return WeedWorkerService.DadosLeitura.CameraFrames;
default:
return new Dictionary<CameraFrameType, OAKCameraFrameModel>();
return new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
}
}
}
public async Task<Bitmap> GetNewFrame(CameraFrameType tipo = CameraFrameType.Rgb)
public async Task<Bitmap> GetNewFrame(TipoFrameCamera tipo = TipoFrameCamera.Rgb)
{
OAKCameraFrameModel frame = null;
switch (Dispositivo)

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

@ -203,6 +203,7 @@ namespace AgroBase.Models
int qtdCamerasSolo,
TiposControladorDirecional tipoControleDirecional,
bool movimentoAutomatico,
bool direcionalAutomatico,
bool pulverizadorAutomatico,
bool frenagemAutomatica)
{
@ -227,8 +228,9 @@ namespace AgroBase.Models
{
SonarAtivado = sonarAtivado,
MovimentoAutomatico = movimentoAutomatico,
DirecionalAutomatico = direcionalAutomatico,
PulverizadorAutomatico = pulverizadorAutomatico,
FrenagemAutomatica = frenagemAutomatica,
FrenagemAutomaticaAoParar = frenagemAutomatica,
AtuDuracaoAtuacao = duracaoAtuacao,
AtuPercentualInicioPulverizacao = percentualAtuacao,
AtuPressaoLinha = pressaoLinha,
@ -364,8 +366,9 @@ namespace AgroBase.Models
Controle = new OperacaoParametrosControleModel()
{
MovimentoAutomatico = false,
DirecionalAutomatico = false,
PulverizadorAutomatico = false,
FrenagemAutomatica = true,
FrenagemAutomaticaAoParar = true,
MovVelocidadeSErvasPercent = 100,
MovVelocidadeCErvasPercent = 15,
DirAnguloMaximo = 25,
@ -472,8 +475,9 @@ namespace AgroBase.Models
Controle = new OperacaoParametrosControleModel()
{
MovimentoAutomatico = true,
DirecionalAutomatico = true,
PulverizadorAutomatico = true,
FrenagemAutomatica = true,
FrenagemAutomaticaAoParar = true,
MovVelocidadeSErvasPercent = 50,
MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30,
@ -622,8 +626,9 @@ namespace AgroBase.Models
Controle = new OperacaoParametrosControleModel()
{
MovimentoAutomatico = true,
DirecionalAutomatico = true,
PulverizadorAutomatico = false,
FrenagemAutomatica = false,
FrenagemAutomaticaAoParar = false,
MovVelocidadeSErvasPercent = 50,
MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30,
@ -753,8 +758,9 @@ namespace AgroBase.Models
Controle = new OperacaoParametrosControleModel()
{
MovimentoAutomatico = true,
DirecionalAutomatico = true,
PulverizadorAutomatico = false,
FrenagemAutomatica = true,
FrenagemAutomaticaAoParar = true,
MovVelocidadeSErvasPercent = 40,
MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30,
@ -1258,7 +1264,11 @@ namespace AgroBase.Models
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico)
{
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov, T_Code.Dir });
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov });
}
if (pControle.DirecionalAutomatico)
{
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Dir });
}
if (pControle.PulverizadorAutomatico)
{
@ -1282,7 +1292,9 @@ namespace AgroBase.Models
continue;
if (Ctrl.Tipo == T_Code.Atu && !pControle.PulverizadorAutomatico)
continue;
if ((Ctrl.Tipo == T_Code.Mov || Ctrl.Tipo == T_Code.Dir) && !pControle.MovimentoAutomatico)
if (Ctrl.Tipo == T_Code.Mov && !pControle.MovimentoAutomatico)
continue;
if (Ctrl.Tipo == T_Code.Dir && !pControle.DirecionalAutomatico)
continue;
}
@ -1314,7 +1326,7 @@ namespace AgroBase.Models
{
case T_Code.Mov:
{
if (pControle.FrenagemAutomatica && _Controle.EmFreio)
if (pControle.FrenagemAutomaticaAoParar && _Controle.EmFreio)
{
_Controle.Direcao = Direcao.EmFreio;
}
@ -1645,7 +1657,11 @@ namespace AgroBase.Models
if (Variaveis.OperacaoEmAndamento.Pausa) return;
if (controleBase.Controle == null) return;
if (Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico && Variaveis.OperacaoEmAndamento.Iniciado) return;
var cmdRover = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.botaoJoy == controleBase.Tecla);
if (cmdRover?.Dispositivo == T_Code.Mov && Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico && Variaveis.OperacaoEmAndamento.Iniciado) return;
if (cmdRover?.Dispositivo == T_Code.Dir && Variaveis.OperacaoEmAndamento.Parametros.Controle.DirecionalAutomatico && Variaveis.OperacaoEmAndamento.Iniciado) return;
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
if (controleBase.Controle.PercentualVelocidadeSP != null) _Controle.PercentualVelocidadeSP = (double)controleBase.Controle.PercentualVelocidadeSP;
@ -1653,7 +1669,7 @@ namespace AgroBase.Models
if (controleBase.Controle.TipoMovimentoDirecional != null) _Controle.TipoMovimento = (TipoMovimentoDirecional)controleBase.Controle.TipoMovimentoDirecional;
if (controleBase.Controle.EmFreio != null) _Controle.EmFreio = (bool)controleBase.Controle.EmFreio;
Keys tecla = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.botaoJoy == controleBase.Tecla)?.key ?? Keys.Escape;
Keys tecla = cmdRover?.key ?? Keys.Escape;
GeneralJoystick.EnviaComandoMotor(tecla, controleBase.Dispositivo, ForcarComando: true, ID: controleBase._comp_id);
}
@ -1881,7 +1897,7 @@ namespace AgroBase.Models
string nome = Variaveis.OperacaoEmAndamento.idxLog.ToString();
Camera.SaveFrames(new List<TipoFrameCamera>() { TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao }, nome, Caminho);
Camera.SaveFrames(new List<TipoFrameCamera>() { TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao }, nome, Caminho); // TipoFrameCamera.Raw4
var cameras_conectadas = JsonConvert.DeserializeObject<Dictionary<string, object>>(RedisService.Get(CtxKey.DadosCameras));
bool conectada = cameras_conectadas.ContainsKey(Camera?.Id ?? "");
@ -2136,17 +2152,8 @@ namespace AgroBase.Models
public void Reiniciar()
{
var _Parametros = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (_Parametros.MovimentoAutomatico)
{
Angulo = 0;
PercentualVelocidadeSP = 0;
}
else
{
Angulo = _Parametros.DirAnguloMaximo;
PercentualVelocidadeSP = _Parametros.MovVelocidadeCErvasPercent;
}
PercentualVelocidadeSP = _Parametros.MovimentoAutomatico ? 0 : _Parametros.MovVelocidadeCErvasPercent;
Angulo = _Parametros.DirecionalAutomatico ? 0 : _Parametros.DirAnguloMaximo;
EmFreio = false;
Direcao = Direcao.Parado;
TipoMovimento = TipoMovimentoDirecional.RodasDianteiras;
@ -2567,10 +2574,10 @@ namespace AgroBase.Models
{
var vw = OperadorVisual?.Clone();
if (vw != null)
vw.CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>();
vw.CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
var ww = OperadorErvas?.Clone();
if (ww != null)
ww.CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>();
ww.CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
return new OperacaoSensoriamentoLogModel()
{

7
AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs
View File

@ -31,10 +31,13 @@ namespace AgroBase.Models.Operacoes
public class OperacaoParametrosControleModel
{
public bool SonarAtivado { get; set; }
public bool MovimentoAutomatico { get; set; }
public bool DirecionalAutomatico { get; set; }
public bool PulverizadorAutomatico { get; set; }
public bool FrenagemAutomatica { get; set; }
public bool SonarAtivado { get; set; }
public bool OakParadaPorObstaculo { get; set; }
public bool ImuParadaPorInclinacao { get; set; }
public bool FrenagemAutomaticaAoParar { get; set; }
public int MovVelocidadeSErvasPercent { get; set; }
public int MovRpmMax

8
AgroBase/AgroBase/Models/Operadores/CameraWorkerModel.cs
View File

@ -59,13 +59,5 @@ namespace AgroBase.Models.Operadores
ScriptCarregado = 1,
}
public enum CameraFrameType
{
Rgb = 1,
Heatmap = 2,
RadarTopDown = 3,
Segmentacao = 4,
Debug = 5
}
}

6
AgroBase/AgroBase/Models/Operadores/VisualWorkerModel.cs
View File

@ -13,7 +13,7 @@ namespace AgroBase.Models.Operadores
public bool Pronto { get; set; }
public DateTime ProntoEm { get; set; }
public DateTime UltimaMensagem { get; set; }
public Dictionary<CameraFrameType, OAKCameraFrameModel> CameraFrames { get; set; } = new Dictionary<CameraFrameType, OAKCameraFrameModel>();
public Dictionary<TipoFrameCamera, OAKCameraFrameModel> CameraFrames { get; set; } = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
public VisualWorkerMessageAnaliseModel Analises { get; set; } = new VisualWorkerMessageAnaliseModel();
public VisualWorkerMessageIMUModel Imu { get; set; } = new VisualWorkerMessageIMUModel();
public StatusModulo Status { get; set; }
@ -27,7 +27,7 @@ namespace AgroBase.Models.Operadores
IniciadoEm = IniciadoEm,
Pronto = Pronto,
ProntoEm = ProntoEm,
CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>(CameraFrames ?? new Dictionary<CameraFrameType, OAKCameraFrameModel>()),
CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>(CameraFrames ?? new Dictionary<TipoFrameCamera, OAKCameraFrameModel>()),
Analises = Analises?.Clone(),
Imu = Imu?.Clone(),
UltimaMensagem = UltimaMensagem,
@ -42,7 +42,7 @@ namespace AgroBase.Models.Operadores
IniciadoEm = DateTime.MinValue;
Pronto = false;
ProntoEm = DateTime.MinValue;
CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>();
CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
Analises = new VisualWorkerMessageAnaliseModel();
Imu = new VisualWorkerMessageIMUModel();
Resumo = new VisualWorkerResumoModel();

7
AgroBase/AgroBase/Models/Operadores/WeedWorkerModel.cs
View File

@ -1,5 +1,6 @@
using System;
using System.Collections.Generic;
using static AgroBase.Models.Enums;
namespace AgroBase.Models.Operadores
{
@ -10,7 +11,7 @@ namespace AgroBase.Models.Operadores
public bool Pronto { get; set; }
public DateTime ProntoEm { get; set; }
public DateTime UltimaMensagem { get; set; }
public Dictionary<CameraFrameType, OAKCameraFrameModel> CameraFrames { get; set; } = new Dictionary<CameraFrameType, OAKCameraFrameModel>();
public Dictionary<TipoFrameCamera, OAKCameraFrameModel> CameraFrames { get; set; } = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
public WeedWorkerAnaliseModel Analise { get; set; } = new WeedWorkerAnaliseModel();
@ -22,7 +23,7 @@ namespace AgroBase.Models.Operadores
IniciadoEm = IniciadoEm,
Pronto = Pronto,
ProntoEm = ProntoEm,
CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>(CameraFrames ?? new Dictionary<CameraFrameType, OAKCameraFrameModel>()),
CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>(CameraFrames ?? new Dictionary<TipoFrameCamera, OAKCameraFrameModel>()),
Analise = Analise.Clone(),
UltimaMensagem = UltimaMensagem,
};
@ -34,7 +35,7 @@ namespace AgroBase.Models.Operadores
IniciadoEm = DateTime.MinValue;
Pronto = false;
ProntoEm = DateTime.MinValue;
CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>();
CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
}
}

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

@ -6,6 +6,7 @@ using Newtonsoft.Json.Linq;
using System;
using System.Collections.Generic;
using System.Linq;
using static AgroBase.Models.CorredorTrajetoriaModel;
using static AgroBase.Models.Enums;
namespace AgroBase.Models
@ -683,7 +684,7 @@ namespace AgroBase.Models
DefinirProximoPonto();
if (PontoAtual == null) goto Inicio;
if (VerificaInicioOperacaoMeioRua()) goto Inicio;
if (VerificaInicioOperacaoMeioRua(true)) goto Inicio;
MarcarPontosIntermediariosNaoVisitados();
DefinirPontoMaisProximo();
@ -724,41 +725,42 @@ namespace AgroBase.Models
return melhoresPontosCorredores.Any(x => x.Value.Item1) ? melhoresPontosCorredores.Where(x => x.Value.Item1).OrderBy(x => x.Value.Item2).FirstOrDefault().Key : -1;
}
private (bool, int) VerificaEquipamentoDentroCorredor()
private (bool, int) VerificaEquipamentoDentroCorredorMontado(GPSModel posicaoAtual)
{
_TrajetoriaJanela = _TrajetoriaFixa;
AtualizarPropriedadesPontosTrajetoria();
AtualizarTrajetoriaJanela();
if (Corredores == null)
return (false, -1);
List<PontoTrajetoriaModel> melhoresPontosCorredores = new List<PontoTrajetoriaModel>();
foreach (var corredor in Corredores)
{
(GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(_TrajetoriaFixa[0].Posicao, corredor);
if (pontoMaisProximo == null)
{
return (false, -1);
}
(GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(posicaoAtual, corredor);
if (pontoMaisProximo == null) continue;
var melhorPonto = _TrajetoriaFixa.FirstOrDefault(x => x.Posicao.Latitude == pontoMaisProximo.Latitude && x.Posicao.Longitude == pontoMaisProximo.Longitude);
var melhorPonto = _Corredores[Corredores.IndexOf(corredor)].Pontos.FirstOrDefault(x => x.Posicao.Latitude == pontoMaisProximo.Latitude && x.Posicao.Longitude == pontoMaisProximo.Longitude);
melhorPonto.AtualizarPropriedades();
melhoresPontosCorredores.Add(melhorPonto);
}
bool pontoNaMargem = melhoresPontosCorredores.Any(x => x.NaMargem);
int idxPonto = !pontoNaMargem ? 0 : melhoresPontosCorredores.Where(x => x.NaMargem).OrderBy(x => x.DistanciaAtual).FirstOrDefault().idxPonto;
var ponto = melhoresPontosCorredores.Where(x => x.NaMargem).OrderBy(x => x.DistanciaAtual).FirstOrDefault();
bool pontoNaMargem = ponto != null;
int idxPonto = !pontoNaMargem ? -1 : ponto.idxPonto;
return (pontoNaMargem, idxPonto);
}
private bool VerificaInicioOperacaoMeioRua()
private bool VerificaInicioOperacaoMeioRua(bool considerarStatus)
{
if (!VerificacaoInicialMeioRuaConcluida && new List<StatusOperacao>() { StatusOperacao.Aguardando, StatusOperacao.EmAndamento }.Contains(Variaveis.OperacaoEmAndamento.StatusAtual))
if (!VerificacaoInicialMeioRuaConcluida && ((considerarStatus && new List<StatusOperacao>() { StatusOperacao.Aguardando, StatusOperacao.EmAndamento }.Contains(Variaveis.OperacaoEmAndamento.StatusAtual)) || !considerarStatus))
{
VerificacaoInicialMeioRuaConcluida = true;
(bool noCoredor, var idxPontoMaisProximo) = VerificaEquipamentoDentroCorredor();
(bool noCoredor, var idxPontoMaisProximo) = VerificaEquipamentoDentroCorredorMontado(_TrajetoriaFixa[0].Posicao);
if (noCoredor)
{
for (int i = 0; i < idxPontoMaisProximo; i++)
for (int i = 0; i <= idxPontoMaisProximo; i++)
{
_TrajetoriaFixa[i].Visitado = true;
}
CorredorAtual.AtualizarDados();
return true;
}
}
@ -771,106 +773,177 @@ namespace AgroBase.Models
{
if (CorredorAtual == null) return;
bool abrirCurva = true;
bool modoSimulador = false && Variaveis.OperacaoEmAndamento.Simulando;
// Limiar de "margem" para considerar proximidade do fim
double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 5.0;
double distanciaCorredor = CorredorAtual.DistanciaTotal;
double distanciaRestanteCorredor = CorredorAtual.DistanciaRestante;
double distanciaUltimoPontoCorredor = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, CorredorAtual.Pontos.LastOrDefault().Posicao);
var OpVisual = Variaveis.OperacaoEmAndamento.Sensoriamento?.OperadorVisual ?? new VisualWorkerModel();
StatusModulo OpVisualStatus = Variaveis.OperacaoEmAndamento.Sensoriamento.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado;
var DadosSegmentacao = OpVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel();
if (OpVisualStatus == StatusModulo.Operante || Variaveis.OperacaoEmAndamento.Simulando) //OpVisual.Iniciado &&
var opVisual = Variaveis.OperacaoEmAndamento.Sensoriamento?.OperadorVisual ?? new VisualWorkerModel();
StatusModulo opVisualStatus = Variaveis.OperacaoEmAndamento.Sensoriamento.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado;
var dadosSegmentacao = opVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel();
// Se não tem visual operante, não está no modoSimulador ou o corredor eh curto demais, não faz nada
if (!(opVisualStatus == StatusModulo.Operante || modoSimulador || distanciaCorredor <= limiarDistanciaMargem))
return;
// Garante estrutura de dados visuais do corredor
var dv = CorredorAtual.DadosVisuais ?? (CorredorAtual.DadosVisuais = new DadosVisuaisCorredor());
StatusCarroMapa statusAtual = dadosSegmentacao.status_corredor;
StatusCarroMapa statusAnterior = dadosSegmentacao.status_corredor_anterior;
var agora = DateTime.Now;
// 1) Atualiza tempo e distância por status (usando o status anterior)
if (dv.UltimoUpdate > DateTime.MinValue)
{
// Contagem de tempo por status em segundos
if (CorredorAtual.TempoPorStatus == null) CorredorAtual.TempoPorStatus = new Dictionary<StatusCarroMapa, double>();
StatusCarroMapa statusCarro = DadosSegmentacao.status_corredor;
StatusCarroMapa statusCarroAnt = DadosSegmentacao.status_corredor_anterior;
if (!CorredorAtual.TempoPorStatus.TryGetValue(statusCarro, out var status))
{
CorredorAtual.TempoPorStatus.Add(statusCarro, 0);
}
if (CorredorAtual.UltimoTempoStatus > DateTime.MinValue)
{
CorredorAtual.TempoPorStatus[statusCarro] += (DateTime.Now - CorredorAtual.UltimoTempoStatus).TotalSeconds;
}
CorredorAtual.UltimoTempoStatus = DateTime.Now;
double TempoTotalContagem = CorredorAtual.TempoPorStatus.Sum(x => x.Value);
double TempoMovimento = CorredorAtual.TempoPorStatus.Where(x => x.Key != StatusCarroMapa.Parado).Sum(x => x.Value);
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.CaminhandoRua, out double TempoCaminhandoRua);
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.EntrandoRua, out double TempoEntrandoRua);
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.SaindoRua, out double TempoSaindoRua);
CorredorAtual.TempoPorStatus.TryGetValue(StatusCarroMapa.Direcionando, out double TempoDirecionando);
double TempoDentroRua = TempoEntrandoRua + TempoCaminhandoRua + TempoSaindoRua;
double PercentualTempoComCana = TempoMovimento == 0 ? 0 : TempoDentroRua / TempoMovimento;
double PercentualTempoSemCana = TempoMovimento == 0 ? 0 : TempoDirecionando / TempoMovimento;
double ts_agora = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0;
double dt = (agora - dv.UltimoUpdate).TotalSeconds;
if (dt < 0) dt = 0;
if (
(
(Variaveis.OperacaoEmAndamento.Simulando ||
(ts_agora - DadosSegmentacao.timestamp < 2.0 && PercentualTempoComCana > 0.4)) // Esteve a pelo menos 40% do tempo em movimento com presenca de cana e o dado esta atualizado
)
&& distanciaCorredor >= limiarDistanciaMargem
&& distanciaRestanteCorredor < limiarDistanciaMargem // Esta a pelo menos x metros do final do corredor
&& distanciaUltimoPontoCorredor < limiarDistanciaMargem // Esta a pelo menos x metros do ultimo ponto do corredor
&& statusCarro == StatusCarroMapa.Direcionando // Carro esta fora do corredor
//&& statusCarroAnt != statusCarro
)
// Aproximação da distância percorrida nesse intervalo pelo mapa
double deltaDist = 0;
if (dv.DistanciaRestanteAnterior >= 0)
{
double anguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo;
if (abrirCurva && !CorredorAtual.Ultimo)
deltaDist = dv.DistanciaRestanteAnterior - distanciaRestanteCorredor;
if (deltaDist < 0) deltaDist = 0; // proteção contra ruído
}
bool emMovimento = statusAnterior != StatusCarroMapa.Parado;
if (emMovimento)
{
switch (statusAnterior)
{
anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda ? -Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo : Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo;
case StatusCarroMapa.CaminhandoRua:
case StatusCarroMapa.EntrandoRua:
case StatusCarroMapa.SaindoRua:
dv.TempoDentroRuaTotal += dt;
dv.DistanciaDentroRuaTotal += deltaDist;
dv.TempoForaRuaContinuo = 0;
dv.DistanciaForaRuaContinuo = 0;
break;
case StatusCarroMapa.Direcionando:
dv.TempoForaRuaTotal += dt;
dv.DistanciaForaRuaTotal += deltaDist;
dv.TempoForaRuaContinuo += dt;
dv.DistanciaForaRuaContinuo += deltaDist;
break;
default:
// Outros estados quebram a continuidade "fora do corredor"
dv.TempoForaRuaContinuo = 0;
dv.DistanciaForaRuaContinuo = 0;
break;
}
}
else
{
// Parado: não acumula movimento, mas quebra janela contínua
dv.TempoForaRuaContinuo = 0;
dv.DistanciaForaRuaContinuo = 0;
}
}
dv.UltimoUpdate = agora;
dv.DistanciaRestanteAnterior = distanciaRestanteCorredor;
// 2) Confirmar se este corredor realmente existiu (teve cana)
const double MIN_DIST_CORREDOR_CONFIRMADO = 5.0; // metros dentro da rua
const double MIN_TEMPO_CORREDOR_CONFIRMADO = 10.0; // segundos dentro da rua
if (!dv.CorredorConfirmado)
{
if (dv.DistanciaDentroRuaTotal >= MIN_DIST_CORREDOR_CONFIRMADO || dv.TempoDentroRuaTotal >= MIN_TEMPO_CORREDOR_CONFIRMADO)
{
dv.CorredorConfirmado = true;
}
}
// 3) Critérios para considerar que chegou ao fim do corredor antes do mapa acabar
double tsAgora = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0;
bool dadoFresco = (tsAgora - dadosSegmentacao.timestamp) < 2.0;
bool pertoDoFim = distanciaRestanteCorredor < limiarDistanciaMargem && distanciaUltimoPontoCorredor < limiarDistanciaMargem;
bool foraDoCorredorAgora = (statusAtual == StatusCarroMapa.Direcionando);
// Limiar de "quanto tempo/distância já estou fora do corredor" para evitar buracos pontuais
const double MIN_TEMPO_FORA_CORREDOR = 2.0; // segundos
const double MIN_DIST_FORA_CORREDOR = 1.0; // metros
bool foraContinuoSuficiente = dv.TempoForaRuaContinuo >= MIN_TEMPO_FORA_CORREDOR || dv.DistanciaForaRuaContinuo >= MIN_DIST_FORA_CORREDOR;
bool podeUsarSegmentacao = modoSimulador || (dadoFresco && dv.CorredorConfirmado);
bool abrirCurva = false; // mantém seu flag por enquanto
if (podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente)
{
// 4) Aqui consideramos que o corredor acabou antes do fim do mapa
double anguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo;
if (abrirCurva && !CorredorAtual.Ultimo)
{
anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda
? -Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo
: Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo;
}
int pontosMarcar = 0;
foreach (var ponto in CorredorAtual.Pontos.Where(x => !x.Visitado))
{
ponto.Visitado = true;
pontosMarcar++;
}
if (!CorredorAtual.Ultimo)
{
// Marca ponto de ligação de entrada do próximo corredor como visitado
_TrajetoriaFixa[CorredorAtual.Pontos[CorredorAtual.QtdPontos - 1].idxPonto + 1].Visitado = true;
// Avança bem a janela para "pular" o restinho do corredor
AtualizarIndicesJanela((pontosMarcar + 2) * 2);
AtualizarTrajetoriaJanela();
DefinirPontoAtual();
AtualizarCorredorAtual();
// Garante que os primeiros pontos do corredor atual estão como visitados
for (int i = 0; i < pontosMarcar && i < CorredorAtual.Pontos.Count; i++)
{
CorredorAtual.Pontos[i].Visitado = true;
}
int pontosMarcar = 0;
foreach (var Ponto in CorredorAtual.Pontos.Where(x => !x.Visitado))
AtualizarIndicesJanela();
AtualizarTrajetoriaJanela();
DefinirPontoAtual();
DefinirProximoPonto();
if (abrirCurva)
{
Ponto.Visitado = true;
pontosMarcar++;
}
if (!CorredorAtual.Ultimo)
{
_TrajetoriaFixa[CorredorAtual.Pontos[CorredorAtual.QtdPontos - 1].idxPonto + 1].Visitado = true; // Marca ponto de ligacao de entrada do proximo corredor como visitado
AtualizarIndicesJanela((pontosMarcar + 2) * 2);
AtualizarTrajetoriaJanela();
DefinirPontoAtual();
AtualizarCorredorAtual();
for (int i = 0; i < pontosMarcar; i++)
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico)
{
CorredorAtual.Pontos[i].Visitado = true;
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent;
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DelayEnvioComandoExtra = 2000;
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov });
}
AtualizarIndicesJanela();
AtualizarTrajetoriaJanela();
DefinirPontoAtual();
DefinirProximoPonto();
if (abrirCurva)
if (pControle.DirecionalAutomatico)
{
Console.WriteLine($"Angulo de controle definido de {Variaveis.OperacaoEmAndamento.Controle.Angulo} para {anguloControle}");
Variaveis.OperacaoEmAndamento.Controle.Angulo = anguloControle;
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent;
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.MovimentoArco;
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).DelayEnvioComandoExtra = 1500;
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DelayEnvioComandoExtra = 2000;
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico)
{
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov, T_Code.Dir });
}
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Dir });
}
}
}
RedisService.AtualizarCampos(
CtxKey.DadosVisualWorker,
("segmentacao.status_corredor_anterior", (int)statusCarro)
);
if (VisualWorkerService.DadosLeitura.Analises.segmentacao != null)
VisualWorkerService.DadosLeitura.Analises.segmentacao.status_corredor_anterior = statusCarro;
}
}
@ -946,20 +1019,26 @@ namespace AgroBase.Models
bool mangerDestravou = novoComando != null && (_Controle.ticks_sem_resposta.AddMilliseconds(min_ticks_sem_resposta) < agora && novoComando.heartbeat != _Controle.heartbeat);
if (novoComando != null && (comandoMudou || mangerDestravou))
{
_Controle.Angulo = novoComando.angulo;
_Controle.TipoMovimento = novoComando.tipo_movimento;
_Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade;
_Controle.EmFreio = novoComando.em_freio;
_Controle.SimulacaoMPC = novoComando.simulacao?.Select(x => new MPCSimulacaoModel() { latitude = x[0], longitude = x[1], orientacao = x[2] }).ToList() ?? new List<MPCSimulacaoModel>();
_Controle.Latencia = novoComando.latencia;
_Controle.ErroLateral = novoComando.erro_lateral;
_Controle.DebugCustoMpc = novoComando.debug_custo;
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico)
{
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov, T_Code.Dir });
_Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade;
_Controle.EmFreio = novoComando.em_freio;
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov });
}
if (pControle.DirecionalAutomatico)
{
_Controle.Angulo = novoComando.angulo;
_Controle.TipoMovimento = novoComando.tipo_movimento;
_Controle.SimulacaoMPC = novoComando.simulacao?.Select(x => new MPCSimulacaoModel() { latitude = x[0], longitude = x[1], orientacao = x[2] }).ToList() ?? new List<MPCSimulacaoModel>();
_Controle.Latencia = novoComando.latencia;
_Controle.ErroLateral = novoComando.erro_lateral;
_Controle.DebugCustoMpc = novoComando.debug_custo;
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Dir });
}
}
if (novoComando != null && novoComando.heartbeat == _Controle.heartbeat)
@ -1718,6 +1797,9 @@ namespace AgroBase.Models
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps;
VerificaInicioOperacaoMeioRua(false);
AtualizarDadosTrajetoria();
// =========================
// REGRA 0 – Está dentro de corredor?
// =========================
@ -2251,8 +2333,7 @@ namespace AgroBase.Models
public int idxRuaEsquerda { get; set; }
public int idxRuaDireita { get; set; }
public double FatorLarguraCorredor { get; set; }
public DateTime UltimoTempoStatus { get; set; } = DateTime.MinValue;
public Dictionary<StatusCarroMapa, double> TempoPorStatus { get; set; }
public DadosVisuaisCorredor DadosVisuais { get; set; } = new DadosVisuaisCorredor();
public void AtualizarDados()
{
@ -2302,8 +2383,7 @@ namespace AgroBase.Models
Pontos = new List<PontoTrajetoriaModel>(),
Ultimo = Ultimo,
FatorLarguraCorredor = FatorLarguraCorredor,
UltimoTempoStatus = UltimoTempoStatus,
TempoPorStatus = new Dictionary<StatusCarroMapa, double>(TempoPorStatus ?? new Dictionary<StatusCarroMapa, double>())
DadosVisuais = DadosVisuais
};
if (clonarPontos)
{
@ -2312,6 +2392,30 @@ namespace AgroBase.Models
return obj;
}
public class DadosVisuaisCorredor
{
// Quanto já andou DENTRO do corredor (EntrandoRua, CaminhandoRua, SaindoRua)
public double TempoDentroRuaTotal { get; set; } = 0;
public double DistanciaDentroRuaTotal { get; set; } = 0;
// Quanto já andou FORA do corredor (Direcionando)
public double TempoForaRuaTotal { get; set; } = 0;
public double DistanciaForaRuaTotal { get; set; } = 0;
// Janela contínua atual fora do corredor (protege contra buracos curtos)
public double TempoForaRuaContinuo { get; set; } = 0;
public double DistanciaForaRuaContinuo { get; set; } = 0;
// Para acumular tempo/distância por intervalo entre chamadas
public DateTime UltimoUpdate { get; set; } = DateTime.MinValue;
public double DistanciaRestanteAnterior { get; set; } = -1;
// Flag: este corredor realmente existiu (teve cana por X tempo/distância)
public bool CorredorConfirmado { get; set; } = false;
}
}
public class PontoTrajetoriaModel
@ -2378,7 +2482,7 @@ namespace AgroBase.Models
// Define o limite de distância para considerar o ponto como visitado
double fatorAjuste = Tipo == TipoPontoRua.CruvaEntreCorredores ? 3.0 : 2.0; // Curvas podem ter menos tolerância
double limiteDistancia = Math.Ceiling(TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras / distanciaEntrePontos) * fatorAjuste * distanciaEntrePontos;
double limiteDistancia = Math.Max(0.1, Math.Ceiling(TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras / distanciaEntrePontos) * fatorAjuste * distanciaEntrePontos);
// Se o ponto está dentro da margem e dentro do limite de distância, marca como visitado
if (NaMargem && DistanciaTrajeto <= limiteDistancia)

2
AgroBase/AgroBase/Models/Variaveis.cs
View File

@ -229,7 +229,7 @@ namespace AgroBase.Models
public static double PercentualReservatorioMinCritio { get; set; } = 5.0;
public static double PercentualToleranciaPressaoLinha { get; set; } = 0.15;
public static int QuantidadeCamerasSolo { get; set; } = 1;
public static int QuantidadeBicosPulverizadores { get; set; } = 3;
public static int QuantidadeBicosPulverizadores { get; set; } = 7;
public static double AlturaBarraPulverizadoraCm { get; set; } = 80;
public static double ComprimentoBarraPulverizadoraCm { get; set; } = 103.7;
public static double DistanciaEntreBicosCm { get; set; } = 50;

1
AgroBase/AgroBase/Services/Operadores/CameraWorkerService.cs
View File

@ -106,6 +106,7 @@ namespace AgroBase.Services.Operadores
{
var camera_str = RedisService.Get(RedisService.CamKey(cam.Key));
var camera = JsonConvert.DeserializeObject<CameraWorkerItemModel>(camera_str);
if (camera.mx_id == null) continue;
CamerasConectadas.Add(new OAKCameraModel()
{
Dispositivo = camera.dispositivo,

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

@ -177,6 +177,7 @@ namespace AgroBase.Services.Operadores
("serial_number", VariaveisEquipamento.Parametros.serial_number),
("largura", VariaveisEquipamento.LarguraEquipamentoMm / 1000.0),
("distancia_entre_eixos", VariaveisEquipamento.DistanciaEntreEixos / 100.0),
("qtd_bicos", VariaveisEquipamento.QuantidadeBicosPulverizadores),
("percentual_reservatorio_min", VariaveisEquipamento.PercentualReservatorioMin),
("percentual_tensao_bateria_min", VariaveisEquipamento.PercentualTensaoBateriaMin),
("camera_caminho_id", Variaveis.OperacaoEmAndamento.DispSen?.Dados?.CameraCaminho?.Id ?? ""),
@ -242,7 +243,10 @@ namespace AgroBase.Services.Operadores
distanciaMargem = p.LarguraCorredor * 0.8
})
.ToList(),
horizonte = 2.5,
horizonte = 2.0,
horizonte_min = 1.2,
horizonte_max = 2.8,
horizonte_parado = 1.2,
angulo_max_graus = pControle.DirAnguloMaximo,
velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeCErvasPercent),
velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeSErvasPercent),
@ -291,8 +295,10 @@ namespace AgroBase.Services.Operadores
CtxKey.DadosControle,
("pulverizador_automatico", pControle.PulverizadorAutomatico),
("movimento_automatico", pControle.MovimentoAutomatico),
("frenagem_automatica", pControle.FrenagemAutomatica),
("sonar_ativado", pControle.SonarAtivado)
("direcional_automatico", pControle.DirecionalAutomatico),
("frenagem_automatica", pControle.FrenagemAutomaticaAoParar),
("oak_parada_por_bloqueio", pControle.OakParadaPorObstaculo),
("imu_parada_por_inclinacao", pControle.ImuParadaPorInclinacao)
);
}

20
AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs
View File

@ -79,16 +79,20 @@ namespace AgroBase.Services.Operadores
bool comandoMudou = (_Controle.Angulo != novoComando.angulo || _Controle.PercentualVelocidadeSP != novoComando.percentual_velocidade || _Controle.TipoMovimento != novoComando.tipo_movimento || _Controle.EmFreio != novoComando.em_freio);
if (novoComando != null && comandoMudou)
{
_Controle.Angulo = novoComando.angulo;
_Controle.TipoMovimento = novoComando.tipo_movimento;
_Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade;
_Controle.EmFreio = novoComando.em_freio;
_Controle.SimulacaoMPC = novoComando.simulacao?.Select(x => new MPCSimulacaoModel() { latitude = x[0], longitude = x[1], orientacao = x[2] }).ToList() ?? new List<MPCSimulacaoModel>();
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico)
{
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov, T_Code.Dir });
_Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade;
_Controle.EmFreio = novoComando.em_freio;
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov });
}
if (pControle.DirecionalAutomatico)
{
_Controle.Angulo = novoComando.angulo;
_Controle.TipoMovimento = novoComando.tipo_movimento;
_Controle.SimulacaoMPC = novoComando.simulacao?.Select(x => new MPCSimulacaoModel() { latitude = x[0], longitude = x[1], orientacao = x[2] }).ToList() ?? new List<MPCSimulacaoModel>();
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Dir });
}
}
}
@ -110,7 +114,7 @@ namespace AgroBase.Services.Operadores
break;
case ManagerWorkerCommandType.EnviarDadosControle:
if (Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico)
if (Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico || Variaveis.OperacaoEmAndamento.Parametros.Controle.DirecionalAutomatico)
{
AtualizarControle(comando.@params);
}

21
AgroBase/AgroBase/Services/Operadores/OperadoresService.cs
View File

@ -79,7 +79,7 @@ namespace AgroBase.Services.Operadores
if (!Conectado)
{
TempoDesconectado++;
if (Variaveis.OperacaoEmAndamento.Iniciado && Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico)
if (Variaveis.OperacaoEmAndamento.Iniciado && (Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico || Variaveis.OperacaoEmAndamento.Parametros.Controle.DirecionalAutomatico))
{
RedisService.AtualizarCampos(
CtxKey.DadosOperacao,
@ -89,9 +89,24 @@ namespace AgroBase.Services.Operadores
);
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
if (Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico)
{
_Controle.PercentualVelocidadeSP = 0;
RedisService.AtualizarCampos(
CtxKey.DadosControle,
("velocidade_sp", _Controle.PercentualVelocidadeSP)
);
}
if (Variaveis.OperacaoEmAndamento.Parametros.Controle.DirecionalAutomatico)
{
_Controle.Angulo = 0;
RedisService.AtualizarCampos(
CtxKey.DadosControle,
("angulo_sp", _Controle.Angulo)
);
}
_Controle.Angulo = 0;
_Controle.PercentualVelocidadeSP = 0;
_Controle.Bicos.ForEach(x => x.ComandoAtuar = false);
RedisService.AtualizarCampos(
CtxKey.DadosControle,

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

@ -68,7 +68,7 @@ namespace AgroBase.Services.Operadores
break;
case VisualWorkerCommandType.GetCameraFrame:
CameraFrameType tipo = (CameraFrameType)Convert.ToInt32(comando.@params["tipo"].ToString());
TipoFrameCamera tipo = (TipoFrameCamera)Convert.ToInt32(comando.@params["tipo"].ToString());
DadosLeitura.CameraFrames[tipo] = new OAKCameraFrameModel()
{
frame = comando.@params["frame"].ToString(),
@ -106,7 +106,7 @@ namespace AgroBase.Services.Operadores
RedisService.Publish(CmdKey.VisualWorkerRx, JsonConvert.SerializeObject(comando));
}
public static async Task<OAKCameraFrameModel> GetCameraFrame(CameraFrameType tipo = CameraFrameType.Rgb)
public static async Task<OAKCameraFrameModel> GetCameraFrame(TipoFrameCamera tipo = TipoFrameCamera.Rgb)
{
DateTime Inicio = DateTime.Now;

5
AgroBase/AgroBase/Services/Operadores/WeedWorkerService.cs
View File

@ -71,6 +71,7 @@ namespace AgroBase.Services.Operadores
}
if (comandoMudou)
{
Console.WriteLine("Comando dos bicos alterado: " + JsonConvert.SerializeObject(novoComando));
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.PulverizadorAutomatico)
{
@ -98,7 +99,7 @@ namespace AgroBase.Services.Operadores
break;
case WeedWorkerCommandType.GetCameraFrame:
CameraFrameType tipo = (CameraFrameType)Convert.ToInt32(comando.@params["tipo"].ToString());
TipoFrameCamera tipo = (TipoFrameCamera)Convert.ToInt32(comando.@params["tipo"].ToString());
DadosLeitura.CameraFrames[tipo] = new OAKCameraFrameModel()
{
frame = comando.@params["frame"].ToString(),
@ -133,7 +134,7 @@ namespace AgroBase.Services.Operadores
Saude = OperadoresModels.AtualizarSaude(CtxKey.DadosWeedWorker);
}
public static async Task<OAKCameraFrameModel> GetCameraFrame(CameraFrameType tipo = CameraFrameType.Rgb)
public static async Task<OAKCameraFrameModel> GetCameraFrame(TipoFrameCamera tipo = TipoFrameCamera.Rgb)
{
DateTime Inicio = DateTime.Now;

2
AgroBase/AgroBase/bin/x64/Debug/Parametros/parametersAtu-1_1.par
View File

File diff suppressed because one or more lines are too long

2
AgroBase/AgroBase/bin/x64/Debug/Parametros/parametersMvd-2_0.par
View File

File diff suppressed because one or more lines are too long

2
AgroBase/AgroBase/bin/x64/Debug/Parametros/parametersSen-1_3.par
View File

File diff suppressed because one or more lines are too long

123
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_gal.py
View File

@ -1,9 +1,12 @@
import numpy as np
import time
import socket
import struct
from camera_worker.raw_segformer_service import make_bgr_preview_from_raw
from shared.enums import StatusModulo, T_Code, TipoFrameCamera
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
from shared.utils import resize_frame
from shared.enums import StatusModulo, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis
from camera_worker.gal_service import GalService
import subprocess
@ -16,9 +19,10 @@ class CameraGal:
self.mostrar_log = mostrar_log
self.gst_proc = None
self.gst_WIDTH = 640
self.gst_HEIGHT = 360
self.gst_BIT_RATE = 500
self.gst_FPS = 5
self.gst_WIDTH = 480
self.gst_HEIGHT = 270
self.gst_BIT_RATE = 300
self._ultimo_envio_gst = 0.0
self.dispositivo = T_Code.Vzo
@ -52,16 +56,26 @@ class CameraGal:
self.modelo = info.get("model")
self.versao = info.get("version_word")
_camera = ContextoGlobalRedis.get_camera(self.mx_id) or {}
_camera["mx_id"] = self.mx_id
_camera["versao"] = self.versao
_camera["iniciando"] = True
_camera["iniciado"] = False
_camera["rodando"] = False
_camera["parametros"] = {}
# Criar processo para transmissao de video
try:
_cfg = ContextoGlobalRedis.get_equipamento() or {}
_ip = _cfg.get("base_ip")
_porta = _cfg.get("base_porta_ervas")
if _ip is not None and _porta is not None:
self._sock = None
self._sock_conectado = False
self._sock_ultima_tentativa_conexao = 0.0
self._sock_intervalo_reconexao = 5.0 # segundos entre tentativas
if False and _ip is not None and _porta is not None:
self.mostrar_log(f"Iniciando GStreamer para {_ip}:{_porta}...")
gst_cmd = [
GST_LAUNCH,
"fdsrc", "fd=0",
#"!", "queue", "leaky=2", "max-size-buffers=1", # dropa frames antigos
"!", "videoparse",
f"width={self.gst_WIDTH}", f"height={self.gst_HEIGHT}",
"format=bgr",
@ -71,6 +85,7 @@ class CameraGal:
"!", f"video/x-raw,width={self.gst_WIDTH},height={self.gst_HEIGHT}",
"!", "x264enc", "tune=zerolatency", "speed-preset=ultrafast",
f"bitrate={self.gst_BIT_RATE}", "key-int-max=20",
#"byte-stream=true", "key-int-max=5", "bframes=0", "aud=true", f"bitrate={self.gst_BIT_RATE}",
"!", "rtph264pay", "config-interval=-1", "pt=96",
#"!", "h264parse", "!", "mpegtsmux",
"!", "udpsink",
@ -85,6 +100,11 @@ class CameraGal:
self.gst_proc = None
self.mostrar_log(f"Erro ao criar script de transmissao de video: {e}")
_camera["modelo"] = self.modelo
_camera["dispositivo"] = self.dispositivo.value
_camera["tem_depht"] = False
_camera["tem_imu"] = False
# Fase 2: criar pipeline e instanciar normalmente
try:
rgb_width = 2592
@ -110,15 +130,20 @@ class CameraGal:
"cm_por_px_y": cm_por_px_y,
}
_camera["parametros"] = self.parametros
self.ultima_saude["timestamp"] = time.time()
self.ultima_saude["status"] = StatusModulo.OPERANTE.value
self.iniciado = True
_camera["iniciado"] = True
_camera["iniciado_em"] = time.time()
except Exception as e:
self.mostrar_log(f"Erro ao iniciar camera: {e}")
pass
#ContextoGlobalRedis.set(ContextoGlobalRedis.CamKey(self.mx_id), _camera)
_camera["iniciando"] = False
ContextoGlobalRedis.set(ContextoGlobalRedis.CamKey(self.mx_id), _camera)
def requisitar_frame_raw(self, force: bool = False, max_age_s: float = None):
"""
@ -348,3 +373,81 @@ class CameraGal:
from camera_worker.manager import definir_saude_camera
definir_saude_camera(self.mx_id, status, saude, motivos, self.rodando, performance, conectado, agora, self.dispositivo)
def _sock_tentar_conectar(self):
_cfg = ContextoGlobalRedis.get_equipamento() or {}
_ip = _cfg.get("base_ip")
_porta = _cfg.get("base_porta_ervas")
# Sem IP/porta configurado, nem tenta
if not _ip or not _porta:
return
agora = time.time()
if (agora - self._sock_ultima_tentativa_conexao) < self._sock_intervalo_reconexao:
# ainda não deu o tempo mínimo, evita flood
return
self._sock_ultima_tentativa_conexao = agora
# Se já tem socket antigo, fecha
if self._sock is not None:
try:
self._sock.close()
except:
pass
self._sock = None
try:
#self.mostrar_log(f"Tentando conectar em {_ip}:{_porta}...")
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
s.settimeout(1.0) # timeout curto pra não travar
s.connect((_ip, _porta))
s.settimeout(None) # depois de conectado, volta p/ blocking normal
self._sock = s
self._sock_conectado = True
self.mostrar_log("Socket de vídeo conectado.")
except Exception as e:
self._sock_conectado = False
self._sock = None
# log mais leve pra não spammar
#self.mostrar_log(f"Falha ao conectar socket de vídeo: {e}")
def enviar_frame_tcp(self, frame_bgr):
_stream_on = (ContextoGlobalRedis.get_camera(self.mx_id) or {}).get("streaming", False)
if not _stream_on:
return # se operador desligar o streaming, sai secão
# Se não está conectado, tenta conectar e sai se ainda assim não conseguir
if not self._sock_conectado or self._sock is None:
self._sock_tentar_conectar()
if not self._sock_conectado or self._sock is None:
return # sem conexão, simplesmente não envia o frame
# 🔹 OPCIONAL: padronizar resolução antes de encodar
frame_bgr = resize_frame(frame_bgr, max_width=640, max_height=360)
# Aqui já consideramos que temos conexão, aí sim vale a pena encodar o frame
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 50] # qualidade ajustável
ok, buf = cv2.imencode(".jpg", frame_bgr, encode_param)
if not ok:
return
data = buf.tobytes()
size = len(data)
header = struct.pack("!I", size) # 4 bytes big-endian
try:
self._sock.sendall(header + data)
except (BrokenPipeError, ConnectionResetError, OSError) as e:
self.mostrar_log(f"Conexão de vídeo perdida: {e}")
# marca como desconectado, próxima chamada vai tentar reconectar
self._sock_conectado = False
try:
self._sock.close()
except:
pass
self._sock = None

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

@ -1,10 +1,13 @@
import numpy as np
import depthai as dai
import time
import socket
import struct
from health_worker.modulos.imu import IMUCamera
from shared.enums import StatusModulo, T_Code, TipoFrameCamera
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
from shared.utils import resize_frame
from shared.enums import StatusModulo, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis
import subprocess
import cv2
@ -17,9 +20,10 @@ class CameraOak:
self.modelo_ia_det = modelo_ia_det
self.gst_proc = None
self.gst_WIDTH = 640
self.gst_HEIGHT = 360
self.gst_BIT_RATE = 500
self.gst_FPS = 5
self.gst_WIDTH = 480
self.gst_HEIGHT = 270
self.gst_BIT_RATE = 300
self._ultimo_envio_gst = 0.0
self.dispositivo = T_Code.Vzo
@ -84,10 +88,11 @@ class CameraOak:
# Criar processo para transmissao de video
try:
_cfg = ContextoGlobalRedis.get_equipamento() or {}
_ip = _cfg.get("base_ip")
_porta = _cfg.get("base_porta_caminho") if self.dispositivo == T_Code.Snr else _cfg.get("base_porta_ervas")
if _ip is not None and _porta is not None:
self._sock = None
self._sock_conectado = False
self._sock_ultima_tentativa_conexao = 0.0
self._sock_intervalo_reconexao = 5.0 # segundos entre tentativas
if False and _ip is not None and _porta is not None:
self.mostrar_log(f"Iniciando GStreamer para {_ip}:{_porta}...")
gst_cmd = [
GST_LAUNCH,
@ -101,6 +106,7 @@ class CameraOak:
"!", f"video/x-raw,width={self.gst_WIDTH},height={self.gst_HEIGHT}",
"!", "x264enc", "tune=zerolatency", "speed-preset=ultrafast",
f"bitrate={self.gst_BIT_RATE}", "key-int-max=20",
#"byte-stream=true", "key-int-max=5", "bframes=0", "aud=true", f"bitrate={self.gst_BIT_RATE}",
"!", "rtph264pay", "config-interval=-1", "pt=96",
#"!", "h264parse", "!", "mpegtsmux",
"!", "udpsink",
@ -180,13 +186,11 @@ class CameraOak:
self.iniciado = True
_camera["iniciado"] = True
_camera["iniciado_em"] = time.time()
except Exception as e:
self.mostrar_log(f"Erro ao iniciar camera: {e}")
pass
_camera["iniciando"] = False
ContextoGlobalRedis.set(ContextoGlobalRedis.CamKey(self.mx_id), _camera)
def _criar_pipeline(self):
@ -697,4 +701,82 @@ class CameraOak:
definir_saude_camera(self.mx_id, status, saude, motivos, self.rodando, performance, conectado, agora, self.dispositivo)
def _sock_tentar_conectar(self):
_cfg = ContextoGlobalRedis.get_equipamento() or {}
_ip = _cfg.get("base_ip")
_porta = _cfg.get("base_porta_caminho")
# Sem IP/porta configurado, nem tenta
if not _ip or not _porta:
return
agora = time.time()
if (agora - self._sock_ultima_tentativa_conexao) < self._sock_intervalo_reconexao:
# ainda não deu o tempo mínimo, evita flood
return
self._sock_ultima_tentativa_conexao = agora
# Se já tem socket antigo, fecha
if self._sock is not None:
try:
self._sock.close()
except:
pass
self._sock = None
try:
#self.mostrar_log(f"Tentando conectar em {_ip}:{_porta}...")
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
s.settimeout(1.0) # timeout curto pra não travar
s.connect((_ip, _porta))
s.settimeout(None) # depois de conectado, volta p/ blocking normal
self._sock = s
self._sock_conectado = True
self.mostrar_log("Socket de vídeo conectado.")
except Exception as e:
self._sock_conectado = False
self._sock = None
# log mais leve pra não spammar
#self.mostrar_log(f"Falha ao conectar socket de vídeo: {e}")
def enviar_frame_tcp(self, frame_bgr):
_stream_on = (ContextoGlobalRedis.get_camera(self.mx_id) or {}).get("streaming", False)
if not _stream_on:
return # se operador desligar o streaming, sai secão
# Se não está conectado, tenta conectar e sai se ainda assim não conseguir
if not self._sock_conectado or self._sock is None:
self._sock_tentar_conectar()
if not self._sock_conectado or self._sock is None:
return # sem conexão, simplesmente não envia o frame
# 🔹 OPCIONAL: padronizar resolução antes de encodar
frame_bgr = resize_frame(frame_bgr, max_width=640, max_height=360)
# Aqui já consideramos que temos conexão, aí sim vale a pena encodar o frame
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 50] # qualidade ajustável
ok, buf = cv2.imencode(".jpg", frame_bgr, encode_param)
if not ok:
return
data = buf.tobytes()
size = len(data)
header = struct.pack("!I", size) # 4 bytes big-endian
try:
self._sock.sendall(header + data)
except (BrokenPipeError, ConnectionResetError, OSError) as e:
self.mostrar_log(f"Conexão de vídeo perdida: {e}")
# marca como desconectado, próxima chamada vai tentar reconectar
self._sock_conectado = False
try:
self._sock.close()
except:
pass
self._sock = None

352
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/bateria.py Normal file
View File

@ -0,0 +1,352 @@
import time
from shared.contexto_global_redis import ContextoGlobalRedis
from shared.enums import StatusModulo, T_Code
from health_worker.modulos.base import ModuloDiagnosticoBase
class ModuloBateria(ModuloDiagnosticoBase):
def __init__(self):
self.t_code = T_Code.Bat
self.nome = "Bateria / BMS Daly"
self.timeout = 10 # segundos para considerar desatualizado
def _status_por_score(self, score: float) -> StatusModulo:
if score >= 80:
return StatusModulo.OPERANTE
elif score >= 50:
return StatusModulo.ALERTA
else:
return StatusModulo.FALHA
def atualizar_saude(self):
try:
SAUDE_MIN_ALERTA = 80
motivos = []
condicoes = []
saude_individual = []
now = time.perf_counter()
saude = 0
dados = ContextoGlobalRedis.get_modulo(self.t_code) or {}
conectado = dados.get("iniciado")
if not conectado:
saude = 0
motivos.append("Desconectado")
else:
momento = float(dados.get("timestamp") or 0.0)
dt = now - momento if momento > 0 else 9999.0
# ------------ LEITURA DOS DADOS DA BATERIA ------------
v_total = dados.get("tensao_total_v")
corrente = dados.get("corrente_a")
soc = dados.get("soc_pct")
v_min = dados.get("tensao_min_celula_v")
v_max = dados.get("tensao_max_celula_v")
idx_vmin = dados.get("idx_celula_min")
idx_vmax = dados.get("idx_celula_max")
t_min = dados.get("temp_min_c")
t_max = dados.get("temp_max_c")
cap_rest = dados.get("cap_restante_ah")
num_cells = dados.get("num_celulas_serie")
alarms = dados.get("alarmes") or {}
bms_life = dados.get("bms_life_raw")
delta_cells = None
if v_min is not None and v_max is not None:
delta_cells = v_max - v_min
# ------------ SCORES PARCIAIS ------------
# 1) Score de comunicação / frescor
if not dados:
score_com = 0.0
motivos.append("Nenhum dado de telemetria da bateria disponível.")
else:
if dt > self.timeout:
score_com = max(0.0, 100.0 - (dt - self.timeout) * 5.0) # cai 5 pts por segundo acima do timeout
motivos.append(f"Dados da bateria desatualizados ({dt:.1f} s sem atualização).")
else:
score_com = 100.0
# 2) Score de SOC
if soc is None:
score_soc = 60.0 # penaliza por não ter SOC
motivos.append("SOC da bateria indisponível / não informado pelo BMS.")
else:
if soc <= 3:
score_soc = 20.0
motivos.append(f"SOC crítico ({soc:.1f}%).")
elif soc <= 10:
score_soc = 40.0
motivos.append(f"SOC muito baixo ({soc:.1f}%).")
elif soc <= 20:
score_soc = 60.0
motivos.append(f"SOC baixo ({soc:.1f}%).")
else:
score_soc = 100.0
# 3) Score de desbalanceamento entre células
if delta_cells is None:
score_bal = 70.0 # não informado, mas não mata
motivos.append("Não foi possível calcular desbalanceamento entre células (Vmin/Vmax ausentes).")
else:
if delta_cells > 0.15:
score_bal = 30.0
motivos.append(
f"Desbalanceamento elevado entre células ({delta_cells:.3f} V) "
f"[Vmin={v_min:.3f} V (#{idx_vmin}), Vmax={v_max:.3f} V (#{idx_vmax})]."
)
elif delta_cells > 0.08:
score_bal = 60.0
motivos.append(
f"Desbalanceamento moderado entre células ({delta_cells:.3f} V)."
)
else:
score_bal = 100.0
# 4) Score de temperatura
if t_max is None:
score_temp = 70.0
motivos.append("Temperatura da bateria não informada.")
else:
if t_max >= 60:
score_temp = 20.0
motivos.append(f"Temperatura crítica da bateria ({t_max:.1f} °C).")
elif t_max >= 55:
score_temp = 40.0
motivos.append(f"Temperatura muito alta da bateria ({t_max:.1f} °C).")
elif t_max >= 45:
score_temp = 70.0
motivos.append(f"Temperatura elevada da bateria ({t_max:.1f} °C).")
else:
score_temp = 100.0
# 5) Score baseado em alarmes do BMS
score_alarm = 100.0
alarmes_ativos = [nome for nome, val in alarms.items() if val]
if alarmes_ativos:
for nome in alarmes_ativos:
if nome in ("soc_low_1", "soc_low_2"):
score_alarm -= 20.0
motivos.append(f"Alarme BMS: {nome} ativo (SOC baixo).")
elif nome in ("ovp", "uvp", "otp", "utp"):
score_alarm -= 40.0
motivos.append(f"Alarme BMS crítico: {nome} ativo.")
else:
score_alarm -= 15.0
motivos.append(f"Alarme BMS: {nome} ativo.")
score_alarm = max(0.0, score_alarm)
# ------------ SAÚDE GLOBAL ------------
# Peso dos sub-scores (total 1.0)
w_com = 0.20 # comunicação
w_soc = 0.25 # estado de carga
w_bal = 0.20 # balanceamento
w_temp = 0.20 # temperatura
w_alarm = 0.15 # alarmes
health = (
w_com * score_com +
w_soc * score_soc +
w_bal * score_bal +
w_temp * score_temp +
w_alarm * score_alarm
)
saude = round(health, 1)
# ------------ SAÚDE INDIVIDUAL / CONDIÇÕES ------------
# 1) SOC
cond_soc = []
if score_soc < SAUDE_MIN_ALERTA:
severidade = int(max(0, 100 - score_soc))
c = {
"label": "Estado de carga (SOC)",
"valor": soc if soc is not None else -1,
"severidade": severidade,
"descricao": "SOC abaixo do ideal para operação segura.",
"acoes": [
"Planejar recarga da bateria antes de continuar a operação.",
"Reduzir velocidade e carga dos atuadores para economizar energia.",
"Evitar iniciar serviços longos com SOC baixo."
]
}
condicoes.append(c)
cond_soc.append(c)
saude_individual.append({
"id": "bat_soc",
"label": "Estado de carga (SOC)",
"status": self._status_por_score(score_soc).value,
"saude": round(score_soc, 1),
"motivos": [m for m in motivos if "SOC " in m or "SOC" in m],
"condicoes_operacionais": cond_soc,
"em_uso": True,
})
# 2) Desbalanceamento entre células
cond_bal = []
if score_bal < SAUDE_MIN_ALERTA:
severidade = int(max(0, 100 - score_bal))
c = {
"label": "Desbalanceamento de células",
"valor": round(delta_cells, 4) if delta_cells is not None else -1,
"severidade": severidade,
"descricao": "Diferença de tensão significativa entre células da bateria.",
"acoes": [
"Permitir período de balanceamento com o carregador conectado.",
"Evitar descargas profundas até reduzir o desbalanceamento.",
"Verificar se alguma célula está degradada ou com problema."
]
}
condicoes.append(c)
cond_bal.append(c)
saude_individual.append({
"id": "bat_cells",
"label": "Balanceamento entre células",
"status": self._status_por_score(score_bal).value,
"saude": round(score_bal, 1),
"motivos": [m for m in motivos if "Desbalanceamento" in m],
"condicoes_operacionais": cond_bal,
"em_uso": True,
})
# 3) Temperatura da bateria
cond_temp = []
if score_temp < SAUDE_MIN_ALERTA:
severidade = int(max(0, 100 - score_temp))
c = {
"label": "Temperatura da bateria",
"valor": t_max if t_max is not None else -273,
"severidade": severidade,
"descricao": "Temperatura da bateria acima da faixa ideal.",
"acoes": [
"Reduzir corrente de descarga (velocidade / esforços dos atuadores).",
"Verificar ventilação / refrigeração do pack de baterias.",
"Interromper operação se a temperatura continuar subindo."
]
}
condicoes.append(c)
cond_temp.append(c)
saude_individual.append({
"id": "bat_temp",
"label": "Temperatura da bateria",
"status": self._status_por_score(score_temp).value,
"saude": round(score_temp, 1),
"motivos": [m for m in motivos if "Temperatura" in m],
"condicoes_operacionais": cond_temp,
"em_uso": True,
})
# 4) Alarmes do BMS
cond_alarm = []
if score_alarm < SAUDE_MIN_ALERTA or alarmes_ativos:
severidade = int(max(0, 100 - score_alarm))
c = {
"label": "Alarmes do BMS",
"valor": len(alarmes_ativos),
"severidade": severidade,
"descricao": "Um ou mais alarmes foram acionados pelo BMS.",
"acoes": [
"Verificar detalhes dos alarmes no painel do BMS.",
"Investigar causas de sobretensão, subtensão ou sobretemperatura.",
"Interromper operação em caso de alarmes críticos persistentes."
]
}
condicoes.append(c)
cond_alarm.append(c)
saude_individual.append({
"id": "bat_alarms",
"label": "Alarmes do BMS",
"status": self._status_por_score(score_alarm).value,
"saude": round(score_alarm, 1),
"motivos": [m for m in motivos if "Alarme BMS" in m],
"condicoes_operacionais": cond_alarm,
"em_uso": True,
})
# 5) Comunicação / frescor dos dados
cond_com = []
if score_com < SAUDE_MIN_ALERTA:
severidade = int(max(0, 100 - score_com))
c = {
"label": "Atualização de telemetria",
"valor": round(dt, 2),
"severidade": severidade,
"descricao": "Dados da bateria desatualizados.",
"acoes": [
"Verificar comunicação CAN entre ECU e BMS.",
"Verificar módulo responsável por requisitar dados da bateria.",
"Checar cabos/conectores e alimentação do BMS."
]
}
condicoes.append(c)
cond_com.append(c)
saude_individual.append({
"id": "bat_telemetry",
"label": "Telemetria da bateria",
"status": self._status_por_score(score_com).value,
"saude": round(score_com, 1),
"motivos": [m for m in motivos if "Dados da bateria" in m or "telemetria" in m],
"condicoes_operacionais": cond_com,
"em_uso": True,
})
# ------------ STATUS GERAL DO MÓDULO ------------
status = StatusModulo.OPERANTE
if not conectado:
status = StatusModulo.DESCONECTADO
elif saude <= 0:
status = StatusModulo.FALHA
elif saude < SAUDE_MIN_ALERTA:
status = StatusModulo.ALERTA
payload = {
"conectado": conectado,
"status": status.value,
"saude": saude,
"motivos": motivos,
"saude_individual": saude_individual,
"condicoes_operacionais": condicoes,
"detalhes": {
"tensao_total_v": locals().get("v_total", 0),
"corrente_a": locals().get("corrente", 0),
"soc_pct": locals().get("soc", 0),
"cap_restante_ah": locals().get("cap_rest", 0),
"num_celulas_serie": locals().get("num_cells", 0),
"tensao_min_celula_v": locals().get("v_min", 0),
"tensao_max_celula_v": locals().get("v_max", 0),
"delta_celulas_v": locals().get("delta_cells", 0),
"temp_min_c": locals().get("t_min", 0),
"temp_max_c": locals().get("t_max", 0),
"bms_life_raw": locals().get("bms_life", 0),
"tempo_desde_ultimo_update_s": locals().get("dt", 0),
"qtd_alarmes_ativos": len(locals().get("alarmes_ativos", [])),
"alarmes_ativos": locals().get("alarmes_ativos", []),
"score_com": round(locals().get("score_com", 0), 1),
"score_soc": round(locals().get("score_soc", 0), 1),
"score_balanceamento": round(locals().get("score_bal", 0), 1),
"score_temp": round(locals().get("score_temp", 0), 1),
"score_alarm": round(locals().get("score_alarm", 0), 1),
}
}
ContextoGlobalRedis.atualizar_ctx_dict(
ContextoGlobalRedis.ModKey(self.t_code),
saude=payload
)
except Exception as e:
print(f"Erro ao atualizar saude do modulo {self.t_code.name}: {e}")

16
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/gerenciador.py
View File

@ -25,15 +25,13 @@ class ManagerWorker:
def executar(self, envia_resposta):
t0 = time.time()
movimento_automatico = ContextoGlobalRedis.get_controle().get("movimento_automatico", False)
if movimento_automatico:
self.definir_processador()
if self.processador:
comando = self.processador.processar()
if comando is not None and envia_resposta:
ContextoGlobalRedis.publicar_comando(CmdKey.ManagerWorkerTx, { "cmd": ManagerWorkerCommandType.EnviarDadosControle.value, "params": comando })
else:
mostrar_log("Processador nao definido")
self.definir_processador()
if self.processador:
comando = self.processador.processar()
if comando is not None and envia_resposta:
ContextoGlobalRedis.publicar_comando(CmdKey.ManagerWorkerTx, { "cmd": ManagerWorkerCommandType.EnviarDadosControle.value, "params": comando })
else:
mostrar_log("Processador nao definido")
t1 = time.time()
latencia = t1 - t0

103
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py
View File

@ -1,6 +1,6 @@
import time
from shared.enums import ModoOperacao, StatusCarroMapa, StatusModulo, StatusOperacao, T_Code, TipoMovimentoDirecional, TiposControladorDirecional
from shared.enums import ModoOperacao, StatusCarroMapa, StatusModulo, T_Code, TipoMovimentoDirecional, TiposControladorDirecional
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
from manager_worker.config import mostrar_log
from manager_worker.modulos.mpc import get_mpc, comando_parado
@ -103,7 +103,7 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
},
"RegrasAtivas": {
"matriz_custo": False,
"deteccao_obstaculos": True,
"deteccao_obstaculos": False,
},
"Equipamento": {
"largura": _equipamento.get("largura", 0.85),
@ -116,19 +116,14 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
#print(contexto["VisualWorker"])
if _tipo_controle == TiposControladorDirecional.MPC:
comando = _regras_taticas(contexto)
if (comando.get("comando_definido", False)):
return _montar_comando_retorno(comando, latencia=latencia)
op_modo = ModoOperacao(_operacao.get("modo", ModoOperacao.NaoDefinido.value))
if op_modo == ModoOperacao.MapaGPS:
return _comando_mapa_gps_mpc(contexto)
elif op_modo == ModoOperacao.MapeamentoVisual:
return _comando_mapeamento_visual(contexto)
else:
op_modo = ModoOperacao(_operacao.get("modo", ModoOperacao.NaoDefinido.value))
if op_modo == ModoOperacao.MapaGPS:
return _comando_mapa_gps_mpc(contexto)
elif op_modo == ModoOperacao.MapeamentoVisual:
return _comando_mapeamento_visual(contexto)
else:
comando = _comando_direcional_parado(True)
return _montar_comando_retorno(comando, latencia=latencia)
comando = _comando_direcional_parado()
return _montar_comando_retorno(comando, latencia=latencia)
elif _tipo_controle == TiposControladorDirecional.PID:
angulo_max = _controle.get("angulo_max", 30)
@ -157,7 +152,7 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
_cmd = {
"comando_definido": True,
"enviar_comando": True,
"parada_necessaria": True,
"parada_necessaria": False,
"erro": False,
"latencia": 0.0,
"angulo": angulo,
@ -169,83 +164,25 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
except Exception as e:
mostrar_log(f"❌ Erro ao definir comando direcional: {e}")
def _comando_direcional_parado(definido: bool, frear: bool = False):
def _comando_direcional_parado():
try:
angulo, tipo = comando_parado()
_cmd = {
"comando_definido": definido,
"enviar_comando": True,
"parada_necessaria": True,
"parada_necessaria": False,
"erro": False,
"latencia": 0.0,
"angulo": angulo,
"tipo": tipo.value,
"simulacao": [],
"frear": frear
"erro_lateral": 0,
"erro_orientacao": 0,
"debug_custo": {},
"candidatos_testados": 0
}
return _cmd
except Exception as e:
mostrar_log(f"❌ Erro ao montar comando direcional parado: {e}")
def _regras_taticas(contexto):
try:
if contexto.get("Operacao", {}).get("Status", StatusOperacao.NaoIniciado.value) != StatusOperacao.EmAndamento.value or contexto.get("Operacao", {}).get("Finalizando", False):
mostrar_log("🟥 Direcional parado: operação não está em andamento ou está finalizando.")
return _comando_direcional_parado(True)
velocidade_media = contexto.get("Carro", {}).get("Velocidade", 0.0)
if velocidade_media < 0.01:
mostrar_log("⚠️ Possível travamento detectado (velocidade quase zero).")
# Pode evoluir para uma lógica de recuperação no futuro
dados_vw = contexto.get("VisualWorker", {})
vw_ativado = dados_vw.get("Ativado", False)
vw_operante = dados_vw.get("Operante", False)
if vw_ativado and not vw_operante:
#mostrar_log("⚠️ Sensores principais inativos. Rodando controle no modo básico.")
return _comando_direcional_parado(False)
imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
if imu is not None and imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) == StatusModulo.OPERANTE.value:
ang_roll_max = contexto.get("Equipamento", {}).get("angulo_roll_max", 15.0)
ang_pitch_max = contexto.get("Equipamento", {}).get("angulo_pitch_max", 30.0)
if abs(imu.get("pitch", 0.0)) > ang_pitch_max or abs(imu.get("roll", 0.0)) > ang_roll_max:
mostrar_log(f"🟥 Inclinação perigosa detectada. Parando movimentação. roll: {imu.get('roll')}, pitch: {imu.get('pitch')}, yaw: {imu.get('yaw')}")
return _comando_direcional_parado(True)
if not vw_ativado:
mostrar_log("⚠️ Sensores principais desativados. Rodando controle no modo básico.")
return _comando_direcional_parado(False)
# >>> NOVO: gate de bloqueio/parede com block + d_obs_min <<<
if vw_operante and contexto.get("RegrasAtivas", {}).get("deteccao_obstaculos", False):
block = dados_vw.get("MatrizCusto", {}).get("Block", None)
if block is not None:
reason = block.get("reason", "none")
decision = block.get("decision", {})
#print(block)
if decision.get("parar", False):
mostrar_log(f"🟥 Parada necessaria por {reason}.")
return _comando_direcional_parado(True, frear=True)
# motivo narrow: não para, mas dá dica lateral
if isinstance(block, dict) and reason == "narrow":
sb = (block.get("side_bias") or {}).get("value", 0.0)
hints = contexto.setdefault("DirecionalHints", {})
if sb > 0.25: hints["vetar_direita"] = True
if sb < -0.25: hints["vetar_esquerda"] = True
# se chegou até aqui, pode seguir
return _comando_direcional_parado(False)
except Exception as e:
mostrar_log(f"❌ Erro ao processar regras taticas do direcional: {e}")
return _comando_direcional_parado(True)
def _comando_mapa_gps_mpc(contexto):
try:
mpc = get_mpc()
@ -278,7 +215,7 @@ def _comando_mapa_gps_mpc(contexto):
def _comando_mapeamento_visual(contexto):
try:
# ⛔ Fallback seguro se visual não operante
comando = _comando_direcional_parado(True)
comando = _comando_direcional_parado()
if not contexto.get("VisualWorker", {}).get("Operante", False):
mostrar_log("Sensores principais inativos! Impossível definir controle")
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"], False
@ -317,7 +254,11 @@ def _comando_mapeamento_visual(contexto):
"latencia": latencia,
"angulo": round(angulo, 2),
"tipo": tipo_movimento.value,
"simulacao": []
"simulacao": [],
"erro_lateral": 0,
"erro_orientacao": 0,
"debug_custo": {},
"candidatos_testados": 0
}
return _montar_comando_retorno(comando)

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

@ -1,41 +1,12 @@
import time
from shared.enums import ModoOperacao, StatusModulo, StatusOperacao, StatusCarroMapa, T_Code
from shared.enums import ModoOperacao, StatusModulo, StatusCarroMapa, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
from manager_worker.config import mostrar_log
from manager_worker.filtros import FiltroVelocidade
def definir_comando(parada_necessaria: bool, erro: bool, filtro_vel: FiltroVelocidade, erro_orientacao: float = None):
def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None):
try:
_operacao = ContextoGlobalRedis.get_operacao()
_controle = ContextoGlobalRedis.get_controle()
status_operacao = StatusOperacao(_operacao.get("status", StatusOperacao.NaoIniciado.value))
finalizando = _operacao.get("finalizando", False)
if erro:
mostrar_log("🟥 Movimento parado: erro ao definir comando direcional.")
return _montar_comando_retorno(0.0, False)
if parada_necessaria:
mostrar_log("🟥 Movimento parado: direcional sem possibilidade de desvio.")
frenagem_automatica = ContextoGlobalRedis.get_controle().get("frenagem_automatica", False)
frear = False
deve_frear = frenagem_automatica and frear
return _montar_comando_retorno(0.0, deve_frear)
if (status_operacao != StatusOperacao.EmAndamento) or finalizando:
mostrar_log(f"🟥 Movimento parado: operação não está em andamento ou finalizando: {status_operacao.name}")
return _montar_comando_retorno(0.0, False)
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
_snr = ContextoGlobalRedis.get_modulo(T_Code.Snr)
sonar_ativado = _controle.get("sonar_ativado", False)
visual_worker_operante = (_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) == StatusModulo.OPERANTE.value
analise_deteccao_atualizada = (time.perf_counter() - _dados_vw.get("matriz_confianca", {}).get("ts", 10.0) <= 1.0)
obstaculo_detectado = _dados_vw.get("matriz_confianca", {}).get("block", {}).get("decision", {}).get("parar", False) if analise_deteccao_atualizada else False
if (sonar_ativado and visual_worker_operante and obstaculo_detectado):
mostrar_log(f"🟥 Movimento parado: obstaculo proximo detectado no sonar")
return _montar_comando_retorno(0.0, True)
op_modo = ModoOperacao(_operacao.get("modo", ModoOperacao.NaoDefinido.value))
_contexto = ContextoGlobalRedis.get_contexto()
@ -70,6 +41,7 @@ def definir_comando(parada_necessaria: bool, erro: bool, filtro_vel: FiltroVeloc
if erro_orientacao is None: erro_orientacao = _trajetoria.get("erro_angular", 0)
pontos_fim_corredor = _trajetoria.get("CorredorAtual", {}).get("pontos_restantes", -1)
elif op_modo == ModoOperacao.MapeamentoVisual:
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
status_carro = StatusCarroMapa(_dados_vw.get("segmentacao", {}).get("status_corredor", StatusCarroMapa.Parado.value))
if erro_orientacao is None: erro_orientacao = _dados_vw.get("segmentacao", {}).get("erro_angular", 0)
@ -101,7 +73,7 @@ def definir_comando(parada_necessaria: bool, erro: bool, filtro_vel: FiltroVeloc
#mostrar_log(f"🚗 Velocidade definida: {velocidade_sp:.1f}%")
return _montar_comando_retorno(velocidade_sp, False)
return _montar_comando_retorno(velocidade_sp)
except Exception as e:
mostrar_log(f"Erro ao definir o comando de movimentacao: {e}")
@ -128,8 +100,7 @@ def calcular_velocidade_relativa(vel_min, vel_max, ang_max, erro_orientacao, k=1
vel = vel_min + (1.0 - k*s) * faixa
return round(vel, 2)
def _montar_comando_retorno(velocidade_sp: float, frear: bool):
def _montar_comando_retorno(velocidade_sp: float):
return {
"velocidade": velocidade_sp,
"frear": frear
"velocidade": velocidade_sp
}

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

@ -41,6 +41,12 @@ class ControladorMPC:
self.velocidade_min = parametros_mpc.get("velocidade_min", 0.4)
self.velocidade_max = parametros_mpc.get("velocidade_max", 1.9)
self.horizonte = parametros_mpc.get("horizonte", 2.5)
self.horizonte_min = parametros_mpc.get("horizonte_min", 0.6)
self.horizonte_max = parametros_mpc.get("horizonte_max", 2.8)
self.horizonte_parado = parametros_mpc.get("horizonte_parado", 0.5)
self._beam_traj_min = parametros_mpc.get("beam_traj_min", 3) # nº mínimo de trajetórias completas que queremos
self._beam_topN_max = parametros_mpc.get("beam_topN_max", 5) # limite superior de candidatos ativos
self._beam_topN_min = parametros_mpc.get("beam_topN_min", 2) # limite inferior
# otimizacoes
self._np_bool = np.bool_
@ -595,15 +601,16 @@ class ControladorMPC:
# tente usar self.vel_max_m_s se já existir; senão, usa 6.8 km/h (~1.889 m/s)
vel_max_ms = getattr(self, "vel_max_m_s", 6.8/3.6)
v = float(velocidade)
if v <= 1.05: # heurística: trata <=1.0 como v_norm
v = max(0.0, min(1.0, v)) * vel_max_ms
else:
v = max(0.0, v) # já está em m/s
#if v <= 1.05: # heurística: trata <=1.0 como v_norm
# v = max(0.0, min(1.0, v)) * vel_max_ms
#else:
# v = max(0.0, v) # já está em m/s
v = max(0.0, v) # já está em m/s
# --- 2) Base polinomial (suave e previsível) ------------------------------
# L_base = L0 + a*v + b*v^2 (clamp em [Lmin, Lmax])
Lmin, Lmax = 0.60, 2.80 # bom para teu range (1.3–6.8 km/h)
L0, a, b = 0.60, 0.70, 0.20
Lmin, Lmax = self.horizonte_min, self.horizonte_max # bom para teu range (1.3–6.8 km/h)
L0, a, b = self.horizonte_parado, 0.70, 0.20
L = L0 + a*v + b*(v*v)
if L < Lmin: L = Lmin
if L > Lmax: L = Lmax
@ -736,300 +743,12 @@ class ControladorMPC:
t_exec = max((now() - self.ultima_atualizacao), 1e-3)
if comando['erro']:
mostrar_log(f"🧭 Direcional MPC | Latencia: {comando['latencia']} | Parada: {comando['parada_necessaria']} | Movimento: {TipoMovimentoDirecional(comando['tipo']).name} | Ângulo: {comando['angulo']}° | Horizonte: {len(comando['simulacao'])} | dt: {dt_raw:.2f} s | freq = {(1.0 / dt_raw):.2f} Hz | t_exec: {t_exec:.2} s | f_exec: {(1.0 / t_exec):.2f} Hz")
mostrar_log(f"🧭 Direcional MPC | Latencia: {comando['latencia']} | Parada: {comando['parada_necessaria']} | Movimento: {TipoMovimentoDirecional(comando['tipo']).name} | Ângulo: {comando['angulo']}° | CT: {comando['candidatos_testados']} ({len(comando['debug_custo'])}) | Horizonte: {len(comando['simulacao'])} | dt: {dt_raw:.2f} s | freq = {(1.0 / dt_raw):.2f} Hz | t_exec: {t_exec:.2} s | f_exec: {(1.0 / t_exec):.2f} Hz")
return comando
except Exception as e:
mostrar_log(f"❌ Erro ao processar compute MPC: {e}")
def _processar_mpc_receding_old(self, contexto, comando_anterior):
now = time.perf_counter
t0 = now()
SLA_MS = getattr(self, "_mpc_sla_ms", 200.0) # 200 ms ⇒ 5 Hz
HEAD_MS = getattr(self, "_mpc_headroom_ms", 25.0)
deadline = t0 + (SLA_MS - HEAD_MS) / 1000.0
def ms_left():
return (deadline - now()) * 1000.0
try:
if not self.pontos_info:
return None
GPS = contexto.get("GPS", {})
pos_lat = GPS.get("Latitude", 0.0)
pos_lon = GPS.get("Longitude", 0.0)
pos_theta = GPS.get("AnguloCarro", 0.0)
pos_passos_atraso = GPS.get("PassosAtraso", 0)
pos_timestamp = GPS.get("Timestamp", 0.0)
pos_latencia = now() - pos_timestamp
LAT_MAX = 2.0
if pos_latencia >= LAT_MAX:
mostrar_log(f"[GPS] Grande latencia entre coordenadas detectado ({pos_latencia:.3f}/{LAT_MAX}): parando por fallback")
return self._comando_fallback_hot_stop(comando_anterior, latencia=pos_latencia)
if (pos_passos_atraso > 3):
mostrar_log(f"[GPS] Atraso detectado de {(pos_passos_atraso / 3.0):.1f} passos, {pos_latencia:.3f} s")
x, y = self.gps_handler.converter_latlon_para_xz(pos_lat, pos_lon)
theta = np.radians(pos_theta)
x0, y0, t0 = x, y, theta
velocidade = contexto.get("Carro", {}).get("Velocidade", 0)
status_carro = contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value)
idx_proximo_ponto_real = contexto.get("Carro", {}).get("IdxProximoPonto", 0)
idx_ponto_alvo = contexto.get("Carro", {}).get("IdxPontoAlvo", 0)
Nmax = 20 if status_carro == StatusCarroMapa.CaminhandoRua.value else 60
# Comando de hold (se não tiver buffer de comandos aplicados)
u_hold = {
"tipo": comando_anterior.get("tipo"),
"angulo": np.radians(comando_anterior.get("angulo", 0.0)),
"v": velocidade,
"omega": self.gps_handler.calcular_omega(velocidade, np.radians(comando_anterior.get("angulo", 0.0)), comando_anterior.get("tipo"))
}
# Opcional: sequência real de comandos aplicados durante a latência (se você tiver)
cmd_seq = None # ou algo como [(0.12, u1), (0.08, u2), ...]
x, y, theta, pts_visitados = self.corrigir_pose_por_latencia(
x, y, theta, self.visitados_execucao,
latency_s=min(pos_latencia, LAT_MAX),
dt=self.dt,
nova_posicao_fn=self._nova_posicao,
u_hold=u_hold,
cmd_seq=cmd_seq,
time_left_ms=lambda: (deadline - now()) * 1000.0,
calc_omega_fn=self.gps_handler.calcular_omega
)
idx_alvo_correcao = self._corrigir_pontos_visitados(x, y, pts_visitados, idx_proximo_ponto_real) # marca pontos antigos como visitados
#print(f"Anterior: x: {x0}, y: {y0}, t: {t0} - Novo: x: {x}, y: {y}, t: {theta}")
#print(GPS)
distancia_m = velocidade * self.dt
ultimo_ponto = idx_proximo_ponto_real >= len(self.pontos_info)
self.tempo_execucao_local = contexto.get("Carro", {}).get("TempoEntreComandos", 0.5)
if ultimo_ponto:
self.passos_horizonte_local = 1
self.qtd_comandos_sucessivos = 3
elif distancia_m > 0:
passos_total = min(int(self.horizonte / distancia_m), Nmax)
self.passos_horizonte_local = max(1, math.ceil(self.tempo_execucao_local / self.dt))
self.qtd_comandos_sucessivos = max(1, passos_total // self.passos_horizonte_local)
else:
self.passos_horizonte_local = 0
self.qtd_comandos_sucessivos = 0
angulo_anterior = np.radians(comando_anterior.get("angulo", 0))
tipo_anterior = TipoMovimentoDirecional(comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value))
simulacao_latlon = []
#mostrar_log(f"previsao_futura em {(t_1 - t_0):.4f}s, correcao_pontos_visitados em {(t_2 - t_1):.4f}")
if ultimo_ponto:
angulo_final, tipo_final = comando_parado()
parada_necessaria = True
else:
candidatos_ativos = [{
"x": x, "y": y, "theta": theta,
"custo": 0.0,
"comandos": [(tipo_anterior, angulo_anterior)],
"trajetoria": [],
"visitados": pts_visitados,
"inicial": True
}]
# quantas expansões cabem no tempo restante deste ciclo
est = max(1.0, getattr(self, "_t_est_passo_ms", 6.0))
exp_budget = max(4, int(ms_left() // est))
#print(f"Comecando, idx_proximo_ponto_real: {idx_proximo_ponto_real}, idx_alvo_correcao: {idx_alvo_correcao}")
for passo in range(self.qtd_comandos_sucessivos):
if ms_left() <= 0:
break
novos_candidatos = []
for candidato in candidatos_ativos:
# encerra passo se orçamento acabou
if exp_budget <= 0 or ms_left() <= 0:
break
x_atual, y_atual, theta_atual = candidato["x"], candidato["y"], candidato["theta"]
visitados = candidato["visitados"].copy()
cmd_anterior = {
"tipo": candidato["comandos"][-1][0],
"angulo": candidato["comandos"][-1][1],
}
idx_alvo = self._corrigir_pontos_visitados(x_atual, y_atual, visitados)
#if passo == 0 and idx_alvo < idx_ponto_alvo:
# print(f"ponto alvo alterado de {idx_alvo} para {idx_ponto_alvo}")
# idx_alvo = idx_ponto_alvo
if idx_alvo < len(self.pontos_info) - 1:
idx_alvo = idx_alvo + 1
ponto_alvo = self.pontos_info[idx_alvo]
tipos, angs, custos_candidatos = self._gerar_angulos_candidatos_receding(ponto_alvo, (x_atual, y_atual, theta_atual), (x, y, theta), contexto, deadline, 15.0)
if ms_left() <= 0 or exp_budget <= 0:
break
# ---------- INÍCIO BLOCO SEM THREADS ----------
ang_array = np.asarray(angs, dtype=np.float32)
graus_r = np.round(np.degrees(ang_array), 2)
pairs_tipo, pairs_ang, pairs_heur = [], [], []
for tipo in tipos:
tv = tipo.value
# heurísticos para TODOS os ângulos deste tipo
h_tmp = np.array(
[float(custos_candidatos.get((tv, float(g)), 0.0)) for g in graus_r],
dtype=np.float32
)
pairs_tipo.append([tipo] * ang_array.size)
pairs_ang.append(ang_array) # sem copy
pairs_heur.append(h_tmp)
if not pairs_tipo:
continue
from itertools import chain
tipos_flat = list(chain.from_iterable(pairs_tipo))
angs_flat = np.concatenate(pairs_ang, axis=0)
heur_flat = np.concatenate(pairs_heur, axis=0)
if heur_flat.size == 0:
continue
ord_idx = np.argsort(heur_flat)
tempo_util_ms = max(0.0, ms_left() - 10.0)
est_local = max(1.0, min(getattr(self, "_t_est_passo_ms", 6.0), 20.0))
Kmax = 12
Kdyn = int(min(Kmax, max(3, tempo_util_ms // est_local)))
Kdyn = min(Kdyn, ord_idx.size)
melhor_custo_local = float("inf")
for k in range(Kdyn):
if ms_left() <= 0 or exp_budget <= 0:
break
i = int(ord_idx[k])
tipo_k = tipos_flat[i]
ang_k = float(angs_flat[i])
heur_k = float(heur_flat[i])
if (candidato["custo"] + heur_k) > (melhor_custo_local * 1.20) and ms_left() < 2*est_local:
continue
t_c_ini = now()
try:
custo, sim, valido, visitados_sim = self._simular_passo(
x_atual, y_atual, theta_atual,
tipo_k, ang_k,
custos_candidatos,
cmd_anterior, contexto,
visitados # <- sem copy aqui
)
if valido:
x_f, y_f, theta_f = sim[-1]
novo = {
"x": x_f, "y": y_f, "theta": theta_f,
"custo": candidato["custo"] + float(custo),
"comandos": candidato["comandos"] + [(tipo_k, ang_k)],
"trajetoria": candidato["trajetoria"] + sim,
"visitados": visitados_sim,
"inicial": False
}
novos_candidatos.append(novo)
if novo["custo"] < melhor_custo_local:
melhor_custo_local = novo["custo"]
except Exception as e:
mostrar_log(f"Erro ao simular passo (sem threads) {tipo_k.name} | {np.degrees(ang_k):.2f}: {e}")
finally:
dt_ms = (now() - t_c_ini) * 1000.0
alpha = getattr(self, "_ema_alpha", 0.2)
self._t_est_passo_ms = (1.0 - alpha)*getattr(self, "_t_est_passo_ms", 6.0) + alpha*dt_ms
exp_budget -= 1
# ---------- FIM BLOCO SEM THREADS ----------
if exp_budget <= 0:
pass # apenas segue para seleção dos melhores
if exp_budget <= 0 or ms_left() <= 0:
break # sai do laço de passos
# Filtro opcional: limitar para os N melhores candidatos
candidatos_ativos = self._filtrar_por_margem_angular(novos_candidatos, margem_graus=2.0)
N_top = 5 if exp_budget > 6 else 3
candidatos_ativos = self._selecionar_melhores(candidatos_ativos, N=N_top)
candidatos_validos = [c for c in candidatos_ativos if not c.get("inicial", False)]
if len(candidatos_validos) > 0:
try:
melhor = min(candidatos_validos, key=lambda c: c["custo"])
except Exception as e:
mostrar_log(f"Erro ao selecionar melhor candidato: {e}")
melhor = None
else:
melhor = None
parada_necessaria = melhor is None
if not parada_necessaria:
_comandos = melhor.get("comandos", [])
if len(_comandos) == 0:
pass
else:
idx_cmd = 0 if len(_comandos) == 1 else 1
tipo_final, angulo_final = _comandos[idx_cmd]
simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(melhor["trajetoria"]) or [])
simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta))
else:
angulo_final = 0
tipo_final = TipoMovimentoDirecional.RodasDianteiras
simulacao_latlon = []
#print(f"Simulacao: {len(simulacao_latlon)}")
_cmd = {
"enviar_comando": True,
"parada_necessaria": parada_necessaria,
"erro": False,
"latencia": pos_latencia,
"angulo": float(round(np.degrees(angulo_final), 2)),
"tipo": tipo_final.value,
"simulacao": simulacao_latlon
}
self._tick_id = getattr(self, "_tick_id", 0) + 1
if (getattr(self, "debug_cost_vis", False)
and contexto.get("RegrasAtivas", {}).get("matriz_custo", False)
and (self._tick_id % 10 == 0)):
dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {})
# reconstroi set de candidatos só pra plot, com orçamento curtinho
deadline_dbg = time.perf_counter() + 0.03 # ~30 ms pra não pesar
tipos_dbg, angs_dbg, _ = self._gerar_angulos_candidatos_receding(
ponto_alvo=self.pontos_info[idx_alvo_correcao],
ponto_atual=(x, y, theta),
ponto_ref=(x, y, theta),
contexto=contexto,
deadline=deadline_dbg,
margem_ms=8.0,
)
angs_dbg_deg = [round(float(np.degrees(a)), 2) for a in angs_dbg]
self.debug_plot_matriz_custo(
dados_matriz_custo,
tipos_dbg,
angs_dbg_deg,
max_linhas=3,
titulo=f"debug_cost_{int(self._tick_id):06d}.png",
highlight=(tipo_final.value, round(float(np.degrees(angulo_final)), 2)),
)
return _cmd
except Exception as e:
mostrar_log(f"❌ Erro ao processar MPC: {e}")
return self._comando_fallback_hot_stop(comando_anterior, latencia=pos_latencia)
def _processar_mpc_receding(self, contexto, comando_anterior):
now = time.perf_counter
t0 = now()
@ -1127,6 +846,18 @@ class ControladorMPC:
# >>> escolha K dentro do [K_lb, K_cap]; pacing do orçamento vem depois
K = max(K_lb, min(K_cap, K_ALVO))
beam_traj_min = int(getattr(self, "_beam_traj_min", 3))
# custo bruto para levar beam_traj_min trajetórias até K passos
exp_min_full = beam_traj_min * K
if exp_budget < exp_min_full:
# aqui temos três opções, em ordem:
# 1) reduzir beam_traj_min, mas garantir pelo menos 1 traj completa
beam_traj_min = max(1, exp_budget // max(1, K))
# 2) se mesmo assim ficar ridículo, melhor nem recalcular nada e reaproveitar o comando anterior
if beam_traj_min <= 0:
# fallback suave: continua com comando_anterior
return comando_anterior
# dt para que K passos cubram S (agora garantido dentro de [DT_MIN, DT_MAX])
dt_pred = (S_ALVO / (v_sim * K))
dt_pred = max(DT_MIN, min(dt_pred, DT_MAX))
@ -1222,6 +953,18 @@ class ControladorMPC:
Kdyn = int(min(Kmax, max(3, tempo_util_ms // est_local)))
Kdyn = min(Kdyn, ord_idx.size)
passos_restantes = self.qtd_comandos_sucessivos - passo
if passos_restantes <= 0:
break
# quantas expansões mínimas queremos garantir por passo
traj_ativas = max(1, len(candidatos_ativos))
exp_min_por_passo = traj_ativas # pelo menos 1 ângulo por candidato
# quantas expansões ainda cabem, por passo restante
exp_por_passo = max(1, exp_budget // passos_restantes)
# Kdyn não pode passar do que cabe no orçamento
Kdyn_orc = max(1, exp_por_passo // max(1, traj_ativas))
Kdyn = int(min(Kdyn, Kdyn_orc, ord_idx.size))
melhor_custo_local = float("inf")
for k in range(Kdyn):
@ -1274,9 +1017,20 @@ class ControladorMPC:
break
# Filtro de diversidade angular e top-N
#candidatos_ativos = self._filtrar_por_margem_angular(novos_candidatos, margem_graus=2.0)
#N_top = 5 if exp_budget > 6 else 3
#candidatos_ativos = self._selecionar_melhores(candidatos_ativos, N=N_top)
beam_topN_max = int(getattr(self, "_beam_topN_max", 5))
beam_topN_min = int(getattr(self, "_beam_topN_min", 2))
candidatos_ativos = self._filtrar_por_margem_angular(novos_candidatos, margem_graus=2.0)
N_top = 5 if exp_budget > 6 else 3
candidatos_ativos = self._selecionar_melhores(candidatos_ativos, N=N_top)
# número alvo de trajetórias ativas
#N_alvo = min(beam_topN_max, max(beam_topN_min, beam_traj_min))
beam_traj_min = max(1, min(beam_traj_min, beam_topN_max)) # sanity
# número alvo de trajetórias ativas: não pode passar de beam_traj_min
N_alvo = min(beam_traj_min, beam_topN_max)
# se quiser um piso suave:
N_alvo = max(1, N_alvo)
candidatos_ativos = self._selecionar_melhores(candidatos_ativos, N=N_alvo)
candidatos_validos = [c for c in candidatos_ativos if not c.get("inicial", False)]
if len(candidatos_validos) > 0:
@ -1319,7 +1073,8 @@ class ControladorMPC:
"simulacao": simulacao_latlon,
"erro_lateral": erro_lateral,
"erro_orientacao": erro_orientacao,
"debug_custo": debug_custo
"debug_custo": debug_custo,
"candidatos_testados": K
}
# -------------------- Debug opcional da matriz de custo --------------------
@ -1369,7 +1124,10 @@ class ControladorMPC:
"angulo": comando_anterior.get("angulo", 0),
"tipo": comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value),
"simulacao": [],
"erro_latreral": 0
"erro_latreral": 0,
"erro_orientacao": 0,
"debug_custo": {},
"candidatos_testados": 0
}
return _cmd
@ -2149,103 +1907,6 @@ class ControladorMPC:
mostrar_log(f"❌ Erro ao filtrar por margem angular: {e}")
return candidatos
def _simular_passo_old(self, x, y, theta, tipo, angulo_testado, custos_candidatos, comando_anterior, contexto, visitados, dt_pred, v_planejado):
"""
Simula UM 'passo' do receding (um comando) cobrindo dt_pred de tempo,
subdividido em passos menores (sub_dt) para estabilidade geométrica.
O custo é acumulado 'por metro' (multiplicando por v_planejado * sub_dt).
"""
try:
custo_total = 0.0
x_sim, y_sim, theta_sim = x, y, theta
self.x_ant, self.y_ant = 0.0, 0.0
# chave para custo heurístico vindo do VisualWorker (por ângulo/tipo)
chave = (tipo.value, round(float(np.degrees(angulo_testado)), 2))
custo_visual_worker = float(custos_candidatos.get(chave, 0.0))
simulacoes = []
prev_ang = float(comando_anterior.get("angulo", 0.0))
prev_tipo = comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value)
pontos_visitados = visitados.copy()
# Contexto usado no custo
angulo_caminho = np.radians(contexto.get("Carro", {}).get("AnguloCaminho", 0.0))
status_carro = StatusCarroMapa(contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value))
dentro_corredor = bool(contexto.get("Carro", {}).get("DentroCorredor", False))
# omega e sub-stepping
omega_const = self.gps_handler.calcular_omega(v_planejado, angulo_testado, tipo)
# Se alguém setar self.passos_horizonte_local > 1, a gente cobre tudo em dt_total:
passos_local = max(1, int(getattr(self, "passos_horizonte_local", 1)))
dt_total = float(dt_pred) * passos_local
SUB_MAX = float(getattr(self, "_dt_sub_max", 0.15)) # ~80 ms por subpasso
n_sub = max(1, int(math.ceil(dt_total / SUB_MAX)))
sub_dt = dt_total / n_sub
dist_sub = v_planejado * sub_dt # usado para custo por metro
for _ in range(n_sub):
# IMPORTANTE: _nova_posicao precisa aceitar dt opcional (ver nota abaixo)
x_sim, y_sim, theta_sim = self._nova_posicao(
x_sim, y_sim, theta_sim,
omega_const, v_planejado, # ordem original: (omega, v, tipo, angulo)
tipo, angulo_testado,
dt=sub_dt # <<--- dt variável
)
simulacoes.append((x_sim, y_sim, theta_sim))
# alvo mais próximo após avançar
idx_alvo_sim = self._corrigir_pontos_visitados(x_sim, y_sim, pontos_visitados, velocidade=v_planejado, dt=sub_dt)
ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"]
# orientação do caminho + erro angular combinado
orient_sim = self.gps_handler.calcular_orientacao((x_sim, y_sim), ponto_alvo_sim)
orient_sim = (orient_sim + np.pi) % (2 * np.pi)
# erros
erro_pos = float(np.linalg.norm([x_sim - ponto_alvo_sim[0], y_sim - ponto_alvo_sim[1]]))
peso_erro_orientacao_caminho = 0.3 if (status_carro == StatusCarroMapa.CaminhandoRua and dentro_corredor) else 0.7
erro_ori_sim = self.gps_handler.erro_angular(orient_sim, theta_sim, angulo_caminho, peso_erro_orientacao_caminho)
erro_lateral = self.cross_track_error_point(x_sim, y_sim)[0]
# pesos dinâmicos
pesos = self._calcular_pesos_movimento(contexto, erro_ori_sim)
peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, peso_movimento = pesos
# ângulo relativo ao alvo e suavidade
vetor_alvo = np.array(ponto_alvo_sim) - np.array([x_sim, y_sim])
vetor_alvo_norm = vetor_alvo / (np.linalg.norm(vetor_alvo) + 1e-9)
vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
cos_angulo = -float(np.dot(vetor_movel, vetor_alvo_norm))
delta_angulo = abs(angulo_testado - prev_ang)
# componentes de custo
custo_pos = erro_pos * peso_erro_pos
custo_ori = ((erro_ori_sim / np.pi) * erro_pos) * peso_erro_ori
custo_suavidade = ((delta_angulo / np.pi) * erro_pos) * peso_suavidade
custo_tipo_movimento = float(peso_movimento[tipo])
erro_re = (1.0 - cos_angulo) if cos_angulo > 0 else cos_angulo
custo_re = -erro_re * erro_pos * peso_fator_re
custo_lateral = abs(erro_lateral) * peso_lateral
# soma local (heurística do visual + demais), e NORMALIZA por metro
custo_mapa = (custo_pos + custo_ori + custo_suavidade + custo_tipo_movimento + custo_re + custo_lateral + custo_visual_worker)
custo_total += custo_mapa * dist_sub # <<< custo POR METRO
#print(f"Angulo: {np.degrees(angulo_testado):.2f}, Tipo: {tipo.name}, Theta: {np.degrees(theta):.2f}, ep: {erro_pos:.4f}, eo: {erro_ori_sim:.4f}, er: {erro_re:.4f}, el: {erro_lateral:.4f}")
prev_ang = angulo_testado
prev_tipo = tipo
return float(custo_total), simulacoes, True, pontos_visitados
except Exception as e:
mostrar_log(f"Erro ao simular passo para {tipo.name} | angulo: {np.degrees(angulo_testado):.2f}: {e}")
return float('inf'), [(0.0, 0.0, 0.0)], False, visitados
def _simular_passo(self, x, y, theta, tipo, angulo_testado, custos_candidatos, comando_anterior, contexto, visitados, dt_pred, v_planejado, passo=-1):
"""
Simula UM 'passo' do receding cobrindo dt_pred, subdividido em subpassos.
@ -2463,18 +2124,6 @@ class ControladorMPC:
mostrar_log(f"Erro ao simular passo para {tipo.name} | angulo: {np.degrees(angulo_testado):.2f}: {e}")
return float('inf'), [(0.0, 0.0, 0.0)], False, visitados
def _nova_posicao_old(self, x, y, theta, omega, velocidade, tipo, angulo_rad, dt=None):
"""
Atualiza a pose integrando por 'dt' (se None, cai em self.dt).
Mantém a ordem dos parâmetros antigos para não quebrar chamadas existentes.
"""
dt_use = self.dt if (dt is None) else float(dt)
distancia_m = velocidade * dt_use
theta_sim = theta + (omega * dt_use)
x_sim = x + (np.sin(theta_sim) * distancia_m)
y_sim = y + (np.cos(theta_sim) * distancia_m)
return x_sim, y_sim, theta_sim
def _nova_posicao(self, x, y, theta, omega, velocidade, tipo, angulo_rad, dt=None):
dt_use = self.dt if (dt is None) else float(dt)
theta_next = theta + omega * dt_use # orientação do corpo (no diagonal, ω≈0 → mantém)

51
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/regras_taticas.py Normal file
View File

@ -0,0 +1,51 @@
import time
from shared.enums import StatusModulo, StatusOperacao, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
from manager_worker.config import mostrar_log
def verifica_controle_liberado():
try:
motivos = []
_operacao = ContextoGlobalRedis.get_operacao()
status_operacao = StatusOperacao(_operacao.get("status", StatusOperacao.NaoIniciado.value))
finalizando = _operacao.get("finalizando", False)
if (status_operacao != StatusOperacao.EmAndamento) or finalizando:
motivos.append(f"Operação não está em andamento ou finalizando: {status_operacao.name}")
_controle = ContextoGlobalRedis.get_controle()
oak_parada_por_bloqueio = _controle.get("oak_parada_por_bloqueio", False)
if oak_parada_por_bloqueio:
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
_snr = ContextoGlobalRedis.get_modulo(T_Code.Snr)
_snr_saude = StatusModulo(_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value))
visual_worker_operante = _snr_saude == StatusModulo.OPERANTE
analise_deteccao_atualizada = (time.perf_counter() - _dados_vw.get("matriz_confianca", {}).get("ts", 10.0) <= 1.0)
obstaculo_detectado = _dados_vw.get("matriz_confianca", {}).get("block", {}).get("decision", {}).get("parar", False) if analise_deteccao_atualizada else False
if (not visual_worker_operante):
motivos.append(f"Sonar mandatório não operante: {_snr_saude.name}")
elif (visual_worker_operante and obstaculo_detectado):
motivos.append(f"Obstáculo próximo detectado no sonar")
imu_parada_por_inclinacao = _controle.get("imu_parada_por_inclinacao", False)
if imu_parada_por_inclinacao:
_imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
_imu_saude = StatusModulo(_imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value))
imu_operante = _imu_saude == StatusModulo.OPERANTE
_equipamento = ContextoGlobalRedis.get_equipamento()
ang_roll_max = _equipamento.get("angulo_roll_max", 15.0)
ang_pitch_max = _equipamento.get("angulo_pitch_max", 30.0)
inclinacao_perigosa = abs(_imu.get("pitch", 0.0)) > ang_pitch_max or abs(_imu.get("roll", 0.0)) > ang_roll_max
if not imu_operante:
motivos.append(f"IMU mandatório não operante: {_imu_saude.name}")
elif imu_operante and inclinacao_perigosa:
motivos.append(f"Inclinação perigosa detectada. roll: {_imu.get('roll')}, pitch: {_imu.get('pitch')}, yaw: {_imu.get('yaw')}")
return motivos
except Exception as e:
mostrar_log(f"Erro ao verificar liberacao do controle: {e}")

3
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_0_nao_iniciado.py
View File

@ -6,4 +6,5 @@ class ProcessadorNaoIniciado(ProcessadorBase):
try:
return comando_parado()
except Exception as e:
print(f"Erro no processador NaoIniciado: {e}")
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador NaoIniciado: {e}")

3
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_1_parametrizando.py
View File

@ -6,4 +6,5 @@ class ProcessadorParametrizando(ProcessadorBase):
try:
return comando_parado()
except Exception as e:
print(f"Erro no processador Parametrizando: {e}")
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador Parametrizando: {e}")

3
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_2_calibrando.py
View File

@ -6,4 +6,5 @@ class ProcessadorCalibrando(ProcessadorBase):
try:
return comando_parado()
except Exception as e:
print(f"Erro no processador Calibrando: {e}")
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador Calibrando: {e}")

3
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_3_aguardando.py
View File

@ -6,4 +6,5 @@ class ProcessadorAguardando(ProcessadorBase):
try:
return comando_parado()
except Exception as e:
print(f"Erro no processador Aguardando: {e}")
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador Aguardando: {e}")

53
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py
View File

@ -1,9 +1,11 @@
import time
from shared.enums import TipoMovimentoDirecional
from shared.contexto_global_redis import ContextoGlobalRedis
from manager_worker.modulos.pid import PIDAdaptativo
from .base import ProcessadorBase
from manager_worker.filtros import FiltroVelocidade
from manager_worker.processadores.padroes import comando_controle
from manager_worker.modulos.regras_taticas import verifica_controle_liberado
from manager_worker.modulos.direcional import definir_comando as definir_comando_dir
from manager_worker.modulos.movimentacao import definir_comando as definir_comando_mov
@ -28,23 +30,50 @@ class ProcessadorEmAndamento(ProcessadorBase):
try:
agora = time.perf_counter()
envio_necessario = (agora - self.ultimo_comando_enviado) > 0.3
comando_dir = definir_comando_dir(self.pid_dir, envio_necessario)
if comando_dir.get("enviar_comando", False):
self.ultimo_comando_enviado = agora
custos = comando_dir.get("debug_custo", {})
e_ori = custos.get(f"{comando_dir['tipo']}_{comando_dir['angulo']:.2f}", {}).get("erro_orientacao", None) or None
comando_mov = definir_comando_mov(comando_dir.get("parada_necessaria", False), comando_dir.get("erro", False), self.filtro_mov, erro_orientacao=e_ori)
motivos_parada = verifica_controle_liberado()
if len(motivos_parada) > 0:
from manager_worker.config import mostrar_log
mostrar_log(f"⛔ Controle bloqueado: {' | '.join(motivos_parada)}")
return comando_controle(
percentual_velocidade=comando_mov.get("velocidade"),
frear=comando_mov.get("frear", False),
angulo=comando_dir.get("angulo"),
tipo_movimento=comando_dir.get("tipo"),
simulacao=comando_dir.get("simulacao"),
percentual_velocidade=0,
frear=True,
angulo=0,
tipo_movimento=TipoMovimentoDirecional.RodasDianteiras.value,
simulacao=[],
erro=False,
latencia=0,
erro_lateral=0,
debug_custo={}
)
_controle = ContextoGlobalRedis.get_controle()
comando_mov = {}
movimento_automatico = _controle.get("movimento_automatico", False)
if movimento_automatico:
comando_mov = definir_comando_mov(self.filtro_mov)
comando_dir = {}
direcional_automatico = _controle.get("direcional_automatico", False)
if direcional_automatico:
comando_dir = definir_comando_dir(self.pid_dir, envio_necessario)
enviar_comando = (direcional_automatico and comando_dir.get("enviar_comando", False)) or not direcional_automatico
if enviar_comando:
self.ultimo_comando_enviado = agora
return comando_controle(
percentual_velocidade=comando_mov.get("velocidade", 0),
frear=comando_dir.get("parada_necessaria", False),
angulo=comando_dir.get("angulo", 0),
tipo_movimento=comando_dir.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value),
simulacao=comando_dir.get("simulacao", []),
erro=comando_dir.get("erro", False),
latencia=comando_dir.get("latencia", 0.0),
erro_lateral=comando_dir.get("erro_lateral", 0.0),
debug_custo=comando_dir.get("debug_custo", {})
)
except Exception as e:
print(f"Erro no processador EmAndamento: {e}")
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador EmAndamento: {e}")
return None

3
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_5_parado.py
View File

@ -6,4 +6,5 @@ class ProcessadorParado(ProcessadorBase):
try:
return comando_parado()
except Exception as e:
print(f"Erro no processador Parado: {e}")
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador Parado: {e}")

3
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_6_concluido.py
View File

@ -6,4 +6,5 @@ class ProcessadorConcluido(ProcessadorBase):
try:
return comando_parado()
except Exception as e:
print(f"Erro no processador Concluido: {e}")
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador Concluido: {e}")

14
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py
View File

@ -4,13 +4,15 @@ from manager_worker.config import mostrar_log
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
def comando_controle(percentual_velocidade: float, frear: bool, angulo: float, tipo_movimento: int, simulacao = [], erro: bool = False, latencia: float = 0.0, erro_lateral: float = 0.0, debug_custo: dict = {}):
hb_atual = ContextoGlobalRedis.get_controle().get("heartbeat", 0)
_controle = ContextoGlobalRedis.get_controle()
hb_atual = _controle.get("heartbeat", 0)
hb_novo = hb_atual
if erro == False:
try:
hb_atual = int(hb_atual) % 10
hb_novo = int(hb_novo) % 10
except:
hb_atual = 0
hb_atual = (hb_atual + 1) % 10
hb_novo = 0
hb_novo = (hb_novo + 1) % 10
ContextoGlobalRedis.atualizar_ctx_dict(
CtxKey.DadosControle,
@ -19,7 +21,7 @@ def comando_controle(percentual_velocidade: float, frear: bool, angulo: float, t
velocidade_sp=percentual_velocidade,
em_freio=frear,
simulacao=simulacao,
heartbeat=hb_atual,
heartbeat=hb_novo,
latencia=latencia,
erro_lateral=erro_lateral,
debug_custo=debug_custo
@ -30,7 +32,7 @@ def comando_controle(percentual_velocidade: float, frear: bool, angulo: float, t
"angulo_sp": angulo,
"tipo_movimento_direcional": tipo_movimento,
"simulacao": simulacao,
"heartbeat": hb_atual,
"heartbeat": hb_novo,
"latencia": latencia,
"erro_lateral": erro_lateral,
"debug_custo": debug_custo

18
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py
View File

@ -478,6 +478,15 @@ class ContextoGlobalRedis:
if (iniciado and not status_iniciado_anterior):
cls._iniciar_operacao()
# Não está liberado para operação
if not op_liberada:
if status_atual != StatusOperacao.Parado:
cls.atualizar_ctx_dict(CtxKey.DadosOperacao, status_anterior_parado=status_atual.value)
mostrar_log(f"Operacao Parada! {_operacao.get('motivo_nao_liberado')}")
if status_atual != StatusOperacao.Parado:
cls.atualizar_ctx_dict(CtxKey.DadosOperacao, status=StatusOperacao.Parado.value)
return
# Verifica se ainda está aguardando tempo inicial
tempo_aguardando = cls.get_operacao().get("tempo_aguardando", 0)
em_espera = (tempo_aguardando + _operacao.get("tempo_aguardar_inicio_operacao", 25)) > agora
@ -496,15 +505,6 @@ class ContextoGlobalRedis:
cls._finalizar_operacao()
return
# Não está liberado para operação
if not op_liberada:
if status_atual != StatusOperacao.Parado:
cls.atualizar_ctx_dict(CtxKey.DadosOperacao, status_anterior_parado=status_atual.value)
mostrar_log(f"Operacao Parada! {_operacao.get('motivo_nao_liberado')}")
if status_atual != StatusOperacao.Parado:
cls.atualizar_ctx_dict(CtxKey.DadosOperacao, status=StatusOperacao.Parado.value)
return
# Estava parado e liberou: volta para aguardando
if status_atual == StatusOperacao.Parado:
cls.atualizar_ctx_dict(

8
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/enums.py
View File

@ -172,13 +172,6 @@ class TipoPontoRua(IntEnum):
CruvaEntreCorredores = 6
Desvio = 7
class CameraFrameType(IntEnum):
Rgb = 1
Heatmap = 2
RadarTopDown = 3
Segmentacao = 4
Debug = 5
class TipoFrameCamera(IntEnum):
Rgb = 0
Segmentacao = 1
@ -188,3 +181,4 @@ class TipoFrameCamera(IntEnum):
MatrizCusto = 5
Deteccoes = 6
Corredor = 7
Raw4 = 8

20
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/utils.py
View File

@ -244,3 +244,23 @@ def get_velocidade_atual_ms():
mostrar_log(f"Erro ao consultar velocidade atual: {e}")
finally:
return velocidade
def resize_frame(frame_bgr, max_width=640, max_height=360):
h, w = frame_bgr.shape[:2]
# se já está menor que o limite, não faz nada
if w <= max_width and h <= max_height:
return frame_bgr
# escala baseada em qual limite "aperta" primeiro
scale_w = max_width / w
scale_h = max_height / h
scale = min(scale_w, scale_h)
new_w = int(w * scale)
new_h = int(h * scale)
frame_resized = cv2.resize(frame_bgr, (new_w, new_h), interpolation=cv2.INTER_AREA)
return frame_resized

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

@ -5,18 +5,13 @@ import time
from concurrent.futures import ThreadPoolExecutor
import cv2
import cupy as cp
import numpy as np
from visual_worker.utils import calcular_threshold_anomalias, converter_valores_numpy, gerar_heatmap
from visual_worker.processamento.analise_solo import AnaliseSoloManager
from visual_worker.processamento.analise_anomalias import AnaliseAnomaliasManager
from visual_worker.processamento.radar_top_down import Radar2DManager
from visual_worker.utils import converter_valores_numpy, gerar_heatmap
from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao, SegmentacaoManager
from camera_worker.segformer_runner import SegformerNavRunner
from visual_worker.processamento.costmap_fuser import CostmapFuser, unpack_snapshot
from shared.enums import StatusModulo, T_Code, TipoFrameCamera
from shared.utils import analisar_linhas_por_profundidade, converter_mask_ids_para_bgr, decode_image_base64, encode_image_base64, fazer_overlay, get_velocidade_atual_ms
from shared.gps_handler import GPSHandler
from shared.utils import converter_mask_ids_para_bgr, get_velocidade_atual_ms
from camera_worker.camera_oak import CameraOak
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
@ -120,7 +115,7 @@ class CameraManager:
self._analisando_deteccao = False
self._iniciar_loop_analise_continua(15.0)
self._iniciar_loop_frame_stream(5.0)
self._iniciar_loop_frame_stream(self.camera.gst_FPS)
self.iniciando = False
self.atualizar_saude_camera()
@ -299,10 +294,13 @@ class CameraManager:
continue
t0 = time.time()
try:
_frame_type = (TipoFrameCamera)((ContextoGlobalRedis.get_camera(self.mx_id) or {}).get("frame_type", TipoFrameCamera.Rgb.value))
_frame = self.get_selected_frame(_frame_type)
if _frame is not None:
self.camera.enviar_frame_stream(_frame)
_camera = (ContextoGlobalRedis.get_camera(self.mx_id) or {})
_stream_on = _camera.get("streaming", False)
if (_stream_on):
_frame_type = TipoFrameCamera(_camera.get("frame_type", TipoFrameCamera.Rgb.value))
_frame = self.get_selected_frame(_frame_type)
if _frame is not None:
self.camera.enviar_frame_tcp(_frame)
from visual_worker.config import load_seg_config, load_det_config
seg_config = load_seg_config()

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

@ -51,15 +51,16 @@ def main():
get_camera_manager().atualizar_saude_camera()
elif acao == VisualWorkerCommandType.GetCameraFrame:
tipo = TipoFrameCamera(dados.get("params", TipoFrameCamera.Rgb.value))
frame, ts = get_camera_manager().get_selected_frame(tipo)
base64_img = encode_image_base64(frame)
if base64_img is not None:
resposta = {
"frame": base64_img,
"timestamp": ts,
"tipo": tipo.value
}
ContextoGlobalRedis.publicar_comando(CmdKey.VisualWorkerTx, { "cmd": VisualWorkerCommandType.GetCameraFrame.value, "params": resposta })
frame = get_camera_manager().get_selected_frame(tipo)
if frame is not None:
base64_img = encode_image_base64(frame)
if base64_img is not None:
resposta = {
"frame": base64_img,
"timestamp": time.time(),
"tipo": tipo.value
}
ContextoGlobalRedis.publicar_comando(CmdKey.VisualWorkerTx, { "cmd": VisualWorkerCommandType.GetCameraFrame.value, "params": resposta })
elif acao == VisualWorkerCommandType.SaveCameraFrames:
nome = dados.get("params", {}).get("nome", "")
pasta = dados.get("params", {}).get("caminho", "frames_salvos")

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

@ -47,6 +47,7 @@ class SegmentacaoManager:
# tamanho máximo da deque (fallback se o FPS variar)
self.max_len = max_len or int(self.janela_s * fps_esperado * 1.5)
self._status_hist = deque(maxlen=self.max_len)
self._status_final_hist = deque(maxlen=2)
def segmentar(self, predictions):
@ -190,7 +191,7 @@ class SegmentacaoManager:
lat_out = round(float(self._ema_lat), 3)
mask_nav = (self.predictions == ClassesSegmentacao.NAVEGAVEL.value)
status_now, status_final, prob, debug = self.classificar_status_corredor(mask_nav)
status_now, status_final, status_before, prob, debug = self.classificar_status_corredor(mask_nav)
return {
"timestamp": time.time(),
@ -199,6 +200,7 @@ class SegmentacaoManager:
"erro_angular": ang_out,
"erro_lateral_pct": lat_out,
"status_corredor": status_final.value,
"status_corredor_anterior": status_before.value,
"centros_corredor": centros,
"larguras_px": larguras,
"confianca": round(float(score), 3),
@ -446,8 +448,10 @@ class SegmentacaoManager:
# Histerese temporal: mantém teu esquema de histórico
self._status_hist.append((status_now, self._now()))
status_final = self._maioria_ultimos()
self._status_final_hist.append(status_final)
status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final
return status_now, status_final, probs, debug
return status_now, status_final, status_before, probs, debug
@staticmethod
def _now():

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

@ -4,9 +4,9 @@ import threading
import time
import cv2
import numpy as np
from camera_worker.raw_segformer_service import RawSegformerService
from shared.enums import StatusModulo, StatusOperacao, T_Code, TipoFrameCamera, WeedWorkerCommandType
from shared.utils import decode_image_base64, encode_image_base64, fazer_overlay
from camera_worker.camera_gal import CameraGal
from shared.contexto_global_redis import CmdKey, ContextoGlobalRedis, CtxKey
from weed_worker.weed_detector import WeedDetector
@ -28,6 +28,7 @@ class CameraManager:
self._ts_segmentacao_anterior = 0
self._ts_ultima_analise = 0
self._ultimo_predictions = None
self._ultimo_raw_base = None
self._ultimo_raw_input = None
self._ultimo_controle = None
self._analisando_segmentacao = False
@ -67,6 +68,7 @@ class CameraManager:
self._ultima_analise = {}
self._ultimo_rgb_frame = None
self._ultimo_predictions = None
self._ultimo_raw_base = None
self._ultimo_raw_input = None
self._ultimo_controle = None
self._analisando_segmentacao = False
@ -77,7 +79,7 @@ class CameraManager:
self.weed_detector = WeedDetector(color_map=colormap_rgb, classes=classes)
self._iniciar_loop_analise_continua(20.0)
self._iniciar_loop_frame_stream(5.0)
self._iniciar_loop_frame_stream(self.camera.gst_FPS)
self.iniciando = False
self.atualizar_saude_camera()
@ -127,12 +129,11 @@ class CameraManager:
t2 = time.time()
predictions = self.model_svc.infer_raw(raw_input)
ts = time.time()
res = {
"raw_input": raw_input
}
res = {}
self._ultimo_predictions = predictions
self._ultimo_raw_base = raw4_base
self._ultimo_raw_input = raw_input
self._ultimo_predictions = predictions
#print(f"[PREDICTIONS] t_total: {ts - t0:.5f} s, t_req: {t1 - t0:.5f} s, t_build_raw: {t2 - t1:.5f} s, t_infer: {ts - t2:.5f} s")
@ -149,15 +150,19 @@ class CameraManager:
def get_selected_frame(self, _frame_type: TipoFrameCamera):
_frame = None
if (_frame_type == TipoFrameCamera.Rgb):
_frame = self._ultimo_rgb_frame
elif (_frame_type in [TipoFrameCamera.Segmentacao, TipoFrameCamera.Overlay]):
if _frame_type == TipoFrameCamera.Segmentacao: _alpha = 1.0
elif _frame_type == TipoFrameCamera.Overlay: _alpha = 0.5
_, _, _frame, _, _ = self.model_svc.preview_infer_cached(self._ultimo_raw_input, self._ultimo_predictions, alpha=_alpha)
elif _frame_type == TipoFrameCamera.Debug:
self.weed_detector._mostrar_debug_bicos_overlay(_frame, [], self._ultimo_controle, show=False)
_frame = self.weed_detector._dbg_img
if (_frame_type in [TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao, TipoFrameCamera.Overlay, TipoFrameCamera.Debug]):
rgb_frame, seg_frame, overlay_frame, _, _ = self.model_svc.preview_infer_cached(self._ultimo_raw_input, self._ultimo_predictions, alpha=0.5)
if _frame_type == TipoFrameCamera.Rgb:
_frame = rgb_frame
elif _frame_type == TipoFrameCamera.Segmentacao:
_frame = seg_frame
elif _frame_type == TipoFrameCamera.Overlay:
_frame = overlay_frame
elif _frame_type == TipoFrameCamera.Debug:
self.weed_detector._mostrar_debug_bicos_overlay(overlay_frame, [], self._ultimo_controle, show=False)
_frame = self.weed_detector._dbg_img
elif _frame_type == TipoFrameCamera.Raw4:
_frame = self._ultimo_raw_base
return _frame
def _iniciar_loop_analise_continua(self, freq):
@ -192,16 +197,20 @@ class CameraManager:
continue
t0 = time.time()
try:
_frame_type = (TipoFrameCamera)((ContextoGlobalRedis.get_camera(self.mx_id) or {}).get("frame_type", TipoFrameCamera.Rgb.value))
_frame = self.get_selected_frame(_frame_type)
if _frame is not None:
self.camera.enviar_frame_stream(_frame)
_camera = (ContextoGlobalRedis.get_camera(self.mx_id) or {})
_stream_on = _camera.get("streaming", False)
if (_stream_on):
_frame_type = TipoFrameCamera(_camera.get("frame_type", TipoFrameCamera.Rgb.value))
_frame = self.get_selected_frame(_frame_type)
if _frame is not None:
self.camera.enviar_frame_tcp(_frame)
from weed_worker.config import load_seg_config
config = load_seg_config()
if (config.get("debug_visual")):
_frame = self.get_selected_frame(TipoFrameCamera.Overlay)
self.weed_detector._mostrar_debug_bicos_overlay(_frame, [], self._ultimo_controle, config, show=True)
frame_dbg = self.get_selected_frame(TipoFrameCamera.Overlay)
if frame_dbg is not None:
self.weed_detector._mostrar_debug_bicos_overlay(frame_dbg, [], self._ultimo_controle, config, show=True)
except Exception as e:
self.mostrar_log(f"Erro no loop de stream: {e}")
finally:
@ -215,7 +224,9 @@ class CameraManager:
try:
t0 = time.time()
predictions, ts, res = self.get_segmentation_predictions()
if ts == self._ts_segmentacao_anterior: return # já analisado
if ts == self._ts_segmentacao_anterior:
self._analisando_segmentacao = False
return
fps = 1.0 / (ts - self._ts_segmentacao_anterior)
self._ts_segmentacao_anterior = ts
if predictions is not None:
@ -281,9 +292,13 @@ class CameraManager:
for _frame_type in tipos:
_frame = self.get_selected_frame(_frame_type)
if _frame is not None and _frame.size > 0:
nome_frame = f"{frame_name}_{(TipoFrameCamera(_frame_type)).name}.jpeg"
caminho = os.path.join(pasta, nome_frame)
cv2.imwrite(caminho, _frame)
nome_frame = f"{frame_name}_{(TipoFrameCamera(_frame_type)).name}"
if _frame_type == TipoFrameCamera.Raw4:
caminho = os.path.join(pasta, f"{nome_frame}.raw")
_frame.astype(np.float32).tofile(caminho)
else:
caminho = os.path.join(pasta, f"{nome_frame}.jpeg")
cv2.imwrite(caminho, _frame)
frames_salvos.append(caminho)
return frames_salvos

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

@ -106,7 +106,8 @@ def load_seg_config(force_reload=False):
}
dadosAtu = ContextoGlobalRedis.get_operacao().get("Atu", {})
contexto = ContextoGlobalRedis.get_contexto()
_CONFIG_CACHE["qtd_bicos"] = dadosAtu.get("qtd_bicos", 4)
equipamento = ContextoGlobalRedis.get_equipamento()
_CONFIG_CACHE["qtd_bicos"] = equipamento.get("qtd_bicos")
_CONFIG_CACHE["velocidade_robo"] = contexto.get("Gerais", {}).get("velocidade_ms", 0.0)
_CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ervas")

19
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/main.py
View File

@ -48,15 +48,16 @@ def main():
get_camera_manager().atualizar_saude_camera()
elif acao == WeedWorkerCommandType.GetCameraFrame:
tipo = TipoFrameCamera(dados.get("params", TipoFrameCamera.Rgb.value))
frame, ts = get_camera_manager().get_selected_frame(tipo)
base64_img = encode_image_base64(frame)
if base64_img is not None:
resposta = {
"frame": base64_img,
"timestamp": ts,
"tipo": tipo.value
}
ContextoGlobalRedis.publicar_comando(CmdKey.WeedWorkerTx, { "cmd": WeedWorkerCommandType.GetCameraFrame.value, "params": resposta })
frame = get_camera_manager().get_selected_frame(tipo)
if frame is not None:
base64_img = encode_image_base64(frame)
if base64_img is not None:
resposta = {
"frame": base64_img,
"timestamp": time.time(),
"tipo": tipo.value
}
ContextoGlobalRedis.publicar_comando(CmdKey.WeedWorkerTx, { "cmd": WeedWorkerCommandType.GetCameraFrame.value, "params": resposta })
elif acao == WeedWorkerCommandType.SaveCameraFrames:
nome = dados.get("params", {}).get("nome", "")
pasta = dados.get("params", {}).get("caminho", "frames_salvos")

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

@ -30,6 +30,7 @@ class WeedDetector:
self.img_mock = "C:\\ZendionInc\\agrobot_base\\AgroBase\\AgroBase\\bin\\x64\\Debug\\Operacoes\\25_07_2025_14_39_14\\Cam0\\85_rgb.jpeg"
self.predictions = None
self.dados_visuais = {}
self._dbg_img = None
self._dbg_img_shape = (706, 560)
self._mostrar_debug = True
@ -451,7 +452,7 @@ class WeedDetector:
faixa_atuacao = float(config.get("area_atuacao_bicos", 0.3))
# --- alocação única dos buffers de debug ---
if not hasattr(self, "_dbg_img") or self._dbg_img.shape[:2] != (self._dbg_img_shape[1], self._dbg_img_shape[0]):
if self._dbg_img is None or self._dbg_img.shape[:2] != (self._dbg_img_shape[1], self._dbg_img_shape[0]):
# _dbg_img_shape = (W, H)
self._dbg_img = np.empty((self._dbg_img_shape[1], self._dbg_img_shape[0], 3), dtype=np.uint8)
self._layer = np.zeros_like(self._dbg_img)

151
AgroBase/OperationControl/Models/TcpVideoReceiver.cs Normal file
View File

@ -0,0 +1,151 @@
using System.IO;
using System.Net;
using System.Net.Sockets;
using System.Windows.Media.Imaging;
namespace OperationControl.Models
{
internal class TcpVideoReceiver
{
private readonly int _port;
private readonly System.Windows.Controls.Image _imageControl;
private volatile bool _rodando;
private Thread _thread;
private TcpListener _listener;
public TcpVideoReceiver(int port, System.Windows.Controls.Image imageControl)
{
_port = port;
_imageControl = imageControl;
}
public void Iniciar()
{
if (_rodando) return;
_rodando = true;
_thread = new Thread(Run) { IsBackground = true };
_thread.Start();
}
public void Parar()
{
_rodando = false;
try
{
_listener?.Stop();
_imageControl.Source = null;
}
catch { /* ignora */ }
}
private void Run()
{
try
{
_listener = new TcpListener(IPAddress.Any, _port);
_listener.Start();
Console.WriteLine($"[VideoReceiver] Ouvindo na porta {_port}...");
while (_rodando)
{
TcpClient client = null;
try
{
// Em vez de bloquear direto no Accept, usamos Pending() + Sleep
if (!_listener.Pending())
{
Thread.Sleep(100);
continue;
}
client = _listener.AcceptTcpClient();
client.NoDelay = true;
Console.WriteLine("[VideoReceiver] Cliente conectado.");
using (var stream = client.GetStream())
using (var br = new BinaryReader(stream))
{
while (_rodando && client.Connected)
{
// 1) lê tamanho (4 bytes big-endian)
byte[] sizeBytes = br.ReadBytes(4);
if (sizeBytes.Length < 4)
break;
int size =
(sizeBytes[0] << 24) |
(sizeBytes[1] << 16) |
(sizeBytes[2] << 8) |
(sizeBytes[3]);
// 2) lê JPEG
byte[] imgBytes = br.ReadBytes(size);
if (imgBytes.Length < size)
break;
_imageControl.Dispatcher.BeginInvoke(new Action(() =>
{
try
{
using (var ms = new MemoryStream(imgBytes))
{
var bmp = new BitmapImage();
bmp.BeginInit();
bmp.CacheOption = BitmapCacheOption.OnLoad;
bmp.StreamSource = ms;
bmp.EndInit();
bmp.Freeze();
_imageControl.Source = bmp;
}
}
catch
{
// ignora frame corrompido ou problema de decode
}
}));
}
}
Console.WriteLine("[VideoReceiver] Cliente desconectado.");
client.Close();
}
catch (SocketException ex)
{
// WSACancelBlockingCall ao parar o listener é normal
if (!_rodando)
break;
Console.WriteLine("[VideoReceiver] SocketException: " + ex.Message);
client?.Close();
Thread.Sleep(500);
}
catch (IOException)
{
// desconexão abrupta do cliente
client?.Close();
Thread.Sleep(200);
}
catch (Exception ex)
{
Console.WriteLine("[VideoReceiver] Erro geral: " + ex.Message);
client?.Close();
Thread.Sleep(500);
}
}
Console.WriteLine("[VideoReceiver] Loop encerrado.");
}
catch (Exception ex)
{
Console.WriteLine("[VideoReceiver] Erro ao iniciar listener: " + ex.Message);
}
finally
{
try { _listener?.Stop(); } catch { }
}
}
}
}

16
AgroBase/OperationControl/Windows/MainWindow.xaml
View File

@ -826,16 +826,24 @@
<Border x:Name="brdCameraFrontal" BorderBrush="Black" BorderThickness="1" CornerRadius="8" Margin="1453,0,7,735">
<Canvas>
<Label Content="Câmera frontal | 12 fps | 230 kbps | 0,00% cana | 43 cm - 27 cm | 12,95° | CaminhandoRua" FontSize="10" HorizontalAlignment="Center" VerticalAlignment="Top" Canvas.Top="-3"/>
<vlc:VideoView x:Name="VideoFront" Background="Black" Height="237" Width="438" HorizontalAlignment="Center" Canvas.Top="25" VerticalAlignment="Top" Canvas.Left="10"/>
<ComboBox x:Name="cmbCameraFrontalFrameTipo" Canvas.Left="10" Canvas.Top="25" HorizontalAlignment="Left" VerticalAlignment="Center" Width="100" />
<!--<vlc:VideoView x:Name="VideoFront" Background="Black" Height="237" Width="438" HorizontalAlignment="Center" Canvas.Top="25" VerticalAlignment="Top" Canvas.Left="10"/>-->
<Grid Width="438" Height="237" Canvas.Top="25" Canvas.Left="10" Background="Black">
<TextBlock Text="SEM SINAL" Foreground="Gray" FontSize="18" HorizontalAlignment="Center" VerticalAlignment="Center"/>
<Image x:Name="imgVideoFront" Stretch="Uniform" RenderOptions.BitmapScalingMode="LowQuality" />
</Grid>
<ComboBox x:Name="cmbCameraFrontalFrameTipo" Canvas.Left="10" Canvas.Top="25" HorizontalAlignment="Left" VerticalAlignment="Center" Width="100" SelectionChanged="cmbCameraFrontalFrameTipo_SelectionChanged" />
</Canvas>
</Border>
<Border x:Name="brdCameraTraseira" BorderBrush="Black" BorderThickness="1" CornerRadius="8" Margin="1453,281,7,491">
<Canvas>
<Label Content="Câmera traseira | 10 fps | 193 kbps | 5,41% erva | 0,00% cana" FontSize="10" HorizontalAlignment="Center" VerticalAlignment="Top" Canvas.Top="-3"/>
<vlc:VideoView x:Name="VideoWeed" Background="Black" Height="200" Width="438" HorizontalAlignment="Center" Canvas.Top="25" VerticalAlignment="Top" Canvas.Left="10"/>
<ComboBox x:Name="cmbCameraTraseiraFrameTipo" Canvas.Left="10" Canvas.Top="25" HorizontalAlignment="Left" VerticalAlignment="Center" Width="100" />
<!--<vlc:VideoView x:Name="VideoWeed" Background="Black" Height="200" Width="438" HorizontalAlignment="Center" Canvas.Top="25" VerticalAlignment="Top" Canvas.Left="10"/>-->
<Grid Width="438" Height="200" Canvas.Top="25" Canvas.Left="10" Background="Black">
<TextBlock Text="SEM SINAL" Foreground="Gray" FontSize="18" HorizontalAlignment="Center" VerticalAlignment="Center"/>
<Image x:Name="imgVideoWeed" Stretch="Uniform" RenderOptions.BitmapScalingMode="LowQuality" />
</Grid>
<ComboBox x:Name="cmbCameraTraseiraFrameTipo" Canvas.Left="10" Canvas.Top="25" HorizontalAlignment="Left" VerticalAlignment="Center" Width="100" SelectionChanged="cmbCameraTraseiraFrameTipo_SelectionChanged" />
</Canvas>
</Border>

171
AgroBase/OperationControl/Windows/MainWindow.xaml.cs
View File

@ -27,11 +27,13 @@ namespace OperationControl.Windows
/// </summary>
public partial class MainWindow : Window, INotifyPropertyChanged
{
private LibVLC _libVLC;
private MediaPlayer _playerFront;
private MediaPlayer _playerWeed;
private Media _mediaFront;
private Media _mediaWeed;
//private LibVLC _libVLC;
//private MediaPlayer _playerFront;
//private MediaPlayer _playerWeed;
//private Media _mediaFront;
//private Media _mediaWeed;
private TcpVideoReceiver videoFront;
private TcpVideoReceiver videoWeed;
public MainWindow()
{
@ -82,17 +84,20 @@ namespace OperationControl.Windows
// Opções focadas em baixa latência
_libVLC = new LibVLC(
"--network-caching=1000", // ms (ajusta depois)
"--clock-jitter=0",
"--clock-synchro=0"
);
//_libVLC = new LibVLC(
// "--network-caching=1000", // ms (ajusta depois)
// "--clock-jitter=0",
// "--clock-synchro=0"
//);
// Player da câmera frontal
_playerFront = new MediaPlayer(_libVLC);
VideoFront.MediaPlayer = _playerFront;
_playerWeed = new MediaPlayer(_libVLC);
VideoWeed.MediaPlayer = _playerWeed;
//_playerFront = new MediaPlayer(_libVLC);
//VideoFront.MediaPlayer = _playerFront;
//_playerWeed = new MediaPlayer(_libVLC);
//VideoWeed.MediaPlayer = _playerWeed;
videoFront = new TcpVideoReceiver(5000, imgVideoFront);
videoWeed = new TcpVideoReceiver(5002, imgVideoWeed);
Loaded += MainWindow_Loaded;
@ -119,7 +124,7 @@ namespace OperationControl.Windows
{
PararStreamCameraFrontal(true);
PararStreamCameraErvas(true);
_libVLC?.Dispose();
//_libVLC?.Dispose();
}
#region DISPOSITIVOS
@ -1779,7 +1784,7 @@ namespace OperationControl.Windows
txtParametros_PctErvaOff.Text = controle.AtuPercentualErvasBicoOff.ToString();
txtParametros_TempoOn.Text = controle.AtuDuracaoAtuacao.ToString();
chbParametros_Pulverizador.IsChecked = controle.PulverizadorAutomatico;
chbParametros_Frenagem.IsChecked = controle.FrenagemAutomatica;
chbParametros_Frenagem.IsChecked = controle.FrenagemAutomaticaAoParar;
chbParametros_Sonar.IsChecked = controle.SonarAtivado;
}
catch (Exception exUi)
@ -1807,7 +1812,7 @@ namespace OperationControl.Windows
{
MovimentoAutomatico = modo != AgroBase.Models.Enums.ModoOperacao.Manual,
SonarAtivado = chbParametros_Sonar.IsChecked ?? false,
FrenagemAutomatica = chbParametros_Frenagem.IsChecked ?? false,
FrenagemAutomaticaAoParar = chbParametros_Frenagem.IsChecked ?? false,
MovVelocidadeCErvasPercent = int.Parse(txtParametros_VelCErvas.Text),
MovVelocidadeSErvasPercent = int.Parse(txtParametros_VelSErvas.Text),
@ -1922,45 +1927,49 @@ namespace OperationControl.Windows
#region CAMERA FRONTAL
private void cmbCameraFrontalFrameTipo_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
if (cmbDispositivo.SelectedItem is null || cmbDispositivo.SelectedItem == VariaveisControleOperacao.BaseMarkerID) return;
PararStreamCameraFrontal();
IniciarStreamCameraFrontal();
}
private void IniciarStreamCameraFrontal()
{
var path = @"streams\camera_frontal.sdp";
if (!File.Exists(path))
{
string stream =
"v=0\r\n" +
"o=- 0 0 IN IP4 0.0.0.0\r\n" +
"s=OAK H264\r\n" +
"c=IN IP4 0.0.0.0\r\n" +
"t=0 0\r\n" +
"m=video 5000 RTP/AVP 96\r\n" +
"a=rtpmap:96 H264/90000\r\n" +
"a=fmtp:96 packetization-mode=1\r\n";
File.WriteAllText(path, stream);
}
_mediaFront?.Dispose(); // limpa antigo, se existir
_mediaFront = new Media(_libVLC, path, FromType.FromPath);
// _mediaFront.AddOption(":network-caching=150"); // se quiser por arquivo
_playerFront.Play(_mediaFront);
//var path = @"streams\camera_frontal.sdp";
//if (!File.Exists(path))
//{
// string stream =
// "v=0\r\n" +
// "o=- 0 0 IN IP4 0.0.0.0\r\n" +
// "s=OAK H264\r\n" +
// "c=IN IP4 0.0.0.0\r\n" +
// "t=0 0\r\n" +
// "m=video 5000 RTP/AVP 96\r\n" +
// "a=rtpmap:96 H264/90000\r\n" +
// "a=fmtp:96 packetization-mode=1\r\n";
// File.WriteAllText(path, stream);
//}
//_mediaFront?.Dispose(); // limpa antigo, se existir
//_mediaFront = new Media(_libVLC, path, FromType.FromPath);
//_playerFront.Play(_mediaFront);
videoFront.Iniciar();
VariaveisControleOperacao.EnviarComandoTransmissaoVideo(AgroBase.Models.Enums.T_Code.Snr, true, ((AgroBase.Models.Enums.TipoFrameCamera)cmbCameraFrontalFrameTipo.SelectedIndex));
}
private void PararStreamCameraFrontal(bool finalizar = false)
{
_playerFront?.Stop();
if (finalizar)
{
_mediaFront?.Dispose();
_mediaFront = null;
_playerFront?.Dispose();
_playerFront = null;
}
//_playerFront?.Stop();
//if (finalizar)
//{
// _mediaFront?.Dispose();
// _mediaFront = null;
// _playerFront?.Dispose();
// _playerFront = null;
//}
videoFront.Parar();
VariaveisControleOperacao.EnviarComandoTransmissaoVideo(AgroBase.Models.Enums.T_Code.Snr, false, ((AgroBase.Models.Enums.TipoFrameCamera)cmbCameraFrontalFrameTipo.SelectedIndex));
}
@ -1968,45 +1977,50 @@ namespace OperationControl.Windows
#region CAMERA ERVAS
private void cmbCameraTraseiraFrameTipo_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
if (cmbDispositivo.SelectedItem is null || cmbDispositivo.SelectedItem == VariaveisControleOperacao.BaseMarkerID) return;
PararStreamCameraErvas();
IniciarStreamCameraErvas();
}
private void IniciarStreamCameraErvas()
{
var path = @"streams\camera_ervas.sdp";
if (!File.Exists(path))
{
string stream =
"v=0\r\n" +
"o=- 0 0 IN IP4 0.0.0.0\r\n" +
"s=OAK H264\r\n" +
"c=IN IP4 0.0.0.0\r\n" +
"t=0 0\r\n" +
"m=video 5002 RTP/AVP 96\r\n" + // <- porta 5001
"a=rtpmap:96 H264/90000\r\n" +
"a=fmtp:96 packetization-mode=1\r\n";
File.WriteAllText(path, stream);
}
_mediaWeed?.Dispose();
_mediaWeed = new Media(_libVLC, path, FromType.FromPath);
// _mediaWeed.AddOption(":network-caching=150");
_playerWeed.Play(_mediaWeed);
//var path = @"streams\camera_ervas.sdp";
//if (!File.Exists(path))
//{
// string stream =
// "v=0\r\n" +
// "o=- 0 0 IN IP4 0.0.0.0\r\n" +
// "s=OAK H264\r\n" +
// "c=IN IP4 0.0.0.0\r\n" +
// "t=0 0\r\n" +
// "m=video 5002 RTP/AVP 96\r\n" + // <- porta 5001
// "a=rtpmap:96 H264/90000\r\n" +
// "a=fmtp:96 packetization-mode=1\r\n";
// File.WriteAllText(path, stream);
//}
//_mediaWeed?.Dispose();
//_mediaWeed = new Media(_libVLC, path, FromType.FromPath);
//_mediaWeed.AddOption(":network-caching=150");
//_playerWeed.Play(_mediaWeed);
videoWeed.Iniciar();
VariaveisControleOperacao.EnviarComandoTransmissaoVideo(AgroBase.Models.Enums.T_Code.Cam, true, ((AgroBase.Models.Enums.TipoFrameCamera)cmbCameraTraseiraFrameTipo.SelectedIndex));
}
private void PararStreamCameraErvas(bool finalizar = false)
{
_playerWeed?.Stop();
if (finalizar)
{
_mediaWeed?.Dispose();
_mediaWeed = null;
_playerWeed?.Dispose();
_playerWeed = null;
}
//_playerWeed?.Stop();
//if (finalizar)
//{
// _mediaWeed?.Dispose();
// _mediaWeed = null;
// _playerWeed?.Dispose();
// _playerWeed = null;
//}
videoWeed.Parar();
VariaveisControleOperacao.EnviarComandoTransmissaoVideo(AgroBase.Models.Enums.T_Code.Cam, false, ((AgroBase.Models.Enums.TipoFrameCamera)cmbCameraTraseiraFrameTipo.SelectedIndex));
}
@ -2246,6 +2260,9 @@ namespace OperationControl.Windows
#endregion

6
Firmware/Modulos/BicoPulverizadorModel.h
View File

@ -107,8 +107,12 @@ class Bico : public ComponenteCAN {
break;
}
case CanMessagePosicaoDados::Dados1: {
int angulo = 0;
if (servoMov != nullptr) {
angulo = servoMov->_Angulo;
}
data.push_back(static_cast<uint8_t>(_Status));
data.push_back(servoMov->_Angulo);
data.push_back(angulo);
break;
}
}