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

revisado fluxo de trajetoria dinamica

This commit is contained in:
Diego Freitas 2024-08-16 10:39:43 -03:00
parent 81cc9cc5a3
commit 8464ac33f2
19 changed files with 1161 additions and 595 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
AgroBase/.vs/AgroBase/FileContentIndex/810aa3b9-5fca-48b5-b9af-b4bfd2a55675.vsidx → AgroBase/.vs/AgroBase/FileContentIndex/ca2a373d-c163-4b46-bb19-2a6913875e42.vsidx
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/FileContentIndex/d1f5893a-1225-4fb6-9ff7-aeeeeb086278.vsidx
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/FileContentIndex/e81c5f44-0d14-4d98-96f0-633bd7ff25e0.vsidx Normal file
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/v17/.suo
View File

Binary file not shown.

211
AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.Designer.cs generated
View File

@ -68,16 +68,27 @@
this.pnlOrientacaoTrajeto = new System.Windows.Forms.Panel();
this.pnlSonar = new System.Windows.Forms.Panel();
this.picSonar = new System.Windows.Forms.PictureBox();
this.pnlZoomMapa = new System.Windows.Forms.Panel();
this.label2 = new System.Windows.Forms.Label();
this.label3 = new System.Windows.Forms.Label();
this.label4 = new System.Windows.Forms.Label();
this.lblRua = new System.Windows.Forms.Label();
this.lblMargem = new System.Windows.Forms.Label();
this.lblProximoPonto = new System.Windows.Forms.Label();
this.lblAproximando = new System.Windows.Forms.Label();
this.lblDistProx = new System.Windows.Forms.Label();
this.lblDistAnt = new System.Windows.Forms.Label();
this.lblStatusOperacao = new System.Windows.Forms.Label();
this.pnlOpcoes.SuspendLayout();
this.gpbSonar.SuspendLayout();
this.gpbGPS.SuspendLayout();
this.pnlMapa.SuspendLayout();
this.pnlSonar.SuspendLayout();
((System.ComponentModel.ISupportInitialize)(this.picSonar)).BeginInit();
this.SuspendLayout();
//
// pnlOpcoes
//
this.pnlOpcoes.Controls.Add(this.lblStatusOperacao);
this.pnlOpcoes.Controls.Add(this.lblDistanciaLateral);
this.pnlOpcoes.Controls.Add(this.btnAtualizarLeitura);
this.pnlOpcoes.Controls.Add(this.lblDistanciaFinal);
@ -89,10 +100,10 @@
this.pnlOpcoes.Controls.Add(this.lblStatus);
this.pnlOpcoes.Controls.Add(this.gpbGPS);
this.pnlOpcoes.Dock = System.Windows.Forms.DockStyle.Bottom;
this.pnlOpcoes.Location = new System.Drawing.Point(0, 479);
this.pnlOpcoes.Location = new System.Drawing.Point(0, 499);
this.pnlOpcoes.Margin = new System.Windows.Forms.Padding(2);
this.pnlOpcoes.Name = "pnlOpcoes";
this.pnlOpcoes.Size = new System.Drawing.Size(1028, 100);
this.pnlOpcoes.Size = new System.Drawing.Size(1379, 100);
this.pnlOpcoes.TabIndex = 0;
//
// lblDistanciaLateral
@ -108,7 +119,7 @@
//
// btnAtualizarLeitura
//
this.btnAtualizarLeitura.Location = new System.Drawing.Point(134, 30);
this.btnAtualizarLeitura.Location = new System.Drawing.Point(134, 5);
this.btnAtualizarLeitura.Margin = new System.Windows.Forms.Padding(2);
this.btnAtualizarLeitura.Name = "btnAtualizarLeitura";
this.btnAtualizarLeitura.Size = new System.Drawing.Size(121, 20);
@ -289,12 +300,12 @@
//
this.lblStatus.AutoSize = true;
this.lblStatus.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblStatus.Location = new System.Drawing.Point(154, 69);
this.lblStatus.Location = new System.Drawing.Point(154, 70);
this.lblStatus.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblStatus.Name = "lblStatus";
this.lblStatus.Size = new System.Drawing.Size(101, 13);
this.lblStatus.Size = new System.Drawing.Size(96, 13);
this.lblStatus.TabIndex = 15;
this.lblStatus.Text = "Status: Aguardando";
this.lblStatus.Text = "Carro: Aguardando";
//
// gpbGPS
//
@ -444,7 +455,7 @@
this.txtLongitude.Name = "txtLongitude";
this.txtLongitude.Size = new System.Drawing.Size(135, 20);
this.txtLongitude.TabIndex = 15;
this.txtLongitude.Text = "-47.3815675";
this.txtLongitude.Text = "-47,1956147249173";
this.txtLongitude.TextAlign = System.Windows.Forms.HorizontalAlignment.Center;
//
// txtLatitude
@ -454,7 +465,7 @@
this.txtLatitude.Name = "txtLatitude";
this.txtLatitude.Size = new System.Drawing.Size(135, 20);
this.txtLatitude.TabIndex = 14;
this.txtLatitude.Text = "-22.2130615";
this.txtLatitude.Text = "-22,0787517060124";
this.txtLatitude.TextAlign = System.Windows.Forms.HorizontalAlignment.Center;
//
// btnIniciarGPS
@ -470,25 +481,20 @@
//
// pnlMapa
//
this.pnlMapa.Anchor = ((System.Windows.Forms.AnchorStyles)((((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Bottom)
| System.Windows.Forms.AnchorStyles.Left)
| System.Windows.Forms.AnchorStyles.Right)));
this.pnlMapa.Controls.Add(this.pnlAnguloControle);
this.pnlMapa.Controls.Add(this.pnlOrientacaoCarro);
this.pnlMapa.Controls.Add(this.pnlOrientacaoTrajeto);
this.pnlMapa.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Bottom)
| System.Windows.Forms.AnchorStyles.Left)));
this.pnlMapa.Location = new System.Drawing.Point(9, 10);
this.pnlMapa.Margin = new System.Windows.Forms.Padding(2);
this.pnlMapa.Name = "pnlMapa";
this.pnlMapa.Size = new System.Drawing.Size(506, 465);
this.pnlMapa.Size = new System.Drawing.Size(506, 485);
this.pnlMapa.TabIndex = 1;
//
// pnlAnguloControle
//
this.pnlAnguloControle.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Right)));
this.pnlAnguloControle.BackColor = System.Drawing.Color.White;
this.pnlAnguloControle.BackgroundImage = global::AgroBase.Properties.Resources.rosa_dos_ventos;
this.pnlAnguloControle.BackgroundImage = global::AgroBase.Properties.Resources.transferidor;
this.pnlAnguloControle.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom;
this.pnlAnguloControle.Location = new System.Drawing.Point(425, 178);
this.pnlAnguloControle.Location = new System.Drawing.Point(787, 11);
this.pnlAnguloControle.Margin = new System.Windows.Forms.Padding(2);
this.pnlAnguloControle.Name = "pnlAnguloControle";
this.pnlAnguloControle.Size = new System.Drawing.Size(78, 84);
@ -497,11 +503,10 @@
//
// pnlOrientacaoCarro
//
this.pnlOrientacaoCarro.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Right)));
this.pnlOrientacaoCarro.BackColor = System.Drawing.Color.White;
this.pnlOrientacaoCarro.BackgroundImage = global::AgroBase.Properties.Resources.rosa_dos_ventos;
this.pnlOrientacaoCarro.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom;
this.pnlOrientacaoCarro.Location = new System.Drawing.Point(425, 90);
this.pnlOrientacaoCarro.Location = new System.Drawing.Point(658, 11);
this.pnlOrientacaoCarro.Margin = new System.Windows.Forms.Padding(2);
this.pnlOrientacaoCarro.Name = "pnlOrientacaoCarro";
this.pnlOrientacaoCarro.Size = new System.Drawing.Size(78, 84);
@ -510,11 +515,10 @@
//
// pnlOrientacaoTrajeto
//
this.pnlOrientacaoTrajeto.Anchor = ((System.Windows.Forms.AnchorStyles)((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Right)));
this.pnlOrientacaoTrajeto.BackColor = System.Drawing.Color.White;
this.pnlOrientacaoTrajeto.BackgroundImage = global::AgroBase.Properties.Resources.rosa_dos_ventos;
this.pnlOrientacaoTrajeto.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom;
this.pnlOrientacaoTrajeto.Location = new System.Drawing.Point(425, 2);
this.pnlOrientacaoTrajeto.Location = new System.Drawing.Point(519, 11);
this.pnlOrientacaoTrajeto.Margin = new System.Windows.Forms.Padding(2);
this.pnlOrientacaoTrajeto.Name = "pnlOrientacaoTrajeto";
this.pnlOrientacaoTrajeto.Size = new System.Drawing.Size(78, 84);
@ -523,13 +527,14 @@
//
// pnlSonar
//
this.pnlSonar.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Bottom)
this.pnlSonar.Anchor = ((System.Windows.Forms.AnchorStyles)((((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Bottom)
| System.Windows.Forms.AnchorStyles.Left)
| System.Windows.Forms.AnchorStyles.Right)));
this.pnlSonar.Controls.Add(this.picSonar);
this.pnlSonar.Location = new System.Drawing.Point(519, 10);
this.pnlSonar.Location = new System.Drawing.Point(870, 10);
this.pnlSonar.Margin = new System.Windows.Forms.Padding(2);
this.pnlSonar.Name = "pnlSonar";
this.pnlSonar.Size = new System.Drawing.Size(500, 465);
this.pnlSonar.Size = new System.Drawing.Size(500, 485);
this.pnlSonar.TabIndex = 2;
//
// picSonar
@ -538,17 +543,154 @@
this.picSonar.Location = new System.Drawing.Point(0, 0);
this.picSonar.Margin = new System.Windows.Forms.Padding(2);
this.picSonar.Name = "picSonar";
this.picSonar.Size = new System.Drawing.Size(500, 465);
this.picSonar.Size = new System.Drawing.Size(500, 485);
this.picSonar.SizeMode = System.Windows.Forms.PictureBoxSizeMode.Zoom;
this.picSonar.TabIndex = 0;
this.picSonar.TabStop = false;
//
// pnlZoomMapa
//
this.pnlZoomMapa.BackColor = System.Drawing.Color.White;
this.pnlZoomMapa.Location = new System.Drawing.Point(520, 113);
this.pnlZoomMapa.Name = "pnlZoomMapa";
this.pnlZoomMapa.Size = new System.Drawing.Size(345, 346);
this.pnlZoomMapa.TabIndex = 15;
//
// label2
//
this.label2.AutoSize = true;
this.label2.Location = new System.Drawing.Point(535, 97);
this.label2.Name = "label2";
this.label2.Size = new System.Drawing.Size(48, 13);
this.label2.TabIndex = 16;
this.label2.Text = "Caminho";
//
// label3
//
this.label3.AutoSize = true;
this.label3.Location = new System.Drawing.Point(681, 97);
this.label3.Name = "label3";
this.label3.Size = new System.Drawing.Size(32, 13);
this.label3.TabIndex = 17;
this.label3.Text = "Carro";
//
// label4
//
this.label4.AutoSize = true;
this.label4.Location = new System.Drawing.Point(805, 97);
this.label4.Name = "label4";
this.label4.Size = new System.Drawing.Size(46, 13);
this.label4.TabIndex = 18;
this.label4.Text = "Controle";
//
// lblRua
//
this.lblRua.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
| System.Windows.Forms.AnchorStyles.Right)));
this.lblRua.AutoSize = true;
this.lblRua.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblRua.Location = new System.Drawing.Point(519, 462);
this.lblRua.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblRua.Name = "lblRua";
this.lblRua.Size = new System.Drawing.Size(54, 13);
this.lblRua.TabIndex = 19;
this.lblRua.Text = "Rua: Fora";
//
// lblMargem
//
this.lblMargem.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
| System.Windows.Forms.AnchorStyles.Right)));
this.lblMargem.AutoSize = true;
this.lblMargem.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblMargem.Location = new System.Drawing.Point(595, 462);
this.lblMargem.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblMargem.Name = "lblMargem";
this.lblMargem.Size = new System.Drawing.Size(71, 13);
this.lblMargem.TabIndex = 20;
this.lblMargem.Text = "Margem: Não";
//
// lblProximoPonto
//
this.lblProximoPonto.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
| System.Windows.Forms.AnchorStyles.Right)));
this.lblProximoPonto.AutoSize = true;
this.lblProximoPonto.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblProximoPonto.Location = new System.Drawing.Point(691, 462);
this.lblProximoPonto.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblProximoPonto.Name = "lblProximoPonto";
this.lblProximoPonto.Size = new System.Drawing.Size(87, 13);
this.lblProximoPonto.TabIndex = 21;
this.lblProximoPonto.Text = "Próximo Ponto: 0";
//
// lblAproximando
//
this.lblAproximando.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
| System.Windows.Forms.AnchorStyles.Right)));
this.lblAproximando.AutoSize = true;
this.lblAproximando.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblAproximando.Location = new System.Drawing.Point(793, 462);
this.lblAproximando.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblAproximando.Name = "lblAproximando";
this.lblAproximando.Size = new System.Drawing.Size(68, 13);
this.lblAproximando.TabIndex = 22;
this.lblAproximando.Text = "Aproximando";
//
// lblDistProx
//
this.lblDistProx.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
| System.Windows.Forms.AnchorStyles.Right)));
this.lblDistProx.AutoSize = true;
this.lblDistProx.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblDistProx.Location = new System.Drawing.Point(519, 482);
this.lblDistProx.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistProx.Name = "lblDistProx";
this.lblDistProx.Size = new System.Drawing.Size(160, 13);
this.lblDistProx.TabIndex = 23;
this.lblDistProx.Text = "Distância Próximo Ponto: 0,00 m";
//
// lblDistAnt
//
this.lblDistAnt.Anchor = ((System.Windows.Forms.AnchorStyles)(((System.Windows.Forms.AnchorStyles.Top | System.Windows.Forms.AnchorStyles.Left)
| System.Windows.Forms.AnchorStyles.Right)));
this.lblDistAnt.AutoSize = true;
this.lblDistAnt.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblDistAnt.Location = new System.Drawing.Point(691, 482);
this.lblDistAnt.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistAnt.Name = "lblDistAnt";
this.lblDistAnt.Size = new System.Drawing.Size(159, 13);
this.lblDistAnt.TabIndex = 24;
this.lblDistAnt.Text = "Distância Ponto Anterior: 0,00 m";
//
// lblStatusOperacao
//
this.lblStatusOperacao.AutoSize = true;
this.lblStatusOperacao.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblStatusOperacao.Location = new System.Drawing.Point(154, 35);
this.lblStatusOperacao.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblStatusOperacao.Name = "lblStatusOperacao";
this.lblStatusOperacao.Size = new System.Drawing.Size(133, 13);
this.lblStatusOperacao.TabIndex = 24;
this.lblStatusOperacao.Text = "Operação: Parametrizando";
//
// frmSimulacaoMapaGPS
//
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
this.ClientSize = new System.Drawing.Size(1028, 579);
this.ClientSize = new System.Drawing.Size(1379, 599);
this.Controls.Add(this.lblDistAnt);
this.Controls.Add(this.lblDistProx);
this.Controls.Add(this.lblAproximando);
this.Controls.Add(this.lblProximoPonto);
this.Controls.Add(this.lblMargem);
this.Controls.Add(this.lblRua);
this.Controls.Add(this.label4);
this.Controls.Add(this.label3);
this.Controls.Add(this.label2);
this.Controls.Add(this.pnlAnguloControle);
this.Controls.Add(this.pnlZoomMapa);
this.Controls.Add(this.pnlOrientacaoCarro);
this.Controls.Add(this.pnlSonar);
this.Controls.Add(this.pnlOrientacaoTrajeto);
this.Controls.Add(this.pnlMapa);
this.Controls.Add(this.pnlOpcoes);
this.Margin = new System.Windows.Forms.Padding(2);
@ -562,10 +704,10 @@
this.gpbSonar.PerformLayout();
this.gpbGPS.ResumeLayout(false);
this.gpbGPS.PerformLayout();
this.pnlMapa.ResumeLayout(false);
this.pnlSonar.ResumeLayout(false);
((System.ComponentModel.ISupportInitialize)(this.picSonar)).EndInit();
this.ResumeLayout(false);
this.PerformLayout();
}
@ -611,5 +753,16 @@
private System.Windows.Forms.TextBox txtAnguloControle;
private System.Windows.Forms.Panel pnlOrientacaoCarro;
private System.Windows.Forms.Panel pnlAnguloControle;
private System.Windows.Forms.Panel pnlZoomMapa;
private System.Windows.Forms.Label label2;
private System.Windows.Forms.Label label3;
private System.Windows.Forms.Label label4;
private System.Windows.Forms.Label lblRua;
private System.Windows.Forms.Label lblMargem;
private System.Windows.Forms.Label lblProximoPonto;
private System.Windows.Forms.Label lblAproximando;
private System.Windows.Forms.Label lblDistProx;
private System.Windows.Forms.Label lblDistAnt;
private System.Windows.Forms.Label lblStatusOperacao;
}
}

