478 lines
20 KiB
C#
478 lines
20 KiB
C#
using AgroBase.Models;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class SonarService
|
|
{
|
|
private double minDepth;
|
|
private int toleranciaDeteccaoSuperior;
|
|
private int toleranciaDeteccaoTrajetoria;
|
|
private static float sfocalLength;
|
|
private static float sensorWidthMM;
|
|
private static int depthFrameWidth;
|
|
|
|
public SonarService(double _minDepth, float _focalLeght, float _sensorWidthMM, int _depthFrameWidth, int _toleranciaCima, int _toleranciaBaixo)
|
|
{
|
|
minDepth = _minDepth;
|
|
toleranciaDeteccaoSuperior = _toleranciaCima;
|
|
toleranciaDeteccaoTrajetoria = _toleranciaBaixo;
|
|
|
|
sfocalLength = _focalLeght;
|
|
sensorWidthMM = _sensorWidthMM;
|
|
depthFrameWidth = _depthFrameWidth;
|
|
}
|
|
|
|
public List<SimulacaoDesvio> RealizarSimulacaoGranular(List<Obstaculo> obstaculosSimulados, float passoSimulacaoCm, float LimiteRiscoAceitavel)
|
|
{
|
|
List<SimulacaoDesvio> simulacoes = new List<SimulacaoDesvio>();
|
|
var obstaculosClone = obstaculosSimulados.Select(o => o.Clone()).ToList();
|
|
float distanciaPercorrida = 0;
|
|
int passo = 0;
|
|
|
|
while (obstaculosClone.Any())
|
|
{
|
|
var obstaculoCritico = obstaculosClone.OrderBy(o => o.DistanciaMedia_mm).First();
|
|
|
|
// Simular um passo
|
|
SimulacaoDesvio simulacaoAtual = SimularPasso(obstaculoCritico.DirecaoDesvio, obstaculoCritico.AnguloParaDesvio, obstaculosClone, passoSimulacaoCm, ref passo);
|
|
simulacoes.Add(simulacaoAtual);
|
|
|
|
distanciaPercorrida += passoSimulacaoCm * 10; // Convertendo de cm para mm
|
|
|
|
// Verificar se o robô ultrapassou o obstáculo crítico
|
|
if (distanciaPercorrida >= (obstaculoCritico.DistanciaMedia_mm - minDepth))
|
|
{
|
|
// Novo obstáculo crítico, ajustar o ângulo de desvio
|
|
obstaculosClone = simulacaoAtual.Obstaculos.Where(o => o.DistanciaMedia_mm > distanciaPercorrida).ToList();
|
|
|
|
if (obstaculosClone.Any())
|
|
{
|
|
obstaculoCritico = obstaculosClone.OrderBy(o => o.DistanciaMedia_mm).First();
|
|
simulacaoAtual = SimularPasso(obstaculoCritico.DirecaoDesvio, obstaculoCritico.AnguloParaDesvio, obstaculosClone, passoSimulacaoCm, ref passo);
|
|
simulacoes.Add(simulacaoAtual);
|
|
}
|
|
else
|
|
{
|
|
break; // Não há mais obstáculos a considerar
|
|
}
|
|
}
|
|
|
|
if (simulacaoAtual.RiscoTotal >= LimiteRiscoAceitavel || !obstaculosClone.Any())
|
|
{
|
|
break;
|
|
}
|
|
|
|
obstaculosClone = simulacaoAtual.Obstaculos.ToList();
|
|
}
|
|
|
|
return simulacoes;
|
|
}
|
|
|
|
private SimulacaoDesvio SimularPasso(Direcao direcaoDesvio, double anguloDesvio, List<Obstaculo> obstaculosSimulados, float passoSimulacaoCm, ref int passo)
|
|
{
|
|
passo++;
|
|
|
|
var obstaculosClone = obstaculosSimulados.Select(o => o.Clone()).ToList();
|
|
double novoAnguloDesvio = double.IsNaN(anguloDesvio) ? 0 : anguloDesvio;
|
|
|
|
// Conversão de ângulo para radianos para cálculos trigonométricos
|
|
double anguloEmRadianos = novoAnguloDesvio * (Math.PI / 180.0);
|
|
|
|
foreach (var obstaculo in obstaculosClone)
|
|
{
|
|
// Calcular o deslocamento em Y (distância até o robô) considerando o passo da simulação
|
|
float deslocamentoYmm = (passoSimulacaoCm * 10); // Convertendo de cm para mm
|
|
obstaculo.DistanciaMedia_mm -= deslocamentoYmm;
|
|
|
|
// Calcular o deslocamento em X (lateral) com base no ângulo de desvio
|
|
float deslocamentoXmm = Convert.ToInt32(deslocamentoYmm * Math.Tan(anguloEmRadianos));
|
|
|
|
// Atualizar as novas dimensões do obstáculo com base na nova distância
|
|
float ratioAtual = CalculatePixelToMMRatio(obstaculo.DistanciaMedia_mm);
|
|
float fatorCrescimento = CalculatePixelToMMRatio(obstaculo.DistanciaMedia_mm + deslocamentoYmm) / ratioAtual;
|
|
|
|
var b = ((obstaculo.Largura_mm * fatorCrescimento) - obstaculo.Largura_mm) / 2;
|
|
int deslocamentoXpx = (int)(deslocamentoXmm / ratioAtual);
|
|
var d = fatorCrescimento * obstaculo.Width;
|
|
var e = (int)((d - obstaculo.Width));
|
|
|
|
var c = ((obstaculo.Altura_mm * fatorCrescimento) - obstaculo.Altura_mm) / 2;
|
|
var f = fatorCrescimento * obstaculo.Height;
|
|
var g = (int)((f - obstaculo.Height) / 2);
|
|
int deslocamentoYpx = (int)(deslocamentoYmm / ratioAtual);
|
|
|
|
obstaculo.Y += (int)g;
|
|
if (direcaoDesvio == Direcao.Esquerda)
|
|
{
|
|
obstaculo.X += (int)(e / 2);
|
|
}
|
|
else if (direcaoDesvio == Direcao.Direita)
|
|
{
|
|
obstaculo.X -= (int)(e * 2);
|
|
}
|
|
|
|
|
|
|
|
obstaculo.Width = Convert.ToInt32(obstaculo.Width * fatorCrescimento);
|
|
obstaculo.Height = Convert.ToInt32(obstaculo.Height * fatorCrescimento);
|
|
}
|
|
|
|
double riscoTotal = CalcularRiscoTotal(obstaculosClone);
|
|
|
|
return new SimulacaoDesvio
|
|
{
|
|
DirecaoDesvio = direcaoDesvio,
|
|
AnguloDesvio = novoAnguloDesvio, // Mantém o ângulo original até a ultrapassagem do obstáculo crítico
|
|
RiscoTotal = riscoTotal,
|
|
Obstaculos = obstaculosClone
|
|
};
|
|
}
|
|
|
|
private double CalcularRiscoTotal(List<Obstaculo> obstaculos)
|
|
{
|
|
double riscoTotal = 0;
|
|
|
|
foreach (var obstaculo in obstaculos)
|
|
{
|
|
if (obstaculo.DirecaoDesvio != Direcao.Parado)
|
|
{
|
|
riscoTotal += (1 / obstaculo.DistanciaMedia_mm) * 1; // obstaculo.PercentualCobertura;
|
|
}
|
|
else
|
|
{
|
|
riscoTotal += 1;
|
|
}
|
|
}
|
|
|
|
return riscoTotal;
|
|
}
|
|
|
|
public static float CalculatePixelToMMRatio(float distance)
|
|
{
|
|
// Exemplo de cálculo simplificado da relação pixels para milímetros
|
|
// A relação exata pode variar e pode precisar de ajustes
|
|
// Aqui assumimos uma relação linear simplificada
|
|
|
|
float pixelToMM = (distance / sfocalLength) * (sensorWidthMM / depthFrameWidth) * 10;
|
|
return pixelToMM;
|
|
}
|
|
|
|
public static Bitmap ExibirObstaculos(Bitmap bitmap, SonarCamModel leitura, bool ExibirTexto = false)
|
|
{
|
|
if (bitmap == null || (!Variaveis.OperacaoEmAndamento.Parametros.Controle.SonarAtivado && Variaveis.OperacaoEmAndamento.Iniciado))
|
|
{
|
|
return bitmap;
|
|
}
|
|
|
|
try
|
|
{
|
|
var _bitmapMap = (Bitmap)bitmap.Clone();
|
|
|
|
using (Graphics g = Graphics.FromImage(_bitmapMap))
|
|
{
|
|
// Calcular fatores de escala
|
|
double fatorX = (double)_bitmapMap.Width / leitura.X_Max;
|
|
double fatorY = (double)_bitmapMap.Height / leitura.Y_Max;
|
|
|
|
// Desenhar a trajetória do robô com os fatores aplicados
|
|
DesenharTrajetoriaRobo(g, leitura.Subdivisoes, fatorX, fatorY);
|
|
|
|
foreach (var obstaculo in leitura.Obstaculos)
|
|
{
|
|
Pen Cor = (obstaculo.Tipo == TipoObstaculo.Ressalto ? Pens.Red : obstaculo.Tipo == TipoObstaculo.Rebaixo ? Pens.Blue : Pens.Black);
|
|
Rectangle rect = new Rectangle(obstaculo.X, obstaculo.Y, obstaculo.Width, obstaculo.Height);
|
|
g.DrawRectangle(Cor, rect);
|
|
|
|
if (ExibirTexto)
|
|
{
|
|
string distanceText = $"D: {obstaculo.DistanciaMedia_mm:F1} mm, L: {obstaculo.Largura_mm:F1} mm, A: {obstaculo.Altura_mm:F1} mm";
|
|
Font font = new Font("Arial", 12);
|
|
SizeF textSize = g.MeasureString(distanceText, font);
|
|
PointF textLocation = new PointF(obstaculo.X + (obstaculo.Width - textSize.Width) / 2, obstaculo.Y + (obstaculo.Height - textSize.Height) / 2);
|
|
g.DrawString(distanceText, font, Brushes.Red, textLocation);
|
|
}
|
|
}
|
|
|
|
if (leitura.DesvioNecessario && leitura.DirecaoDesvio != Direcao.Parado)
|
|
{
|
|
// Desenhar a seta
|
|
DesenharSeta(g, _bitmapMap.Width, _bitmapMap.Height, 90 - (float)leitura.AnguloDesvio);
|
|
}
|
|
}
|
|
|
|
return _bitmapMap;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
|
|
}
|
|
|
|
return bitmap;
|
|
}
|
|
|
|
private static void DesenharSeta(Graphics g, int largura, int altura, float angulo)
|
|
{
|
|
// Ponto de início (base da seta)
|
|
PointF pontoInicio = new PointF(largura / 2, altura);
|
|
|
|
// Comprimento da seta (2/3 da altura da tela)
|
|
float comprimentoSeta = altura * 1 / 3;
|
|
|
|
// Cálculo do ponto final da seta com base no ângulo
|
|
float anguloRad = (float)(Math.PI * angulo / 180.0);
|
|
PointF pontoFinal = new PointF(
|
|
pontoInicio.X + comprimentoSeta * (float)Math.Cos(anguloRad),
|
|
pontoInicio.Y - comprimentoSeta * (float)Math.Sin(anguloRad)
|
|
);
|
|
|
|
// Desenhar a linha da seta
|
|
Pen penSeta = new Pen(Color.Orange, 4);
|
|
g.DrawLine(penSeta, pontoInicio, pontoFinal);
|
|
|
|
// Desenhar as pontas da seta
|
|
float tamanhoPonta = 20;
|
|
PointF ponta1 = new PointF(
|
|
pontoFinal.X + tamanhoPonta * (float)Math.Cos(anguloRad + Math.PI * 3 / 4),
|
|
pontoFinal.Y - tamanhoPonta * (float)Math.Sin(anguloRad + Math.PI * 3 / 4)
|
|
);
|
|
PointF ponta2 = new PointF(
|
|
pontoFinal.X + tamanhoPonta * (float)Math.Cos(anguloRad - Math.PI * 3 / 4),
|
|
pontoFinal.Y - tamanhoPonta * (float)Math.Sin(anguloRad - Math.PI * 3 / 4)
|
|
);
|
|
|
|
g.DrawLine(penSeta, pontoFinal, ponta1);
|
|
g.DrawLine(penSeta, pontoFinal, ponta2);
|
|
}
|
|
|
|
public static void DesenharTrajetoriaRobo(Graphics g, List<Subdivisao> subdivisoes, double fatorX, double fatorY)
|
|
{
|
|
var subdivisoesTrajetoria = subdivisoes.Where(x => !x.IsZonaSuperior).ToList();
|
|
DesenharSubdivisoes(g, subdivisoesTrajetoria, subdivisoesTrajetoria.FirstOrDefault()?.Tolerancia ?? 100, Color.Green, Color.Green, fatorX, fatorY);
|
|
|
|
var subdivisoesSuperior = subdivisoes.Where(x => x.IsZonaSuperior).ToList();
|
|
DesenharSubdivisoes(g, subdivisoesSuperior, subdivisoesSuperior.FirstOrDefault()?.Tolerancia ?? 100, Color.Red, Color.Red, fatorX, fatorY);
|
|
}
|
|
|
|
// Atualizado para aplicar fatorX e fatorY nas posições e tamanhos
|
|
private static void DesenharSubdivisoes(Graphics g, List<Subdivisao> subdivisoes, int tolerancia, Color corContorno, Color corPreenchimento, double fatorX, double fatorY)
|
|
{
|
|
foreach (var subdivisao in subdivisoes)
|
|
{
|
|
Color cor = subdivisao.ObstaculoDetectado ? Color.FromArgb(128, corPreenchimento) : Color.Transparent;
|
|
|
|
Brush brush = new SolidBrush(cor);
|
|
Pen pen = new Pen(corContorno);
|
|
|
|
// Aplicando os fatores de escala
|
|
int x = (int)(subdivisao.X * fatorX);
|
|
int y = (int)(subdivisao.Y * fatorY);
|
|
int largura = (int)(subdivisao.Largura * fatorX);
|
|
int altura = (int)(subdivisao.Altura * fatorY);
|
|
|
|
Rectangle rect = new Rectangle(x, y, largura, altura);
|
|
g.FillRectangle(brush, rect);
|
|
g.DrawRectangle(pen, rect);
|
|
}
|
|
}
|
|
|
|
|
|
public List<Obstaculo> IdentificarObstaculos(List<Subdivisao> subdivisoes, bool sonarAtivado)
|
|
{
|
|
if (!sonarAtivado || subdivisoes == null)
|
|
{
|
|
return new List<Obstaculo>();
|
|
}
|
|
|
|
double tolerancia = 100.0;
|
|
|
|
List<List<Subdivisao>> AgrupamentoPorProfundidade = new List<List<Subdivisao>>();
|
|
foreach (var sub in subdivisoes.Where(x => x.ObstaculoDetectado))
|
|
{
|
|
bool add = false;
|
|
foreach (var grupo in AgrupamentoPorProfundidade)
|
|
{
|
|
if (grupo.Any(x => Math.Abs(x.ProfundidadeMedia - sub.ProfundidadeMedia) <= tolerancia))
|
|
{
|
|
grupo.Add(sub);
|
|
add = true;
|
|
continue;
|
|
}
|
|
}
|
|
if (!add)
|
|
{
|
|
AgrupamentoPorProfundidade.Add(new List<Subdivisao>() { sub });
|
|
}
|
|
}
|
|
|
|
List<List<Subdivisao>> AgrupamentoPorAdjacencia = new List<List<Subdivisao>>();
|
|
foreach (var grupo in AgrupamentoPorProfundidade)
|
|
{
|
|
while (grupo.Any())
|
|
{
|
|
var subgrupo = new List<Subdivisao> { grupo[0] };
|
|
grupo.RemoveAt(0);
|
|
|
|
bool adicionou = true;
|
|
while (adicionou)
|
|
{
|
|
adicionou = false;
|
|
var adjacentes = grupo.Where(s =>
|
|
subgrupo.Any(x =>
|
|
Math.Abs(x.X - s.X) <= Math.Max(x.Largura, s.Largura) &&
|
|
(x.IsZonaSuperior != s.IsZonaSuperior ?
|
|
VerificarContinuidade(x, s) :
|
|
Math.Abs(x.Y - s.Y) <= (Math.Max(x.Altura, s.Altura) + 1)))).ToList();
|
|
|
|
if (adjacentes.Any())
|
|
{
|
|
subgrupo.AddRange(adjacentes);
|
|
foreach (var adj in adjacentes)
|
|
{
|
|
grupo.Remove(adj);
|
|
}
|
|
adicionou = true;
|
|
}
|
|
}
|
|
AgrupamentoPorAdjacencia.Add(subgrupo);
|
|
}
|
|
}
|
|
|
|
List<Obstaculo> obstaculos = new List<Obstaculo>();
|
|
foreach (var grupoSubdivisoes in AgrupamentoPorAdjacencia)
|
|
{
|
|
var obstaculo = CriarObstaculo(grupoSubdivisoes);
|
|
if (obstaculo.DistanciaMedia_mm > 0 && obstaculo.DesvioNecessario)
|
|
{
|
|
obstaculo.AtualizarDados(depthFrameWidth, minDepth);
|
|
|
|
obstaculos.Add(obstaculo);
|
|
}
|
|
}
|
|
|
|
return obstaculos;
|
|
}
|
|
|
|
// Método auxiliar para verificar continuidade entre zonas superior e inferior
|
|
private bool VerificarContinuidade(Subdivisao sub1, Subdivisao sub2)
|
|
{
|
|
// Se as subdivisões estão na mesma zona, são adjacentes se tocarem fisicamente
|
|
if (sub1.IsZonaSuperior == sub2.IsZonaSuperior)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// Se uma subdivisão está na zona superior e a outra na inferior, verifique se estão alinhadas verticalmente
|
|
// Isso considera a adjacência na transição entre as zonas
|
|
if (sub1.IsZonaSuperior && !sub2.IsZonaSuperior)
|
|
{
|
|
return sub1.idxY == sub2.idxY - 1;
|
|
}
|
|
|
|
if (!sub1.IsZonaSuperior && sub2.IsZonaSuperior)
|
|
{
|
|
return sub1.idxY - 1 == sub2.idxY;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
private Obstaculo CriarObstaculo(List<Subdivisao> grupoObstaculo)
|
|
{
|
|
int minX = grupoObstaculo.Min(s => s.X);
|
|
int minY = grupoObstaculo.Min(s => s.Y);
|
|
int maxX = grupoObstaculo.Max(s => s.X + s.Largura);
|
|
int maxY = grupoObstaculo.Max(s => s.Y + s.Altura);
|
|
|
|
int larguraPixels = maxX - minX;
|
|
int alturaPixels = maxY - minY;
|
|
|
|
// Calcular a distância média do obstáculo
|
|
float distanciaMedia = (float)grupoObstaculo.Average(s => s.ProfundidadeMedia);
|
|
|
|
// Calcular a relação pixels para milímetros
|
|
float pixelToMM = CalculatePixelToMMRatio(distanciaMedia);
|
|
|
|
// Converter dimensões de pixels para milímetros
|
|
float larguraMM = larguraPixels * pixelToMM;
|
|
float alturaMM = alturaPixels * pixelToMM;
|
|
|
|
Obstaculo obstaculo = new Obstaculo
|
|
{
|
|
X = minX,
|
|
Y = minY,
|
|
Width = larguraPixels,
|
|
Height = alturaPixels,
|
|
DistanciaMedia_mm = distanciaMedia,
|
|
Largura_mm = larguraMM,
|
|
Altura_mm = alturaMM,
|
|
ObstaculoSuperior = grupoObstaculo.Any(x => x.IsZonaSuperior),
|
|
Tolerancia = grupoObstaculo.Any(x => x.IsZonaSuperior) ? toleranciaDeteccaoSuperior : toleranciaDeteccaoTrajetoria,
|
|
ObstaculoTrajetoria = grupoObstaculo.Any(x => !x.IsZonaSuperior),
|
|
Tipo = grupoObstaculo.Count(x => x.Tipo == TipoObstaculo.Ressalto) > grupoObstaculo.Count(x => x.Tipo == TipoObstaculo.Rebaixo) ? TipoObstaculo.Ressalto : TipoObstaculo.Rebaixo,
|
|
};
|
|
|
|
return obstaculo;
|
|
}
|
|
|
|
public List<Subdivisao> ConverterSubdivisoes(OAKDLiteSubdivisoesModel modelo)
|
|
{
|
|
var lista = new List<Subdivisao>();
|
|
|
|
if (modelo == null)
|
|
{
|
|
return lista;
|
|
}
|
|
|
|
if (modelo?.subdivisoes_cima?.celulas != null)
|
|
{
|
|
foreach (var celula in modelo.subdivisoes_cima.celulas)
|
|
{
|
|
lista.Add(new Subdivisao
|
|
{
|
|
idxX = celula.coluna,
|
|
idxY = celula.linha,
|
|
X = celula.x,
|
|
Y = celula.y,
|
|
Largura = celula.largura,
|
|
Altura = celula.altura,
|
|
ProfundidadeMedia = (int)Math.Round(celula.profundidade_media),
|
|
ProfundidadeEsperada = (int)Math.Round(celula.profundidade_calibragem),
|
|
Tolerancia = toleranciaDeteccaoSuperior,
|
|
IsZonaSuperior = true
|
|
});
|
|
}
|
|
}
|
|
|
|
if (modelo?.subdivisoes_chao?.celulas != null)
|
|
{
|
|
foreach (var celula in modelo.subdivisoes_chao.celulas)
|
|
{
|
|
lista.Add(new Subdivisao
|
|
{
|
|
idxX = celula.coluna,
|
|
idxY = celula.linha,
|
|
X = celula.x,
|
|
Y = celula.y,
|
|
Largura = celula.largura,
|
|
Altura = celula.altura,
|
|
ProfundidadeMedia = (int)Math.Round(celula.profundidade_media),
|
|
ProfundidadeEsperada = (int)Math.Round(celula.profundidade_calibragem),
|
|
Tolerancia = toleranciaDeteccaoTrajetoria,
|
|
IsZonaSuperior = false
|
|
});
|
|
}
|
|
}
|
|
|
|
return lista;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
}
|