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

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

@ -198,7 +198,7 @@ namespace AgroBase.Forms.Operacoes
ReqFrameCam = true; ReqFrameCam = true;
try 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); Panel pnl = FuncoesGlobais.FindControlRecursive<Panel>(flwCamerasSolo, "pnlCamSolo_" + Variaveis.OperacaoEmAndamento.DispSen.Dados.CamerasSolo[0].Name);
AtualizarImagemPainel(pnl, frame?.image()); AtualizarImagemPainel(pnl, frame?.image());
} }
@ -213,7 +213,7 @@ namespace AgroBase.Forms.Operacoes
ReqFrameSnr = true; ReqFrameSnr = true;
try try
{ {
var frameRgb = await VisualWorkerService.GetCameraFrame(CameraFrameType.Debug); var frameRgb = await VisualWorkerService.GetCameraFrame(TipoFrameCamera.Overlay);
AtualizarImagemPainel(pnlCameraRua, frameRgb?.image()); AtualizarImagemPainel(pnlCameraRua, frameRgb?.image());
//var frameSeg = await VisualWorkerService.GetCameraFrame(CameraFrameType.Segmentacao); //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"); 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(); 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"; //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"; //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"; //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"; //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"; //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"; //txtTempoManobrando.Text = Log.Trajetoria.CorredorAtual.TempoPorStatus.FirstOrDefault(x => x.Key == Enums.StatusCarroMapa.Manobrando).Value.ToString("0.00") + " s";
pnlCaminho.BackgroundImage = CarregarImagemCamera(pathImagensCaminho, "_segmentacao"); pnlCaminho.BackgroundImage = CarregarImagemCamera(pathImagensCaminho, "_segmentacao");
pnlCaminho.BackgroundImageLayout = ImageLayout.Zoom; 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)); System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(frmSimulacaoMapaGPS));
this.pnlOpcoes = new System.Windows.Forms.Panel(); 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.lblDelayPosicao = new System.Windows.Forms.Label();
this.txtDelayPosicao = new System.Windows.Forms.TextBox(); this.txtDelayPosicao = new System.Windows.Forms.TextBox();
this.lblCarregado = new System.Windows.Forms.Label(); this.lblCarregado = new System.Windows.Forms.Label();
@ -105,9 +108,6 @@
this.lblTempoRestante = new System.Windows.Forms.Label(); this.lblTempoRestante = new System.Windows.Forms.Label();
this.lblPercentualRua = new System.Windows.Forms.Label(); this.lblPercentualRua = new System.Windows.Forms.Label();
this.lblPercentualOperacao = 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.pnlOpcoes.SuspendLayout();
this.gpbSonar.SuspendLayout(); this.gpbSonar.SuspendLayout();
this.gpbGPS.SuspendLayout(); this.gpbGPS.SuspendLayout();
@ -151,6 +151,39 @@
this.pnlOpcoes.Size = new System.Drawing.Size(1379, 100); this.pnlOpcoes.Size = new System.Drawing.Size(1379, 100);
this.pnlOpcoes.TabIndex = 0; 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 // lblDelayPosicao
// //
this.lblDelayPosicao.AutoSize = true; 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblTempoOperacao.AutoSize = true; this.lblTempoOperacao.AutoSize = true;
this.lblTempoOperacao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblTempoOperacao.Name = "lblTempoOperacao"; this.lblTempoOperacao.Name = "lblTempoOperacao";
this.lblTempoOperacao.Size = new System.Drawing.Size(148, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblStatusOperacao.AutoSize = true; this.lblStatusOperacao.AutoSize = true;
this.lblStatusOperacao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblStatusOperacao.Name = "lblStatusOperacao"; this.lblStatusOperacao.Name = "lblStatusOperacao";
this.lblStatusOperacao.Size = new System.Drawing.Size(133, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDistanciaFinal.AutoSize = true; this.lblDistanciaFinal.AutoSize = true;
this.lblDistanciaFinal.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistanciaFinal.Name = "lblDistanciaFinal"; this.lblDistanciaFinal.Name = "lblDistanciaFinal";
this.lblDistanciaFinal.Size = new System.Drawing.Size(135, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDistanciaOperacao.AutoSize = true; this.lblDistanciaOperacao.AutoSize = true;
this.lblDistanciaOperacao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistanciaOperacao.Name = "lblDistanciaOperacao"; this.lblDistanciaOperacao.Name = "lblDistanciaOperacao";
this.lblDistanciaOperacao.Size = new System.Drawing.Size(189, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDirecao.AutoSize = true; this.lblDirecao.AutoSize = true;
this.lblDirecao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDirecao.Name = "lblDirecao"; this.lblDirecao.Name = "lblDirecao";
this.lblDirecao.Size = new System.Drawing.Size(84, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblStatus.AutoSize = true; this.lblStatus.AutoSize = true;
this.lblStatus.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblStatus.Name = "lblStatus"; this.lblStatus.Name = "lblStatus";
this.lblStatus.Size = new System.Drawing.Size(96, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblRua.AutoSize = true; this.lblRua.AutoSize = true;
this.lblRua.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblRua.Name = "lblRua"; this.lblRua.Name = "lblRua";
this.lblRua.Size = new System.Drawing.Size(74, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblMargem.AutoSize = true; this.lblMargem.AutoSize = true;
this.lblMargem.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblMargem.Name = "lblMargem"; this.lblMargem.Name = "lblMargem";
this.lblMargem.Size = new System.Drawing.Size(71, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblProximoPonto.AutoSize = true; this.lblProximoPonto.AutoSize = true;
this.lblProximoPonto.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblProximoPonto.Name = "lblProximoPonto"; this.lblProximoPonto.Name = "lblProximoPonto";
this.lblProximoPonto.Size = new System.Drawing.Size(87, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblAproximando.AutoSize = true; this.lblAproximando.AutoSize = true;
this.lblAproximando.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblAproximando.Name = "lblAproximando"; this.lblAproximando.Name = "lblAproximando";
this.lblAproximando.Size = new System.Drawing.Size(68, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDistProx.AutoSize = true; this.lblDistProx.AutoSize = true;
this.lblDistProx.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistProx.Name = "lblDistProx"; this.lblDistProx.Name = "lblDistProx";
this.lblDistProx.Size = new System.Drawing.Size(160, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblDistAnt.AutoSize = true; this.lblDistAnt.AutoSize = true;
this.lblDistAnt.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistAnt.Name = "lblDistAnt"; this.lblDistAnt.Name = "lblDistAnt";
this.lblDistAnt.Size = new System.Drawing.Size(159, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblTipoMovimento.AutoSize = true; this.lblTipoMovimento.AutoSize = true;
this.lblTipoMovimento.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblTipoMovimento.Name = "lblTipoMovimento"; this.lblTipoMovimento.Name = "lblTipoMovimento";
this.lblTipoMovimento.Size = new System.Drawing.Size(75, 13); 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.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Bottom | System.Windows.Forms.AnchorStyles.Left)));
this.lblPercentualRua.AutoSize = true; this.lblPercentualRua.AutoSize = true;
this.lblPercentualRua.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F); 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.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblPercentualRua.Name = "lblPercentualRua"; this.lblPercentualRua.Name = "lblPercentualRua";
this.lblPercentualRua.Size = new System.Drawing.Size(62, 13); this.lblPercentualRua.Size = new System.Drawing.Size(62, 13);
@ -994,39 +1027,6 @@
this.lblPercentualOperacao.TabIndex = 42; this.lblPercentualOperacao.TabIndex = 42;
this.lblPercentualOperacao.Text = "Operação: 0,00%"; 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 // frmSimulacaoMapaGPS
// //
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); 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; 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(); lblTipoMovimento.Text = (_Sensoriamento.Controle?.TipoMovimento ?? TipoMovimentoDirecional.Diagnostico).ToString();
if (_Trajetoria == null) if (_Trajetoria == null)
@ -136,21 +137,21 @@ namespace AgroBase.Forms
if (_Sensoriamento.Trajetoria == null) return; if (_Sensoriamento.Trajetoria == null) return;
lblStatus.Text = "Carro: " + Enum.GetName(typeof(StatusCarroMapa), _Sensoriamento.Trajetoria.StatusCarro); lblStatus.Text = $"Carro: {_Sensoriamento.Trajetoria.StatusCarro} ({_Sensoriamento.OperadorVisual.Analises.segmentacao.status_corredor})";
lblDirecao.Text = "Direção: " + Enum.GetName(typeof(DirecaoCarroRua), _Sensoriamento.Trajetoria.DirecaoCaminho); lblDirecao.Text = $"Direção: {_Sensoriamento.Trajetoria.DirecaoCaminho}";
lblDistanciaOperacao.Text = "Distância Operação: " + _Sensoriamento.Trajetoria.DistanciaPercorrida.ToString("0.00") + " m de " + _Sensoriamento.Trajetoria.DistanciaTotal.ToString("0.00") + " m"; 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.ToString("0.00") + " m"; lblDistanciaFinal.Text = $"Distância Restante: {_Sensoriamento.Trajetoria.DistanciaRestante:F2} 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"; 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"); lblRua.Text = $"Corredor: " + (_Sensoriamento.Trajetoria.CorredorAtual.Dentro ? "Dentro" : "Fora");
lblMargem.Text = "Margem: " + (_Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não"); lblMargem.Text = $"Margem: " + (_Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não");
lblProximoPonto.Text = "Próximo Ponto: " + (_Sensoriamento.Trajetoria.ProximoPonto.idxPonto); lblProximoPonto.Text = $"Próximo Ponto: {_Sensoriamento.Trajetoria.ProximoPonto.idxPonto}";
lblAproximando.Text = (_Sensoriamento.Trajetoria.ProximoPonto.Aproximando ? "Aproximando" : "Afastando"); lblAproximando.Text = (_Sensoriamento.Trajetoria.ProximoPonto.Aproximando ? "Aproximando" : "Afastando");
lblDistProx.Text = "Distância Próximo Ponto: " + _Sensoriamento.Trajetoria.ProximoPonto.DistanciaTrajeto.ToString("0.00") + " m"; lblDistProx.Text = $"Distância Próximo Ponto: {_Sensoriamento.Trajetoria.ProximoPonto.DistanciaTrajeto:F2} m";
lblDistAnt.Text = "Distância Ponto Anterior: " + _Sensoriamento.Trajetoria.PontoMaisProximo.DistanciaTrajeto.ToString("0.00") + " m"; lblDistAnt.Text = $"Distância Ponto Anterior: {_Sensoriamento.Trajetoria.PontoMaisProximo.DistanciaTrajeto:F2} m";
lblTempoRestante.Text = "Tempo Restante: " + _Sensoriamento.Trajetoria.TempoEstimadoRestante; lblTempoRestante.Text = $"Tempo Restante: {_Sensoriamento.Trajetoria.TempoEstimadoRestante}";
lblTempoOperacao.Text = "Tempo: " + _Sensoriamento.TempoDecorrido.ToString("HH:mm:ss") + " de " + _Sensoriamento.Trajetoria.TempoEstimadoOperacao; lblTempoOperacao.Text = $"Tempo: " + _Sensoriamento.TempoDecorrido.ToString("HH:mm:ss") + $" de {_Sensoriamento.Trajetoria.TempoEstimadoOperacao}";
lblPercentualRua.Text = "Rua: " + _Sensoriamento.Trajetoria.CorredorAtual.Progresso.ToString("0.00") + "%"; lblPercentualRua.Text = $"Rua: {_Sensoriamento.Trajetoria.CorredorAtual.Progresso:F2}%";
lblPercentualOperacao.Text = "Operação: " + _Sensoriamento.Trajetoria.ProgressoTrajeto.ToString("0.00") + "%"; lblPercentualOperacao.Text = $"Operação: {_Sensoriamento.Trajetoria.ProgressoTrajeto:F2}%";
pnlOrientacaoTrajeto.Invalidate(); pnlOrientacaoTrajeto.Invalidate();
@ -544,6 +545,11 @@ namespace AgroBase.Forms
private void btnRetornarBase_Click(object sender, EventArgs e) private void btnRetornarBase_Click(object sender, EventArgs e)
{ {
Variaveis.OperacaoEmAndamento.Trajetoria.IniciarRetornoBase(VariaveisOperacao.PosicaoBase); 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) 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.Text;
using System.Threading.Tasks; using System.Threading.Tasks;
using System.Windows.Forms; using System.Windows.Forms;
using static AgroBase.Models.Enums;
namespace AgroBase.Forms.Simuladores namespace AgroBase.Forms.Simuladores
{ {
@ -68,7 +69,7 @@ namespace AgroBase.Forms.Simuladores
{ {
Task.Run(async () => 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; var dadosSeg = Variaveis.OperacaoEmAndamento.Sensoriamento.OperadorVisual.Analises.segmentacao;
DesenharCorredor(frame?.image(), dadosSeg.centros_corredor, dadosSeg.width, dadosSeg.height, 80); 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.Threading.Tasks;
using System.Windows.Forms; using System.Windows.Forms;
using System.Windows.Forms.DataVisualization.Charting; using System.Windows.Forms.DataVisualization.Charting;
using static AgroBase.Models.Enums;
namespace AgroBase.Forms namespace AgroBase.Forms
{ {
@ -79,51 +80,51 @@ namespace AgroBase.Forms
private async void btnLeituraRGB_Click(object sender, EventArgs e) private async void btnLeituraRGB_Click(object sender, EventArgs e)
{ {
EnviarComandoReqFrame(CameraFrameType.Rgb); EnviarComandoReqFrame(TipoFrameCamera.Rgb);
} }
private void btnLeituraHeatmap_Click(object sender, EventArgs e) private void btnLeituraHeatmap_Click(object sender, EventArgs e)
{ {
EnviarComandoReqFrame(CameraFrameType.Heatmap); EnviarComandoReqFrame(TipoFrameCamera.Heatmap);
} }
private void btnLeituraRadar2D_Click(object sender, EventArgs e) private void btnLeituraRadar2D_Click(object sender, EventArgs e)
{ {
EnviarComandoReqFrame(CameraFrameType.RadarTopDown); //EnviarComandoReqFrame(TipoFrameCamera.RadarTopDown);
} }
private void btnLeituraSegmentacaoSemantica_Click(object sender, EventArgs e) 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); OAKCameraFrameModel frame = await VisualWorkerService.GetCameraFrame(tipo);
PictureBox pic = PictureBox pic =
tipo == CameraFrameType.Rgb ? picRGB : tipo == TipoFrameCamera.Rgb ? picRGB :
tipo == CameraFrameType.Heatmap ? picHeatmap : tipo == TipoFrameCamera.Heatmap ? picHeatmap :
tipo == CameraFrameType.RadarTopDown ? picRadar2D : //tipo == TipoFrameCamera.RadarTopDown ? picRadar2D :
tipo == CameraFrameType.Segmentacao ? picSegmentacaoSemantica : tipo == TipoFrameCamera.Segmentacao ? picSegmentacaoSemantica :
new PictureBox(); new PictureBox();
Label lbl = Label lbl =
tipo == CameraFrameType.Rgb ? lblLeituraRGB : tipo == TipoFrameCamera.Rgb ? lblLeituraRGB :
tipo == CameraFrameType.Heatmap ? lblLeituraHeatmap : tipo == TipoFrameCamera.Heatmap ? lblLeituraHeatmap :
tipo == CameraFrameType.RadarTopDown ? lblLeituraRadar2D : //tipo == TipoFrameCamera.RadarTopDown ? lblLeituraRadar2D :
tipo == CameraFrameType.Segmentacao ? lblLeituraSegmentacaoSemantica : tipo == TipoFrameCamera.Segmentacao ? lblLeituraSegmentacaoSemantica :
new Label(); new Label();
pic.Image?.Dispose(); pic.Image?.Dispose();
try { pic.Image = frame?.image(); } catch { } try { pic.Image = frame?.image(); } catch { }
lbl.Text = $"Leitura em: " + frame?.timestamp.ToString("dd/MM/yyyy HH:mm:ss"); 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) private void btnReqStatusDispositivo_Click(object sender, EventArgs e)
@ -152,7 +153,7 @@ namespace AgroBase.Forms
var analise = VisualWorkerService.DadosLeitura.Analises; var analise = VisualWorkerService.DadosLeitura.Analises;
lblLeituraDebug.Text = $"Leitura em: " + (analise?.timestamp ?? DateTime.MinValue).ToString("dd/MM/yyyy HH:mm:ss"); 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?.Dispose();
picDebug.Image = analise.PlotarAnalise(frame); picDebug.Image = analise.PlotarAnalise(frame);

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

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

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

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

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

@ -2055,8 +2055,8 @@ namespace AgroBase.Models.Modules
loRaService.Configurado = false; loRaService.Configurado = false;
loRaService.ParametrosGet.UltimaLeitura = DateTime.MinValue; loRaService.ParametrosGet.UltimaLeitura = DateTime.MinValue;
loRaService.MensagensRecebidasAvulsas = new List<LoRaMensagemModel>(); loRaService.MensagensRecebidasAvulsas = new List<LoRaMensagemModel>();
VisualWorkerService.DadosLeitura.CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>(); VisualWorkerService.DadosLeitura.CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
WeedWorkerService.DadosLeitura.CameraFrames = new Dictionary<CameraFrameType, OAKCameraFrameModel>(); WeedWorkerService.DadosLeitura.CameraFrames = new Dictionary<TipoFrameCamera, OAKCameraFrameModel>();
ReiniciarLeituras(Dados); 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 DistanciaMinimaVisao { get; set; } = 500;
public double PercentualAlturaSolo { get; set; } = 30; public double PercentualAlturaSolo { get; set; } = 30;
public Dictionary<CameraFrameType, OAKCameraFrameModel> frames public Dictionary<TipoFrameCamera, OAKCameraFrameModel> frames
{ {
get get
{ {
@ -47,13 +47,13 @@ namespace AgroBase.Models
case T_Code.Cam: case T_Code.Cam:
return WeedWorkerService.DadosLeitura.CameraFrames; return WeedWorkerService.DadosLeitura.CameraFrames;
default: 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; OAKCameraFrameModel frame = null;
switch (Dispositivo) switch (Dispositivo)

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

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

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

@ -31,10 +31,13 @@ namespace AgroBase.Models.Operacoes
public class OperacaoParametrosControleModel public class OperacaoParametrosControleModel
{ {
public bool SonarAtivado { get; set; }
public bool MovimentoAutomatico { get; set; } public bool MovimentoAutomatico { get; set; }
public bool DirecionalAutomatico { get; set; }
public bool PulverizadorAutomatico { 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 MovVelocidadeSErvasPercent { get; set; }
public int MovRpmMax public int MovRpmMax

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

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

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

@ -1,5 +1,6 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using static AgroBase.Models.Enums;
namespace AgroBase.Models.Operadores namespace AgroBase.Models.Operadores
{ {
@ -10,7 +11,7 @@ namespace AgroBase.Models.Operadores
public bool Pronto { get; set; } public bool Pronto { get; set; }
public DateTime ProntoEm { get; set; } public DateTime ProntoEm { get; set; }
public DateTime UltimaMensagem { 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(); public WeedWorkerAnaliseModel Analise { get; set; } = new WeedWorkerAnaliseModel();
@ -22,7 +23,7 @@ namespace AgroBase.Models.Operadores
IniciadoEm = IniciadoEm, IniciadoEm = IniciadoEm,
Pronto = Pronto, Pronto = Pronto,
ProntoEm = ProntoEm, 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(), Analise = Analise.Clone(),
UltimaMensagem = UltimaMensagem, UltimaMensagem = UltimaMensagem,
}; };
@ -34,7 +35,7 @@ namespace AgroBase.Models.Operadores
IniciadoEm = DateTime.MinValue; IniciadoEm = DateTime.MinValue;
Pronto = false; Pronto = false;
ProntoEm = DateTime.MinValue; 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;
using System.Collections.Generic; using System.Collections.Generic;
using System.Linq; using System.Linq;
using static AgroBase.Models.CorredorTrajetoriaModel;
using static AgroBase.Models.Enums; using static AgroBase.Models.Enums;
namespace AgroBase.Models namespace AgroBase.Models
@ -683,7 +684,7 @@ namespace AgroBase.Models
DefinirProximoPonto(); DefinirProximoPonto();
if (PontoAtual == null) goto Inicio; if (PontoAtual == null) goto Inicio;
if (VerificaInicioOperacaoMeioRua()) goto Inicio; if (VerificaInicioOperacaoMeioRua(true)) goto Inicio;
MarcarPontosIntermediariosNaoVisitados(); MarcarPontosIntermediariosNaoVisitados();
DefinirPontoMaisProximo(); 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; 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; if (Corredores == null)
AtualizarPropriedadesPontosTrajetoria(); return (false, -1);
AtualizarTrajetoriaJanela();
List<PontoTrajetoriaModel> melhoresPontosCorredores = new List<PontoTrajetoriaModel>(); List<PontoTrajetoriaModel> melhoresPontosCorredores = new List<PontoTrajetoriaModel>();
foreach (var corredor in Corredores) foreach (var corredor in Corredores)
{ {
(GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(_TrajetoriaFixa[0].Posicao, corredor); (GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(posicaoAtual, corredor);
if (pontoMaisProximo == null) if (pontoMaisProximo == null) continue;
{
return (false, -1);
}
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); melhoresPontosCorredores.Add(melhorPonto);
} }
bool pontoNaMargem = melhoresPontosCorredores.Any(x => x.NaMargem); var ponto = melhoresPontosCorredores.Where(x => x.NaMargem).OrderBy(x => x.DistanciaAtual).FirstOrDefault();
int idxPonto = !pontoNaMargem ? 0 : melhoresPontosCorredores.Where(x => x.NaMargem).OrderBy(x => x.DistanciaAtual).FirstOrDefault().idxPonto; bool pontoNaMargem = ponto != null;
int idxPonto = !pontoNaMargem ? -1 : ponto.idxPonto;
return (pontoNaMargem, 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; VerificacaoInicialMeioRuaConcluida = true;
(bool noCoredor, var idxPontoMaisProximo) = VerificaEquipamentoDentroCorredor(); (bool noCoredor, var idxPontoMaisProximo) = VerificaEquipamentoDentroCorredorMontado(_TrajetoriaFixa[0].Posicao);
if (noCoredor) if (noCoredor)
{ {
for (int i = 0; i < idxPontoMaisProximo; i++) for (int i = 0; i <= idxPontoMaisProximo; i++)
{ {
_TrajetoriaFixa[i].Visitado = true; _TrajetoriaFixa[i].Visitado = true;
} }
CorredorAtual.AtualizarDados();
return true; return true;
} }
} }
@ -771,106 +773,177 @@ namespace AgroBase.Models
{ {
if (CorredorAtual == null) return; 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 limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 5.0;
double distanciaCorredor = CorredorAtual.DistanciaTotal; double distanciaCorredor = CorredorAtual.DistanciaTotal;
double distanciaRestanteCorredor = CorredorAtual.DistanciaRestante; double distanciaRestanteCorredor = CorredorAtual.DistanciaRestante;
double distanciaUltimoPontoCorredor = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, CorredorAtual.Pontos.LastOrDefault().Posicao); 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 double dt = (agora - dv.UltimoUpdate).TotalSeconds;
if (CorredorAtual.TempoPorStatus == null) CorredorAtual.TempoPorStatus = new Dictionary<StatusCarroMapa, double>(); if (dt < 0) dt = 0;
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;
if ( // Aproximação da distância percorrida nesse intervalo pelo mapa
( double deltaDist = 0;
(Variaveis.OperacaoEmAndamento.Simulando || if (dv.DistanciaRestanteAnterior >= 0)
(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
)
{ {
double anguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo; deltaDist = dv.DistanciaRestanteAnterior - distanciaRestanteCorredor;
if (abrirCurva && !CorredorAtual.Ultimo) 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; AtualizarIndicesJanela();
foreach (var Ponto in CorredorAtual.Pontos.Where(x => !x.Visitado)) AtualizarTrajetoriaJanela();
DefinirPontoAtual();
DefinirProximoPonto();
if (abrirCurva)
{ {
Ponto.Visitado = true; var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
pontosMarcar++;
} if (pControle.MovimentoAutomatico)
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++)
{ {
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(); if (pControle.DirecionalAutomatico)
DefinirProximoPonto();
if (abrirCurva)
{ {
Console.WriteLine($"Angulo de controle definido de {Variaveis.OperacaoEmAndamento.Controle.Angulo} para {anguloControle}"); Console.WriteLine($"Angulo de controle definido de {Variaveis.OperacaoEmAndamento.Controle.Angulo} para {anguloControle}");
Variaveis.OperacaoEmAndamento.Controle.Angulo = anguloControle; Variaveis.OperacaoEmAndamento.Controle.Angulo = anguloControle;
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent;
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.MovimentoArco; 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.Dir).DelayEnvioComandoExtra = 1500;
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DelayEnvioComandoExtra = 2000; Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Dir });
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico)
{
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov, 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); bool mangerDestravou = novoComando != null && (_Controle.ticks_sem_resposta.AddMilliseconds(min_ticks_sem_resposta) < agora && novoComando.heartbeat != _Controle.heartbeat);
if (novoComando != null && (comandoMudou || mangerDestravou)) 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; var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico) 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) if (novoComando != null && novoComando.heartbeat == _Controle.heartbeat)
@ -1718,6 +1797,9 @@ namespace AgroBase.Models
GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps; GPSModel posicaoAtual = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps;
VerificaInicioOperacaoMeioRua(false);
AtualizarDadosTrajetoria();
// ========================= // =========================
// REGRA 0 – Está dentro de corredor? // REGRA 0 – Está dentro de corredor?
// ========================= // =========================
@ -2251,8 +2333,7 @@ namespace AgroBase.Models
public int idxRuaEsquerda { get; set; } public int idxRuaEsquerda { get; set; }
public int idxRuaDireita { get; set; } public int idxRuaDireita { get; set; }
public double FatorLarguraCorredor { get; set; } public double FatorLarguraCorredor { get; set; }
public DateTime UltimoTempoStatus { get; set; } = DateTime.MinValue; public DadosVisuaisCorredor DadosVisuais { get; set; } = new DadosVisuaisCorredor();
public Dictionary<StatusCarroMapa, double> TempoPorStatus { get; set; }
public void AtualizarDados() public void AtualizarDados()
{ {
@ -2302,8 +2383,7 @@ namespace AgroBase.Models
Pontos = new List<PontoTrajetoriaModel>(), Pontos = new List<PontoTrajetoriaModel>(),
Ultimo = Ultimo, Ultimo = Ultimo,
FatorLarguraCorredor = FatorLarguraCorredor, FatorLarguraCorredor = FatorLarguraCorredor,
UltimoTempoStatus = UltimoTempoStatus, DadosVisuais = DadosVisuais
TempoPorStatus = new Dictionary<StatusCarroMapa, double>(TempoPorStatus ?? new Dictionary<StatusCarroMapa, double>())
}; };
if (clonarPontos) if (clonarPontos)
{ {
@ -2312,6 +2392,30 @@ namespace AgroBase.Models
return obj; 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 public class PontoTrajetoriaModel
@ -2378,7 +2482,7 @@ namespace AgroBase.Models
// Define o limite de distância para considerar o ponto como visitado // 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 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 // Se o ponto está dentro da margem e dentro do limite de distância, marca como visitado
if (NaMargem && DistanciaTrajeto <= limiteDistancia) 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 PercentualReservatorioMinCritio { get; set; } = 5.0;
public static double PercentualToleranciaPressaoLinha { get; set; } = 0.15; public static double PercentualToleranciaPressaoLinha { get; set; } = 0.15;
public static int QuantidadeCamerasSolo { get; set; } = 1; 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 AlturaBarraPulverizadoraCm { get; set; } = 80;
public static double ComprimentoBarraPulverizadoraCm { get; set; } = 103.7; public static double ComprimentoBarraPulverizadoraCm { get; set; } = 103.7;
public static double DistanciaEntreBicosCm { get; set; } = 50; 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_str = RedisService.Get(RedisService.CamKey(cam.Key));
var camera = JsonConvert.DeserializeObject<CameraWorkerItemModel>(camera_str); var camera = JsonConvert.DeserializeObject<CameraWorkerItemModel>(camera_str);
if (camera.mx_id == null) continue;
CamerasConectadas.Add(new OAKCameraModel() CamerasConectadas.Add(new OAKCameraModel()
{ {
Dispositivo = camera.dispositivo, 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), ("serial_number", VariaveisEquipamento.Parametros.serial_number),
("largura", VariaveisEquipamento.LarguraEquipamentoMm / 1000.0), ("largura", VariaveisEquipamento.LarguraEquipamentoMm / 1000.0),
("distancia_entre_eixos", VariaveisEquipamento.DistanciaEntreEixos / 100.0), ("distancia_entre_eixos", VariaveisEquipamento.DistanciaEntreEixos / 100.0),
("qtd_bicos", VariaveisEquipamento.QuantidadeBicosPulverizadores),
("percentual_reservatorio_min", VariaveisEquipamento.PercentualReservatorioMin), ("percentual_reservatorio_min", VariaveisEquipamento.PercentualReservatorioMin),
("percentual_tensao_bateria_min", VariaveisEquipamento.PercentualTensaoBateriaMin), ("percentual_tensao_bateria_min", VariaveisEquipamento.PercentualTensaoBateriaMin),
("camera_caminho_id", Variaveis.OperacaoEmAndamento.DispSen?.Dados?.CameraCaminho?.Id ?? ""), ("camera_caminho_id", Variaveis.OperacaoEmAndamento.DispSen?.Dados?.CameraCaminho?.Id ?? ""),
@ -242,7 +243,10 @@ namespace AgroBase.Services.Operadores
distanciaMargem = p.LarguraCorredor * 0.8 distanciaMargem = p.LarguraCorredor * 0.8
}) })
.ToList(), .ToList(),
horizonte = 2.5, horizonte = 2.0,
horizonte_min = 1.2,
horizonte_max = 2.8,
horizonte_parado = 1.2,
angulo_max_graus = pControle.DirAnguloMaximo, angulo_max_graus = pControle.DirAnguloMaximo,
velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeCErvasPercent), velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeCErvasPercent),
velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeSErvasPercent), velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeSErvasPercent),
@ -291,8 +295,10 @@ namespace AgroBase.Services.Operadores
CtxKey.DadosControle, CtxKey.DadosControle,
("pulverizador_automatico", pControle.PulverizadorAutomatico), ("pulverizador_automatico", pControle.PulverizadorAutomatico),
("movimento_automatico", pControle.MovimentoAutomatico), ("movimento_automatico", pControle.MovimentoAutomatico),
("frenagem_automatica", pControle.FrenagemAutomatica), ("direcional_automatico", pControle.DirecionalAutomatico),
("sonar_ativado", pControle.SonarAtivado) ("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); 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) 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; var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico) 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; break;
case ManagerWorkerCommandType.EnviarDadosControle: case ManagerWorkerCommandType.EnviarDadosControle:
if (Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico) if (Variaveis.OperacaoEmAndamento.Parametros.Controle.MovimentoAutomatico || Variaveis.OperacaoEmAndamento.Parametros.Controle.DirecionalAutomatico)
{ {
AtualizarControle(comando.@params); AtualizarControle(comando.@params);
} }

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

@ -79,7 +79,7 @@ namespace AgroBase.Services.Operadores
if (!Conectado) if (!Conectado)
{ {
TempoDesconectado++; 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( RedisService.AtualizarCampos(
CtxKey.DadosOperacao, CtxKey.DadosOperacao,
@ -89,9 +89,24 @@ namespace AgroBase.Services.Operadores
); );
var _Controle = Variaveis.OperacaoEmAndamento.Controle; 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); _Controle.Bicos.ForEach(x => x.ComandoAtuar = false);
RedisService.AtualizarCampos( RedisService.AtualizarCampos(
CtxKey.DadosControle, CtxKey.DadosControle,

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

@ -68,7 +68,7 @@ namespace AgroBase.Services.Operadores
break; break;
case VisualWorkerCommandType.GetCameraFrame: 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() DadosLeitura.CameraFrames[tipo] = new OAKCameraFrameModel()
{ {
frame = comando.@params["frame"].ToString(), frame = comando.@params["frame"].ToString(),
@ -106,7 +106,7 @@ namespace AgroBase.Services.Operadores
RedisService.Publish(CmdKey.VisualWorkerRx, JsonConvert.SerializeObject(comando)); 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; DateTime Inicio = DateTime.Now;

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

@ -71,6 +71,7 @@ namespace AgroBase.Services.Operadores
} }
if (comandoMudou) if (comandoMudou)
{ {
Console.WriteLine("Comando dos bicos alterado: " + JsonConvert.SerializeObject(novoComando));
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.PulverizadorAutomatico) if (pControle.PulverizadorAutomatico)
{ {
@ -98,7 +99,7 @@ namespace AgroBase.Services.Operadores
break; break;
case WeedWorkerCommandType.GetCameraFrame: 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() DadosLeitura.CameraFrames[tipo] = new OAKCameraFrameModel()
{ {
frame = comando.@params["frame"].ToString(), frame = comando.@params["frame"].ToString(),
@ -133,7 +134,7 @@ namespace AgroBase.Services.Operadores
Saude = OperadoresModels.AtualizarSaude(CtxKey.DadosWeedWorker); 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; 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 numpy as np
import time import time
import socket
import struct
from camera_worker.raw_segformer_service import make_bgr_preview_from_raw from camera_worker.raw_segformer_service import make_bgr_preview_from_raw
from shared.enums import StatusModulo, T_Code, TipoFrameCamera from shared.utils import resize_frame
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey from shared.enums import StatusModulo, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis
from camera_worker.gal_service import GalService from camera_worker.gal_service import GalService
import subprocess import subprocess
@ -16,9 +19,10 @@ class CameraGal:
self.mostrar_log = mostrar_log self.mostrar_log = mostrar_log
self.gst_proc = None self.gst_proc = None
self.gst_WIDTH = 640 self.gst_FPS = 5
self.gst_HEIGHT = 360 self.gst_WIDTH = 480
self.gst_BIT_RATE = 500 self.gst_HEIGHT = 270
self.gst_BIT_RATE = 300
self._ultimo_envio_gst = 0.0 self._ultimo_envio_gst = 0.0
self.dispositivo = T_Code.Vzo self.dispositivo = T_Code.Vzo
@ -52,16 +56,26 @@ class CameraGal:
self.modelo = info.get("model") self.modelo = info.get("model")
self.versao = info.get("version_word") 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 # Criar processo para transmissao de video
try: try:
_cfg = ContextoGlobalRedis.get_equipamento() or {} self._sock = None
_ip = _cfg.get("base_ip") self._sock_conectado = False
_porta = _cfg.get("base_porta_ervas") self._sock_ultima_tentativa_conexao = 0.0
if _ip is not None and _porta is not None: 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}...") self.mostrar_log(f"Iniciando GStreamer para {_ip}:{_porta}...")
gst_cmd = [ gst_cmd = [
GST_LAUNCH, GST_LAUNCH,
"fdsrc", "fd=0", "fdsrc", "fd=0",
#"!", "queue", "leaky=2", "max-size-buffers=1", # dropa frames antigos
"!", "videoparse", "!", "videoparse",
f"width={self.gst_WIDTH}", f"height={self.gst_HEIGHT}", f"width={self.gst_WIDTH}", f"height={self.gst_HEIGHT}",
"format=bgr", "format=bgr",
@ -71,6 +85,7 @@ class CameraGal:
"!", f"video/x-raw,width={self.gst_WIDTH},height={self.gst_HEIGHT}", "!", f"video/x-raw,width={self.gst_WIDTH},height={self.gst_HEIGHT}",
"!", "x264enc", "tune=zerolatency", "speed-preset=ultrafast", "!", "x264enc", "tune=zerolatency", "speed-preset=ultrafast",
f"bitrate={self.gst_BIT_RATE}", "key-int-max=20", 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", "!", "rtph264pay", "config-interval=-1", "pt=96",
#"!", "h264parse", "!", "mpegtsmux", #"!", "h264parse", "!", "mpegtsmux",
"!", "udpsink", "!", "udpsink",
@ -85,6 +100,11 @@ class CameraGal:
self.gst_proc = None self.gst_proc = None
self.mostrar_log(f"Erro ao criar script de transmissao de video: {e}") 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 # Fase 2: criar pipeline e instanciar normalmente
try: try:
rgb_width = 2592 rgb_width = 2592
@ -110,15 +130,20 @@ class CameraGal:
"cm_por_px_y": cm_por_px_y, "cm_por_px_y": cm_por_px_y,
} }
_camera["parametros"] = self.parametros
self.ultima_saude["timestamp"] = time.time() self.ultima_saude["timestamp"] = time.time()
self.ultima_saude["status"] = StatusModulo.OPERANTE.value self.ultima_saude["status"] = StatusModulo.OPERANTE.value
self.iniciado = True self.iniciado = True
_camera["iniciado"] = True
_camera["iniciado_em"] = time.time()
except Exception as e: except Exception as e:
self.mostrar_log(f"Erro ao iniciar camera: {e}") self.mostrar_log(f"Erro ao iniciar camera: {e}")
pass 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): 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 from camera_worker.manager import definir_saude_camera
definir_saude_camera(self.mx_id, status, saude, motivos, self.rodando, performance, conectado, agora, self.dispositivo) 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 numpy as np
import depthai as dai import depthai as dai
import time import time
import socket
import struct
from health_worker.modulos.imu import IMUCamera from health_worker.modulos.imu import IMUCamera
from shared.enums import StatusModulo, T_Code, TipoFrameCamera from shared.utils import resize_frame
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey from shared.enums import StatusModulo, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis
import subprocess import subprocess
import cv2 import cv2
@ -17,9 +20,10 @@ class CameraOak:
self.modelo_ia_det = modelo_ia_det self.modelo_ia_det = modelo_ia_det
self.gst_proc = None self.gst_proc = None
self.gst_WIDTH = 640 self.gst_FPS = 5
self.gst_HEIGHT = 360 self.gst_WIDTH = 480
self.gst_BIT_RATE = 500 self.gst_HEIGHT = 270
self.gst_BIT_RATE = 300
self._ultimo_envio_gst = 0.0 self._ultimo_envio_gst = 0.0
self.dispositivo = T_Code.Vzo self.dispositivo = T_Code.Vzo
@ -84,10 +88,11 @@ class CameraOak:
# Criar processo para transmissao de video # Criar processo para transmissao de video
try: try:
_cfg = ContextoGlobalRedis.get_equipamento() or {} self._sock = None
_ip = _cfg.get("base_ip") self._sock_conectado = False
_porta = _cfg.get("base_porta_caminho") if self.dispositivo == T_Code.Snr else _cfg.get("base_porta_ervas") self._sock_ultima_tentativa_conexao = 0.0
if _ip is not None and _porta is not None: 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}...") self.mostrar_log(f"Iniciando GStreamer para {_ip}:{_porta}...")
gst_cmd = [ gst_cmd = [
GST_LAUNCH, GST_LAUNCH,
@ -101,6 +106,7 @@ class CameraOak:
"!", f"video/x-raw,width={self.gst_WIDTH},height={self.gst_HEIGHT}", "!", f"video/x-raw,width={self.gst_WIDTH},height={self.gst_HEIGHT}",
"!", "x264enc", "tune=zerolatency", "speed-preset=ultrafast", "!", "x264enc", "tune=zerolatency", "speed-preset=ultrafast",
f"bitrate={self.gst_BIT_RATE}", "key-int-max=20", 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", "!", "rtph264pay", "config-interval=-1", "pt=96",
#"!", "h264parse", "!", "mpegtsmux", #"!", "h264parse", "!", "mpegtsmux",
"!", "udpsink", "!", "udpsink",
@ -180,13 +186,11 @@ class CameraOak:
self.iniciado = True self.iniciado = True
_camera["iniciado"] = True _camera["iniciado"] = True
_camera["iniciado_em"] = time.time() _camera["iniciado_em"] = time.time()
except Exception as e: except Exception as e:
self.mostrar_log(f"Erro ao iniciar camera: {e}") self.mostrar_log(f"Erro ao iniciar camera: {e}")
pass pass
_camera["iniciando"] = False _camera["iniciando"] = False
ContextoGlobalRedis.set(ContextoGlobalRedis.CamKey(self.mx_id), _camera) ContextoGlobalRedis.set(ContextoGlobalRedis.CamKey(self.mx_id), _camera)
def _criar_pipeline(self): def _criar_pipeline(self):
@ -696,5 +700,83 @@ class CameraOak:
from camera_worker.manager import definir_saude_camera from camera_worker.manager import definir_saude_camera
definir_saude_camera(self.mx_id, status, saude, motivos, self.rodando, performance, conectado, agora, self.dispositivo) 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): def executar(self, envia_resposta):
t0 = time.time() t0 = time.time()
movimento_automatico = ContextoGlobalRedis.get_controle().get("movimento_automatico", False) self.definir_processador()
if movimento_automatico: if self.processador:
self.definir_processador() comando = self.processador.processar()
if self.processador: if comando is not None and envia_resposta:
comando = self.processador.processar() ContextoGlobalRedis.publicar_comando(CmdKey.ManagerWorkerTx, { "cmd": ManagerWorkerCommandType.EnviarDadosControle.value, "params": comando })
if comando is not None and envia_resposta: else:
ContextoGlobalRedis.publicar_comando(CmdKey.ManagerWorkerTx, { "cmd": ManagerWorkerCommandType.EnviarDadosControle.value, "params": comando }) mostrar_log("Processador nao definido")
else:
mostrar_log("Processador nao definido")
t1 = time.time() t1 = time.time()
latencia = t1 - t0 latencia = t1 - t0

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

@ -1,6 +1,6 @@
import time 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 shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
from manager_worker.config import mostrar_log from manager_worker.config import mostrar_log
from manager_worker.modulos.mpc import get_mpc, comando_parado from manager_worker.modulos.mpc import get_mpc, comando_parado
@ -103,7 +103,7 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
}, },
"RegrasAtivas": { "RegrasAtivas": {
"matriz_custo": False, "matriz_custo": False,
"deteccao_obstaculos": True, "deteccao_obstaculos": False,
}, },
"Equipamento": { "Equipamento": {
"largura": _equipamento.get("largura", 0.85), "largura": _equipamento.get("largura", 0.85),
@ -116,19 +116,14 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
#print(contexto["VisualWorker"]) #print(contexto["VisualWorker"])
if _tipo_controle == TiposControladorDirecional.MPC: if _tipo_controle == TiposControladorDirecional.MPC:
comando = _regras_taticas(contexto) op_modo = ModoOperacao(_operacao.get("modo", ModoOperacao.NaoDefinido.value))
if op_modo == ModoOperacao.MapaGPS:
if (comando.get("comando_definido", False)): return _comando_mapa_gps_mpc(contexto)
return _montar_comando_retorno(comando, latencia=latencia) elif op_modo == ModoOperacao.MapeamentoVisual:
return _comando_mapeamento_visual(contexto)
else: else:
op_modo = ModoOperacao(_operacao.get("modo", ModoOperacao.NaoDefinido.value)) comando = _comando_direcional_parado()
if op_modo == ModoOperacao.MapaGPS: return _montar_comando_retorno(comando, latencia=latencia)
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)
elif _tipo_controle == TiposControladorDirecional.PID: elif _tipo_controle == TiposControladorDirecional.PID:
angulo_max = _controle.get("angulo_max", 30) angulo_max = _controle.get("angulo_max", 30)
@ -157,7 +152,7 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
_cmd = { _cmd = {
"comando_definido": True, "comando_definido": True,
"enviar_comando": True, "enviar_comando": True,
"parada_necessaria": True, "parada_necessaria": False,
"erro": False, "erro": False,
"latencia": 0.0, "latencia": 0.0,
"angulo": angulo, "angulo": angulo,
@ -169,83 +164,25 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
except Exception as e: except Exception as e:
mostrar_log(f"❌ Erro ao definir comando direcional: {e}") mostrar_log(f"❌ Erro ao definir comando direcional: {e}")
def _comando_direcional_parado(definido: bool, frear: bool = False): def _comando_direcional_parado():
try: try:
angulo, tipo = comando_parado() angulo, tipo = comando_parado()
_cmd = { _cmd = {
"comando_definido": definido, "enviar_comando": True,
"enviar_comando": True, "parada_necessaria": False,
"parada_necessaria": True, "erro": False,
"erro": False, "angulo": angulo,
"latencia": 0.0, "tipo": tipo.value,
"angulo": angulo, "simulacao": [],
"tipo": tipo.value, "erro_lateral": 0,
"simulacao": [], "erro_orientacao": 0,
"frear": frear "debug_custo": {},
"candidatos_testados": 0
} }
return _cmd return _cmd
except Exception as e: except Exception as e:
mostrar_log(f"❌ Erro ao montar comando direcional parado: {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): def _comando_mapa_gps_mpc(contexto):
try: try:
mpc = get_mpc() mpc = get_mpc()
@ -278,7 +215,7 @@ def _comando_mapa_gps_mpc(contexto):
def _comando_mapeamento_visual(contexto): def _comando_mapeamento_visual(contexto):
try: try:
# ⛔ Fallback seguro se visual não operante # ⛔ Fallback seguro se visual não operante
comando = _comando_direcional_parado(True) comando = _comando_direcional_parado()
if not contexto.get("VisualWorker", {}).get("Operante", False): if not contexto.get("VisualWorker", {}).get("Operante", False):
mostrar_log("Sensores principais inativos! Impossível definir controle") mostrar_log("Sensores principais inativos! Impossível definir controle")
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"], False return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"], False
@ -311,13 +248,17 @@ def _comando_mapeamento_visual(contexto):
# 🔧 Monta comando final # 🔧 Monta comando final
comando = { comando = {
"enviar_comando": True, "enviar_comando": True,
"parada_necessaria": parada_necessaria, "parada_necessaria": parada_necessaria,
"erro": False, "erro": False,
"latencia": latencia, "latencia": latencia,
"angulo": round(angulo, 2), "angulo": round(angulo, 2),
"tipo": tipo_movimento.value, "tipo": tipo_movimento.value,
"simulacao": [] "simulacao": [],
"erro_lateral": 0,
"erro_orientacao": 0,
"debug_custo": {},
"candidatos_testados": 0
} }
return _montar_comando_retorno(comando) return _montar_comando_retorno(comando)

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

@ -1,42 +1,13 @@
import time from shared.enums import ModoOperacao, StatusModulo, StatusCarroMapa, T_Code
from shared.enums import ModoOperacao, StatusModulo, StatusOperacao, StatusCarroMapa, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
from manager_worker.config import mostrar_log from manager_worker.config import mostrar_log
from manager_worker.filtros import FiltroVelocidade 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: try:
_operacao = ContextoGlobalRedis.get_operacao() _operacao = ContextoGlobalRedis.get_operacao()
_controle = ContextoGlobalRedis.get_controle() _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)) op_modo = ModoOperacao(_operacao.get("modo", ModoOperacao.NaoDefinido.value))
_contexto = ContextoGlobalRedis.get_contexto() _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) if erro_orientacao is None: erro_orientacao = _trajetoria.get("erro_angular", 0)
pontos_fim_corredor = _trajetoria.get("CorredorAtual", {}).get("pontos_restantes", -1) pontos_fim_corredor = _trajetoria.get("CorredorAtual", {}).get("pontos_restantes", -1)
elif op_modo == ModoOperacao.MapeamentoVisual: elif op_modo == ModoOperacao.MapeamentoVisual:
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
status_carro = StatusCarroMapa(_dados_vw.get("segmentacao", {}).get("status_corredor", StatusCarroMapa.Parado.value)) 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) 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}%") #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: except Exception as e:
mostrar_log(f"Erro ao definir o comando de movimentacao: {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 vel = vel_min + (1.0 - k*s) * faixa
return round(vel, 2) return round(vel, 2)
def _montar_comando_retorno(velocidade_sp: float, frear: bool): def _montar_comando_retorno(velocidade_sp: float):
return { return {
"velocidade": velocidade_sp, "velocidade": velocidade_sp
"frear": frear
} }

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_min = parametros_mpc.get("velocidade_min", 0.4)
self.velocidade_max = parametros_mpc.get("velocidade_max", 1.9) self.velocidade_max = parametros_mpc.get("velocidade_max", 1.9)
self.horizonte = parametros_mpc.get("horizonte", 2.5) 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 # otimizacoes
self._np_bool = np.bool_ 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) # 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) vel_max_ms = getattr(self, "vel_max_m_s", 6.8/3.6)
v = float(velocidade) v = float(velocidade)
if v <= 1.05: # heurística: trata <=1.0 como v_norm #if v <= 1.05: # heurística: trata <=1.0 como v_norm
v = max(0.0, min(1.0, v)) * vel_max_ms # v = max(0.0, min(1.0, v)) * vel_max_ms
else: #else:
v = max(0.0, v) # já está em m/s # 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) ------------------------------ # --- 2) Base polinomial (suave e previsível) ------------------------------
# L_base = L0 + a*v + b*v^2 (clamp em [Lmin, Lmax]) # 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) Lmin, Lmax = self.horizonte_min, self.horizonte_max # bom para teu range (1.3–6.8 km/h)
L0, a, b = 0.60, 0.70, 0.20 L0, a, b = self.horizonte_parado, 0.70, 0.20
L = L0 + a*v + b*(v*v) L = L0 + a*v + b*(v*v)
if L < Lmin: L = Lmin if L < Lmin: L = Lmin
if L > Lmax: L = Lmax if L > Lmax: L = Lmax
@ -736,300 +743,12 @@ class ControladorMPC:
t_exec = max((now() - self.ultima_atualizacao), 1e-3) t_exec = max((now() - self.ultima_atualizacao), 1e-3)
if comando['erro']: 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 return comando
except Exception as e: except Exception as e:
mostrar_log(f"❌ Erro ao processar compute MPC: {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): def _processar_mpc_receding(self, contexto, comando_anterior):
now = time.perf_counter now = time.perf_counter
t0 = now() t0 = now()
@ -1127,6 +846,18 @@ class ControladorMPC:
# >>> escolha K dentro do [K_lb, K_cap]; pacing do orçamento vem depois # >>> escolha K dentro do [K_lb, K_cap]; pacing do orçamento vem depois
K = max(K_lb, min(K_cap, K_ALVO)) 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 para que K passos cubram S (agora garantido dentro de [DT_MIN, DT_MAX])
dt_pred = (S_ALVO / (v_sim * K)) dt_pred = (S_ALVO / (v_sim * K))
dt_pred = max(DT_MIN, min(dt_pred, DT_MAX)) dt_pred = max(DT_MIN, min(dt_pred, DT_MAX))
@ -1221,6 +952,18 @@ class ControladorMPC:
Kmax = 12 Kmax = 12
Kdyn = int(min(Kmax, max(3, tempo_util_ms // est_local))) Kdyn = int(min(Kmax, max(3, tempo_util_ms // est_local)))
Kdyn = min(Kdyn, ord_idx.size) 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") melhor_custo_local = float("inf")
@ -1274,9 +1017,20 @@ class ControladorMPC:
break break
# Filtro de diversidade angular e top-N # 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) candidatos_ativos = self._filtrar_por_margem_angular(novos_candidatos, margem_graus=2.0)
N_top = 5 if exp_budget > 6 else 3 # número alvo de trajetórias ativas
candidatos_ativos = self._selecionar_melhores(candidatos_ativos, N=N_top) #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)] candidatos_validos = [c for c in candidatos_ativos if not c.get("inicial", False)]
if len(candidatos_validos) > 0: if len(candidatos_validos) > 0:
@ -1319,7 +1073,8 @@ class ControladorMPC:
"simulacao": simulacao_latlon, "simulacao": simulacao_latlon,
"erro_lateral": erro_lateral, "erro_lateral": erro_lateral,
"erro_orientacao": erro_orientacao, "erro_orientacao": erro_orientacao,
"debug_custo": debug_custo "debug_custo": debug_custo,
"candidatos_testados": K
} }
# -------------------- Debug opcional da matriz de custo -------------------- # -------------------- Debug opcional da matriz de custo --------------------
@ -1369,7 +1124,10 @@ class ControladorMPC:
"angulo": comando_anterior.get("angulo", 0), "angulo": comando_anterior.get("angulo", 0),
"tipo": comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value), "tipo": comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value),
"simulacao": [], "simulacao": [],
"erro_latreral": 0 "erro_latreral": 0,
"erro_orientacao": 0,
"debug_custo": {},
"candidatos_testados": 0
} }
return _cmd return _cmd
@ -2149,103 +1907,6 @@ class ControladorMPC:
mostrar_log(f"❌ Erro ao filtrar por margem angular: {e}") mostrar_log(f"❌ Erro ao filtrar por margem angular: {e}")
return candidatos 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): 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. 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}") 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 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): 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) 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) 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: try:
return comando_parado() return comando_parado()
except Exception as e: 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: try:
return comando_parado() return comando_parado()
except Exception as e: 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: try:
return comando_parado() return comando_parado()
except Exception as e: 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: try:
return comando_parado() return comando_parado()
except Exception as e: 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 import time
from shared.enums import TipoMovimentoDirecional
from shared.contexto_global_redis import ContextoGlobalRedis from shared.contexto_global_redis import ContextoGlobalRedis
from manager_worker.modulos.pid import PIDAdaptativo from manager_worker.modulos.pid import PIDAdaptativo
from .base import ProcessadorBase from .base import ProcessadorBase
from manager_worker.filtros import FiltroVelocidade from manager_worker.filtros import FiltroVelocidade
from manager_worker.processadores.padroes import comando_controle 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.direcional import definir_comando as definir_comando_dir
from manager_worker.modulos.movimentacao import definir_comando as definir_comando_mov from manager_worker.modulos.movimentacao import definir_comando as definir_comando_mov
@ -28,23 +30,50 @@ class ProcessadorEmAndamento(ProcessadorBase):
try: try:
agora = time.perf_counter() agora = time.perf_counter()
envio_necessario = (agora - self.ultimo_comando_enviado) > 0.3 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): motivos_parada = verifica_controle_liberado()
self.ultimo_comando_enviado = agora if len(motivos_parada) > 0:
custos = comando_dir.get("debug_custo", {}) from manager_worker.config import mostrar_log
e_ori = custos.get(f"{comando_dir['tipo']}_{comando_dir['angulo']:.2f}", {}).get("erro_orientacao", None) or None mostrar_log(f"⛔ Controle bloqueado: {' | '.join(motivos_parada)}")
comando_mov = definir_comando_mov(comando_dir.get("parada_necessaria", False), comando_dir.get("erro", False), self.filtro_mov, erro_orientacao=e_ori)
return comando_controle( return comando_controle(
percentual_velocidade=comando_mov.get("velocidade"), percentual_velocidade=0,
frear=comando_mov.get("frear", False), frear=True,
angulo=comando_dir.get("angulo"), angulo=0,
tipo_movimento=comando_dir.get("tipo"), tipo_movimento=TipoMovimentoDirecional.RodasDianteiras.value,
simulacao=comando_dir.get("simulacao"), 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), erro=comando_dir.get("erro", False),
latencia=comando_dir.get("latencia", 0.0), latencia=comando_dir.get("latencia", 0.0),
erro_lateral=comando_dir.get("erro_lateral", 0.0), erro_lateral=comando_dir.get("erro_lateral", 0.0),
debug_custo=comando_dir.get("debug_custo", {}) debug_custo=comando_dir.get("debug_custo", {})
) )
except Exception as e: 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 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: try:
return comando_parado() return comando_parado()
except Exception as e: 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: try:
return comando_parado() return comando_parado()
except Exception as e: 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 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 = {}): 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: if erro == False:
try: try:
hb_atual = int(hb_atual) % 10 hb_novo = int(hb_novo) % 10
except: except:
hb_atual = 0 hb_novo = 0
hb_atual = (hb_atual + 1) % 10 hb_novo = (hb_novo + 1) % 10
ContextoGlobalRedis.atualizar_ctx_dict( ContextoGlobalRedis.atualizar_ctx_dict(
CtxKey.DadosControle, CtxKey.DadosControle,
@ -19,7 +21,7 @@ def comando_controle(percentual_velocidade: float, frear: bool, angulo: float, t
velocidade_sp=percentual_velocidade, velocidade_sp=percentual_velocidade,
em_freio=frear, em_freio=frear,
simulacao=simulacao, simulacao=simulacao,
heartbeat=hb_atual, heartbeat=hb_novo,
latencia=latencia, latencia=latencia,
erro_lateral=erro_lateral, erro_lateral=erro_lateral,
debug_custo=debug_custo debug_custo=debug_custo
@ -30,7 +32,7 @@ def comando_controle(percentual_velocidade: float, frear: bool, angulo: float, t
"angulo_sp": angulo, "angulo_sp": angulo,
"tipo_movimento_direcional": tipo_movimento, "tipo_movimento_direcional": tipo_movimento,
"simulacao": simulacao, "simulacao": simulacao,
"heartbeat": hb_atual, "heartbeat": hb_novo,
"latencia": latencia, "latencia": latencia,
"erro_lateral": erro_lateral, "erro_lateral": erro_lateral,
"debug_custo": debug_custo "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): if (iniciado and not status_iniciado_anterior):
cls._iniciar_operacao() 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 # Verifica se ainda está aguardando tempo inicial
tempo_aguardando = cls.get_operacao().get("tempo_aguardando", 0) tempo_aguardando = cls.get_operacao().get("tempo_aguardando", 0)
em_espera = (tempo_aguardando + _operacao.get("tempo_aguardar_inicio_operacao", 25)) > agora em_espera = (tempo_aguardando + _operacao.get("tempo_aguardar_inicio_operacao", 25)) > agora
@ -496,15 +505,6 @@ class ContextoGlobalRedis:
cls._finalizar_operacao() cls._finalizar_operacao()
return 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 # Estava parado e liberou: volta para aguardando
if status_atual == StatusOperacao.Parado: if status_atual == StatusOperacao.Parado:
cls.atualizar_ctx_dict( 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 CruvaEntreCorredores = 6
Desvio = 7 Desvio = 7
class CameraFrameType(IntEnum):
Rgb = 1
Heatmap = 2
RadarTopDown = 3
Segmentacao = 4
Debug = 5
class TipoFrameCamera(IntEnum): class TipoFrameCamera(IntEnum):
Rgb = 0 Rgb = 0
Segmentacao = 1 Segmentacao = 1
@ -188,3 +181,4 @@ class TipoFrameCamera(IntEnum):
MatrizCusto = 5 MatrizCusto = 5
Deteccoes = 6 Deteccoes = 6
Corredor = 7 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}") mostrar_log(f"Erro ao consultar velocidade atual: {e}")
finally: finally:
return velocidade 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 from concurrent.futures import ThreadPoolExecutor
import cv2 import cv2
import cupy as cp
import numpy as np import numpy as np
from visual_worker.utils import calcular_threshold_anomalias, converter_valores_numpy, gerar_heatmap from visual_worker.utils import 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.processamento.segmentacao_semantica import ClassesSegmentacao, SegmentacaoManager from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao, SegmentacaoManager
from camera_worker.segformer_runner import SegformerNavRunner from camera_worker.segformer_runner import SegformerNavRunner
from visual_worker.processamento.costmap_fuser import CostmapFuser, unpack_snapshot from visual_worker.processamento.costmap_fuser import CostmapFuser, unpack_snapshot
from shared.enums import StatusModulo, T_Code, TipoFrameCamera 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.utils import converter_mask_ids_para_bgr, get_velocidade_atual_ms
from shared.gps_handler import GPSHandler
from camera_worker.camera_oak import CameraOak from camera_worker.camera_oak import CameraOak
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
@ -120,7 +115,7 @@ class CameraManager:
self._analisando_deteccao = False self._analisando_deteccao = False
self._iniciar_loop_analise_continua(15.0) 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.iniciando = False
self.atualizar_saude_camera() self.atualizar_saude_camera()
@ -299,10 +294,13 @@ class CameraManager:
continue continue
t0 = time.time() t0 = time.time()
try: try:
_frame_type = (TipoFrameCamera)((ContextoGlobalRedis.get_camera(self.mx_id) or {}).get("frame_type", TipoFrameCamera.Rgb.value)) _camera = (ContextoGlobalRedis.get_camera(self.mx_id) or {})
_frame = self.get_selected_frame(_frame_type) _stream_on = _camera.get("streaming", False)
if _frame is not None: if (_stream_on):
self.camera.enviar_frame_stream(_frame) _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 from visual_worker.config import load_seg_config, load_det_config
seg_config = load_seg_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() get_camera_manager().atualizar_saude_camera()
elif acao == VisualWorkerCommandType.GetCameraFrame: elif acao == VisualWorkerCommandType.GetCameraFrame:
tipo = TipoFrameCamera(dados.get("params", TipoFrameCamera.Rgb.value)) tipo = TipoFrameCamera(dados.get("params", TipoFrameCamera.Rgb.value))
frame, ts = get_camera_manager().get_selected_frame(tipo) frame = get_camera_manager().get_selected_frame(tipo)
base64_img = encode_image_base64(frame) if frame is not None:
if base64_img is not None: base64_img = encode_image_base64(frame)
resposta = { if base64_img is not None:
"frame": base64_img, resposta = {
"timestamp": ts, "frame": base64_img,
"tipo": tipo.value "timestamp": time.time(),
} "tipo": tipo.value
ContextoGlobalRedis.publicar_comando(CmdKey.VisualWorkerTx, { "cmd": VisualWorkerCommandType.GetCameraFrame.value, "params": resposta }) }
ContextoGlobalRedis.publicar_comando(CmdKey.VisualWorkerTx, { "cmd": VisualWorkerCommandType.GetCameraFrame.value, "params": resposta })
elif acao == VisualWorkerCommandType.SaveCameraFrames: elif acao == VisualWorkerCommandType.SaveCameraFrames:
nome = dados.get("params", {}).get("nome", "") nome = dados.get("params", {}).get("nome", "")
pasta = dados.get("params", {}).get("caminho", "frames_salvos") 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) # 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.max_len = max_len or int(self.janela_s * fps_esperado * 1.5)
self._status_hist = deque(maxlen=self.max_len) self._status_hist = deque(maxlen=self.max_len)
self._status_final_hist = deque(maxlen=2)
def segmentar(self, predictions): def segmentar(self, predictions):
@ -190,7 +191,7 @@ class SegmentacaoManager:
lat_out = round(float(self._ema_lat), 3) lat_out = round(float(self._ema_lat), 3)
mask_nav = (self.predictions == ClassesSegmentacao.NAVEGAVEL.value) 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 { return {
"timestamp": time.time(), "timestamp": time.time(),
@ -199,6 +200,7 @@ class SegmentacaoManager:
"erro_angular": ang_out, "erro_angular": ang_out,
"erro_lateral_pct": lat_out, "erro_lateral_pct": lat_out,
"status_corredor": status_final.value, "status_corredor": status_final.value,
"status_corredor_anterior": status_before.value,
"centros_corredor": centros, "centros_corredor": centros,
"larguras_px": larguras, "larguras_px": larguras,
"confianca": round(float(score), 3), "confianca": round(float(score), 3),
@ -446,8 +448,10 @@ class SegmentacaoManager:
# Histerese temporal: mantém teu esquema de histórico # Histerese temporal: mantém teu esquema de histórico
self._status_hist.append((status_now, self._now())) self._status_hist.append((status_now, self._now()))
status_final = self._maioria_ultimos() 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 @staticmethod
def _now(): 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 time
import cv2 import cv2
import numpy as np
from camera_worker.raw_segformer_service import RawSegformerService from camera_worker.raw_segformer_service import RawSegformerService
from shared.enums import StatusModulo, StatusOperacao, T_Code, TipoFrameCamera, WeedWorkerCommandType 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 camera_worker.camera_gal import CameraGal
from shared.contexto_global_redis import CmdKey, ContextoGlobalRedis, CtxKey from shared.contexto_global_redis import CmdKey, ContextoGlobalRedis, CtxKey
from weed_worker.weed_detector import WeedDetector from weed_worker.weed_detector import WeedDetector
@ -28,6 +28,7 @@ class CameraManager:
self._ts_segmentacao_anterior = 0 self._ts_segmentacao_anterior = 0
self._ts_ultima_analise = 0 self._ts_ultima_analise = 0
self._ultimo_predictions = None self._ultimo_predictions = None
self._ultimo_raw_base = None
self._ultimo_raw_input = None self._ultimo_raw_input = None
self._ultimo_controle = None self._ultimo_controle = None
self._analisando_segmentacao = False self._analisando_segmentacao = False
@ -67,6 +68,7 @@ class CameraManager:
self._ultima_analise = {} self._ultima_analise = {}
self._ultimo_rgb_frame = None self._ultimo_rgb_frame = None
self._ultimo_predictions = None self._ultimo_predictions = None
self._ultimo_raw_base = None
self._ultimo_raw_input = None self._ultimo_raw_input = None
self._ultimo_controle = None self._ultimo_controle = None
self._analisando_segmentacao = False self._analisando_segmentacao = False
@ -77,7 +79,7 @@ class CameraManager:
self.weed_detector = WeedDetector(color_map=colormap_rgb, classes=classes) self.weed_detector = WeedDetector(color_map=colormap_rgb, classes=classes)
self._iniciar_loop_analise_continua(20.0) 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.iniciando = False
self.atualizar_saude_camera() self.atualizar_saude_camera()
@ -127,12 +129,11 @@ class CameraManager:
t2 = time.time() t2 = time.time()
predictions = self.model_svc.infer_raw(raw_input) predictions = self.model_svc.infer_raw(raw_input)
ts = time.time() ts = time.time()
res = { res = {}
"raw_input": raw_input
}
self._ultimo_predictions = predictions self._ultimo_raw_base = raw4_base
self._ultimo_raw_input = raw_input 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") #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): def get_selected_frame(self, _frame_type: TipoFrameCamera):
_frame = None _frame = None
if (_frame_type == TipoFrameCamera.Rgb): if (_frame_type in [TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao, TipoFrameCamera.Overlay, TipoFrameCamera.Debug]):
_frame = self._ultimo_rgb_frame rgb_frame, seg_frame, overlay_frame, _, _ = self.model_svc.preview_infer_cached(self._ultimo_raw_input, self._ultimo_predictions, alpha=0.5)
elif (_frame_type in [TipoFrameCamera.Segmentacao, TipoFrameCamera.Overlay]): if _frame_type == TipoFrameCamera.Rgb:
if _frame_type == TipoFrameCamera.Segmentacao: _alpha = 1.0 _frame = rgb_frame
elif _frame_type == TipoFrameCamera.Overlay: _alpha = 0.5 elif _frame_type == TipoFrameCamera.Segmentacao:
_, _, _frame, _, _ = self.model_svc.preview_infer_cached(self._ultimo_raw_input, self._ultimo_predictions, alpha=_alpha) _frame = seg_frame
elif _frame_type == TipoFrameCamera.Debug: elif _frame_type == TipoFrameCamera.Overlay:
self.weed_detector._mostrar_debug_bicos_overlay(_frame, [], self._ultimo_controle, show=False) _frame = overlay_frame
_frame = self.weed_detector._dbg_img 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 return _frame
def _iniciar_loop_analise_continua(self, freq): def _iniciar_loop_analise_continua(self, freq):
@ -192,16 +197,20 @@ class CameraManager:
continue continue
t0 = time.time() t0 = time.time()
try: try:
_frame_type = (TipoFrameCamera)((ContextoGlobalRedis.get_camera(self.mx_id) or {}).get("frame_type", TipoFrameCamera.Rgb.value)) _camera = (ContextoGlobalRedis.get_camera(self.mx_id) or {})
_frame = self.get_selected_frame(_frame_type) _stream_on = _camera.get("streaming", False)
if _frame is not None: if (_stream_on):
self.camera.enviar_frame_stream(_frame) _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 from weed_worker.config import load_seg_config
config = load_seg_config() config = load_seg_config()
if (config.get("debug_visual")): if (config.get("debug_visual")):
_frame = self.get_selected_frame(TipoFrameCamera.Overlay) frame_dbg = self.get_selected_frame(TipoFrameCamera.Overlay)
self.weed_detector._mostrar_debug_bicos_overlay(_frame, [], self._ultimo_controle, config, show=True) 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: except Exception as e:
self.mostrar_log(f"Erro no loop de stream: {e}") self.mostrar_log(f"Erro no loop de stream: {e}")
finally: finally:
@ -215,7 +224,9 @@ class CameraManager:
try: try:
t0 = time.time() t0 = time.time()
predictions, ts, res = self.get_segmentation_predictions() 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) fps = 1.0 / (ts - self._ts_segmentacao_anterior)
self._ts_segmentacao_anterior = ts self._ts_segmentacao_anterior = ts
if predictions is not None: if predictions is not None:
@ -281,9 +292,13 @@ class CameraManager:
for _frame_type in tipos: for _frame_type in tipos:
_frame = self.get_selected_frame(_frame_type) _frame = self.get_selected_frame(_frame_type)
if _frame is not None and _frame.size > 0: if _frame is not None and _frame.size > 0:
nome_frame = f"{frame_name}_{(TipoFrameCamera(_frame_type)).name}.jpeg" nome_frame = f"{frame_name}_{(TipoFrameCamera(_frame_type)).name}"
caminho = os.path.join(pasta, nome_frame) if _frame_type == TipoFrameCamera.Raw4:
cv2.imwrite(caminho, _frame) 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) frames_salvos.append(caminho)
return frames_salvos 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", {}) dadosAtu = ContextoGlobalRedis.get_operacao().get("Atu", {})
contexto = ContextoGlobalRedis.get_contexto() 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["velocidade_robo"] = contexto.get("Gerais", {}).get("velocidade_ms", 0.0)
_CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ervas") _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() get_camera_manager().atualizar_saude_camera()
elif acao == WeedWorkerCommandType.GetCameraFrame: elif acao == WeedWorkerCommandType.GetCameraFrame:
tipo = TipoFrameCamera(dados.get("params", TipoFrameCamera.Rgb.value)) tipo = TipoFrameCamera(dados.get("params", TipoFrameCamera.Rgb.value))
frame, ts = get_camera_manager().get_selected_frame(tipo) frame = get_camera_manager().get_selected_frame(tipo)
base64_img = encode_image_base64(frame) if frame is not None:
if base64_img is not None: base64_img = encode_image_base64(frame)
resposta = { if base64_img is not None:
"frame": base64_img, resposta = {
"timestamp": ts, "frame": base64_img,
"tipo": tipo.value "timestamp": time.time(),
} "tipo": tipo.value
ContextoGlobalRedis.publicar_comando(CmdKey.WeedWorkerTx, { "cmd": WeedWorkerCommandType.GetCameraFrame.value, "params": resposta }) }
ContextoGlobalRedis.publicar_comando(CmdKey.WeedWorkerTx, { "cmd": WeedWorkerCommandType.GetCameraFrame.value, "params": resposta })
elif acao == WeedWorkerCommandType.SaveCameraFrames: elif acao == WeedWorkerCommandType.SaveCameraFrames:
nome = dados.get("params", {}).get("nome", "") nome = dados.get("params", {}).get("nome", "")
pasta = dados.get("params", {}).get("caminho", "frames_salvos") 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.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.predictions = None
self.dados_visuais = {} self.dados_visuais = {}
self._dbg_img = None
self._dbg_img_shape = (706, 560) self._dbg_img_shape = (706, 560)
self._mostrar_debug = True self._mostrar_debug = True
@ -451,7 +452,7 @@ class WeedDetector:
faixa_atuacao = float(config.get("area_atuacao_bicos", 0.3)) faixa_atuacao = float(config.get("area_atuacao_bicos", 0.3))
# --- alocação única dos buffers de debug --- # --- 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) # _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._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) 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"> <Border x:Name="brdCameraFrontal" BorderBrush="Black" BorderThickness="1" CornerRadius="8" Margin="1453,0,7,735">
<Canvas> <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"/> <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"/> <!--<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" /> <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> </Canvas>
</Border> </Border>
<Border x:Name="brdCameraTraseira" BorderBrush="Black" BorderThickness="1" CornerRadius="8" Margin="1453,281,7,491"> <Border x:Name="brdCameraTraseira" BorderBrush="Black" BorderThickness="1" CornerRadius="8" Margin="1453,281,7,491">
<Canvas> <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"/> <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"/> <!--<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" /> <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> </Canvas>
</Border> </Border>

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

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

6
Firmware/Modulos/BicoPulverizadorModel.h
View File

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