288
AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs
View File

@ -1,6 +1,7 @@
using AgroBase.Models;
using AgroBase.Services;
using CefSharp;
using SharpDX.Mathematics.Interop;
using System;
using System.Collections.Generic;
using System.ComponentModel;
@ -12,6 +13,7 @@ using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using static AgroBase.Models.Enums;
using static Assimp.Metadata;
namespace AgroBase.Forms
{
@ -19,6 +21,11 @@ namespace AgroBase.Forms
{
Timer tmrSonar;
StatusCarroMapa StatusCarro = StatusCarroMapa.Parado;
private double _zoom = 1000000.0; // Nível de zoom
private const double ZoomFactor = 1.1; // Fator de zoom
private bool _isDragging = false; // Controle de arrasto
private Point _startPoint = new Point(); // Posição inicial do mouse
private Point _mapOffset = new Point(); // Deslocamento do mapa
public frmSimulacaoMapaGPS()
{
@ -31,29 +38,280 @@ namespace AgroBase.Forms
/*tmrSonar = new Timer() { Interval = 100 };
tmrSonar.Tick += tmrSonar_Tick;
tmrSonar.Start();*/
this.pnlZoomMapa.Paint += PnlZoomMapa_Paint;
this.pnlZoomMapa.MouseWheel += PnlZoomMapa_MouseWheel;
this.pnlZoomMapa.MouseDown += PnlZoomMapa_MouseDown;
this.pnlZoomMapa.MouseMove += PnlZoomMapa_MouseMove;
this.pnlZoomMapa.MouseUp += PnlZoomMapa_MouseUp;
// Ativa o double buffering
this.DoubleBuffered = true;
this.SetStyle(ControlStyles.AllPaintingInWmPaint, true);
this.SetStyle(ControlStyles.UserPaint, true);
this.SetStyle(ControlStyles.OptimizedDoubleBuffer, true);
pnlZoomMapa.GetType().GetMethod("SetStyle", System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic).Invoke(pnlZoomMapa, new object[] { ControlStyles.UserPaint | ControlStyles.AllPaintingInWmPaint | ControlStyles.OptimizedDoubleBuffer, true });
}
private void PnlZoomMapa_MouseUp(object sender, MouseEventArgs e)
{
if (e.Button == MouseButtons.Left)
{
_isDragging = false; // Termina o arrasto
Cursor = Cursors.Default;
}
}
private void PnlZoomMapa_MouseMove(object sender, MouseEventArgs e)
{
Panel panel1 = (Panel)sender;
if (_isDragging)
{
// Calcula o deslocamento do mapa com base no movimento do mouse
_mapOffset.X += e.X - _startPoint.X;
_mapOffset.Y += e.Y - _startPoint.Y;
// Atualiza a posição inicial do mouse
_startPoint = e.Location;
// Redesenha o panel para refletir o novo deslocamento
panel1.Invalidate();
Cursor = Cursors.Cross;
}
}
private void PnlZoomMapa_MouseDown(object sender, MouseEventArgs e)
{
if (e.Button == MouseButtons.Left)
{
_isDragging = true;
_startPoint = e.Location; // Salva a posição inicial do mouse
Cursor = Cursors.Hand;
}
}
private void PnlZoomMapa_MouseWheel(object sender, MouseEventArgs e)
{
Panel panel1 = (Panel)sender;
if (e.Delta > 0)
{
_zoom *= ZoomFactor; // Aumenta o zoom
}
else if (e.Delta < 0)
{
_zoom /= ZoomFactor; // Diminui o zoom
}
panel1.Invalidate();
}
private void PnlZoomMapa_Paint(object sender, PaintEventArgs e)
{
var Mapa = Variaveis.OperacaoEmAndamento.Mapa;
if (!Mapa.TrajetoriaProjetada.Any())
{
return;
}
Panel panel1 = (Panel)sender;
var _trajetoriaRuaAtual = Mapa.TrajetoriaProjetada.FirstOrDefault();
Graphics g = e.Graphics;
// Defina as dimensões do panel
int width = panel1.Width;
int height = panel1.Height;
// Coordenadas centrais (onde o robô estará), ajustadas pelo deslocamento do mapa
float centerX = width / 2 + _mapOffset.X;
float centerY = height / 2 + _mapOffset.Y;
// Desenha os pontos da trajetória projetada
DrawTrajectory(g, Mapa.TrajetoriaDinamica, centerX, centerY, Color.Purple, Pens.MediumPurple);
// Desenha os pontos da trajetória planejada da rua atual
DrawTrajectory(g, _trajetoriaRuaAtual, centerX, centerY, Color.Green, Pens.Blue);
// Desenha os pontos da trajetória percorrida
DrawTrajectory(g, Variaveis.OperacaoEmAndamento.GPSTrajetoria, centerX, centerY, Color.Orange, Pens.OrangeRed);
// Desenha a posição atual do robô (ponto vermelho)
float PxToCm = DrawRobot(g, centerX, centerY, Color.Red, Color.Red, (float)VariaveisEquipamento.LarguraEsquerda, (float)VariaveisEquipamento.LarguraDireita, (float)VariaveisEquipamento.LarguraEsquerda, (float)VariaveisEquipamento.LarguraDireita, (float)Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro);
if (KinectService.Iniciado && Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.StatusDetecao)
{
var obstaculo = KinectService.Leitura.ObstaculoCritico;
DrawObstacle(g, centerX, centerY, Color.Cyan, Color.Cyan, (float)Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho - (float)obstaculo.AnguloParaDesvio, obstaculo, PxToCm);
}
}
private void DrawTrajectory(Graphics g, List<GPSModel> trajectory, float centerX, float centerY, Color pointColor, Pen linePen)
{
var _posicaoAtual = GPSService.UltimaLeitura;
for (int i = 0; i < trajectory.Count; i++)
{
var trajPoint = trajectory[i];
// Calcule a posição no panel
float x = centerX + (float)((trajPoint.Longitude - _posicaoAtual.Longitude) * _zoom);
float y = centerY - (float)((trajPoint.Latitude - _posicaoAtual.Latitude) * _zoom);
// Desenha o ponto da trajetória
DrawPoint(g, x, y, pointColor);
// Desenha a linha conectando os pontos
if (i > 0)
{
var prevPoint = trajectory[i - 1];
float prevX = centerX + (float)((prevPoint.Longitude - _posicaoAtual.Longitude) * _zoom);
float prevY = centerY - (float)((prevPoint.Latitude - _posicaoAtual.Latitude) * _zoom);
g.DrawLine(linePen, prevX, prevY, x, y);
}
}
}
private void DrawPoint(Graphics g, float x, float y, Color color)
{
float size = 5;
using (Brush brush = new SolidBrush(color))
{
g.FillEllipse(brush, x - size / 2, y - size / 2, size, size);
}
}
private float DrawRobot(Graphics g, float x, float y, Color pointColor, Color borderColor, float larguraEsquerda, float larguraDireita, float comprimentoFrente, float comprimentoTras, float angle)
{
// Desenhar o ponto GPS
DrawPoint(g, x, y, pointColor);
GPSModel posicaoRobo = GPSService.UltimaLeitura;
int agPlus = Convert.ToInt32(135 + angle);
(float xET, float yET) = CalcularPontosLateraisRobo(posicaoRobo, larguraEsquerda, comprimentoTras, x, y, agPlus + 90);
(float xDT, float yDT) = CalcularPontosLateraisRobo(posicaoRobo, larguraDireita, comprimentoTras, x, y, agPlus + 0);
(float xEF, float yEF) = CalcularPontosLateraisRobo(posicaoRobo, larguraEsquerda, comprimentoFrente, x, y, agPlus + 180);
(float xDF, float yDF) = CalcularPontosLateraisRobo(posicaoRobo, larguraDireita, comprimentoFrente, x, y, agPlus + 270);
// Calcular os cantos do retângulo considerando o ângulo
PointF[] corners = new PointF[4];
// Cantos do retângulo com as novas medidas
corners[0] = new PointF(xET, yET); // Superior esquerdo
corners[1] = new PointF(xDT, yDT); // Superior direito
corners[2] = new PointF(xDF, yDF); // Inferior direito
corners[3] = new PointF(xEF, yEF); // Inferior esquerdo
// Desenhar o contorno do robô
using (Pen pen = new Pen(borderColor, 2))
{
g.DrawPolygon(pen, corners);
}
// Desenhar a linha indicando a frente do robô (do ponto central para a frente)
DrawFrontLine(g, x, y, xDF, yDF, xEF, yEF, borderColor);
float PxToCm = Math.Abs(xET - xDT) / (larguraDireita + larguraEsquerda);
return PxToCm;
}
private void DrawObstacle(Graphics g, float xRobot, float yRobot, Color pointColor, Color borderColor, float angle, Obstaculo obstaculo, float PxToCm)
{
GPSModel posicaoRobo = GPSService.UltimaLeitura;
GPSModel posicaoObstaculo = Variaveis.OperacaoEmAndamento.Mapa.GerarPontoDeslocado(posicaoRobo, angle, obstaculo.DistanciaMedia_mm / 1000);
float x = xRobot + (float)((posicaoObstaculo.Longitude - posicaoRobo.Longitude) * _zoom);
float y = yRobot - (float)((posicaoObstaculo.Latitude - posicaoRobo.Latitude) * _zoom);
// Desenhar o ponto GPS
DrawPoint(g, x, y, pointColor);
int agPlus = Convert.ToInt32(135 + angle);
float largura = obstaculo.Largura_mm / 20;
(float xET, float yET) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 90);
(float xDT, float yDT) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 0);
(float xEF, float yEF) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 180);
(float xDF, float yDF) = CalcularPontosLateraisRobo(posicaoObstaculo, largura, largura, x, y, agPlus + 270);
// Calcular os cantos do retângulo considerando o ângulo
PointF[] corners = new PointF[4];
// Cantos do retângulo com as novas medidas
corners[0] = new PointF(xET, yET); // Superior esquerdo
corners[1] = new PointF(xDT, yDT); // Superior direito
corners[2] = new PointF(xDF, yDF); // Inferior direito
corners[3] = new PointF(xEF, yEF); // Inferior esquerdo
// Desenhar o contorno do robô
using (Pen pen = new Pen(borderColor, 2))
{
g.DrawPolygon(pen, corners);
}
}
private void DrawFrontLine(Graphics g, float x, float y, float xDF, float yDF, float xEF, float yEF, Color borderColor)
{
// Calcular o ponto médio entre DF e EF
float midX = (xDF + xEF) / 2;
float midY = (yDF + yEF) / 2;
// Desenhar a linha do ponto central para a frente do robô
using (Pen frontPen = new Pen(borderColor, 2))
{
g.DrawLine(frontPen, x, y, midX, midY);
}
}
private (float, float) CalcularPontosLateraisRobo(GPSModel posicaoRobo, float Largura, float Comprimento, float x, float y, int offAngulo)
{
float hP = (float)Math.Sqrt(Math.Pow(Largura, 2) + Math.Pow(Comprimento, 2)) / 100f;
GPSModel pP = Variaveis.OperacaoEmAndamento.Mapa.GerarPontoDeslocado(posicaoRobo, offAngulo, hP);
float xP = x + (float)((pP.Longitude - posicaoRobo.Longitude) * _zoom);
float yP = y - (float)((pP.Latitude - posicaoRobo.Latitude) * _zoom);
return (xP, yP);
}
private void Tmr_Tick(object sender, EventArgs e)
{
btnCalcularAngulo_Click(sender, e);
StatusCarro = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.StatusAtual;
lblStatus.Text = "Status: " + Enum.GetName(typeof(StatusCarroMapa), StatusCarro);
lblStatusOperacao.Text = "Operação: " + Enum.GetName(typeof(StatusOperacao), Variaveis.OperacaoEmAndamento.StatusAtual);
lblStatus.Text = "Carro: " + Enum.GetName(typeof(StatusCarroMapa), StatusCarro);
lblDirecao.Text = "Direção: " + Enum.GetName(typeof(DirecaoCarroRua), Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.Direcao);
lblDistanciaOperacao.Text = "Distância Operação: " + (Variaveis.OperacaoEmAndamento.Mapa == null ? 0 : Variaveis.OperacaoEmAndamento.Mapa.DistanciaTotal).ToString("00.00") + " m";
lblDistanciaFinal.Text = "Distância Final: " + (GPSService.DistanciaDoTrecho(Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica)).ToString("00.00") + " m";
lblDistanciaLateral.Text = "Esq: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda.ToString("0.00") + " m - Dir: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita.ToString("0.00") + " m";
lblRua.Text = "Rua: " + (Variaveis.OperacaoEmAndamento.Mapa.DentroDaRua ? "Dentro" : "Fora");
lblMargem.Text = "Margem: " + (Variaveis.OperacaoEmAndamento.Mapa.NaMargemEntradaRua ? "Sim" : "Não");
lblProximoPonto.Text = "Próximo Ponto: " + (Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria + 1);
lblAproximando.Text = (Variaveis.OperacaoEmAndamento.Mapa.AproximandoUltimoPontoTrajetoria ? "Aproximando" : "Afastando");
lblDistProx.Text = "Distância Próximo Ponto: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaAteProximoPonto.ToString("0.00") + " m";
lblDistAnt.Text = "Distância Ponto Anterior: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaAtePontoAnterior.ToString("0.00") + " m";
if (Variaveis.OperacaoEmAndamento.Iniciado)
{
if (StatusCarro != StatusCarroMapa.Manobrando && StatusCarro != StatusCarroMapa.SaindoRua)
{
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
}
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
pnlOrientacaoTrajeto.Invalidate();
pnlAnguloControle.Invalidate();
pnlOrientacaoCarro.Invalidate();
pnlZoomMapa.Invalidate();
}
else
{
@ -80,6 +338,7 @@ namespace AgroBase.Forms
{
tmrSonar.Stop();
}
Variaveis.OperacaoEmAndamento.Iniciado = false;
}
private void btnCarregar_Click(object sender, EventArgs e)
@ -162,9 +421,7 @@ namespace AgroBase.Forms
txtDistanciaSonar.Text = distSonar.ToString();
}
btnCalcularAngulo_Click(sender, e);
btnAtualizarLeitura_Click(sender, e);
Tmr_Tick(sender, e);
}
private void btnIniciarSimulacao_Click(object sender, EventArgs e)
@ -174,9 +431,17 @@ namespace AgroBase.Forms
MessageBox.Show("Selecione as ruas para realizar a operação!");
return;
}
Variaveis.OperacaoEmAndamento.Iniciado = !Variaveis.OperacaoEmAndamento.Iniciado;
Variaveis.OperacaoEmAndamento.Simulando = true;
Variaveis.OperacaoEmAndamento.Iniciado = btnIniciarSimulacao.Text == "Iniciar Simulação";
btnIniciarSimulacao.Text = Variaveis.OperacaoEmAndamento.Iniciado ? "Parar Simulação" : "Iniciar Simulação";
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
if (Variaveis.OperacaoEmAndamento.Iniciado)
{
GPSService.PenultimaLeitura.Latitude = GPSService.UltimaLeitura.Latitude;
GPSService.PenultimaLeitura.Longitude = GPSService.UltimaLeitura.Longitude;
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
Tmr_Tick(sender, e);
}
}
private void btnCarregarMapa_Click(object sender, EventArgs e)
@ -358,6 +623,7 @@ namespace AgroBase.Forms
txtDistanciaGPS.Text = distancia.ToString("0.00");
pnlOrientacaoTrajeto.Invalidate();
pnlOrientacaoCarro.Invalidate();
}

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

