512 lines
20 KiB
C#
512 lines
20 KiB
C#
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models
|
|
{
|
|
public class SonarCamModel
|
|
{
|
|
public double minDepth = 25;
|
|
private double maxDepth = 10000;
|
|
private float focalLength = 440.0f;
|
|
private float sensorWidthMM = 5.76f;
|
|
public int depthFrameWidth = 640;
|
|
private static int toleranciaDeteccaoSuperior = 200;
|
|
private static int toleranciaDeteccaoTrajetoria = 100;
|
|
private float distProjetarTrajetoriaMm = 2000f;
|
|
public static double MargemSegurancaDesvio = 5;
|
|
public static int AnguloDesvioMaximo = 60;
|
|
public SonarService service;
|
|
|
|
public SonarCamModel()
|
|
{
|
|
service = new SonarService(minDepth, focalLength, sensorWidthMM, depthFrameWidth, toleranciaDeteccaoSuperior, toleranciaDeteccaoTrajetoria);
|
|
}
|
|
|
|
public DateTime Momento { get; set; }
|
|
public bool Iniciado { get; set; }
|
|
private void AtualizarIniciado()
|
|
{
|
|
Iniciado = Variaveis.OperacaoEmAndamento.CameraCaminho?.Iniciada ?? false;
|
|
}
|
|
|
|
public int X_Max { get; set; }
|
|
public int Y_Max { get; set; }
|
|
private void AtualizarLarguraAltura(int x_max, int y_max)
|
|
{
|
|
X_Max = x_max;
|
|
Y_Max = y_max;
|
|
}
|
|
|
|
public List<Subdivisao> Subdivisoes { get; set; } = new List<Subdivisao>();
|
|
public List<Obstaculo> Obstaculos { get; set; } = new List<Obstaculo>();
|
|
public Obstaculo ObstaculoCritico { get; set; }
|
|
private void AtualizarObstaculoCritico()
|
|
{
|
|
// Ordenar os obstáculos por criticidade
|
|
var obstaculoCritico = Obstaculos
|
|
.OrderByDescending(o => o.DesvioNecessario) // Primeiro, priorizar os que precisam de desvio
|
|
.ThenBy(o => o.DistanciaMedia_mm) // Priorizar os mais próximos
|
|
.ThenByDescending(o => o.Largura_mm) // Depois, os mais largos
|
|
.ThenByDescending(o => o.Altura_mm) // Em seguida, os mais altos
|
|
.ThenByDescending(o => Math.Abs(o.AnguloParaDesvio)) // Finalmente, os que requerem maior ângulo de desvio
|
|
.FirstOrDefault();
|
|
ObstaculoCritico = obstaculoCritico;
|
|
}
|
|
public bool DesvioNecessario { get; set; }
|
|
private void AtualizarDesvioNecessario()
|
|
{
|
|
DesvioNecessario = Obstaculos.Any(x => x.DesvioNecessario);
|
|
}
|
|
public double AnguloDesvio { get; set; }
|
|
public Direcao DirecaoDesvio { get; set; }
|
|
private void AtualizarAnguloDirecaoDesvio()
|
|
{
|
|
var ObstaculosDetectados = Obstaculos.Where(x => x.DesvioNecessario).ToList();
|
|
|
|
if (!ObstaculosDetectados.Any())
|
|
{
|
|
AnguloDesvio = 0;
|
|
DirecaoDesvio = Direcao.Frente;
|
|
}
|
|
if (ObstaculosDetectados.Any(x => x.DirecaoDesvio == Direcao.Parado))
|
|
{
|
|
AnguloDesvio = 0;
|
|
DirecaoDesvio = Direcao.Parado;
|
|
}
|
|
if (ObstaculosDetectados.Count() == 1)
|
|
{
|
|
AnguloDesvio = ObstaculoCritico.AnguloParaDesvio;
|
|
DirecaoDesvio = ObstaculoCritico.DirecaoDesvio;
|
|
}
|
|
|
|
var obstaculosLista = ObstaculosDetectados.Select(o => o.Clone()).ToList();
|
|
float LimiteRiscoAceitavel = 0.1f;
|
|
|
|
//var Simulacoes = RealizarSimulacao(obstaculosLista, LimiteRiscoAceitavel);
|
|
var Simulacoes = service.RealizarSimulacaoGranular(obstaculosLista, 10, LimiteRiscoAceitavel);
|
|
|
|
if (Simulacoes.Any(x => x.DirecaoDesvio == Direcao.Parado || x.RiscoTotal > LimiteRiscoAceitavel))
|
|
{
|
|
AnguloDesvio = 0;
|
|
DirecaoDesvio = Direcao.Parado;
|
|
}
|
|
|
|
// Se a primeira simulação for segura
|
|
var SimulacaoSegura = Simulacoes.First();
|
|
AnguloDesvio = SimulacaoSegura.AnguloDesvio;
|
|
DirecaoDesvio = SimulacaoSegura.DirecaoDesvio;
|
|
}
|
|
public float EspacoLivreEsquerda { get; set; }
|
|
private void AtualizarEspacoLivreEsquerda()
|
|
{
|
|
float dE = (depthFrameWidth / 2.0f);
|
|
float rE = SonarService.CalculatePixelToMMRatio(distProjetarTrajetoriaMm);
|
|
float espacoLivreE = dE * rE;
|
|
|
|
var obstaculosE = Obstaculos.Where(o => o.DirecaoDesvio == Direcao.Direita).ToList();
|
|
|
|
EspacoLivreEsquerda = obstaculosE.Any() ? obstaculosE.Min(x => x.DistanciaLivreEsquerda) : espacoLivreE;
|
|
}
|
|
public float EspacoLivreDireita { get; set; }
|
|
private void AtualizarEspacoLivreDireita()
|
|
{
|
|
float dD = (depthFrameWidth / 2.0f);
|
|
float rD = SonarService.CalculatePixelToMMRatio(distProjetarTrajetoriaMm);
|
|
float espacoLivreD = dD * rD;
|
|
|
|
var obstaculosD = Obstaculos.Where(o => o.DirecaoDesvio == Direcao.Esquerda).ToList();
|
|
|
|
EspacoLivreDireita = obstaculosD.Any() ? obstaculosD.Min(x => x.DistanciaLivreDireita) : espacoLivreD;
|
|
}
|
|
|
|
public double AnguloRuaCamera { get; set; } = 0;
|
|
|
|
private void AtualizarDadosObstaculos(int x_max, int y_max)
|
|
{
|
|
AtualizarIniciado();
|
|
AtualizarLarguraAltura(x_max, y_max);
|
|
|
|
if (Obstaculos == null || !Obstaculos.Any())
|
|
{
|
|
ObstaculoCritico = null;
|
|
DesvioNecessario = false;
|
|
DirecaoDesvio = Direcao.Frente;
|
|
AnguloDesvio = 0;
|
|
EspacoLivreDireita = 0;
|
|
EspacoLivreEsquerda = 0;
|
|
return;
|
|
}
|
|
|
|
AtualizarObstaculoCritico();
|
|
AtualizarDesvioNecessario();
|
|
AtualizarAnguloDirecaoDesvio();
|
|
AtualizarEspacoLivreEsquerda();
|
|
AtualizarEspacoLivreDireita();
|
|
}
|
|
|
|
public SonarCamModel Clone()
|
|
{
|
|
return new SonarCamModel()
|
|
{
|
|
Momento = Momento,
|
|
Iniciado = Iniciado,
|
|
X_Max = X_Max,
|
|
Y_Max = Y_Max,
|
|
AnguloDesvio = AnguloDesvio,
|
|
depthFrameWidth = depthFrameWidth,
|
|
distProjetarTrajetoriaMm = distProjetarTrajetoriaMm,
|
|
focalLength = focalLength,
|
|
maxDepth = maxDepth,
|
|
minDepth = minDepth,
|
|
Obstaculos = new List<Obstaculo>(Obstaculos),
|
|
sensorWidthMM = sensorWidthMM,
|
|
Subdivisoes = new List<Subdivisao>(Subdivisoes),
|
|
DesvioNecessario = DesvioNecessario,
|
|
DirecaoDesvio = DirecaoDesvio,
|
|
ObstaculoCritico = ObstaculoCritico?.Clone(),
|
|
EspacoLivreDireita = EspacoLivreDireita,
|
|
EspacoLivreEsquerda = EspacoLivreEsquerda,
|
|
AnguloRuaCamera = AnguloRuaCamera,
|
|
};
|
|
}
|
|
|
|
public void DetectarObstaculos(OAKDLiteSubdivisoesModel modelo)
|
|
{
|
|
Subdivisoes = service.ConverterSubdivisoes(modelo);
|
|
Obstaculos = service.IdentificarObstaculos(Subdivisoes, Variaveis.OperacaoEmAndamento.Controle.SonarAtivado);
|
|
|
|
AtualizarDadosObstaculos(modelo?.x_max ?? 640, modelo?.y_max ?? 480);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
public class Obstaculo
|
|
{
|
|
public int X { get; set; }
|
|
public int Y { get; set; }
|
|
public int Width { get; set; }
|
|
public int Height { get; set; }
|
|
public float DistanciaMedia_mm { get; set; }
|
|
public float Largura_mm { get; set; }
|
|
public float Altura_mm { get; set; }
|
|
public bool ObstaculoTrajetoria { get; set; }
|
|
public bool ObstaculoSuperior { get; set; }
|
|
public int Tolerancia { get; set; }
|
|
public TipoObstaculo Tipo { get; set; }
|
|
public bool DesvioNecessario { get; set; }
|
|
private void AtualizarDesvioNecessario()
|
|
{
|
|
bool larguraMinima = Largura_mm >= Tolerancia;
|
|
bool alutraMinima = Altura_mm >= Tolerancia; // (VariaveisEquipamento.DiametroRoda * 1000 / 2); // No minimo precisa ser mais alto que a metade da roda
|
|
|
|
DesvioNecessario = larguraMinima && alutraMinima;
|
|
}
|
|
public float PercentualEsquerda { get; set; }
|
|
public float PercentualDireita { get; set; }
|
|
public Direcao DirecaoDesvio { get; set; }
|
|
private void AtualizarDirecaoDesvio(int depthFrameWidth, double minDepth)
|
|
{
|
|
Direcao direcaoDesvio = Direcao.Frente;
|
|
|
|
if (DesvioNecessario)
|
|
{
|
|
// Calcular as posições extremas do obstáculo
|
|
float posicaoInicioX = X;
|
|
float posicaoFimX = X + Width;
|
|
|
|
float centro = depthFrameWidth / 2;
|
|
|
|
// Calcular quanto do obstáculo está à esquerda e à direita do centro
|
|
float percentualEsquerda = 0;
|
|
float percentualDireita = 0;
|
|
|
|
if (posicaoFimX <= centro)
|
|
{
|
|
// Obstáculo todo à esquerda
|
|
percentualEsquerda = 1;
|
|
}
|
|
else if (posicaoInicioX >= centro)
|
|
{
|
|
// Obstáculo todo à direita
|
|
percentualDireita = 1;
|
|
}
|
|
else
|
|
{
|
|
// Parte do obstáculo está à esquerda e parte à direita
|
|
float larguraEsquerda = centro - posicaoInicioX;
|
|
float larguraDireita = posicaoFimX - centro;
|
|
|
|
percentualEsquerda = (larguraEsquerda / Width);
|
|
percentualDireita = (larguraDireita / Width);
|
|
}
|
|
|
|
// Definir a direção do desvio com base nos percentuais
|
|
if (DistanciaMedia_mm < (minDepth * 1.1) && (DistanciaDaLateralDireita < VariaveisEquipamento.LarguraDireita || DistanciaLivreEsquerda < VariaveisEquipamento.LarguraEsquerda))
|
|
{
|
|
direcaoDesvio = Direcao.Parado;
|
|
}
|
|
else if (percentualDireita > percentualEsquerda)
|
|
{
|
|
direcaoDesvio = Direcao.Esquerda; // Desviar para a esquerda
|
|
}
|
|
else
|
|
{
|
|
direcaoDesvio = Direcao.Direita; // Desviar para a direita
|
|
}
|
|
|
|
// Armazenar os percentuais como novas variáveis
|
|
PercentualEsquerda = percentualEsquerda;
|
|
PercentualDireita = percentualDireita;
|
|
}
|
|
|
|
DirecaoDesvio = direcaoDesvio;
|
|
}
|
|
public float DistanciaLivreEsquerda { get; set; }
|
|
private void AtualizarDistanciaLivreEsquerda()
|
|
{
|
|
float ratio = SonarService.CalculatePixelToMMRatio(DistanciaMedia_mm);
|
|
DistanciaLivreEsquerda = (float)(X * ratio);
|
|
}
|
|
public float DistanciaLivreDireita { get; set; }
|
|
private void AtualizarDistanciaLivreDireita(int depthFrameWidth)
|
|
{
|
|
float ratio = SonarService.CalculatePixelToMMRatio(DistanciaMedia_mm);
|
|
int tamanho = depthFrameWidth - (X + Width);
|
|
DistanciaLivreDireita = (float)((tamanho) * ratio);
|
|
}
|
|
public double DistanciaDaLateralDireita { get; set; }
|
|
private void AtualizarDistanciaLateralDireita(int depthFrameWidth)
|
|
{
|
|
// Coordenadas do ponto central do robô (origem)
|
|
int centroX = depthFrameWidth / 2;
|
|
double profundidadeZ = DistanciaMedia_mm;
|
|
|
|
float ratio = SonarService.CalculatePixelToMMRatio((float)profundidadeZ);
|
|
DistanciaDaLateralDireita = (((X + Width) - centroX) * ratio) / 10;
|
|
}
|
|
public double DistanciaDaLateralEsquerda { get; set; }
|
|
private void AtualizarDistanciaLateralEsquerda(int depthFrameWidth)
|
|
{
|
|
// Coordenadas do ponto central do robô (origem)
|
|
int centroX = depthFrameWidth / 2;
|
|
double profundidadeZ = DistanciaMedia_mm;
|
|
|
|
float ratio = SonarService.CalculatePixelToMMRatio((float)profundidadeZ);
|
|
DistanciaDaLateralEsquerda = ((centroX - X) * ratio) / 10;
|
|
}
|
|
public bool Adireita { get; set; }
|
|
private void AtualizarADireita(int depthFrameWidth)
|
|
{
|
|
int origemX = depthFrameWidth / 2;
|
|
Adireita = X + Width > origemX;
|
|
}
|
|
public bool Aesquerda { get; set; }
|
|
private void AtualizarAEsquerda(int depthFrameWidth)
|
|
{
|
|
int origemX = depthFrameWidth / 2;
|
|
Aesquerda = X < origemX;
|
|
}
|
|
public double DeslocamentoNecessarioDesvio { get; set; }
|
|
private void AtualizarDeslocamentoNecessarioDesvio()
|
|
{
|
|
if (DirecaoDesvio == Direcao.Direita)
|
|
{
|
|
DeslocamentoNecessarioDesvio = DeslocamentoNecessarioDireita;
|
|
}
|
|
else if (DirecaoDesvio == Direcao.Esquerda)
|
|
{
|
|
DeslocamentoNecessarioDesvio = DeslocamentoNecessarioEsquerda;
|
|
}
|
|
else
|
|
{
|
|
DeslocamentoNecessarioDesvio = 0;
|
|
}
|
|
}
|
|
public double DeslocamentoNecessarioEsquerda { get; set; }
|
|
private void AtualizarDeslocamentoNecessarioEsquerda()
|
|
{
|
|
if (PercentualDireita > 0 || PercentualEsquerda > 0)
|
|
{
|
|
double deslocamento = ((((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2.0) + DistanciaDaLateralEsquerda) * (1 + PercentualEsquerda)) + SonarCamModel.MargemSegurancaDesvio;
|
|
if (deslocamento < 0)
|
|
{
|
|
deslocamento = 0;
|
|
}
|
|
DeslocamentoNecessarioEsquerda = deslocamento;
|
|
}
|
|
else
|
|
{
|
|
DeslocamentoNecessarioEsquerda = 0;
|
|
}
|
|
}
|
|
public double DeslocamentoNecessarioDireita { get; set; }
|
|
private void AtualizarDeslocamentoNecessarioDireita()
|
|
{
|
|
if (PercentualDireita > 0 || PercentualEsquerda > 0)
|
|
{
|
|
double deslocamento = ((((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2.0) + DistanciaDaLateralDireita) * (1 + PercentualDireita)) + SonarCamModel.MargemSegurancaDesvio;
|
|
if (deslocamento < 0)
|
|
{
|
|
deslocamento = 0;
|
|
}
|
|
DeslocamentoNecessarioDireita = deslocamento;
|
|
}
|
|
else
|
|
{
|
|
DeslocamentoNecessarioDireita = 0;
|
|
}
|
|
}
|
|
public double AnguloParaDesvio { get; set; }
|
|
private void AtualizarAnguloParaDesvio()
|
|
{
|
|
double anguloDesvio = 0;
|
|
Direcao direcaoDesvio = DirecaoDesvio;
|
|
|
|
if (direcaoDesvio == Direcao.Esquerda)
|
|
{
|
|
anguloDesvio = AnguloParaDesvioEsquerda;
|
|
}
|
|
else if (direcaoDesvio == Direcao.Direita)
|
|
{
|
|
anguloDesvio = AnguloParaDesvioDireita;
|
|
}
|
|
|
|
AnguloParaDesvio = anguloDesvio;
|
|
}
|
|
public double AnguloParaDesvioEsquerda { get; set; }
|
|
private void AtualizarAnguloParaDesvioEsquerda()
|
|
{
|
|
// Usando trigonometria para calcular o ângulo necessário
|
|
// Nota: Assegure-se de converter o resultado de arco seno para graus, se necessário
|
|
double anguloRad = Math.Atan((DistanciaMedia_mm / 10) / DeslocamentoNecessarioEsquerda);
|
|
double anguloDesvio = (anguloRad * (180 / Math.PI)); // Convertendo radianos para graus
|
|
|
|
anguloDesvio = anguloDesvio - 90;
|
|
|
|
// Atribuição do ângulo calculado ao ângulo de desvio, com verificação de limites
|
|
AnguloParaDesvioEsquerda = Math.Min(anguloDesvio, SonarCamModel.AnguloDesvioMaximo);
|
|
}
|
|
public double AnguloParaDesvioDireita { get; set; }
|
|
private void AtualizarAnguloParaDesvioDireita()
|
|
{
|
|
// Usando trigonometria para calcular o ângulo necessário
|
|
// Nota: Assegure-se de converter o resultado de arco seno para graus, se necessário
|
|
double anguloRad = Math.Atan((DistanciaMedia_mm / 10) / DeslocamentoNecessarioDireita);
|
|
double anguloDesvio = (anguloRad * (180 / Math.PI)); // Convertendo radianos para graus
|
|
|
|
anguloDesvio = 90 - anguloDesvio;
|
|
|
|
// Atribuição do ângulo calculado ao ângulo de desvio, com verificação de limites
|
|
AnguloParaDesvioDireita = Math.Min(anguloDesvio, SonarCamModel.AnguloDesvioMaximo);
|
|
}
|
|
|
|
public void AtualizarDados(int depthFrameWidth, double minDepth)
|
|
{
|
|
AtualizarDesvioNecessario();
|
|
AtualizarDirecaoDesvio(depthFrameWidth, minDepth);
|
|
AtualizarDistanciaLivreEsquerda();
|
|
AtualizarDistanciaLivreDireita(depthFrameWidth);
|
|
AtualizarDistanciaLateralDireita(depthFrameWidth);
|
|
AtualizarDistanciaLateralEsquerda(depthFrameWidth);
|
|
AtualizarADireita(depthFrameWidth);
|
|
AtualizarAEsquerda(depthFrameWidth);
|
|
AtualizarDeslocamentoNecessarioEsquerda();
|
|
AtualizarDeslocamentoNecessarioDireita();
|
|
AtualizarDeslocamentoNecessarioDesvio();
|
|
AtualizarAnguloParaDesvioEsquerda();
|
|
AtualizarAnguloParaDesvioDireita();
|
|
AtualizarAnguloParaDesvio();
|
|
}
|
|
|
|
public Obstaculo Clone()
|
|
{
|
|
return new Obstaculo()
|
|
{
|
|
Tolerancia = Tolerancia,
|
|
Altura_mm = Altura_mm,
|
|
DistanciaMedia_mm = DistanciaMedia_mm,
|
|
Height = Height,
|
|
Largura_mm = Largura_mm,
|
|
ObstaculoSuperior = ObstaculoSuperior,
|
|
ObstaculoTrajetoria = ObstaculoTrajetoria,
|
|
PercentualDireita = PercentualDireita,
|
|
PercentualEsquerda = PercentualEsquerda,
|
|
Tipo = Tipo,
|
|
Width = Width,
|
|
X = X,
|
|
Y = Y,
|
|
AnguloParaDesvioDireita = AnguloParaDesvioDireita,
|
|
AnguloParaDesvioEsquerda = AnguloParaDesvioEsquerda,
|
|
AnguloParaDesvio = AnguloParaDesvio,
|
|
DeslocamentoNecessarioEsquerda = DeslocamentoNecessarioEsquerda,
|
|
DeslocamentoNecessarioDesvio = DeslocamentoNecessarioDesvio,
|
|
Aesquerda = Aesquerda,
|
|
Adireita = Adireita,
|
|
DeslocamentoNecessarioDireita = DeslocamentoNecessarioDireita,
|
|
DesvioNecessario = DesvioNecessario,
|
|
DirecaoDesvio = DirecaoDesvio,
|
|
DistanciaDaLateralDireita = DistanciaDaLateralDireita,
|
|
DistanciaDaLateralEsquerda = DistanciaDaLateralEsquerda,
|
|
DistanciaLivreDireita = DistanciaLivreDireita,
|
|
DistanciaLivreEsquerda = DistanciaLivreEsquerda
|
|
};
|
|
}
|
|
|
|
}
|
|
|
|
public class Subdivisao
|
|
{
|
|
public int idxX { get; set; }
|
|
public int idxY { get; set; }
|
|
public int X { get; set; }
|
|
public int Y { get; set; }
|
|
public int Largura { get; set; }
|
|
public int Altura { get; set; }
|
|
public int ProfundidadeMedia { get; set; }
|
|
public int ProfundidadeEsperada { get; set; }
|
|
public int Tolerancia { get; set; }
|
|
public bool IsZonaSuperior { get; set; }
|
|
public int DiferencaProfundidade
|
|
{
|
|
get
|
|
{
|
|
return ProfundidadeMedia - ProfundidadeEsperada;
|
|
}
|
|
}
|
|
public bool ObstaculoDetectado
|
|
{
|
|
get
|
|
{
|
|
if (IsZonaSuperior)
|
|
{
|
|
return ProfundidadeMedia > 0 && (ProfundidadeMedia + Tolerancia) < ProfundidadeEsperada;
|
|
}
|
|
else
|
|
{
|
|
return Math.Abs(DiferencaProfundidade) > Tolerancia;
|
|
}
|
|
}
|
|
}
|
|
public TipoObstaculo Tipo
|
|
{
|
|
get
|
|
{
|
|
if (IsZonaSuperior)
|
|
{
|
|
return ObstaculoDetectado ? TipoObstaculo.Ressalto : TipoObstaculo.Rebaixo;
|
|
}
|
|
else
|
|
{
|
|
return ObstaculoDetectado && DiferencaProfundidade > 0 ? TipoObstaculo.Rebaixo : TipoObstaculo.Ressalto;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
}
|