agrobot_base/AgroBase/AgroBase/Forms/Simuladores/frmTreinamentoIA.cs

532 lines
21 KiB
C#

using AgroBase.Models;
using AgroBase.Services;
using Emgu.CV;
using Newtonsoft.Json;
using System;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Windows.Forms;
using static AgroBase.Models.Enums;
namespace AgroBase.Forms
{
public partial class frmTreinamentoIA : Form
{
string mqtt_topico_state = "robot/state";
string mqtt_topico_action = "robot/actions";
string latInicial = "-22,1789853333333";
string longInicial = "-47,3721381666667";
double anguloInicial = 273;
double distanciaInicial = 0.1;
StatusCarroMapa StatusCarro = StatusCarroMapa.Parado;
MapaDinamicoModel MapaDinamico;
bool calc = false;
bool parar = false;
double ultimaLat = 0;
double ultimaLon = 0;
double ultimoAnguloGps = 0;
DateTime UltimaAtualizacaoGps = DateTime.MinValue;
double tempoGps = (1.0 / GPSService.TaxaAmostragemHz);
int ticksControle = GPSService.TaxaAmostragemHz;
TreinamentoIAModel _robotState = new TreinamentoIAModel();
public frmTreinamentoIA()
{
InitializeComponent();
// Ativa o double buffering
this.DoubleBuffered = true;
this.SetStyle(ControlStyles.AllPaintingInWmPaint, true);
this.SetStyle(ControlStyles.UserPaint, true);
this.SetStyle(ControlStyles.OptimizedDoubleBuffer, true);
MapaDinamico = new MapaDinamicoModel(this, pnlZoomMapa);
}
private async void frmTreinamentoIA_Load(object sender, EventArgs e)
{
Variaveis.OperacaoEmAndamento.Mapa = new MapasModel()
{
pnlMapa = pnlMapa,
};
ReiniciarModelo();
await Variaveis.MqttService.AdicionarNovoTopico(mqtt_topico_state);
await Variaveis.MqttService.AdicionarNovoTopico(mqtt_topico_action, true, 2, async (message) =>
{
Console.WriteLine("Mensagem recebida pelo modelo de IA: " + message);
//btnAcrescentarGPS_Click(new Button(), new EventArgs());
var payload = message.Mensagem;
var action = JsonConvert.DeserializeObject<RobotControl>(payload);
await FuncoesGlobais.ExecutarMetodoComVerificacaoCrossThreadAsync(this, async () => ProcessRobotAction(action));
});
}
private async void frmTreinamentoIA_FormClosing(object sender, FormClosingEventArgs e)
{
var t0 = Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == mqtt_topico_state);
await Variaveis.MqttService.UnsubscribeAsync(t0);
var t1 = Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == mqtt_topico_action);
await Variaveis.MqttService.UnsubscribeAsync(t1);
Variaveis.OperacaoEmAndamento.Treinando = false;
}
private void ReiniciarModelo()
{
ultimaLat = double.Parse(latInicial);
ultimaLon = double.Parse(longInicial);
txtLatitude.Text = latInicial.ToString();
txtLongitude.Text = longInicial.ToString();
txtAnguloGPS.Text = anguloInicial.ToString("0.00");
txtDistanciaGPS.Text = distanciaInicial.ToString("0.00");
Variaveis.OperacaoEmAndamento.GPSTrajetoria = new System.Collections.Generic.List<GPSModel>();
Variaveis.OperacaoEmAndamento.Trajetoria = new TrajetoriaMapaOperacaoModel(Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa);
Variaveis.OperacaoEmAndamento.Trajetoria.ProjetarTrajetoriaFixa();
_robotState = new TreinamentoIAModel();
}
private void btnCarregarMapa_Click(object sender, EventArgs e)
{
Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click((Control)sender, e);
}
private void btnIniciarSimulacao_Click(object sender, EventArgs e)
{
if (!Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Any())
{
MessageBox.Show("Selecione as ruas para realizar a operação!");
return;
}
if (!Variaveis.OperacaoEmAndamento.Iniciado && !parar)
{
Variaveis.OperacaoEmAndamento.IniciarTreinamento();
}
btnIniciarSimulacao.Text = Variaveis.OperacaoEmAndamento.Iniciado ? "Parar Treinamento" : "Iniciar Treinamento";
if (Variaveis.OperacaoEmAndamento.Iniciado)
{
GPSService.PenultimaLeitura.Latitude = GPSService.UltimaLeitura.Latitude;
GPSService.PenultimaLeitura.Longitude = GPSService.UltimaLeitura.Longitude;
AtualizarDadosTela();
}
}
private void AtualizarDadosTela()
{
//btnCalcularAngulo_Click(new object(), new EventArgs());
StatusCarro = _robotState.Current_State.Status;
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.Trajetoria?.DirecaoCaminho ?? DirecaoCarroRua.Parado);
lblDistanciaOperacao.Text = "Distância Operação: " + (Variaveis.OperacaoEmAndamento.Trajetoria?.DistanciaTotal ?? 0).ToString("00.00") + " m";
lblDistanciaFinal.Text = "Distância Final: " + (Variaveis.OperacaoEmAndamento.Trajetoria?.DistanciaRestante ?? 0).ToString("00.00") + " m";
lblDistanciaLateral.Text = "Esq: " + _robotState.Current_State.DistanceToEdges.Left.ToString("0.00") + " m - Dir: " + _robotState.Current_State.DistanceToEdges.Right.ToString("0.00") + " m";
lblRua.Text = "Rua: " + (_robotState.Current_State.Street.InsideStreet ? "Dentro" : "Fora");
lblMargem.Text = "Margem: " + (_robotState.Current_State.Street.EdgeStreet ? "Sim" : "Não");
lblProximoPonto.Text = "Próximo Ponto: " + (Variaveis.OperacaoEmAndamento.Trajetoria.ProximoPonto.idxPontoCorredor);
lblAproximando.Text = (Variaveis.OperacaoEmAndamento.Trajetoria.ProximoPonto.Aproximando ? "Aproximando" : "Afastando");
lblDistProx.Text = "Distância Próximo Ponto: " + (_robotState.Current_State.NextPoints.FirstOrDefault()?.Distance ?? 0).ToString("0.00") + " m";
lblDistAnt.Text = "Distância Ponto Anterior: " + Variaveis.OperacaoEmAndamento.Trajetoria.PontoAtual.DistanciaAtual.ToString("0.00") + " m";
lblTempoEstimado.Text = "Tempo Estimado: " + Variaveis.OperacaoEmAndamento.Trajetoria?.TempoEstimadoRestante ?? "";
if (Variaveis.OperacaoEmAndamento.Iniciado)
{
pnlOrientacaoTrajeto.Invalidate();
pnlAnguloControle.Invalidate();
pnlOrientacaoCarro.Invalidate();
MapaDinamico.AtualizarDados(
KinectService.Iniciado && KinectService.Leitura.StatusDetecao ? KinectService.Leitura.ObstaculoCritico : null,
(float)_robotState.Current_State.Street.Orientation,
(float)_robotState.Current_State.Position.Orientation,
new GPSModel()
{
Latitude = _robotState.Current_State.Position.Latitude,
Longitude = _robotState.Current_State.Position.Longitude,
CursoVerdadeiro = _robotState.Current_State.Position.Orientation,
Momento = DateTime.Now
},
Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaDinamica,
Variaveis.OperacaoEmAndamento.Trajetoria.CorredorAtual.Pontos,
Variaveis.OperacaoEmAndamento.GPSTrajetoria,
Variaveis.OperacaoEmAndamento.Trajetoria.RuasPlantacao
);
}
else
{
if (btnIniciarSimulacao.Text.Contains("Parar"))
{
parar = true;
btnIniciarSimulacao_Click(new object(), new EventArgs());
parar = false;
}
Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas();
}
}
private void btnAcrescentarGPS_Click(object sender, EventArgs e)
{
if (chbEnviarDadosModelo.Checked)
{
double velocidadeMs = double.Parse(txtDistanciaGPS.Text) / tempoGps; // metros por segundo
ProcessRobotAction(new RobotControl()
{
Angle = double.Parse(txtAnguloControle.Text),
Speed = velocidadeMs,
MovementType = TipoMovimentoDirecional.RodasDianteiras
});
}
else
{
AtualizarDadosDeEstado();
}
}
private void pnlOrientacaoTrajeto_Paint(object sender, PaintEventArgs e)
{
try
{
double angulo = Variaveis.OperacaoEmAndamento.Trajetoria.AnguloCaminho;
FuncoesGlobais.DesenharInclinacao((Panel)sender, e, (float)angulo, 90);
}
catch { }
}
private void pnlOrientacaoCarro_Paint(object sender, PaintEventArgs e)
{
try
{
double angulo = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps.AnguloCarroDefinido;
FuncoesGlobais.DesenharInclinacao((Panel)sender, e, (float)angulo, 90);
}
catch { }
}
private void pnlAnguloControle_Paint(object sender, PaintEventArgs e)
{
try
{
double angulo = _robotState.Current_State.Control.Angle;
FuncoesGlobais.DesenharInclinacao((Panel)sender, e, (float)angulo, 90);
}
catch { }
}
private async void ProcessRobotAction(RobotControl action)
{
if (action != null)
{
if (action.Reset)
{
ReiniciarModelo();
}
else
{
_robotState.SavePreviousState();
_robotState.AddControl(action);
}
AtualizarDadosDeEstado();
if (chbEnviarDadosModelo.Checked)
{
for (int i = ticksControle; i <= GPSService.TaxaAmostragemHz - 1; i++)
{
AtualizarDadosDeEstado();
}
EnviarDadosDeEstadoParaModelo();
}
}
}
private void AtualizarDadosDeEstado_old()
{
bool atualizarCoordenadas = true;
double anguloControle = _robotState.Current_State.Control.Angle;
double anguloAtual = double.Parse(txtAnguloGPS.Text);
double novoAngulo = (anguloAtual + anguloControle) % 360;
double anguloCarro = novoAngulo;
txtAnguloGPS.Text = anguloCarro.ToString("0.00");
double angulo = _robotState.Current_State.NextPoints.FirstOrDefault()?.Orientation ?? _robotState.Current_State.Street.Orientation;
if (txtAnguloGPS.Text != "")
{
angulo = atualizarCoordenadas ? double.Parse(txtAnguloGPS.Text) : ultimoAnguloGps;
}
if (_robotState.Current_State.Control.MovementType == TipoMovimentoDirecional.MovimentoArco)
{
angulo += double.Parse(txtAnguloControle.Text);
}
double velocidadeMs = _robotState.Current_State.Control.Speed;
double velocidade = FuncoesMatematicas.ConverteMsParaKmh(velocidadeMs);
txtVelocidade.Text = velocidade.ToString("0.00");
txtAnguloControle.Text = _robotState.Current_State.Control.Angle.ToString("0.00");
// Calcula a distância com base no tempo passado (em segundos)
double distancia = velocidadeMs * tempoGps;
if (distancia == 0 && double.Parse(txtDistanciaGPS.Text) > 0)
{
distancia = double.Parse(txtDistanciaGPS.Text);
}
else
{
txtDistanciaGPS.Text = distancia.ToString("0.00");
}
//double distancia = double.Parse(txtDistanciaGPS.Text); // Distância em metros
//angulo -= 180; // Ângulo em graus
double latitude = ultimaLat;
double longitude = ultimaLon;
// Conversão de ângulo de graus para radianos
double anguloRad = angulo * (Math.PI / 180);
// Raio da Terra em metros
double raioTerra = GPSUtils.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);
if (atualizarCoordenadas)
{
// Atualizar os TextBoxes com as novas coordenadas
txtLatitude.Text = _latAtt.ToString();
txtLongitude.Text = _longAtt.ToString();
ultimoAnguloGps = angulo;
AtualizarPosicaoGPS();
calc = true;
}
else
{
ultimaLat = _latAtt;
ultimaLon = _longAtt;
}
AtualizarDadosTela();
calc = false;
}
private void AtualizarDadosDeEstado()
{
double anguloControle = _robotState.Current_State.Control.Angle;
txtAnguloControle.Text = anguloControle.ToString("0.00");
// Obtém os parâmetros
double anguloAtual = double.Parse(txtAnguloGPS.Text);
//double percentSpeedRpm = _robotState.Current_State.Control.Speed * VariaveisEquipamento.RPM_Max_Roda;
//double velocidade = FuncoesMatematicas.CalculaVelocidadeRPM(percentSpeedRpm);
//double velocidadeMs = FuncoesMatematicas.ConverteKmhParaMs(velocidade);
//double velocidadeMs = _robotState.Current_State.Control.Speed;
//double velocidade = FuncoesMatematicas.ConverteMsParaKmh(velocidadeMs);
double velocidade = FuncoesMatematicas.CalculaVelocidadeRPM(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle);
double velocidadeMs = FuncoesMatematicas.ConverteKmhParaMs(velocidade);
txtVelocidade.Text = velocidade.ToString("0.00");
// Calcula o novo ângulo do robô
double novoAngulo = CalcularNovaOrientacao(
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento,
anguloControle,
velocidadeMs,
tempoGps,
anguloAtual,
VariaveisEquipamento.DistanciaEntreEixos
);
// Atualiza a interface gráfica
txtAnguloGPS.Text = novoAngulo.ToString("0.00");
// Calcula a distância com base no tempo passado (em segundos)
double distancia = velocidadeMs * tempoGps;
if (distancia == 0 && double.Parse(txtDistanciaGPS.Text) > 0)
{
distancia = double.Parse(txtDistanciaGPS.Text);
}
else
{
txtDistanciaGPS.Text = distancia.ToString("0.0000");
}
double latitude = ultimaLat;
double longitude = ultimaLon;
// Conversão de ângulo de graus para radianos
double anguloRad = anguloAtual * (Math.PI / 180);
// Raio da Terra em metros
double raioTerra = GPSUtils.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);
// Atualizar os TextBoxes com as novas coordenadas
txtLatitude.Text = _latAtt.ToString();
txtLongitude.Text = _longAtt.ToString();
UltimaAtualizacaoGps = DateTime.Now;
AtualizarPosicaoGPS();
AtualizarDadosTela();
}
public double CalcularNovaOrientacao(TipoMovimentoDirecional tipoMovimento, double anguloControle, double velocidadeMs, double tempoDelta, double anguloAtual, double distanciaEntreEixos)
{
// Converte o ângulo de controle para radianos
double anguloControleRad = anguloControle * (Math.PI / 180.0);
// Evita divisão por zero (caso o ângulo seja muito pequeno)
double R = Math.Abs(anguloControleRad) < 0.01 ? 99999 : (distanciaEntreEixos / 100.0) / Math.Tan(anguloControleRad);
// Ajusta o raio de curva de acordo com o tipo de movimento
switch (tipoMovimento)
{
case TipoMovimentoDirecional.RodasDianteiras:
// Apenas as rodas dianteiras viram -> usa o raio padrão
break;
case TipoMovimentoDirecional.RodasTraseiras:
// Apenas as rodas traseiras viram -> mesmo efeito que as dianteiras
break;
case TipoMovimentoDirecional.MovimentoArco:
// Rodas dianteiras e traseiras viram em sentidos opostos, reduzindo muito o raio
R *= 0.5; // Reduz o raio pela metade para uma curva mais fechada
break;
case TipoMovimentoDirecional.MovimentoDiagonal:
// Todas as rodas viram na mesma direção, o robô desliza sem mudar a frente
// Isso significa que o ângulo do robô **não muda** ao longo do tempo
return anguloAtual; // Mantém a orientação original
}
// Calcula a rotação angular com base na velocidade e no raio
double omega = velocidadeMs / R;
// Calcula a variação angular no tempo (em radianos)
double deltaTheta = omega * tempoDelta;
// Converte para graus e atualiza a orientação do robô
double novoAngulo = GPSUtils.NormalizarAngulo(anguloAtual + (deltaTheta * (180.0 / Math.PI)));
return novoAngulo;
}
private async void EnviarDadosDeEstadoParaModelo()
{
_robotState.AddPosition(new Position()
{
Latitude = GPSService.UltimaLeitura.Latitude,
Longitude = GPSService.UltimaLeitura.Longitude,
Orientation = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps.AnguloCarroDefinido
});
_robotState.UpdateCurrentRobotState();
await Variaveis.MqttService.PublishAsync(
Variaveis.MqttService.Topicos.First(x => x.Topico == mqtt_topico_state),
JsonConvert.SerializeObject(_robotState)
);
}
private void btnIniciarGPS_Click(object sender, EventArgs e)
{
AtualizarPosicaoGPS();
btnAcrescentarGPS.Enabled = true;
txtAnguloGPS.Enabled = true;
txtDistanciaGPS.Enabled = true;
btnIniciarGPS.Enabled = false;
txtLatitude.ReadOnly = true;
txtLongitude.ReadOnly = true;
}
private void AtualizarPosicaoGPS()
{
GPSService.PenultimaLeitura = new GPSModel()
{
DataHora = GPSService.UltimaLeitura.DataHora,
Longitude = GPSService.UltimaLeitura.Longitude,
Latitude = GPSService.UltimaLeitura.Latitude,
};
if (!GPSService.Iniciado || (txtLongitude.Text != "" && txtLatitude.Text != ""))
{
GPSService.UltimaLeitura = new GPSModel()
{
DataHora = DateTime.Now,
Longitude = double.Parse(txtLongitude.Text.Replace(".", ",")),
Latitude = double.Parse(txtLatitude.Text.Replace(".", ",")),
};
}
ultimaLat = GPSService.UltimaLeitura.Latitude;
ultimaLon = GPSService.UltimaLeitura.Longitude;
GPSService.AtualizarCoordenadasGPS();
}
private void btnAtualizarLeitura_Click(object sender, EventArgs e)
{
AtualizarDadosTela();
}
}
}