@ -22,11 +22,15 @@ namespace AgroBase.Models
private PictureBox picLoading { get; set; }
public Label lblMapa { get; set; }
public MapasService mapaService { get; set; } = new MapasService();
public List<List<GPSModel>> Trajetoria { get; set; } = new List<List<GPSModel>>();
public List<List<GPSModel>> TrajetoriaMapa { get; set; } = new List<List<GPSModel>>();
public List<List<GPSModel>> TrajetoriaProjetada { get; set; } = new List<List<GPSModel>>();
public List<GPSModel> TrajetoriaDinamica { get; set; } = new List<GPSModel>();
public List<string> RuasPercorrer { get; set; } = new List<string>();
public List<string> RuasPercorridas { get; set; } = new List<string>();
public string RuaEmAndamento { get; set; }
public int IdxRuaAtual { get; set; } = 0;
public int IdxUltimoPontoTrajetoria { get; set;} = 0;
public bool AproximandoUltimoPontoTrajetoria { get; set;} = false;
public int IdxUltimoPontoTrajetoriaDinamica { get; set;} = 0;
public double DistanciaTotal
{
@ -34,7 +38,7 @@ namespace AgroBase.Models
{
double distancia = 0.0;
foreach (var trecho in Trajetoria)
foreach (var trecho in TrajetoriaProjetada)
{
distancia += GPSService.DistanciaDoTrecho(trecho);
}
@ -55,7 +59,7 @@ namespace AgroBase.Models
}
else
{
if (Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ExtremoMaisProximo == 0)
if (ExtremoMaisProximoMapa == 0)
{
direcaoCaminho = DirecaoCarroRua.Ida;
}
@ -71,9 +75,9 @@ namespace AgroBase.Models
{
get
{
if (mapaService.DadosMapa != null && RuasPercorrer.Count() > IdxRuaAtual)
if (mapaService.DadosMapa != null && RuasPercorrer.Count() > IdxRuaAtual && TrajetoriaProjetada.Any())
{
try
/*try
{
string IDruaAtual = RuasPercorrer[IdxRuaAtual];
var Rua = mapaService.DadosMapa.features.Where(x => x.geometry.id == IDruaAtual).FirstOrDefault();
@ -120,7 +124,25 @@ namespace AgroBase.Models
catch
{
return false;
}*/
var trajetoria = TrajetoriaProjetada[0];
for (int i = 0; i < trajetoria.Count - 1; i++)
{
var pontoA = trajetoria[i];
var pontoB = trajetoria[i + 1];
var a = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoA);
var b = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoB);
// Verifica se a posição atual está entre os pontos A e B da trajetória
if (GPSService.EstaEntrePontos(GPSService.UltimaLeitura, pontoA, pontoB))
{
return true; // O ponto está dentro da rua
}
}
return false; // O ponto está fora da rua
}
else
{
@ -128,10 +150,78 @@ namespace AgroBase.Models
}
}
}
public GPSModel PrimeiroPontoRua
public double MargemLimiteEntradaRua { get; } = 1.0;
public double DistanciaProjecaoRua
{
get
{
return MargemLimiteEntradaRua * 3.0;
}
}
public bool NaMargemEntradaRua
{
get
{
bool NaMargem = false;
GPSModel PontoRua = ExtremoMaisProximoRuaProjetada == 0 ? PrimeiroPontoRuaProjetada : UltimoPontoRuaProjetada;
double Distancia = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PontoRua);
double DistanciaAnterior = GPSService.DistanciaEntrePontos(GPSService.PenultimaLeitura, PontoRua);
double DistanciaFinal = 0;
if (Distancia > DistanciaAnterior && (Distancia <= (MargemLimiteEntradaRua * 2)))
{
DistanciaFinal = Distancia * -1;
}
else
{
DistanciaFinal = Distancia;
}
if (DistanciaFinal <= MargemLimiteEntradaRua)
{
NaMargem = true;
}
return NaMargem;
}
}
public GPSModel PrimeiroPontoRuaProjetada
{
get
{
if (TrajetoriaProjetada.Any(x => x.Any()))
{
var primeiroPontoRua = TrajetoriaProjetada[0].First();
return primeiroPontoRua;
}
else
{
return new GPSModel();
}
}
}
public GPSModel UltimoPontoRuaProjetada
{
get
{
if (TrajetoriaProjetada.Any(x => x.Any()))
{
var ultimoPontoRua = TrajetoriaProjetada[0].Last();
return ultimoPontoRua;
}
else
{
return new GPSModel();
}
}
}
public GPSModel PrimeiroPontoRuaMapa
{
get
{
var Trajetoria = TrajetoriaMapa;
if (Trajetoria.Any(x => x.Any()) && Trajetoria.Count() > IdxRuaAtual)
{
var primeiroPontoRua = Trajetoria.Skip(IdxRuaAtual).First().First();
@ -144,10 +234,12 @@ namespace AgroBase.Models
}
}
public GPSModel UltimoPontoRua
public GPSModel UltimoPontoRuaMapa
{
get
{
var Trajetoria = TrajetoriaMapa;
if (Trajetoria.Any(x => x.Any()) && Trajetoria.Count() > IdxRuaAtual)
{
var ultimoPontoRua = Trajetoria.Skip(IdxRuaAtual).First().Last();
@ -159,12 +251,24 @@ namespace AgroBase.Models
}
}
}
public int ExtremoMaisProximo
public int ExtremoMaisProximoMapa
{
get
{
double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRua);
double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRua);
double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRuaMapa);
double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRuaMapa);
// 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
// 1 para mais próximo do último ponto da rua (SAINDO)
return (distPP < distUP) ? 0 : 1;
}
}
public int ExtremoMaisProximoRuaProjetada
{
get
{
double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRuaProjetada);
double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRuaProjetada);
// 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
// 1 para mais próximo do último ponto da rua (SAINDO)
@ -206,46 +310,72 @@ namespace AgroBase.Models
}
}
}
public double AnguloRuaAtual
{
get
{
try
{
if (!TrajetoriaProjetada.Any())
{
return 0;
}
var RuaAtual = TrajetoriaProjetada[0];
if (RuaAtual.Any())
{
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(RuaAtual, TrajetoriaTipo.Rua);
int idx = RuaAtual.IndexOf(PontoMaisProximo);
if ((idx + 1) >= RuaAtual.Count())
{
return 0;
}
double angulo = GPSService.CalcularOrientacao(RuaAtual[idx], RuaAtual[idx + 1]);
if (angulo == 0)
{
angulo = GPSService.CalcularOrientacao(RuaAtual[idx + 1], RuaAtual[idx + 2]);
}
return (!(angulo >= 0 || angulo <= 0)) ? 0 : angulo;
}
else
{
return 0;
}
}
catch
{
return 0;
}
}
}
public double DistanciaEsquerda
{
get
{
if (IdxRuaAtual >= Trajetoria.Count())
if (IdxRuaAtual >= TrajetoriaMapa.Count())
{
return 0;
}
double distancia = 0;
DirecaoCarroRua direcaoCaminho = DirecaoCaminho;
List<GPSModel> RuaEsquerda = new List<GPSModel>();
if (Trajetoria.Any())
if (direcaoCaminho == DirecaoCarroRua.Ida)
{
if (DirecaoCaminho == DirecaoCarroRua.Ida)
{
RuaEsquerda = Trajetoria[IdxRuaAtual];
}
else if (DirecaoCaminho == DirecaoCarroRua.Volta)
{
if (IdxRuaAtual > 0)
{
(GPSModel pontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual - 1], TrajetoriaTipo.Geral);
double distanciaP0 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoMaisProximo);
if (distanciaP0 < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua)
{
Trajetoria[IdxRuaAtual - 1].ForEach(x => RuaEsquerda.Add(x));
RuaEsquerda.Reverse();
}
}
}
distancia = DistanciaAteRua(TrajetoriaMapa[IdxRuaAtual], VariaveisEquipamento.LarguraEsquerda);
}
if (RuaEsquerda.Any())
else if (direcaoCaminho == DirecaoCarroRua.Volta)
{
distancia = DistanciaAteRua(RuaEsquerda, VariaveisEquipamento.LarguraEsquerda);
}
else
{
distancia = DistanciaEntreRuas(Trajetoria) / 2;
if (IdxRuaAtual == 0)
{
distancia = DistanciaEntreRuas(TrajetoriaMapa) / 2.0;
}
else
{
distancia = DistanciaAteRua(TrajetoriaMapa[IdxRuaAtual - 1], VariaveisEquipamento.LarguraEsquerda);
}
}
return distancia;
@ -255,47 +385,119 @@ namespace AgroBase.Models
{
get
{
if (IdxRuaAtual >= Trajetoria.Count())
if (IdxRuaAtual >= TrajetoriaMapa.Count())
{
return 0;
}
double distancia = 0;
DirecaoCarroRua direcaoCaminho = DirecaoCaminho;
List<GPSModel> RuaDireita = new List<GPSModel>();
if (Trajetoria.Any())
if (direcaoCaminho == DirecaoCarroRua.Ida)
{
if (DirecaoCaminho == DirecaoCarroRua.Volta)
if (IdxRuaAtual == 0)
{
Trajetoria[IdxRuaAtual].ForEach(x => RuaDireita.Add(x));
RuaDireita.Reverse();
distancia = DistanciaEntreRuas(TrajetoriaMapa) / 2.0;
}
else if (DirecaoCaminho == DirecaoCarroRua.Ida)
else
{
if (IdxRuaAtual > 0)
{
(GPSModel pontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual - 1], TrajetoriaTipo.Geral);
double distanciaP0 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoMaisProximo);
if (distanciaP0 < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua)
{
RuaDireita = Trajetoria[IdxRuaAtual - 1];
}
}
distancia = DistanciaAteRua(TrajetoriaMapa[IdxRuaAtual - 1], VariaveisEquipamento.LarguraDireita);
}
}
if (RuaDireita.Any())
else if (direcaoCaminho == DirecaoCarroRua.Volta)
{
distancia = DistanciaAteRua(RuaDireita, VariaveisEquipamento.LarguraDireita);
}
else
{
distancia = DistanciaEntreRuas(Trajetoria) / 2;
distancia = DistanciaAteRua(TrajetoriaMapa[IdxRuaAtual], VariaveisEquipamento.LarguraDireita);
}
return distancia;
}
}
private DateTime UltimaLeituraDistancia;
private int idxPontoAproximadoRua = 0;
public int IdxPontoAproximadoRua
{
get
{
if (!TrajetoriaProjetada.Any())
{
return 0;
}
try
{
if (UltimaLeituraDistancia == null)
{
UltimaLeituraDistancia = DateTime.Now.Add(new TimeSpan(0, 0, -10));
}
else if (UltimaLeituraDistancia.Add(new TimeSpan(0, 0, 5)) < DateTime.Now)
{
int idx = -1;
double menorDistancia = 9999;
double distanciaAnterior = 9999;
var Rua = TrajetoriaProjetada[0];
foreach (var Ponto in Rua)
{
double dist = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, Ponto);
if (dist < menorDistancia)
{
menorDistancia = dist;
idx = Rua.IndexOf(Ponto);
}
else if (dist > distanciaAnterior)
{
break;
}
distanciaAnterior = dist;
}
idxPontoAproximadoRua = idx;
UltimaLeituraDistancia = DateTime.Now;
}
}
catch
{
}
return idxPontoAproximadoRua;
}
}
public double DistanciaAteProximoPonto
{
get
{
if (!TrajetoriaProjetada.Any() || (TrajetoriaProjetada[0].Count() - 1) <= IdxUltimoPontoTrajetoria)
{
return 0;
}
GPSModel ponto = IdxUltimoPontoTrajetoria == 0 ? TrajetoriaProjetada[0][0] : TrajetoriaProjetada[0][IdxUltimoPontoTrajetoria + 1];
double distanciaProximoPonto = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, ponto);
return distanciaProximoPonto;
}
}
public double DistanciaAtePontoAnterior
{
get
{
if (!TrajetoriaProjetada.Any() || (TrajetoriaProjetada[0].Count() - 1) <= IdxUltimoPontoTrajetoria || IdxUltimoPontoTrajetoria == 0)
{
return 0;
}
GPSModel ponto = IdxUltimoPontoTrajetoria == TrajetoriaProjetada[0].Count() - 1 ? TrajetoriaProjetada[0].Last() : TrajetoriaProjetada[0][IdxUltimoPontoTrajetoria - 1];
double distanciaPontoAnterior = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, ponto);
return distanciaPontoAnterior;
}
}
public void btnCarregar_Click(object sender, EventArgs e, string caminhoArquivo = "", bool monitoramento = false)
{
@ -361,14 +563,14 @@ namespace AgroBase.Models
lblMapa.Text = "Mapa carregado: " + mapaService.NomeArquivos;
}
PopularTrajetoria();
PopularTrajetoriaMapa();
}
IniciarProcessamento();
}
}
public void PopularTrajetoria()
public void PopularTrajetoriaMapa()
{
if (mapaService.DadosMapa != null)
{
@ -378,14 +580,14 @@ namespace AgroBase.Models
{
RuasPercorrer = mapaService.DadosMapa.features.Select(x => x.geometry.id).ToList();
}*/
Trajetoria = new List<List<GPSModel>>();
TrajetoriaMapa = new List<List<GPSModel>>();
foreach (var ID in RuasPercorrer)
{
var rua = mapaService.DadosMapa.features.FirstOrDefault(x => ID == x.geometry.id);
if (rua != null)
{
Trajetoria.Add(new List<GPSModel>());
TrajetoriaMapa.Add(new List<GPSModel>());
foreach (var ponto in rua.geometry.coordinates)
{
GPSModel item = new GPSModel()
@ -393,14 +595,14 @@ namespace AgroBase.Models
Longitude = ponto[0],
Latitude = ponto[1],
};
Trajetoria[Trajetoria.Count - 1].Add(item);
TrajetoriaMapa[TrajetoriaMapa.Count - 1].Add(item);
/*if (Trajetoria[Trajetoria.Count - 1].Count > 1)
{
DistanciaTotal += GPSService.DistanciaEntrePontos(Trajetoria[Trajetoria.Count - 1][Trajetoria[Trajetoria.Count - 1].Count - 2], item);
}*/
}
var Rua = Trajetoria[Trajetoria.Count - 1];
var Rua = TrajetoriaMapa[TrajetoriaMapa.Count - 1];
if (OrientacaoRuas == -999)
{
OrientacaoRuas = GPSService.CalcularOrientacao(Rua[Rua.Count - 1], Rua[Rua.Count - 2]);
@ -506,18 +708,21 @@ namespace AgroBase.Models
public void GerarTrajetoriaDinamica()
{
double distanciaEntreRuas = DistanciaEntreRuas(Trajetoria) / 2;
double distanciaEntreRuas = DistanciaEntreRuas(TrajetoriaMapa) / 2.0;
List<GPSModel> Caminho = new List<GPSModel>();
List<List<GPSModel>> _TrajetoriaProjetada = new List<List<GPSModel>>();
DirecaoCarroRua direcaoRua = TrajetoriaDinamica.Any() ? TrajetoriaDinamica[0].DirecaoTrecho : ExtremoMaisProximo == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
StatusCarroMapa statusCarro = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.StatusAtual;
DirecaoCarroRua direcaoRua = TrajetoriaDinamica.Any() && TrajetoriaDinamica[0].DirecaoTrecho != DirecaoCarroRua.Parado ? TrajetoriaDinamica[0].DirecaoTrecho : ExtremoMaisProximoMapa == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
for (int i = IdxRuaAtual; i < Trajetoria.Count(); i++)
for (int i = IdxRuaAtual; i < TrajetoriaMapa.Count(); i++)
{
List<GPSModel> RuaAtual = Trajetoria.Skip(i).First();
List<GPSModel> RuaAtual = TrajetoriaMapa.Skip(i).First();
double anguloProjetar = GPSService.CalcularOrientacao(RuaAtual.First(), RuaAtual.Skip(1).First()) + 90;
List<GPSModel> RuaProjetada = ProjetarRua(RuaAtual, distanciaEntreRuas, anguloProjetar);
_TrajetoriaProjetada.Add(RuaProjetada);
if (Caminho.Any())
{
@ -535,63 +740,129 @@ namespace AgroBase.Models
{
RuaProjetada.Reverse();
}
double anguloProjetar1 = GPSService.CalcularOrientacao(RuaProjetada.Skip(1).First(), RuaProjetada.First());
GPSModel PrimeiroPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.First(), 3, anguloProjetar1);
GPSModel PrimeiroPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.First(), DistanciaProjecaoRua, anguloProjetar1);
double anguloProjetar2 = GPSService.CalcularOrientacao(RuaProjetada.Skip(RuaProjetada.Count() - 2).First(), RuaProjetada.Last());
GPSModel UltimoPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.Last(), 3, anguloProjetar2);
GPSModel UltimoPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.Last(), DistanciaProjecaoRua, anguloProjetar2);
if (!Caminho.Any())
{
bool dentroRua = DentroDaRua;
(GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(RuaProjetada, TrajetoriaTipo.Rua);
int idxPontoMaisProximo = RuaProjetada.IndexOf(PontoMaisProximo);
GPSModel inicio = GPSService.UltimaLeitura;
List<GPSModel> _caminho = new List<GPSModel>();
if (idxPontoMaisProximo == 0 && aproximando)
GPSModel PontoComparar = new GPSModel();
// Se o robo estiver se aproximando do ponto mais proximo, e nao estiver vindo de fora da rua
if (aproximando && statusCarro != StatusCarroMapa.Direcionando)
{
//_caminho.Add(PrimeiroPonto);
List<GPSModel> _pontos = GPSService.InterpolarPontos(inicio, PrimeiroPonto, 1.0);
if (_pontos.Count() == 1 || GPSService.CompararDirecaoTrajeto(_pontos, RuaProjetada))
PontoComparar = PontoMaisProximo;
}
// Se o robo estiver se aproximando do ponto mais proximo, e estiver vindo de fora da rua
else if (aproximando && statusCarro == StatusCarroMapa.Direcionando)
{
var _a = GPSService.DistanciaEntrePontos(inicio, PrimeiroPonto);
var _b = GPSService.DistanciaEntrePontos(inicio, PontoMaisProximo);
bool _c = GPSService.EstaEntrePontos(inicio, PontoMaisProximo, PrimeiroPonto);
// Verifica se o robo esta na margem da rua projetada e se a distancia do robo ate o ponto mais proximo da rua (0) e menor que a distancia ate o primeiro ponto projetado, ou entao se o robo esta entre ambos
if ((_b < DistanciaProjecaoRua && _b < _a) || _c)
{
_caminho.AddRange(_pontos);
PontoComparar = PontoMaisProximo;
}
else
{
PontoComparar = PrimeiroPonto;
}
}
//bool dentroRua = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.DentroDaRua;
bool dentroRua = Variaveis.OperacaoEmAndamento.Mapa.DentroDaRua;
// Se estiver manobrando e a manobra ainda não tiver sido concluída, OU, o ponto mais próximo é o ultimo ponto da rua, e já esta fora da rua
if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ManobraConcluida || ((idxPontoMaisProximo == (RuaProjetada.Count() - 1)) && !dentroRua))
// Se o robo estiver se afastando do ponto mais proximo, e estiver dentro da rua, e houver mais pontos na trajetoria
else if (!aproximando && dentroRua && RuaProjetada.Count() > (idxPontoMaisProximo + 1))
{
_caminho.Add(inicio);
// Interpolar a posicao atual ate o proximo ponto, pois ja passou pelo ponto mais proximo
idxPontoMaisProximo++;
PontoComparar = RuaProjetada[idxPontoMaisProximo];
}
else if (!aproximando && !dentroRua && RuaProjetada.Count() == (idxPontoMaisProximo + 1))
{
PontoComparar = UltimoPonto;
}
else
{
PontoComparar = PontoMaisProximo;
}
_caminho.AddRange(GPSService.InterpolarPontos(inicio, PontoComparar, 1.0));
// se a distancia entre a posicao do robo e proximo ponto for menor que 1 metro, significa que o ponto interpolado esta mais adiante que o proximo ponto, entao tambem ignoramos ele
var a = GPSService.DistanciaEntrePontos(inicio, _caminho.Last());
var b = GPSService.DistanciaEntrePontos(inicio, PontoComparar);
bool Acrescenta = false;
bool AddUltimoPonto = true;
if (a == 0 || a > b)
{
idxPontoMaisProximo++;
Acrescenta = true;
}
else
{
// Verifica se esta se afastando do ultimo ponto gerado
var a1 = GPSService.DistanciaEntrePontos(GPSService.PenultimaLeitura, _caminho.Last());
if (a1 < a)
{
Acrescenta = true;
}
}
if (!Acrescenta || (Acrescenta && RuaProjetada.Count() > (idxPontoMaisProximo + 1)))
{
_caminho.AddRange(RuaProjetada.Skip(idxPontoMaisProximo));
}
else
{
AddUltimoPonto = GPSService.EstaEntrePontos(inicio, GPSService.PenultimaLeitura, UltimoPonto);
}
// Se não for adicionar o ultimo ponto, significa que o robô já passou por ele
if (!AddUltimoPonto)
{
Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.VerificaFimRuaAtual();
if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.Concluido)
{
GerarTrajetoriaDinamica();
}
return;
}
if (AddUltimoPonto)
{
_caminho.Add(UltimoPonto);
}
_caminho.ForEach(x => x.DirecaoTrecho = direcaoRua);
Caminho.AddRange(_caminho);
//direcaoRua = direcaoRua == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
}
else
{
List<GPSModel> _caminho = new List<GPSModel>();
// Verifica se a distância entre o ultimo ponto da rua anterior e o primeiro ponto da nova rua é curto o bastante para gerar a curva de manobra
double distancia = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], PrimeiroPonto);
bool distanciaMinima = distancia < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua * 2;
bool distanciaMinima = distancia < MargemLimiteEntradaRua * 3;
// Verifica se o angulo formado entre a rua anterior e a rua atual é grande o bastante para gerar a curva de manobra
double anguloEntreRuas = GPSService.CalcularOrientacao(Caminho.Last(), PrimeiroPonto);
double anguloFinalRuaAnterior = GPSService.CalcularOrientacao(Caminho.Count() > 1 ? Caminho[Caminho.Count() - 2] : Caminho.First(), Caminho.Last());
double difAngulo = anguloFinalRuaAnterior - anguloEntreRuas;
bool anguloMinimo = difAngulo >= -45 && difAngulo <= 45;
bool anguloMinimo = difAngulo >= -45.0 && difAngulo <= 45.0;
if (distanciaMinima && !anguloMinimo)
{
GPSModel P0 = CalcularPontoControle(Caminho.Last(), PrimeiroPonto, distanciaEntreRuas * 2);
bool CurvaParaEsquerda = ExtremoMaisProximoMapa == 0;
GPSModel P0 = CalcularPontoControle(Caminho.Last(), PrimeiroPonto, distanciaEntreRuas * 2, CurvaParaEsquerda);
List<GPSModel> CurvaConexao = GerarCurvaConexao(Caminho.Last(), PrimeiroPonto, P0, 6);
_caminho.AddRange(CurvaConexao);
@ -613,6 +884,7 @@ namespace AgroBase.Models
// Limpa a trajetória dinâmica existente e adiciona os novos pontos
TrajetoriaProjetada = new List<List<GPSModel>>(_TrajetoriaProjetada);
TrajetoriaDinamica.Clear();
Caminho.ForEach(x =>
{
@ -624,9 +896,16 @@ namespace AgroBase.Models
});
Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoriaDinamica = 0;
if (Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.StatusDetecao)
if (KinectService.Iniciado && Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.StatusDetecao)
{
AjustarTrajetoriaParaDesvio(Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.ObstaculoCritico);
if (Variaveis.OperacaoEmAndamento.OpMapaGPS.StatusDentroRua.Contains(statusCarro))
{
TrajetoriaDinamica.Clear();
}
else
{
AjustarTrajetoriaParaDesvio(Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.ObstaculoCritico);
}
}
GPSService.AtualizarTrajetoriaDinamica();
@ -669,18 +948,22 @@ namespace AgroBase.Models
return new GPSModel { Latitude = x, Longitude = y };
}
public static GPSModel CalcularPontoControle(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
public static GPSModel CalcularPontoControle(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia, bool ParaDentro)
{
double angulo = GPSService.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
double lat = (ultimoPontoRuaAtual.Latitude + primeiroPontoProximaRua.Latitude) / 2;
double lon = (ultimoPontoRuaAtual.Longitude + primeiroPontoProximaRua.Longitude) / 2;
GPSModel pontoMedio = new GPSModel()
{
Latitude = lat,
Latitude = lat,
Longitude = lon,
};
// Projetar um ponto a uma certa distância na direção calculada
return GPSService.ProjetarPontoDeslocado(pontoMedio, distancia, angulo + 270);
// Ajuste o ângulo com base na direção da curva
double ajusteAngulo = (ParaDentro ? 270 : 90);
// Projetar um ponto a uma certa distância na direção ajustada
return GPSService.ProjetarPontoDeslocado(pontoMedio, distancia, angulo + ajusteAngulo);
}
public static List<GPSModel> GerarCurvaConexao(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, GPSModel pontoControle, int numeroPontos)
@ -725,14 +1008,39 @@ namespace AgroBase.Models
{
GPSModel posicaoAtual = GPSService.UltimaLeitura;
double distanciaObstaculo = ((double)obstaculo.DistanciaMedia_mm / 1000);
double larguraObstaculo = ((double)obstaculo.Largura_mm / 1000);
double anguloCaminho = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
double anguloDesvioInicial = anguloCaminho + obstaculo.AnguloParaDesvio;
double anguloDesvioFinal = anguloCaminho - obstaculo.AnguloParaDesvio;
GPSModel ponto1 = GerarPontoDeslocado(posicaoAtual, anguloDesvioInicial, distanciaObstaculo);
GPSModel ponto2 = GerarPontoDeslocado(ponto1, anguloCaminho, larguraObstaculo);
GPSModel ponto3 = GerarPontoDeslocado(ponto2, anguloDesvioFinal, distanciaObstaculo);
List<GPSModel> lista = new List<GPSModel>()
{
ponto1,
ponto2,
ponto3
};
var idxRemover = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, ponto3, distanciaObstaculo);
TrajetoriaDinamica.RemoveRange(1, idxRemover);
TrajetoriaDinamica.InsertRange(1, lista);
return;
// Identificar o waypoint mais próximo para iniciar o desvio
int indiceInicioDesvio = EncontrarIndiceInicioDesvio(TrajetoriaDinamica, posicaoAtual, ((double)obstaculo.DistanciaMedia_mm / 1000));
int indiceInicioDesvio = EncontrarIndiceInicioDesvio(TrajetoriaDinamica, posicaoAtual, distanciaObstaculo);
// Calcular os waypoints de desvio
var waypointsDesvio = CalcularWaypointsDesvio(posicaoAtual, obstaculo, indiceInicioDesvio);
// Determinar o intervalo de waypoints a serem removidos
int indiceFinalDesvio = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, waypointsDesvio.Last(), ((double)obstaculo.DistanciaMedia_mm / 1000) + ((double)obstaculo.DeslocamentoNecessarioDesvio * 2 / 100));
double distanciaSeguranca = ((double)obstaculo.DeslocamentoNecessarioDesvio * 2 / 100);
int indiceFinalDesvio = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, waypointsDesvio.Last(), distanciaSeguranca);
int quantidadeWaypointsRemover = indiceFinalDesvio - indiceInicioDesvio;
// Remover waypoints obsoletos
@ -768,6 +1076,16 @@ namespace AgroBase.Models
private int EncontrarIndiceInicioDesvio(List<GPSModel> trajetoria, GPSModel posicaoAtual, double distanciaObstaculo)
{
foreach (var wp in trajetoria)
{
double distancia = GPSService.DistanciaEntrePontos(wp, posicaoAtual);
if (distancia < distanciaObstaculo)
{
return trajetoria.IndexOf(wp);
}
}
// Este é um método simplificado. Você precisará implementar a lógica para encontrar o waypoint mais adequado.
return trajetoria.FindIndex(wp => GPSService.DistanciaEntrePontos(wp, posicaoAtual) < distanciaObstaculo);
}
@ -779,7 +1097,7 @@ namespace AgroBase.Models
double anguloCaminho = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
// Calcular o ângulo e distância para o desvio
double anguloDesvio = anguloCaminho + ((obstaculo.DirecaoDesvio == Direcao.Direita ? 270 : 90) + obstaculo.AnguloParaDesvio);
double anguloDesvio = anguloCaminho + ((obstaculo.DirecaoDesvio == Direcao.Direita ? 90 : 270) + obstaculo.AnguloParaDesvio);
double deslocamento = (double)obstaculo.DistanciaMedia_mm / 1000; // Math.Sqrt(Math.Pow(((double)obstaculo.DeslocamentoNecessarioDesvio / 100), 2) + Math.Pow(((double)obstaculo.DistanciaMedia / 100), 2));
// Waypoint de início do desvio
@ -960,6 +1278,35 @@ namespace AgroBase.Models
public GPSModel GerarPontoDeslocado(GPSModel pontoInicial, double angulo, double distancia)
{
double latitude = pontoInicial.Latitude;
double longitude = pontoInicial.Longitude;
// Conversão de ângulo de graus para radianos
double anguloRad = angulo * (Math.PI / 180);
// Raio da Terra em metros
double raioTerra = GPSService.RaioDaTerra;
// Calcular deslocamento de latitude em radianos
double deltaLat = distancia * Math.Cos(anguloRad) / raioTerra;
// Converter de radianos para graus
double latAtt = latitude + deltaLat * (180 / Math.PI);
// Calcular deslocamento de longitude em radianos
double deltaLong = distancia * Math.Sin(anguloRad) / (raioTerra * Math.Cos(latitude * (Math.PI / 180)));
// Converter de radianos para graus
double longAtt = longitude + deltaLong * (180 / Math.PI);
return new GPSModel()
{
Latitude = latAtt,
Longitude = longAtt,
};
}
}
public class MapaFeatureCollectionModel

