diff --git a/AgroBase/.vs/AgroBase/FileContentIndex/810aa3b9-5fca-48b5-b9af-b4bfd2a55675.vsidx b/AgroBase/.vs/AgroBase/FileContentIndex/ca2a373d-c163-4b46-bb19-2a6913875e42.vsidx similarity index 76% rename from AgroBase/.vs/AgroBase/FileContentIndex/810aa3b9-5fca-48b5-b9af-b4bfd2a55675.vsidx rename to AgroBase/.vs/AgroBase/FileContentIndex/ca2a373d-c163-4b46-bb19-2a6913875e42.vsidx index ded9a2b61..530386864 100644 Binary files a/AgroBase/.vs/AgroBase/FileContentIndex/810aa3b9-5fca-48b5-b9af-b4bfd2a55675.vsidx and b/AgroBase/.vs/AgroBase/FileContentIndex/ca2a373d-c163-4b46-bb19-2a6913875e42.vsidx differ diff --git a/AgroBase/.vs/AgroBase/FileContentIndex/d1f5893a-1225-4fb6-9ff7-aeeeeb086278.vsidx b/AgroBase/.vs/AgroBase/FileContentIndex/d1f5893a-1225-4fb6-9ff7-aeeeeb086278.vsidx deleted file mode 100644 index 934313277..000000000 Binary files a/AgroBase/.vs/AgroBase/FileContentIndex/d1f5893a-1225-4fb6-9ff7-aeeeeb086278.vsidx and /dev/null differ diff --git a/AgroBase/.vs/AgroBase/FileContentIndex/e81c5f44-0d14-4d98-96f0-633bd7ff25e0.vsidx b/AgroBase/.vs/AgroBase/FileContentIndex/e81c5f44-0d14-4d98-96f0-633bd7ff25e0.vsidx new file mode 100644 index 000000000..d12a31fa8 Binary files /dev/null and b/AgroBase/.vs/AgroBase/FileContentIndex/e81c5f44-0d14-4d98-96f0-633bd7ff25e0.vsidx differ diff --git a/AgroBase/.vs/AgroBase/v17/.suo b/AgroBase/.vs/AgroBase/v17/.suo index 70f50d861..9467b48cf 100644 Binary files a/AgroBase/.vs/AgroBase/v17/.suo and b/AgroBase/.vs/AgroBase/v17/.suo differ diff --git a/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.Designer.cs b/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.Designer.cs index f9f949197..449b72246 100644 --- a/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.Designer.cs +++ b/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.Designer.cs @@ -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; } } \ No newline at end of file diff --git a/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs b/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs index 75bb511b6..7975ec12f 100644 --- a/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs +++ b/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs @@ -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 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(); } diff --git a/AgroBase/AgroBase/Models/MapasModel.cs b/AgroBase/AgroBase/Models/MapasModel.cs index 7c8e07d7a..93aead3a3 100644 --- a/AgroBase/AgroBase/Models/MapasModel.cs +++ b/AgroBase/AgroBase/Models/MapasModel.cs @@ -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> Trajetoria { get; set; } = new List>(); + public List> TrajetoriaMapa { get; set; } = new List>(); + public List> TrajetoriaProjetada { get; set; } = new List>(); public List TrajetoriaDinamica { get; set; } = new List(); public List RuasPercorrer { get; set; } = new List(); + public List RuasPercorridas { get; set; } = new List(); + 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 RuaEsquerda = new List(); - 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 RuaDireita = new List(); - 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>(); + TrajetoriaMapa = new List>(); foreach (var ID in RuasPercorrer) { var rua = mapaService.DadosMapa.features.FirstOrDefault(x => ID == x.geometry.id); if (rua != null) { - Trajetoria.Add(new List()); + TrajetoriaMapa.Add(new List()); 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 Caminho = new List(); + List> _TrajetoriaProjetada = new List>(); - 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 RuaAtual = Trajetoria.Skip(i).First(); + List RuaAtual = TrajetoriaMapa.Skip(i).First(); double anguloProjetar = GPSService.CalcularOrientacao(RuaAtual.First(), RuaAtual.Skip(1).First()) + 90; List 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 _caminho = new List(); - 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 _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 _caminho = new List(); // 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 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>(_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 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 lista = new List() + { + 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 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 diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoMapaGPSModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoMapaGPSModel.cs index 072b85aaf..6666361ea 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoMapaGPSModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoMapaGPSModel.cs @@ -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(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 _EtapasAnteriores = new List() { 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 trechoCarro = new List() - { - GPSService.PenultimaLeitura, - GPSService.UltimaLeitura - }; - List trechoCaminho = new List() - { - 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() : - 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 Trecho = new List(); - 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 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 diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index c84c627dd..1586a670e 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -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, diff --git a/AgroBase/AgroBase/Services/APIService.cs b/AgroBase/AgroBase/Services/APIService.cs index 82df9ebe3..9d19b785b 100644 --- a/AgroBase/AgroBase/Services/APIService.cs +++ b/AgroBase/AgroBase/Services/APIService.cs @@ -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(); } } } diff --git a/AgroBase/AgroBase/Services/GPSService.cs b/AgroBase/AgroBase/Services/GPSService.cs index 605baf200..641fa68e6 100644 --- a/AgroBase/AgroBase/Services/GPSService.cs +++ b/AgroBase/AgroBase/Services/GPSService.cs @@ -283,9 +283,9 @@ namespace AgroBase.Services public static (GPSModel, bool) PontoMaisProximoRuaProjetada(List 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> PontosAproximacao = new Dictionary>(); + // 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() { { 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 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 PontosAproximacao = new Dictionary(); + + // 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 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; diff --git a/AgroBase/AgroBase/Services/KinectService.cs b/AgroBase/AgroBase/Services/KinectService.cs index 68a5c06e1..88fa20228 100644 --- a/AgroBase/AgroBase/Services/KinectService.cs +++ b/AgroBase/AgroBase/Services/KinectService.cs @@ -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; diff --git a/AgroBase/AgroBase/bin/Debug/AgroBase.pdb b/AgroBase/AgroBase/bin/Debug/AgroBase.pdb index 02c0d4642..d5f48ba17 100644 Binary files a/AgroBase/AgroBase/bin/Debug/AgroBase.pdb and b/AgroBase/AgroBase/bin/Debug/AgroBase.pdb differ diff --git a/AgroBase/AgroBase/bin/Debug/DawnCache/data_1 b/AgroBase/AgroBase/bin/Debug/DawnCache/data_1 index 9c2316a7c..f442e6bf5 100644 Binary files a/AgroBase/AgroBase/bin/Debug/DawnCache/data_1 and b/AgroBase/AgroBase/bin/Debug/DawnCache/data_1 differ diff --git a/AgroBase/AgroBase/bin/Debug/GPUCache/data_1 b/AgroBase/AgroBase/bin/Debug/GPUCache/data_1 index fed2a190a..6793d741c 100644 Binary files a/AgroBase/AgroBase/bin/Debug/GPUCache/data_1 and b/AgroBase/AgroBase/bin/Debug/GPUCache/data_1 differ diff --git a/AgroBase/AgroBase/bin/Debug/Python/Output/mapa_interativo.html b/AgroBase/AgroBase/bin/Debug/Python/Output/mapa_interativo.html index 0d818f5ea..3188b9fda 100644 --- a/AgroBase/AgroBase/bin/Debug/Python/Output/mapa_interativo.html +++ b/AgroBase/AgroBase/bin/Debug/Python/Output/mapa_interativo.html @@ -25,7 +25,7 @@