551
AgroBase/AgroBase/Models/Operacoes/OperacaoMapaGPSModel.cs
View File

@ -2,6 +2,7 @@
using AgroBase.Models.Modules;
using AgroBase.Services;
using CefSharp.DevTools.Database;
using Emgu.CV;
using Emgu.CV.ML;
using Newtonsoft.Json;
using System;
@ -24,7 +25,7 @@ namespace AgroBase.Models.Operacoes
{
get
{
return Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual >= Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Count - 1;
return Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual >= Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa.Count();
}
}
public OperacaoRefRuaGPS ReferencialRuaGPS { get; set; } = new OperacaoRefRuaGPS();
@ -53,7 +54,7 @@ namespace AgroBase.Models.Operacoes
// Calcule o erro lateral baseado nas distâncias
double erroLateral = Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda - Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita;
double erroLateral = statusCarroRua == StatusCarroMapa.CaminhandoRua ? Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda - Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita : 0;
@ -87,12 +88,12 @@ namespace AgroBase.Models.Operacoes
// Manter o erro lateral sempre com duas casas decimais
double multiplicadorErroLateral = erroLateral < 1 ? 10 : erroLateral > 10 && erroLateral < 100 ? 0 : erroLateral > 100 ? 0.1 : 1;
double erroPosicaoLateral =
Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Count() == 1 ? 0 :
double erroPosicaoLateral =
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa.Count() == 1 ? 0 :
(erroLateral * (carroDentroRua ? multiplicadorErroLateral : 1));
double PesoLateralDentroRua = erroPosicaoLateral / 10;
if (PesoLateralDentroRua > 1)
if (PesoLateralDentroRua > 1)
{
PesoLateralDentroRua = 1;
}
@ -129,7 +130,7 @@ namespace AgroBase.Models.Operacoes
}*/
}
}
direcaoVirar = erroCombinado < anguloCarro ? Direcao.Esquerda : Direcao.Direita;
@ -154,53 +155,58 @@ namespace AgroBase.Models.Operacoes
CalcularAnguloInclinacao(statusAtual);
var Controle = Variaveis.OperacaoEmAndamento.Controle;
if (statusAtual == StatusCarroMapa.Parado)
{
var BicosNaoAtuados = new List<AtuadorBicoModel>(Variaveis.OperacaoEmAndamento.DispAtu.Dados.BicosPulverizadores);
BicosNaoAtuados.ForEach(x => x.Atuado = false);
Variaveis.OperacaoEmAndamento.Controle.BicosAtuados = BicosNaoAtuados.ToList();
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = 0;
Variaveis.OperacaoEmAndamento.Controle.Angulo = 0;
Controle.BicosAtuados = BicosNaoAtuados.ToList();
Controle.RPM_SP = 0;
Controle.Angulo = 0;
}
else if (statusAtual == StatusCarroMapa.Direcionando)
{
if (Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.DesvioNecessario)
//bool Manobrando = (ReferencialRuaGPS.EmManobra && Variaveis.OperacaoEmAndamento.Mapa.DistanciaAteProximoPonto <= Variaveis.OperacaoEmAndamento.Mapa.DistanciaProjecaoRua);
bool Manobrando = Variaveis.OperacaoEmAndamento.Mapa.DistanciaAteProximoPonto <= Variaveis.OperacaoEmAndamento.Mapa.DistanciaProjecaoRua;
bool DesvioSonar = (KinectService.Iniciado && Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.DesvioNecessario);
if (Manobrando || DesvioSonar)
{
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = Variaveis.OperacaoEmAndamento.Controle.RPM_Min;
Controle.RPM_SP = Controle.RPM_Min;
}
else
{
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = Variaveis.OperacaoEmAndamento.Controle.RPM_Max;
Controle.RPM_SP = Controle.RPM_Max;
}
}
else if (statusAtual == StatusCarroMapa.EntrandoRua)
{
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = Variaveis.OperacaoEmAndamento.Controle.RPM_Min;
Controle.RPM_SP = Controle.RPM_Min;
}
else if (statusAtual == StatusCarroMapa.SaindoRua)
{
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = Variaveis.OperacaoEmAndamento.Controle.RPM_Min;
Controle.RPM_SP = Controle.RPM_Min;
}
else if (statusAtual == StatusCarroMapa.CaminhandoRua)
{
if (Sensoriamento.ErvasNoRadar == 0)
{
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = Variaveis.OperacaoEmAndamento.Controle.RPM_Max;
Controle.RPM_SP = Controle.RPM_Max;
}
else
{
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = Convert.ToInt32(Variaveis.OperacaoEmAndamento.Controle.RPM_Max * 0.85);
Controle.RPM_SP = Convert.ToInt32(Controle.RPM_Max * 0.85);
}
double margemAngulo = 5;
if (Variaveis.OperacaoEmAndamento.Controle.Angulo > margemAngulo || Variaveis.OperacaoEmAndamento.Controle.Angulo < (margemAngulo * -1))
if (Controle.Angulo > margemAngulo || Controle.Angulo < (margemAngulo * -1))
{
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = Variaveis.OperacaoEmAndamento.Controle.RPM_Min;
Controle.RPM_SP = Controle.RPM_Min;
}
}
else if (statusAtual == StatusCarroMapa.Manobrando)
{
Variaveis.OperacaoEmAndamento.Controle.RPM_SP = Variaveis.OperacaoEmAndamento.Controle.RPM_Max / 2;
Controle.RPM_SP = Controle.RPM_Max / 2;
}
}
@ -218,7 +224,7 @@ namespace AgroBase.Models.Operacoes
if (ids.Count != Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Count)
{
Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer = ids;
Variaveis.OperacaoEmAndamento.Mapa.PopularTrajetoria();
Variaveis.OperacaoEmAndamento.Mapa.PopularTrajetoriaMapa();
}
}
@ -228,249 +234,98 @@ namespace AgroBase.Models.Operacoes
public class OperacaoRefRuaGPS
{
public bool ManobraConcluida { get; set; } = true;
private List<StatusCarroMapa> _EtapasAnteriores = new List<StatusCarroMapa>() { StatusCarroMapa.Parado };
private StatusCarroMapa _EtapaAnterior
{
get
{
var _Anterior = _EtapasAnteriores.Last();
if (_Anterior == _EtapaAtual && _EtapasAnteriores.Count() > 1)
{
_Anterior = _EtapasAnteriores.Skip(_EtapasAnteriores.Count() - 2).First();
}
return _Anterior;
}
}
private void AtualizaEtapaAtual(StatusCarroMapa _Atual)
{
_EtapaAtual = _Atual;
var _EtapaAnterior = _EtapasAnteriores.Last();
if (_EtapaAtual != _EtapaAnterior)
{
_EtapasAnteriores.Add(_EtapaAtual);
}
}
private StatusCarroMapa _EtapaAtual;
public StatusCarroMapa StatusAtual
{
get
{
bool dentroDaRua = Variaveis.OperacaoEmAndamento.Mapa.DentroDaRua;
StatusCarroMapa etapaAtual = _EtapaAtual;
// Se o status da operação não estiver Em Andamento
if (Variaveis.OperacaoEmAndamento.StatusAtual != StatusOperacao.EmAndamento)
{
etapaAtual = StatusCarroMapa.Parado;
AtualizaEtapaAtual(StatusCarroMapa.Parado);
return _EtapaAtual;
}
else
var Mapa = Variaveis.OperacaoEmAndamento.Mapa;
bool dentroDaRua = Mapa.DentroDaRua;
bool naMargem = Mapa.NaMargemEntradaRua;
if (dentroDaRua)
{
// Robô está manobrando para seguir a próxima rota do mapa
if (_EtapaAtual == StatusCarroMapa.Manobrando)
if (KinectService.Iniciado && Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.StatusDetecao)
{
// Aguarda a mudança de rua até que o robô tenha terminado a manobra e tenha ficado parado em frente à rua
/*if (ManobraConcluida && Direcao == DirecaoCarroRua.Parado)
{
Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
ManobraConcluida = false;
etapaAtual = StatusCarroMapa.EntrandoRua;
GPSService.AtualizaLeituraSimulacao();
System.Threading.Thread.Sleep(500);
}*/
int idxProximaRua = Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual + 1;
List<GPSModel> trechoCarro = new List<GPSModel>()
{
GPSService.PenultimaLeitura,
GPSService.UltimaLeitura
};
List<GPSModel> trechoCaminho = new List<GPSModel>()
{
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Where(x => x.idxRua == idxProximaRua).FirstOrDefault(),
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Where(x => x.idxRua == idxProximaRua).Skip(1).FirstOrDefault()
};
if (GPSService.CompararDirecaoTrajeto(trechoCarro, trechoCaminho))
{
Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria = 0;
ManobraConcluida = true;
etapaAtual = StatusCarroMapa.Direcionando;
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Clear();
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
}
}
// Se o robô não estiver dentro da entrelinha de cana
else if (!dentroDaRua)
{
if (Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Count() <= 3)
{
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
if (Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Count() <= 3)
{
etapaAtual = StatusCarroMapa.Parado;
Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
}
}
else
{// Está próximo o bastante e alinhado para entrar na rua
if (NaMargemEntradaRua)
{
DirecaoCarroRua direcao = Direcao;
// Está mais próximo do ponto inicial da rota, e a direção do carro é IDA ou então PARADO, logo está entrando na rua, do começo para o final
if (ExtremoMaisProximo == 0 && (direcao == DirecaoCarroRua.Ida || direcao == DirecaoCarroRua.Parado) && etapaAtual != StatusCarroMapa.SaindoRua)
{
etapaAtual = StatusCarroMapa.EntrandoRua;
}
// Está mais próximo do ponto final da rota, e a direção do carro é VOLTA ou então PARADO, logo está entrando na rua, do final para o começo
else if (ExtremoMaisProximo == 1 && (direcao == DirecaoCarroRua.Volta || direcao == DirecaoCarroRua.Parado) && etapaAtual != StatusCarroMapa.SaindoRua)
{
etapaAtual = StatusCarroMapa.EntrandoRua;
}
// Está mais próximo ao ponto final da rua, e a direção do carro é IDA, mas a rua será iniciada no sentido VOLTA, então para o carro para que ele vire e entre na rua
/*else if (ExtremoMaisProximo == 1 && (direcao == DirecaoCarroRua.Ida && Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Volta))
{
etapaAtual = StatusCarroMapa.Manobrando;
}
else if (ExtremoMaisProximo == 0 && (direcao == DirecaoCarroRua.Volta && Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Ida))
{
etapaAtual = StatusCarroMapa.Manobrando;
}*/
// Robô não está entrando na rua do começo para o fim, nem do fim para o começo
else
{
// Se a etapa anterior era CaminhandoRua, ou seja, robô estava na rua e agora não está mais, então esta saindo da rua
if (_EtapaAtual == StatusCarroMapa.CaminhandoRua)
{
etapaAtual = StatusCarroMapa.SaindoRua;
}
// Se a etapa anterior não for CaminhandoRua, então significa que ele já iniciou a etapa de sair da rua
else
{
// Ainda tem ruas a serem percorridas, logo, o robô precisa realizar a manobra para seguir para a próxima rua
if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.Concluido)
{
etapaAtual = StatusCarroMapa.Manobrando;
ManobraConcluida = false;
}
}
}
}
// Estará se direcionando para o ponto inicial
else
{
// Saiu da margem de saída da rua
if (_EtapaAtual == StatusCarroMapa.SaindoRua)
{
// Ainda tem ruas a serem percorridas, logo, o robô precisa realizar a manobra para seguir para a próxima rua
if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.Concluido)
{
etapaAtual = StatusCarroMapa.Manobrando;
ManobraConcluida = false;
}
}
// Não está na margem de entrada ou saída da rua, então está em estado de direcionamento até a rua da operação
else
{
etapaAtual = StatusCarroMapa.Direcionando;
}
}
}
}
// Se o robô estiver dentro da entrelinha da cana
else if (dentroDaRua)
{
etapaAtual = StatusCarroMapa.CaminhandoRua;
}
}
_EtapaAtual = etapaAtual;
//_EtapaAtual = StatusCarroMapa.Direcionando;
return _EtapaAtual;
}
}
public int PontosConsiderarAngulo { get; set; } = 2;
public double Distancia { get; set; } = 0;
/*public double AnguloCarro
{
get
{
if (Variaveis.OperacaoEmAndamento.DispSen._Porta.IsOpen && Variaveis.OperacaoEmAndamento.DispSen.Dados.Sensores.Any(x => x.Componente == S_Code.sMAG && x.Inicializado))
{
return Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroBussola;
}
else
{
return Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPS;
}
}
}*/
/*public double AnguloCarroGPS
{
get
{
try
{
double somaAngulo = 0;
var Pontos = !Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Any() || Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual >= Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Count() ? new List<GPSModel>() :
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].Count > PontosConsiderarAngulo ?
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].OrderByDescending(x => x.DataHora).Take(PontosConsiderarAngulo).ToList() :
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].OrderByDescending(x => x.DataHora).ToList();
Pontos = Pontos.OrderBy(x => x.DataHora).ToList();
for (int i = 0; i < Pontos.Count - 1; i++)
{
somaAngulo += GPSService.CalcularOrientacao(Pontos[i + 1], Pontos[i]);
}
double mediaAngulo = somaAngulo / (Pontos.Count - 1);
return (!(mediaAngulo >= 0 || mediaAngulo <= 0)) ? 0 : mediaAngulo;
}
catch
{
return 0;
}
}
}*/
public double AnguloCaminho
{
get
{
try
{
if (_EtapaAtual == StatusCarroMapa.Direcionando)
{
double somaAnguloRua = 0;
var Rua = Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica;
var PontosRua = PontosConsiderarAngulo < Rua.Count ?
Rua.Take(PontosConsiderarAngulo).ToList() :
Rua.Skip(PontosConsiderarAngulo).ToList();
for (int i = 0; i < PontosRua.Count - 1; i++)
{
somaAnguloRua += GPSService.CalcularOrientacao(Rua[0], Rua[1]);
}
double mediaAnguloRua = somaAnguloRua / (PontosRua.Count - 1);
return (!(mediaAnguloRua >= 0 || mediaAnguloRua <= 0)) ? 0 : mediaAnguloRua;
_EtapaAtual = StatusCarroMapa.Parado;
}
else
{
double somaAnguloRua = 0;
var Rua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Skip(Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).First();
var PontosRua = IdxPontoAproximadoRua + PontosConsiderarAngulo < Rua.Count ?
Rua.Skip(IdxPontoAproximadoRua).Take(PontosConsiderarAngulo).ToList() :
Rua.Skip(IdxPontoAproximadoRua - PontosConsiderarAngulo).ToList();
/*var Pontos = Rua.Count > PontosConsiderarAngulo ?
Rua.OrderByDescending(x => x.DataHora).Take(PontosConsiderarAngulo).ToList() :
Rua.OrderByDescending(x => x.DataHora).ToList();*/
for (int i = 0; i < PontosRua.Count - 1; i++)
if (!naMargem)
{
somaAnguloRua += GPSService.CalcularOrientacao(PontosRua[i + 1], PontosRua[i]);
AtualizaEtapaAtual(StatusCarroMapa.CaminhandoRua);
}
double mediaAnguloRua = somaAnguloRua / (PontosRua.Count - 1);
double angulo = (!(mediaAnguloRua >= 0 || mediaAnguloRua <= 0)) ? 0 : mediaAnguloRua;
/*double anguloOposto = (angulo + 180) % 360;
if (Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Volta)
else
{
angulo = anguloOposto;
}*/
if (_EtapaAtual == StatusCarroMapa.CaminhandoRua && (_EtapaAnterior == StatusCarroMapa.EntrandoRua))
{
AtualizaEtapaAtual(StatusCarroMapa.SaindoRua);
}
return angulo;
if (_EtapaAnterior == StatusCarroMapa.CaminhandoRua)
{
AtualizaEtapaAtual(StatusCarroMapa.SaindoRua);
}
else
{
AtualizaEtapaAtual(StatusCarroMapa.EntrandoRua);
}
}
}
}
catch
else
{
return 0;
if (!naMargem)
{
AtualizaEtapaAtual(StatusCarroMapa.Direcionando);
}
else
{
if (_EtapaAtual == StatusCarroMapa.Direcionando && (_EtapaAnterior == StatusCarroMapa.Parado || _EtapaAnterior == StatusCarroMapa.SaindoRua))
{
AtualizaEtapaAtual(StatusCarroMapa.EntrandoRua);
}
if (_EtapaAnterior == StatusCarroMapa.Direcionando)
{
AtualizaEtapaAtual(StatusCarroMapa.EntrandoRua);
}
else
{
AtualizaEtapaAtual(StatusCarroMapa.SaindoRua);
}
}
}
return _EtapaAtual;
}
}
public double MargemErroAngulo { get; set; } = 90;
@ -483,26 +338,20 @@ namespace AgroBase.Models.Operacoes
{
return DirecaoCarroRua.Parado;
}
var Mapa = Variaveis.OperacaoEmAndamento.Mapa;
double mediaAngulo = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro;
double anguloOposto = (mediaAngulo + 180) % 360;
double mediaAnguloRua = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
double mediaAnguloRua = Mapa.AnguloRuaAtual;
DirecaoCarroRua direcaoTrecho =
!Mapa.TrajetoriaDinamica.Any() && !Mapa.TrajetoriaProjetada.Any() ? DirecaoCarroRua.Ida :
Mapa.TrajetoriaProjetada[0] != null ? Mapa.TrajetoriaProjetada[0][0].DirecaoTrecho :
Mapa.TrajetoriaDinamica.Any(x => x.idxRua == Mapa.IdxRuaAtual) ?
Mapa.TrajetoriaDinamica.FirstOrDefault(x => x.idxRua == Mapa.IdxRuaAtual).DirecaoTrecho :
Mapa.TrajetoriaDinamica.FirstOrDefault().DirecaoTrecho;
/*if ((mediaAngulo + MargemErroAngulo) >= mediaAnguloRua && (mediaAngulo - MargemErroAngulo) <= mediaAnguloRua)
{
return DirecaoCarroRua.Ida;
}
else if ((anguloOposto + MargemErroAngulo) >= mediaAnguloRua && (anguloOposto - MargemErroAngulo) <= mediaAnguloRua)
{
return DirecaoCarroRua.Volta;
}
else
{
return DirecaoCarroRua.Parado;
}*/
DirecaoCarroRua direcaoTrecho = !Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any() ? DirecaoCarroRua.Ida :
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any(x => x.idxRua == Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual) ?
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.FirstOrDefault(x => x.idxRua == Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).DirecaoTrecho :
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.FirstOrDefault().DirecaoTrecho;
if ((mediaAngulo + MargemErroAngulo) >= mediaAnguloRua && (mediaAngulo - MargemErroAngulo) <= mediaAnguloRua)
{
return direcaoTrecho;
@ -517,181 +366,39 @@ namespace AgroBase.Models.Operacoes
}
}
}
public GPSModel PrimeiroPontoRua
public bool EmManobra
{
get
{
try
{
var primeiroPontoRua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Skip(Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).First().First();
return primeiroPontoRua;
}
catch
{
return new GPSModel();
}
// Caso o robo esteja manobrando em direcao a uma rua, qualquer movimento feito sera considerado aproximacao do primeiro ponto da rua
return Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria == 0 && StatusAtual == StatusCarroMapa.Direcionando;
}
}
}
}
public GPSModel UltimoPontoRua
public void VerificaFimRuaAtual()
{
get
if (_EtapaAtual == StatusCarroMapa.Direcionando && (_EtapaAnterior == StatusCarroMapa.SaindoRua))
{
try
var Mapa = Variaveis.OperacaoEmAndamento.Mapa;
string RuaAtual = Mapa.RuasPercorrer[Mapa.IdxRuaAtual];
if (!Mapa.RuasPercorridas.Contains(RuaAtual))
{
var ultimoPontoRua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Skip(Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).First().Last();
return ultimoPontoRua;
}
catch
{
return new GPSModel();
}
}
}
public bool DentroDaRua
{
get
{
if (Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Count() > Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual && !Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any())
{
try
Mapa.RuasPercorridas.Add(RuaAtual);
Mapa.IdxRuaAtual++;
Mapa.TrajetoriaDinamica.Clear();
Mapa.IdxUltimoPontoTrajetoria = 0;
Mapa.IdxUltimoPontoTrajetoriaDinamica = 0;
GPSService.PenultimaLeitura = new GPSModel()
{
string IDruaAtual = Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual];
var Rua = Variaveis.OperacaoEmAndamento.Mapa.mapaService.DadosMapa.features.Where(x => x.geometry.id == IDruaAtual).FirstOrDefault();
if (Rua != null)
{
bool dentro = false;
List<GPSModel> Trecho = new List<GPSModel>();
var PontosRua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].ToList();
int idx = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.IdxPontoAproximadoRua;
if (Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual % 2 != 0)
{
PontosRua.Reverse();
idx = (PontosRua.Count - 1) - idx;
}
for (int i = 0; i <= idx; i++)
{
Trecho.Add(PontosRua[i]);
}
double distTrecho = GPSService.DistanciaDoTrecho(Trecho);
dentro = distTrecho > 0 && distTrecho < Rua.properties.Length;
return dentro;
}
else
{
return false;
}
}
catch
{
return false;
}
}
else
{
return false;
DataHora = GPSService.UltimaLeitura.DataHora,
Longitude = GPSService.UltimaLeitura.Longitude,
Latitude = GPSService.UltimaLeitura.Latitude,
};
}
}
}
public int ExtremoMaisProximo
{
get
{
double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRua);
double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRua);
// 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
// 1 para mais próximo do último ponto da rua (SAINDO)
return (distPP < distUP) ? 0 : 1;
}
}
private DateTime UltimaLeituraDistancia;
private int idxPontoAproximadoRua = 0;
public int IdxPontoAproximadoRua
{
get
{
if (!Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Any())
{
return 0;
}
try
{
if (UltimaLeituraDistancia == null)
{
UltimaLeituraDistancia = DateTime.Now.Add(new TimeSpan(0, 0, -10));
}
else if (UltimaLeituraDistancia.Add(new TimeSpan(0, 0, 5)) < DateTime.Now)
{
int idx = -1;
double menorDistancia = 9999;
double distanciaAnterior = 9999;
var Rua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual];
foreach (var Ponto in Rua)
{
double dist = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, Ponto);
if (dist < menorDistancia)
{
menorDistancia = dist;
idx = Rua.IndexOf(Ponto);
}
else if (dist > distanciaAnterior)
{
break;
}
distanciaAnterior = dist;
}
idxPontoAproximadoRua = idx;
UltimaLeituraDistancia = DateTime.Now;
}
}
catch
{
}
return idxPontoAproximadoRua;
}
}
public double MargemLimiteEntradaRua { get; } = 2.0;
public bool NaMargemEntradaRua
{
get
{
bool NaMargem = false;
List<GPSModel> Rua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual];
GPSModel PontoRua = ExtremoMaisProximo == 0 ? PrimeiroPontoRua : UltimoPontoRua;
double Distancia = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PontoRua);
double DistanciaAnterior = GPSService.DistanciaEntrePontos(GPSService.PenultimaLeitura, PontoRua);
double DistanciaFinal = 0;
if (Distancia > DistanciaAnterior && (Distancia <= (MargemLimiteEntradaRua * 2)))
{
DistanciaFinal = Distancia * -1;
}
else
{
DistanciaFinal = Distancia;
}
if (DistanciaFinal <= MargemLimiteEntradaRua)
{
NaMargem = true;
}
else if (DistanciaFinal <= (MargemLimiteEntradaRua * 2) && (_EtapaAtual == StatusCarroMapa.CaminhandoRua || _EtapaAtual == StatusCarroMapa.SaindoRua))
{
NaMargem = true;
}
return NaMargem;
}
}
}
public class OperacaoMapaGPSSensoriamentoModel

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

@ -238,6 +238,7 @@ namespace AgroBase.Models
}
}
public bool Iniciado { get; set; }
public bool Simulando { get; set; } = false;
public long TimestampInicio { get; set; }
public long TimestampFim { get; set; }
public TimeSpan Tempo
@ -799,6 +800,7 @@ namespace AgroBase.Models
public void IniciarOperacao(OperacaoControleModel controle = null, bool DaIHM = true)
{
Iniciado = true;
Simulando = false;
TimestampInicio = DateTime.Now.Ticks;
TempoAguardando = DateTime.Now;
TimestampFim = 0;
@ -1039,33 +1041,35 @@ namespace AgroBase.Models
private void AtualizarDadosOperacao()
{
var Mapa = Variaveis.OperacaoEmAndamento.Mapa;
DirecaoCarroRua direcaoCarro = OpMapaGPS.ReferencialRuaGPS.Direcao;
try
{
if (!Variaveis.OperacaoEmAndamento.Mapa.mapaService.Carregado || Variaveis.OperacaoEmAndamento.Mapa.DistanciaTotal == 0)
if (!Mapa.mapaService.Carregado || Mapa.DistanciaTotal == 0)
{
Sensoriamento.ProgressoTrajeto = 0;
}
else
{
Sensoriamento.ProgressoTrajeto = Variaveis.OperacaoEmAndamento.Sensoriamento.DistanciaPercorrida.Sum() / Variaveis.OperacaoEmAndamento.Mapa.DistanciaTotal * 100;
Sensoriamento.ProgressoTrajeto = Variaveis.OperacaoEmAndamento.Sensoriamento.DistanciaPercorrida.Sum() / Mapa.DistanciaTotal * 100;
}
if (!Variaveis.OperacaoEmAndamento.Mapa.mapaService.Carregado || Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual >= Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Count() || (OpMapaGPS.ReferencialRuaGPS.Direcao != DirecaoCarroRua.Ida && OpMapaGPS.ReferencialRuaGPS.Direcao != DirecaoCarroRua.Volta))
if (!Mapa.mapaService.Carregado || Mapa.IdxRuaAtual >= Mapa.TrajetoriaProjetada.Count() || (direcaoCarro != DirecaoCarroRua.Ida && direcaoCarro != DirecaoCarroRua.Volta))
{
Sensoriamento.ProgressoRua = 0;
}
else if (OpMapaGPS.ReferencialRuaGPS.Direcao == DirecaoCarroRua.Ida)
else if (direcaoCarro == DirecaoCarroRua.Ida)
{
Sensoriamento.ProgressoRua = (
(double)OpMapaGPS.ReferencialRuaGPS.IdxPontoAproximadoRua /
(double)(Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].Count - 1)
(double)Mapa.IdxPontoAproximadoRua /
(double)(Mapa.TrajetoriaProjetada[0].Count() - 1)
) * 100;
}
else if (OpMapaGPS.ReferencialRuaGPS.Direcao == DirecaoCarroRua.Volta)
else if (direcaoCarro == DirecaoCarroRua.Volta)
{
Sensoriamento.ProgressoRua = (
(double)(Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].Count - 1 - OpMapaGPS.ReferencialRuaGPS.IdxPontoAproximadoRua) /
(double)(Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].Count - 1)
(double)(Mapa.TrajetoriaProjetada[0].Count() - 1 - Mapa.IdxPontoAproximadoRua) /
(double)(Mapa.TrajetoriaProjetada[0].Count() - 1)
) * 100;
}
}
@ -1127,7 +1131,11 @@ namespace AgroBase.Models
}
AtualizaInformacoesControleOperacao();
if (!Variaveis.OperacaoEmAndamento.Simulando)
{
AtualizaInformacoesControleOperacao();
}
}
private void AtualizaLeituraCameraSolo(CameraSoloModel CameraSolo)
@ -1210,6 +1218,12 @@ namespace AgroBase.Models
try
{
var cameraCaminho = Variaveis.OperacaoEmAndamento.CameraCaminho;
if (cameraCaminho == null || !cameraCaminho.Iniciada)
{
return;
}
var MqttTopico = Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == cameraCaminho.MqttTopico);
if (MqttTopico != null && MqttTopico.Mensagens.Any(x => x.Momento > cameraCaminho.camera.IniciadoEm))
{
@ -1471,7 +1485,7 @@ namespace AgroBase.Models
Direcao = OpMapaGPS.ReferencialRuaGPS.Direcao,
AnguloRua = Mapa.AnguloCaminho,
DentroDaRua = Mapa.DentroDaRua,
IdxPontoAproximadoRua = OpMapaGPS.ReferencialRuaGPS.IdxPontoAproximadoRua,
IdxPontoAproximadoRua = Variaveis.OperacaoEmAndamento.Mapa.IdxPontoAproximadoRua,
IdxRuaAtual = Mapa.IdxRuaAtual,
AnguloDif = Sensoriamento.AnguloDif,
TipoMovimento = Controle.TipoMovimento,

2
AgroBase/AgroBase/Services/APIService.cs
View File

@ -101,7 +101,7 @@ namespace AgroBase.Services
if (ids.Count != Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Count)
{
Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer = ids;
Variaveis.OperacaoEmAndamento.Mapa.PopularTrajetoria();
Variaveis.OperacaoEmAndamento.Mapa.PopularTrajetoriaMapa();
}
}
}

84
AgroBase/AgroBase/Services/GPSService.cs
View File

@ -283,9 +283,9 @@ namespace AgroBase.Services
public static (GPSModel, bool) PontoMaisProximoRuaProjetada(List<GPSModel> RuaProjetada, TrajetoriaTipo trajetoriaTipo)
{
int idxUltimoPonto =
trajetoriaTipo == TrajetoriaTipo.Rua ? Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria :
trajetoriaTipo == TrajetoriaTipo.Dinamica ? Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoriaDinamica :
int idxUltimoPonto =
trajetoriaTipo == TrajetoriaTipo.Rua ? Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria :
trajetoriaTipo == TrajetoriaTipo.Dinamica ? Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoriaDinamica :
0;
// Verifica se a lista está vazia
@ -302,13 +302,26 @@ namespace AgroBase.Services
GPSModel posicaoAnterior = PenultimaLeitura;
bool seAproximandoAnterior = false;
Dictionary<int, Dictionary<GPSModel, bool>> PontosAproximacao = new Dictionary<int, Dictionary<GPSModel, bool>>();
// Itera sobre os pontos da rua projetada
foreach (var ponto in RuaProjetada.Skip(idxUltimoPonto))
{
int idxPonto = RuaProjetada.IndexOf(ponto);
double distanciaAtual = DistanciaEntrePontos(posicaoAtual, ponto);
double distanciaAnterior = DistanciaEntrePontos(posicaoAnterior, ponto);
bool seAproximando = distanciaAtual < distanciaAnterior;
bool mesmaPosicao = distanciaAtual == distanciaAnterior;
bool seAproximando = (distanciaAtual < distanciaAnterior) || (mesmaPosicao && idxUltimoPonto == 0);
// Caso o robo esteja indo ate a entrada de uma nova rua, considerar que sempre esta se aproximando do primeiro ponto
if (!seAproximando)
{
//seAproximando = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.StatusAtual == StatusCarroMapa.Direcionando;
seAproximando = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.EmManobra;
}
PontosAproximacao.Add(idxPonto, new Dictionary<GPSModel, bool>() { { ponto, seAproximando } });
if (distanciaAtual <= menorDistancia)
{
menorDistancia = distanciaAtual;
@ -361,16 +374,55 @@ namespace AgroBase.Services
{
case TrajetoriaTipo.Rua:
Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria = idxUltimoPonto;
Variaveis.OperacaoEmAndamento.Mapa.AproximandoUltimoPontoTrajetoria = PontosAproximacao[idxUltimoPonto].First().Value;
break;
case TrajetoriaTipo.Dinamica:
Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoriaDinamica = idxUltimoPonto;
break;
}
return (pontoMaisProximo, seAproximandoAnterior);
return (PontosAproximacao[idxUltimoPonto].First().Key, PontosAproximacao[idxUltimoPonto].First().Value);
}
public static GPSModel PontoMaisProximoRuaMapa(List<GPSModel> RuaMapa)
{
int idxUltimoPonto = 0;
// Verifica se a lista está vazia
if (RuaMapa == null || RuaMapa.Count == 0)
{
return new GPSModel();
}
// Inicializa variáveis
GPSModel pontoMaisProximo = RuaMapa[idxUltimoPonto];
GPSModel posicaoAtual = UltimaLeitura;
GPSModel posicaoAnterior = PenultimaLeitura;
Dictionary<GPSModel, double> PontosAproximacao = new Dictionary<GPSModel, double>();
// Itera sobre os pontos da rua projetada
foreach (var ponto in RuaMapa.Skip(idxUltimoPonto))
{
int idxPonto = RuaMapa.IndexOf(ponto);
double distanciaAtual = DistanciaEntrePontos(posicaoAtual, ponto);
double distanciaAnterior = DistanciaEntrePontos(posicaoAnterior, ponto);
bool mesmaPosicao = distanciaAtual == distanciaAnterior;
bool seAproximando = (distanciaAtual < distanciaAnterior) || (mesmaPosicao && idxUltimoPonto == 0);
if (seAproximando)
{
PontosAproximacao.Add(ponto, distanciaAtual);
}
}
var PontosAproximando = PontosAproximacao.OrderBy(x => x.Value).ToList();
return PontosAproximando.FirstOrDefault().Key;
}
public static GPSModel PontoMaisProximoRuaProjetada2(List<GPSModel> RuaProjetada)
{
// Verifica se a lista está vazia
@ -637,6 +689,28 @@ namespace AgroBase.Services
};
}
public static bool EstaEntrePontos(GPSModel posicao, GPSModel pontoA, GPSModel pontoB)
{
/*// Calcule a distância entre os pontos usando a fórmula de Haversine ou similar
double distanciaAToB = DistanciaEntrePontos(pontoA, pontoB);
double distanciaAToPonto = DistanciaEntrePontos(pontoA, posicao);
double distanciaBToPonto = DistanciaEntrePontos(pontoB, posicao);
// Verifica se a soma das distâncias do pontoA ao ponto e do ponto ao pontoB
// é aproximadamente igual à distância entre pontoA e pontoB
return Math.Abs((distanciaAToPonto + distanciaBToPonto) - distanciaAToB) < 0.001;*/
double margem = 0.000001;
// Verifica se o ponto atual está entre pontoA e pontoB em latitude e longitude, com uma margem de erro
bool dentroLatitude = (posicao.Latitude >= Math.Min(pontoA.Latitude, pontoB.Latitude) - margem &&
posicao.Latitude <= Math.Max(pontoA.Latitude, pontoB.Latitude) + margem);
bool dentroLongitude = (posicao.Longitude >= Math.Min(pontoA.Longitude, pontoB.Longitude) - margem &&
posicao.Longitude <= Math.Max(pontoA.Longitude, pontoB.Longitude) + margem);
return dentroLatitude && dentroLongitude;
}
private static MapaFeatureCollectionModel TrajetoFake = new MapaFeatureCollectionModel();
private static int idxPonto = 0;

5
AgroBase/AgroBase/Services/KinectService.cs
View File

@ -177,6 +177,11 @@ namespace AgroBase.Services
{
Leitura.Momento = DateTime.Now;
if (kinectSensor == null)
{
return;
}
var Acelerometro = kinectSensor.AccelerometerGetCurrentReading();
Leitura.AccX = Acelerometro.X;
Leitura.AccY = Acelerometro.Y;

BIN
AgroBase/AgroBase/bin/Debug/AgroBase.pdb
View File

Binary file not shown.

BIN
AgroBase/AgroBase/bin/Debug/DawnCache/data_1
View File

Binary file not shown.

BIN
AgroBase/AgroBase/bin/Debug/GPUCache/data_1
View File

Binary file not shown.

36
AgroBase/AgroBase/bin/Debug/Python/Output/mapa_interativo.html
View File

File diff suppressed because one or more lines are too long

BIN
AgroBase/AgroBase/obj/Debug/AgroBase.csproj.AssemblyReference.cache
View File

Binary file not shown.

BIN
AgroBase/AgroBase/obj/Debug/AgroBase.csproj.GenerateResource.cache
View File

Binary file not shown.

BIN
AgroBase/AgroBase/obj/Debug/AgroBase.pdb
View File

Binary file not shown.