agora MPC e simulador passam a considerar a transicao de movimento na troca do tipo de movimento
This commit is contained in:
Diego Freitas
parent
5f04f9c5c6
commit
b894622bca
|
|
@ -764,9 +764,7 @@
|
|||
<Compile Include="Models\Modules\PinoutModel.cs" />
|
||||
<Compile Include="Models\Modules\SensoriamentoModel.cs" />
|
||||
<Compile Include="Models\MotorComum.cs" />
|
||||
<Compile Include="Models\MPCController.cs" />
|
||||
<Compile Include="Models\MPCControllerAprimorado.cs" />
|
||||
<Compile Include="Models\MPCControllerSimple.cs" />
|
||||
<Compile Include="Models\MPCModel.cs" />
|
||||
<Compile Include="Models\OAKCameraModel.cs" />
|
||||
<Compile Include="Models\OIDModel.cs" />
|
||||
<Compile Include="Models\Operacoes\OperacaoModel.cs" />
|
||||
|
|
|
|||
|
|
@ -54,8 +54,25 @@ namespace AgroBase.Forms
|
|||
new SemaphoreSlim(1, 1);
|
||||
|
||||
private readonly object _estadoSimulacaoLock = new object();
|
||||
private readonly Queue<double> historicoAngulosControle =
|
||||
new Queue<double>();
|
||||
|
||||
private struct ComandoDirecionalSimulacao
|
||||
{
|
||||
public double AnguloCanonicoDeg;
|
||||
public TipoMovimentoDirecional TipoMovimento;
|
||||
|
||||
public ComandoDirecionalSimulacao(
|
||||
double anguloCanonicoDeg,
|
||||
TipoMovimentoDirecional tipoMovimento)
|
||||
{
|
||||
AnguloCanonicoDeg = anguloCanonicoDeg;
|
||||
TipoMovimento = tipoMovimento;
|
||||
}
|
||||
}
|
||||
|
||||
private readonly Queue<ComandoDirecionalSimulacao>
|
||||
historicoComandosDirecionais =
|
||||
new Queue<ComandoDirecionalSimulacao>();
|
||||
|
||||
private readonly Queue<GPSModel> _trilhaVisual =
|
||||
new Queue<GPSModel>();
|
||||
|
||||
|
|
@ -88,8 +105,19 @@ namespace AgroBase.Forms
|
|||
private int idxPassoOperacao;
|
||||
|
||||
private double velocidadeCarroMs;
|
||||
|
||||
// O comando canônico continua existindo para compatibilidade com a IHM,
|
||||
// telemetria e depuração. A física, porém, passa a ser mantida por eixo.
|
||||
private double _ultimoAnguloControleAplicado;
|
||||
private double _ultimoAnguloControleAlvoAtrasado;
|
||||
private TipoMovimentoDirecional _ultimoTipoMovimentoAlvoAtrasado =
|
||||
TipoMovimentoDirecional.RodasDianteiras;
|
||||
|
||||
// Estado FÍSICO real da direção simulada. Estes dois valores são os que
|
||||
// entram na cinemática 4WS. Em uma troca de modo, cada eixo percorre o
|
||||
// próprio caminho até o novo alvo, em vez de a geometria trocar instantaneamente.
|
||||
private double _anguloFisicoDianteiroAplicado;
|
||||
private double _anguloFisicoTraseiroAplicado;
|
||||
|
||||
// Modelo simples do atuador direcional: a fila existente representa o
|
||||
// atraso de transporte; este limite representa o tempo físico de giro.
|
||||
|
|
@ -404,18 +432,60 @@ namespace AgroBase.Forms
|
|||
|
||||
if (atualizarAnguloControle)
|
||||
{
|
||||
_ultimoAnguloControleAlvoAtrasado = CalcularAnguloControleAtrasado(
|
||||
Volatile.Read(ref _delayComandoPassos)
|
||||
);
|
||||
ComandoDirecionalSimulacao comandoAtrasado =
|
||||
CalcularComandoDirecionalAtrasado(
|
||||
Volatile.Read(ref _delayComandoPassos)
|
||||
);
|
||||
|
||||
_ultimoAnguloControleAlvoAtrasado =
|
||||
comandoAtrasado.AnguloCanonicoDeg;
|
||||
|
||||
_ultimoTipoMovimentoAlvoAtrasado =
|
||||
comandoAtrasado.TipoMovimento;
|
||||
}
|
||||
|
||||
// Diferente da versão anterior, o fim da fila vira um alvo e
|
||||
// não um salto instantâneo. A pose usa somente o ângulo físico.
|
||||
anguloControle = AtualizarAnguloControleFisico(
|
||||
// O comando canônico é convertido em dois alvos FÍSICOS. Cada
|
||||
// eixo então avança independentemente com a velocidade real do
|
||||
// atuador. Isso é essencial nas trocas de modo direcional.
|
||||
double alvoDianteiroDeg;
|
||||
double alvoTraseiroDeg;
|
||||
|
||||
ConverterComandoParaAngulosFisicos(
|
||||
_ultimoTipoMovimentoAlvoAtrasado,
|
||||
_ultimoAnguloControleAlvoAtrasado,
|
||||
tempoPassadoSegundos
|
||||
out alvoDianteiroDeg,
|
||||
out alvoTraseiroDeg
|
||||
);
|
||||
|
||||
double anguloDianteiroFisicoDeg;
|
||||
double anguloTraseiroFisicoDeg;
|
||||
|
||||
AtualizarAngulosFisicosDirecionais(
|
||||
alvoDianteiroDeg,
|
||||
alvoTraseiroDeg,
|
||||
tempoPassadoSegundos,
|
||||
out anguloDianteiroFisicoDeg,
|
||||
out anguloTraseiroFisicoDeg
|
||||
);
|
||||
|
||||
// Mantém um ângulo canônico representativo apenas para IHM,
|
||||
// logs e consumidores legados. A pose não usa mais esse valor.
|
||||
anguloControle = CalcularAnguloCanonicoRepresentativo(
|
||||
_ultimoTipoMovimentoAlvoAtrasado,
|
||||
anguloDianteiroFisicoDeg,
|
||||
anguloTraseiroFisicoDeg
|
||||
);
|
||||
|
||||
lock (_estadoSimulacaoLock)
|
||||
_ultimoAnguloControleAplicado = anguloControle;
|
||||
|
||||
// Publica o estado físico por eixo. O HealthWorker usa estes
|
||||
// valores quando op.Simulando=true, exatamente como usa a
|
||||
// telemetria dos MKS no rover real.
|
||||
operacao.SimulacaoAnguloDianteiroFisico = anguloDianteiroFisicoDeg;
|
||||
operacao.SimulacaoAnguloTraseiroFisico = anguloTraseiroFisicoDeg;
|
||||
operacao.SimulacaoAngulosFisicosDirecionaisValidos = true;
|
||||
|
||||
if (operacao.DispMvd?.Dados != null)
|
||||
{
|
||||
operacao.DispMvd.Dados.AnguloDirecionalSimuladoCanonico =
|
||||
|
|
@ -433,12 +503,9 @@ namespace AgroBase.Forms
|
|||
? gps.OrientacaoReal
|
||||
: 0.0;
|
||||
|
||||
TipoMovimentoDirecional tipoMovimento =
|
||||
operacao.Controle.TipoMovimento;
|
||||
|
||||
novaPosicao = VariaveisEquipamento.SimularNovaPosicao(
|
||||
tipoMovimento,
|
||||
anguloControle,
|
||||
novaPosicao = SimularNovaPosicaoComAngulosFisicos(
|
||||
anguloDianteiroFisicoDeg,
|
||||
anguloTraseiroFisicoDeg,
|
||||
velocidadeFisica,
|
||||
tempoPassadoSegundos,
|
||||
anguloAtual,
|
||||
|
|
@ -468,7 +535,6 @@ namespace AgroBase.Forms
|
|||
{
|
||||
posicaoAtual = novaPosicao.Clone();
|
||||
velocidadeCarroMs = velocidadeCalculada;
|
||||
_ultimoAnguloControleAplicado = anguloControle;
|
||||
|
||||
if (posicaoCorrigida != null)
|
||||
{
|
||||
|
|
@ -1614,9 +1680,13 @@ namespace AgroBase.Forms
|
|||
passosOperacao = passos ?? new List<GPSModel>();
|
||||
idxPassoOperacao = 0;
|
||||
_trilhaVisual.Clear();
|
||||
historicoAngulosControle.Clear();
|
||||
historicoComandosDirecionais.Clear();
|
||||
}
|
||||
|
||||
var operacao = Variaveis.OperacaoEmAndamento;
|
||||
if (operacao != null)
|
||||
operacao.SimulacaoAngulosFisicosDirecionaisValidos = false;
|
||||
|
||||
Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1);
|
||||
}
|
||||
|
||||
|
|
@ -1630,48 +1700,133 @@ namespace AgroBase.Forms
|
|||
{
|
||||
AtualizarConfiguracaoSimulacaoDaTela();
|
||||
|
||||
double angulo = CalcularAnguloControleAtrasado(
|
||||
Volatile.Read(ref _delayComandoPassos)
|
||||
ComandoDirecionalSimulacao comando =
|
||||
CalcularComandoDirecionalAtrasado(
|
||||
Volatile.Read(ref _delayComandoPassos)
|
||||
);
|
||||
|
||||
double alvoDianteiroDeg;
|
||||
double alvoTraseiroDeg;
|
||||
|
||||
ConverterComandoParaAngulosFisicos(
|
||||
comando.TipoMovimento,
|
||||
comando.AnguloCanonicoDeg,
|
||||
out alvoDianteiroDeg,
|
||||
out alvoTraseiroDeg
|
||||
);
|
||||
|
||||
lock (_estadoSimulacaoLock)
|
||||
{
|
||||
_ultimoAnguloControleAlvoAtrasado = angulo;
|
||||
_ultimoAnguloControleAplicado = angulo;
|
||||
_ultimoAnguloControleAlvoAtrasado =
|
||||
comando.AnguloCanonicoDeg;
|
||||
_ultimoTipoMovimentoAlvoAtrasado =
|
||||
comando.TipoMovimento;
|
||||
|
||||
// O botão é uma ferramenta de depuração manual. Mantém a
|
||||
// semântica histórica de aplicar imediatamente o valor atual.
|
||||
_anguloFisicoDianteiroAplicado = alvoDianteiroDeg;
|
||||
_anguloFisicoTraseiroAplicado = alvoTraseiroDeg;
|
||||
_ultimoAnguloControleAplicado =
|
||||
comando.AnguloCanonicoDeg;
|
||||
}
|
||||
|
||||
txtAnguloControle.Text = angulo.ToString("0.00");
|
||||
var operacao = Variaveis.OperacaoEmAndamento;
|
||||
if (operacao != null)
|
||||
{
|
||||
operacao.SimulacaoAnguloDianteiroFisico = alvoDianteiroDeg;
|
||||
operacao.SimulacaoAnguloTraseiroFisico = alvoTraseiroDeg;
|
||||
operacao.SimulacaoAngulosFisicosDirecionaisValidos = true;
|
||||
}
|
||||
|
||||
txtAnguloControle.Text =
|
||||
comando.AnguloCanonicoDeg.ToString("0.00");
|
||||
}
|
||||
|
||||
private double CalcularAnguloControleAtrasado(int errosConsiderar)
|
||||
private ComandoDirecionalSimulacao CalcularComandoDirecionalAtrasado(
|
||||
int errosConsiderar)
|
||||
{
|
||||
var operacao = Variaveis.OperacaoEmAndamento;
|
||||
|
||||
double novoAngulo =
|
||||
Variaveis.OperacaoEmAndamento?.SimulacaoAnguloControle ?? 0.0;
|
||||
operacao?.SimulacaoAnguloControle ?? 0.0;
|
||||
|
||||
if (!ValorFinito(novoAngulo))
|
||||
novoAngulo = 0.0;
|
||||
|
||||
TipoMovimentoDirecional novoTipo =
|
||||
operacao?.SimulacaoTipoMovimentoControle ??
|
||||
TipoMovimentoDirecional.RodasDianteiras;
|
||||
|
||||
var novoComando = new ComandoDirecionalSimulacao(
|
||||
novoAngulo,
|
||||
novoTipo
|
||||
);
|
||||
|
||||
lock (_estadoSimulacaoLock)
|
||||
{
|
||||
historicoAngulosControle.Enqueue(novoAngulo);
|
||||
historicoComandosDirecionais.Enqueue(novoComando);
|
||||
|
||||
while (historicoAngulosControle.Count > 1 &&
|
||||
(historicoAngulosControle.Count > errosConsiderar ||
|
||||
while (historicoComandosDirecionais.Count > 1 &&
|
||||
(historicoComandosDirecionais.Count > errosConsiderar ||
|
||||
errosConsiderar == 0))
|
||||
{
|
||||
historicoAngulosControle.Dequeue();
|
||||
historicoComandosDirecionais.Dequeue();
|
||||
}
|
||||
|
||||
return historicoAngulosControle.Peek();
|
||||
return historicoComandosDirecionais.Peek();
|
||||
}
|
||||
}
|
||||
|
||||
private double AtualizarAnguloControleFisico(
|
||||
double anguloAlvo,
|
||||
double dtSegundos)
|
||||
private static void ConverterComandoParaAngulosFisicos(
|
||||
TipoMovimentoDirecional tipoMovimento,
|
||||
double anguloCanonicoDeg,
|
||||
out double anguloDianteiroDeg,
|
||||
out double anguloTraseiroDeg)
|
||||
{
|
||||
if (!ValorFinito(anguloAlvo))
|
||||
anguloAlvo = 0.0;
|
||||
anguloDianteiroDeg = 0.0;
|
||||
anguloTraseiroDeg = 0.0;
|
||||
|
||||
switch (tipoMovimento)
|
||||
{
|
||||
case TipoMovimentoDirecional.RodasDianteiras:
|
||||
anguloDianteiroDeg = anguloCanonicoDeg;
|
||||
break;
|
||||
|
||||
case TipoMovimentoDirecional.RodasTraseiras:
|
||||
// Convenção do projeto: comando canônico positivo produz
|
||||
// yaw positivo. No eixo traseiro isso exige esterço físico oposto.
|
||||
anguloTraseiroDeg = -anguloCanonicoDeg;
|
||||
break;
|
||||
|
||||
case TipoMovimentoDirecional.MovimentoArco:
|
||||
anguloDianteiroDeg = anguloCanonicoDeg;
|
||||
anguloTraseiroDeg = -anguloCanonicoDeg;
|
||||
break;
|
||||
|
||||
case TipoMovimentoDirecional.MovimentoDiagonal:
|
||||
case TipoMovimentoDirecional.MovimentoLateral:
|
||||
anguloDianteiroDeg = anguloCanonicoDeg;
|
||||
anguloTraseiroDeg = anguloCanonicoDeg;
|
||||
break;
|
||||
|
||||
default:
|
||||
anguloDianteiroDeg = anguloCanonicoDeg;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private void AtualizarAngulosFisicosDirecionais(
|
||||
double alvoDianteiroDeg,
|
||||
double alvoTraseiroDeg,
|
||||
double dtSegundos,
|
||||
out double anguloDianteiroDeg,
|
||||
out double anguloTraseiroDeg)
|
||||
{
|
||||
if (!ValorFinito(alvoDianteiroDeg))
|
||||
alvoDianteiroDeg = 0.0;
|
||||
|
||||
if (!ValorFinito(alvoTraseiroDeg))
|
||||
alvoTraseiroDeg = 0.0;
|
||||
|
||||
double passoMaximo =
|
||||
VelocidadeAngularDirecionalGrausSegundo *
|
||||
|
|
@ -1679,13 +1834,231 @@ namespace AgroBase.Forms
|
|||
|
||||
lock (_estadoSimulacaoLock)
|
||||
{
|
||||
double erro = anguloAlvo - _ultimoAnguloControleAplicado;
|
||||
double passo = Math.Max(-passoMaximo, Math.Min(passoMaximo, erro));
|
||||
_ultimoAnguloControleAplicado += passo;
|
||||
return _ultimoAnguloControleAplicado;
|
||||
_anguloFisicoDianteiroAplicado = AproximarLinear(
|
||||
_anguloFisicoDianteiroAplicado,
|
||||
alvoDianteiroDeg,
|
||||
passoMaximo
|
||||
);
|
||||
|
||||
_anguloFisicoTraseiroAplicado = AproximarLinear(
|
||||
_anguloFisicoTraseiroAplicado,
|
||||
alvoTraseiroDeg,
|
||||
passoMaximo
|
||||
);
|
||||
|
||||
anguloDianteiroDeg = _anguloFisicoDianteiroAplicado;
|
||||
anguloTraseiroDeg = _anguloFisicoTraseiroAplicado;
|
||||
}
|
||||
}
|
||||
|
||||
private static double AproximarLinear(
|
||||
double atual,
|
||||
double alvo,
|
||||
double passoMaximo)
|
||||
{
|
||||
if (passoMaximo <= 0.0)
|
||||
return atual;
|
||||
|
||||
double erro = alvo - atual;
|
||||
double passo = Math.Max(
|
||||
-passoMaximo,
|
||||
Math.Min(passoMaximo, erro)
|
||||
);
|
||||
|
||||
return atual + passo;
|
||||
}
|
||||
|
||||
private static double CalcularAnguloCanonicoRepresentativo(
|
||||
TipoMovimentoDirecional tipoMovimento,
|
||||
double anguloDianteiroDeg,
|
||||
double anguloTraseiroDeg)
|
||||
{
|
||||
switch (tipoMovimento)
|
||||
{
|
||||
case TipoMovimentoDirecional.RodasDianteiras:
|
||||
return anguloDianteiroDeg;
|
||||
|
||||
case TipoMovimentoDirecional.RodasTraseiras:
|
||||
return -anguloTraseiroDeg;
|
||||
|
||||
case TipoMovimentoDirecional.MovimentoArco:
|
||||
return 0.5 *
|
||||
(anguloDianteiroDeg - anguloTraseiroDeg);
|
||||
|
||||
case TipoMovimentoDirecional.MovimentoDiagonal:
|
||||
case TipoMovimentoDirecional.MovimentoLateral:
|
||||
return 0.5 *
|
||||
(anguloDianteiroDeg + anguloTraseiroDeg);
|
||||
|
||||
default:
|
||||
return anguloDianteiroDeg;
|
||||
}
|
||||
}
|
||||
|
||||
private GPSModel SimularNovaPosicaoComAngulosFisicos(
|
||||
double anguloDianteiroDeg,
|
||||
double anguloTraseiroDeg,
|
||||
double velocidadeMs,
|
||||
double tempoDelta,
|
||||
double anguloAtualDeg,
|
||||
GPSModel pos)
|
||||
{
|
||||
if (pos == null)
|
||||
return GPSService.GetSnapshot();
|
||||
|
||||
double L = Math.Max(
|
||||
0.05,
|
||||
VariaveisEquipamento.DistanciaEntreEixosCm / 100.0
|
||||
);
|
||||
|
||||
double df = anguloDianteiroDeg * Math.PI / 180.0;
|
||||
double dr = anguloTraseiroDeg * Math.PI / 180.0;
|
||||
|
||||
double tf = Math.Tan(df);
|
||||
double tr = Math.Tan(dr);
|
||||
|
||||
if (Math.Abs(tf) < 1e-12)
|
||||
tf = 0.0;
|
||||
|
||||
if (Math.Abs(tr) < 1e-12)
|
||||
tr = 0.0;
|
||||
|
||||
// Cinemática 4WS no ponto médio entre eixos. Diferente do método
|
||||
// legado, df/dr chegam aqui como estados FÍSICOS e podem representar
|
||||
// corretamente uma configuração intermediária durante a troca de modo.
|
||||
double beta = Math.Atan(0.5 * (tf + tr));
|
||||
|
||||
double kappaGeom =
|
||||
Math.Cos(beta) * (tf - tr) / L;
|
||||
|
||||
if (Math.Abs(kappaGeom) < 1e-12)
|
||||
kappaGeom = 0.0;
|
||||
|
||||
double ku = Math.Max(
|
||||
0.0,
|
||||
VariaveisEquipamento.KuDirecional
|
||||
);
|
||||
|
||||
double kappaEf =
|
||||
kappaGeom /
|
||||
(1.0 + ku * velocidadeMs * velocidadeMs);
|
||||
|
||||
double omega = velocidadeMs * kappaEf;
|
||||
|
||||
double theta0 = anguloAtualDeg * Math.PI / 180.0;
|
||||
double dtheta = omega * tempoDelta;
|
||||
double theta1 = theta0 + dtheta;
|
||||
double phi0 = theta0 + beta;
|
||||
|
||||
double dx;
|
||||
double dy;
|
||||
|
||||
if (Math.Abs(omega) <= 1e-9)
|
||||
{
|
||||
double distancia = velocidadeMs * tempoDelta;
|
||||
dx = Math.Sin(phi0) * distancia;
|
||||
dy = Math.Cos(phi0) * distancia;
|
||||
}
|
||||
else
|
||||
{
|
||||
double phi1 = theta1 + beta;
|
||||
double raioVel = velocidadeMs / omega;
|
||||
|
||||
dx =
|
||||
raioVel *
|
||||
(Math.Cos(phi0) - Math.Cos(phi1));
|
||||
|
||||
dy =
|
||||
raioVel *
|
||||
(Math.Sin(phi1) - Math.Sin(phi0));
|
||||
}
|
||||
|
||||
GPSModel ultimaPosicao =
|
||||
GPSService.GetSnapshot() ?? pos;
|
||||
|
||||
double raioTerra = GPSUtils.RaioDaTerra;
|
||||
|
||||
double dLat =
|
||||
(dy / raioTerra) *
|
||||
180.0 / Math.PI;
|
||||
|
||||
double cosLat =
|
||||
Math.Cos(pos.LatitudeAnt * Math.PI / 180.0);
|
||||
|
||||
if (Math.Abs(cosLat) < 1e-9)
|
||||
cosLat = cosLat >= 0.0 ? 1e-9 : -1e-9;
|
||||
|
||||
double dLon =
|
||||
(dx / (raioTerra * cosLat)) *
|
||||
180.0 / Math.PI;
|
||||
|
||||
double latitude = pos.LatitudeAnt + dLat;
|
||||
double longitude = pos.LongitudeAnt + dLon;
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
double agoraMono =
|
||||
Stopwatch.GetTimestamp() /
|
||||
(double)Stopwatch.Frequency;
|
||||
|
||||
double orientacaoRealDeg =
|
||||
GPSUtils.NormalizarAngulo(
|
||||
theta1 * 180.0 / Math.PI
|
||||
);
|
||||
|
||||
double orientacaoMovimentoDeg =
|
||||
GPSUtils.NormalizarAngulo(
|
||||
(theta1 + beta) * 180.0 / Math.PI
|
||||
);
|
||||
|
||||
var timestampOri = ultimaPosicao.TimestampOri?.Clone();
|
||||
var timestampPos = ultimaPosicao.TimestampPos?.Clone();
|
||||
|
||||
GPSModel novaPosicao = new GPSModel
|
||||
{
|
||||
Momento = agora,
|
||||
DataHora = agora,
|
||||
UltimoComandoRespondido = agora,
|
||||
|
||||
Lat0 = GPSService.UltimaLeitura.Lat0,
|
||||
Lon0 = GPSService.UltimaLeitura.Lon0,
|
||||
|
||||
LatitudeAnt = latitude,
|
||||
LongitudeAnt = longitude,
|
||||
|
||||
OrientacaoReal = orientacaoRealDeg,
|
||||
AnguloCarroDefinido = orientacaoRealDeg,
|
||||
OrientacaoMovimento = orientacaoMovimentoDeg,
|
||||
|
||||
TipoOrientacao = "A",
|
||||
Distancia = velocidadeMs * tempoDelta,
|
||||
Velocidade = velocidadeMs,
|
||||
|
||||
Heartbeat = ultimaPosicao.Heartbeat + 1,
|
||||
|
||||
TimestampOri = timestampOri,
|
||||
TimestampPos = timestampPos,
|
||||
};
|
||||
|
||||
var corrigida =
|
||||
GPSService.LeverArm.FixLeverArmLatLon_Fast(
|
||||
novaPosicao.LatitudeAnt,
|
||||
novaPosicao.LongitudeAnt,
|
||||
novaPosicao.OrientacaoReal,
|
||||
Math.Max(1, GPSService.TaxaAmostragemHz)
|
||||
);
|
||||
|
||||
novaPosicao.Latitude = corrigida.lat;
|
||||
novaPosicao.Longitude = corrigida.lon;
|
||||
|
||||
if (novaPosicao.TimestampOri != null)
|
||||
novaPosicao.TimestampOri.valor = agoraMono;
|
||||
|
||||
if (novaPosicao.TimestampPos != null)
|
||||
novaPosicao.TimestampPos.valor = agoraMono;
|
||||
|
||||
return novaPosicao;
|
||||
}
|
||||
|
||||
private double ObterUltimoAnguloControle()
|
||||
{
|
||||
lock (_estadoSimulacaoLock)
|
||||
|
|
|
|||
|
|
@ -1,209 +0,0 @@
|
|||
using AgroBase.Models;
|
||||
using AgroBase.Services;
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Threading.Tasks;
|
||||
using static AgroBase.Models.Enums;
|
||||
|
||||
public class MPCController
|
||||
{
|
||||
private string TopicoRota = "mpc/rota";
|
||||
private string TopicoPosicao = "mpc/posicao";
|
||||
private string TopicoComando = "mpc/comando";
|
||||
private double Horizonte = 4.0;
|
||||
private double _dt = 0.0;
|
||||
private DateTime _ultimaAtualizacao = DateTime.MinValue;
|
||||
public Process pythonProcess;
|
||||
public readonly object processLock = new object();
|
||||
public bool Iniciado = false;
|
||||
|
||||
public MPCComandoModel ComandoAtual = new MPCComandoModel();
|
||||
|
||||
public MPCController()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
public async Task<bool> Inicializar()
|
||||
{
|
||||
await Variaveis.MqttServiceLocal.AdicionarNovoTopico(TopicoRota);
|
||||
await Variaveis.MqttServiceLocal.AdicionarNovoTopico(TopicoPosicao);
|
||||
await Variaveis.MqttServiceLocal.AdicionarNovoTopico(TopicoComando, true, 2, async (mensagem) =>
|
||||
{
|
||||
if (mensagem.Mensagem == "OK")
|
||||
{
|
||||
Iniciado = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
try
|
||||
{
|
||||
ComandoAtual = JsonConvert.DeserializeObject<MPCComandoModel>(mensagem.Mensagem);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine("Erro ao deserializar resposta do MPC: " + ex.Message.ToString());
|
||||
}
|
||||
_ultimaAtualizacao = DateTime.Now;
|
||||
}
|
||||
});
|
||||
|
||||
lock (processLock)
|
||||
{
|
||||
//pythonProcess = PythonService.RunScript(PythonService.ScriptMPCController, new string[] {});
|
||||
}
|
||||
|
||||
bool ScriptIniciado() => Iniciado;
|
||||
await FuncoesGlobais.AguardarCondicaoAsync(ScriptIniciado);
|
||||
|
||||
return Iniciado;
|
||||
}
|
||||
|
||||
public async Task EnviarDadosTrajetoria(List<PontoTrajetoriaModel> _trajetoria)
|
||||
{
|
||||
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
|
||||
|
||||
var trajetoria = new
|
||||
{
|
||||
pontos = _trajetoria
|
||||
.Select(p => new
|
||||
{
|
||||
lat = p.Posicao.Latitude,
|
||||
lon = p.Posicao.Longitude,
|
||||
tipo = (int)p.Tipo,
|
||||
distanciaMargem = p.LarguraCorredor * 0.8
|
||||
})
|
||||
.ToList(),
|
||||
horizonte = Horizonte,
|
||||
angulo_max_graus = pControle.DirAnguloMaximo,
|
||||
distancia_entre_eixos = (VariaveisEquipamento.DistanciaEntreEixosCm / 100.0),
|
||||
velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeCErvasPercent),
|
||||
velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeSErvasPercent)
|
||||
};
|
||||
|
||||
await Variaveis.MqttServiceLocal.PublishAsync(
|
||||
Variaveis.MqttServiceLocal.Topicos.First(x => x.Topico == TopicoRota),
|
||||
JsonConvert.SerializeObject(trajetoria)
|
||||
);
|
||||
}
|
||||
|
||||
public async Task<MPCComandoModel> Compute(double _lat, double _long, double _theta, double _velocidadeMs)
|
||||
{
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
|
||||
DateTime EnviadoEm = DateTime.Now;
|
||||
|
||||
double taxaHz = GPSService.TaxaAmostragemHz; // ex: 10 Hz
|
||||
double tempoIdeal = 1.0 / taxaHz; // = 0.1 s
|
||||
double tempoMinimo = tempoIdeal * 0.5; // 50% abaixo
|
||||
double tempoMaximo = tempoIdeal * 2.0; // até 2x acima
|
||||
_dt = (EnviadoEm - _ultimaAtualizacao).TotalSeconds;
|
||||
double dtCalc = Math.Max(tempoMinimo, Math.Min(tempoMaximo, _dt));
|
||||
|
||||
var _Sensoriamento = op.Sensoriamento;
|
||||
|
||||
var dadosEnvio = new
|
||||
{
|
||||
lat = _lat,
|
||||
lon = _long,
|
||||
theta = _theta,
|
||||
velocidade = _velocidadeMs,
|
||||
dt = dtCalc,
|
||||
comando_anterior = new
|
||||
{
|
||||
angulo = op.Controle.Angulo,
|
||||
tipo = op.Controle.TipoMovimento,
|
||||
},
|
||||
contexto = new
|
||||
{
|
||||
StatusCarro = (int)_Sensoriamento.Trajetoria.StatusCarro,
|
||||
DentroCorredor = _Sensoriamento.Trajetoria.CorredorAtual.Dentro,
|
||||
ManobrandoEntreRuas = _Sensoriamento.Trajetoria.ManobrandoEntreRuas
|
||||
}
|
||||
};
|
||||
|
||||
await Variaveis.MqttServiceLocal.PublishAsync(
|
||||
Variaveis.MqttServiceLocal.Topicos.First(x => x.Topico == TopicoPosicao),
|
||||
JsonConvert.SerializeObject(dadosEnvio)
|
||||
);
|
||||
|
||||
bool Respondido() => _ultimaAtualizacao > EnviadoEm;
|
||||
await FuncoesGlobais.AguardarCondicaoAsync(Respondido, 200, 10);
|
||||
|
||||
return ComandoAtual;
|
||||
}
|
||||
|
||||
public MPCController Clone()
|
||||
{
|
||||
return new MPCController()
|
||||
{
|
||||
ComandoAtual = ComandoAtual?.Clone() ?? new MPCComandoModel(),
|
||||
Horizonte = Horizonte,
|
||||
Iniciado = Iniciado,
|
||||
_ultimaAtualizacao = _ultimaAtualizacao,
|
||||
_dt = _dt
|
||||
}
|
||||
; }
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
lock (processLock)
|
||||
{
|
||||
if (pythonProcess != null && !pythonProcess.HasExited)
|
||||
{
|
||||
pythonProcess?.Kill();
|
||||
}
|
||||
pythonProcess?.Dispose();
|
||||
pythonProcess = null;
|
||||
}
|
||||
|
||||
Iniciado = false;
|
||||
ComandoAtual = new MPCComandoModel();
|
||||
Task.Run(async () => {
|
||||
Variaveis.MqttServiceLocal.Topicos.Remove(Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == TopicoRota));
|
||||
Variaveis.MqttServiceLocal.Topicos.Remove(Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == TopicoPosicao));
|
||||
await Variaveis.MqttServiceLocal.UnsubscribeAsync(Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == TopicoComando));
|
||||
Variaveis.MqttServiceLocal.Topicos.Remove(Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == TopicoComando));
|
||||
});
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public class MPCComandoModel
|
||||
{
|
||||
public TipoMovimentoDirecional tipo { get; set; } = TipoMovimentoDirecional.RodasDianteiras;
|
||||
public double angulo { get; set; } = 0;
|
||||
public List<MPCSimulacaoModel> simulacao { get; set; } = new List<MPCSimulacaoModel>();
|
||||
|
||||
public MPCComandoModel Clone()
|
||||
{
|
||||
return new MPCComandoModel()
|
||||
{
|
||||
tipo = tipo,
|
||||
angulo = angulo,
|
||||
simulacao = new List<MPCSimulacaoModel>(simulacao)
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
public class MPCSimulacaoModel
|
||||
{
|
||||
public double latitude { get; set; }
|
||||
public double longitude { get; set; }
|
||||
public double orientacao { get; set; }
|
||||
}
|
||||
|
||||
public class MPCPesosModel
|
||||
{
|
||||
public double posicao { get; set; }
|
||||
public double orientacao { get; set; }
|
||||
public double suavidade_min { get; set; }
|
||||
public double suavidade_max { get; set; }
|
||||
public double fator_re { get; set; }
|
||||
public double ideal { get; set; }
|
||||
public double lateral_min { get; set; }
|
||||
public double lateral_max { get; set; }
|
||||
}
|
||||
|
|
@ -1,178 +0,0 @@
|
|||
using AgroBase.Models;
|
||||
using AgroBase.Services;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
|
||||
public class MPCControllerAprimorado
|
||||
{
|
||||
private double distanciaPrevisao = 2.0;
|
||||
private int horizonte;
|
||||
private double maxAnguloDirecao;
|
||||
private double maxAngulo4Rodas;
|
||||
private double velocidadeMin;
|
||||
private double velocidadeMax;
|
||||
|
||||
private double pesoOrientacao = 2.5;
|
||||
private double pesoLateral = 1.5;
|
||||
private double pesoDistancia = 0.8;
|
||||
private double pesoSuavidade = 0.5;
|
||||
private double limiteErroOrientacao = 45.0;
|
||||
private double limiteErroLateral = 2.5; // metros
|
||||
|
||||
private DateTime ultimaAtualizacao = DateTime.MinValue;
|
||||
|
||||
public MPCControllerAprimorado()
|
||||
{
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
|
||||
this.maxAnguloDirecao = op.Parametros?.Controle?.DirAnguloMaximo ?? 30.0;
|
||||
this.maxAngulo4Rodas = this.maxAnguloDirecao * 0.8;
|
||||
this.velocidadeMin = op.Parametros?.Controle?.MovVelocidadeCErvasPercent ?? 20.0;
|
||||
this.velocidadeMax = op.Parametros?.Controle?.MovVelocidadeSErvasPercent ?? 100.0;
|
||||
}
|
||||
|
||||
public (double anguloControle, Enums.TipoMovimentoDirecional modo) CalcularControle(GPSModel posicalAtual, double velocidadeAtual, List<GPSModel> proximaTrajetoria, Enums.TipoMovimentoDirecional[] modosPermitidos)
|
||||
{
|
||||
if (proximaTrajetoria == null || proximaTrajetoria.Count < 2)
|
||||
return (0, Enums.TipoMovimentoDirecional.RodasDianteiras);
|
||||
|
||||
if (proximaTrajetoria.Count > 3)
|
||||
{
|
||||
proximaTrajetoria = new List<GPSModel>(proximaTrajetoria.Take(3));
|
||||
}
|
||||
|
||||
if (ultimaAtualizacao == DateTime.MinValue)
|
||||
{
|
||||
ultimaAtualizacao = DateTime.Now.AddMilliseconds(-(1.0 / GPSService.TaxaAmostragemHz));
|
||||
}
|
||||
|
||||
double dt = (DateTime.Now - ultimaAtualizacao).TotalSeconds;
|
||||
double tempoMax = (1.0 / GPSService.TaxaAmostragemHz) * 1.5;
|
||||
dt = Math.Max(0.5, Math.Min(dt, tempoMax));
|
||||
|
||||
double velMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(velocidadeAtual);
|
||||
double passoDist = velMs * dt;
|
||||
|
||||
horizonte = Math.Min(Convert.ToInt32(distanciaPrevisao / passoDist), proximaTrajetoria.Count);
|
||||
|
||||
var candidatos = GerarCandidatos(modosPermitidos);
|
||||
double melhorCusto = double.MaxValue;
|
||||
(double angulo, Enums.TipoMovimentoDirecional modo) melhorControle = (0, Enums.TipoMovimentoDirecional.RodasDianteiras);
|
||||
|
||||
foreach (var candidato in candidatos)
|
||||
{
|
||||
double custo = SimularTrajetoriaComReavaliacao(posicalAtual, proximaTrajetoria, posicalAtual.AnguloCarroDefinido, candidato.angulo, candidato.modo, velMs, dt);
|
||||
//Console.WriteLine($"Candidato: {candidato.angulo}, Custo: {custo.ToString()}");
|
||||
if (custo < melhorCusto)
|
||||
{
|
||||
melhorCusto = custo;
|
||||
melhorControle = candidato;
|
||||
}
|
||||
}
|
||||
|
||||
ultimaAtualizacao = DateTime.Now;
|
||||
return melhorControle;
|
||||
}
|
||||
|
||||
private List<(double angulo, Enums.TipoMovimentoDirecional modo)> GerarCandidatos(Enums.TipoMovimentoDirecional[] modos)
|
||||
{
|
||||
var candidatos = new List<(double, Enums.TipoMovimentoDirecional)>();
|
||||
|
||||
foreach (var modo in modos)
|
||||
{
|
||||
double maxAng = (modo == Enums.TipoMovimentoDirecional.MovimentoArco || modo == Enums.TipoMovimentoDirecional.MovimentoDiagonal)
|
||||
? maxAngulo4Rodas : maxAnguloDirecao;
|
||||
|
||||
for (double ang = -maxAng; ang <= maxAng; ang += 5.0)
|
||||
{
|
||||
candidatos.Add((ang, modo));
|
||||
}
|
||||
}
|
||||
|
||||
return candidatos;
|
||||
}
|
||||
|
||||
private double SimularTrajetoriaComReavaliacao(GPSModel posicaoInicial, List<GPSModel> trajeto, double anguloInicial, double anguloControleInicial, Enums.TipoMovimentoDirecional modoInicial, double velocidadeMs, double dt)
|
||||
{
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
|
||||
double custoTotal = 0;
|
||||
GPSModel posAtual = posicaoInicial;
|
||||
double anguloAtual = anguloInicial;
|
||||
double anguloControle = anguloControleInicial;
|
||||
Enums.TipoMovimentoDirecional modo = modoInicial;
|
||||
|
||||
posAtual = VariaveisEquipamento.SimularNovaPosicao(modoInicial, anguloControleInicial, velocidadeMs, dt, anguloAtual, posAtual);
|
||||
anguloAtual = posAtual.OrientacaoReal;
|
||||
|
||||
for (int i = 0; i < horizonte; i++)
|
||||
{
|
||||
double melhorCustoIteracao = double.MaxValue;
|
||||
|
||||
foreach (var candidato in GerarCandidatos(new[] { modo }))
|
||||
{
|
||||
var novaPos = VariaveisEquipamento.SimularNovaPosicao(candidato.modo, candidato.angulo, velocidadeMs, dt, anguloAtual, posAtual);
|
||||
var pontoAlvo = trajeto[Math.Min(i + 1, trajeto.Count - 1)];
|
||||
|
||||
double orientacaoAlvo = GPSUtils.CalcularOrientacao(posAtual, pontoAlvo);
|
||||
|
||||
double d0 = GPSUtils.DistanciaEntrePontos(posAtual, novaPos);
|
||||
double a0 = GPSUtils.CalcularOrientacao(posAtual, novaPos);
|
||||
|
||||
double d1 = GPSUtils.DistanciaEntrePontos(posAtual, pontoAlvo);
|
||||
double a1 = GPSUtils.CalcularOrientacao(posAtual, pontoAlvo);
|
||||
|
||||
double d2 = GPSUtils.DistanciaEntrePontos(novaPos, pontoAlvo);
|
||||
double a2 = GPSUtils.CalcularOrientacao(novaPos, pontoAlvo);
|
||||
|
||||
double erroDist = d2 - d1;
|
||||
double erroOri = Math.Abs(GPSUtils.CalcularDiferencaAngulo(novaPos.OrientacaoReal, orientacaoAlvo));
|
||||
|
||||
(GPSModel ponto, int idx) = GPSUtils.PontoMaisProximoTrechoComIndice(posAtual, trajeto);
|
||||
int idxNovoPonto = op.Trajetoria.PontoAtual.idxPonto + idx;
|
||||
PontoTrajetoriaModel pontoAtual = op.Trajetoria._TrajetoriaFixa[idxNovoPonto].Clone();
|
||||
|
||||
int idxRuaEsquerda = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaEsquerda;
|
||||
double distEsq = op.Trajetoria.CalcularDistanciaLateral(true, posAtual, idxRuaEsquerda, pontoAtual.Posicao);
|
||||
|
||||
int idxRuaDireita = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaDireita;
|
||||
double distDir = op.Trajetoria.CalcularDistanciaLateral(false, posAtual, idxRuaDireita, pontoAtual.Posicao);
|
||||
|
||||
double erroLat = Math.Abs(distEsq - distDir);
|
||||
|
||||
double deltaAngulo = Math.Abs(GPSUtils.CalcularDiferencaAngulo(novaPos.OrientacaoReal, anguloAtual));
|
||||
|
||||
//Console.WriteLine($"Angulo Inicial: {anguloControleInicial}, Passo: {i}, Candidato: {candidato.angulo}, Erro lateral: {erroLat}, Erro orientacao: {erroOri}");
|
||||
|
||||
if (false && (erroOri > limiteErroOrientacao || erroLat > limiteErroLateral))
|
||||
{
|
||||
continue; // Ignora este candidato
|
||||
}
|
||||
|
||||
double custo = (erroOri * pesoOrientacao) + (erroLat * pesoLateral) + (erroDist * pesoDistancia) + (deltaAngulo * pesoSuavidade);
|
||||
|
||||
if (custo < melhorCustoIteracao)
|
||||
{
|
||||
melhorCustoIteracao = custo;
|
||||
posAtual = novaPos;
|
||||
anguloAtual = novaPos.OrientacaoReal;
|
||||
anguloControle = candidato.angulo;
|
||||
modo = candidato.modo;
|
||||
|
||||
//Console.WriteLine($"Angulo Inicial: {anguloControleInicial}, Passo: {i}, Candidato: {candidato.angulo}, Custo: {custo}, Erro lateral: {erroLat}, Erro orientacao: {erroOri}, Erro distancia: {erroDist}");
|
||||
}
|
||||
}
|
||||
|
||||
custoTotal += melhorCustoIteracao;
|
||||
|
||||
if (melhorCustoIteracao == double.MaxValue)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return custoTotal;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,167 +0,0 @@
|
|||
using AgroBase.Models;
|
||||
using AgroBase.Services;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
public class MPCControllerSimple
|
||||
{
|
||||
private int horizon;
|
||||
private double maxSteeringAngle;
|
||||
private double maxSteeringAngleFourWheels;
|
||||
private double minVelocity;
|
||||
private double maxVelocity;
|
||||
private double accelerationLimit;
|
||||
|
||||
// Pesos da função de custo ajustados
|
||||
private double distancePredict = 5.0;
|
||||
private double weightPosition = 1.0;
|
||||
private double weightAlignment = 0.8;
|
||||
private double weightSteering = 0.5;
|
||||
private double weightVelocity = 0.3;
|
||||
private double weightAcceleration = 1.0; // Penaliza mudanças bruscas de velocidade
|
||||
private double weightObstacle = 2.5; // Maior penalização para obstáculos
|
||||
private double weightLaneCentering = 1.2; // Mantém o robô no centro do corredor
|
||||
|
||||
private DateTime _ultimaAtualizacao = DateTime.MinValue;
|
||||
|
||||
public MPCControllerSimple()
|
||||
{
|
||||
// Definições baseadas em variáveis globais
|
||||
this.horizon = Convert.ToInt32(distancePredict / TrajetoriaMapaOperacaoModel.DistanciaEntrePontos);
|
||||
this.maxSteeringAngle = Variaveis.OperacaoEmAndamento.Parametros.Controle?.DirAnguloMaximo ?? 30.0;
|
||||
this.maxSteeringAngleFourWheels = this.maxSteeringAngle * 1.0;
|
||||
this.minVelocity = Variaveis.OperacaoEmAndamento.Parametros.Controle?.MovVelocidadeCErvasPercent ?? 20.0;
|
||||
this.maxVelocity = Variaveis.OperacaoEmAndamento.Parametros.Controle?.MovVelocidadeSErvasPercent ?? 100.0;
|
||||
this.accelerationLimit = 1.5; // Limita aceleração a valores realistas
|
||||
}
|
||||
|
||||
public (double steeringAngle, Enums.TipoMovimentoDirecional mode) ComputeControl(
|
||||
double currentX, double currentY, double currentTheta,
|
||||
double targetX, double targetY, double targetTheta,
|
||||
double currentVelocity, double leftEdge, double rightEdge, List<Obstaculo> obstacles)
|
||||
{
|
||||
if (_ultimaAtualizacao == DateTime.MinValue)
|
||||
{
|
||||
_ultimaAtualizacao = DateTime.Now.AddMilliseconds(-(1.0 / GPSService.TaxaAmostragemHz));
|
||||
}
|
||||
|
||||
double dt = (DateTime.Now - _ultimaAtualizacao).TotalSeconds;
|
||||
double maxTime = (1.0 / GPSService.TaxaAmostragemHz) * 1.5;
|
||||
dt = Math.Max(0.01, Math.Min(dt, maxTime));
|
||||
|
||||
List<(double steering, Enums.TipoMovimentoDirecional mode)> candidates = GenerateControlCandidates();
|
||||
double bestCost = double.MaxValue;
|
||||
(double steeringAngle, Enums.TipoMovimentoDirecional mode) bestControl = (0, Enums.TipoMovimentoDirecional.RodasDianteiras);
|
||||
bool insideStreet = Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria.CorredorAtual.Dentro;
|
||||
|
||||
double velocity = FuncoesMatematicas.CalculaVelocidadeMsPercentual(currentVelocity);
|
||||
double distanciaPasso = dt * velocity;
|
||||
|
||||
this.horizon = Convert.ToInt32(distancePredict / distanciaPasso);
|
||||
|
||||
foreach (var control in candidates)
|
||||
{
|
||||
double cost = SimulateTrajectory(
|
||||
currentX, currentY, currentTheta,
|
||||
targetX, targetY, targetTheta,
|
||||
velocity, control.steering, control.mode,
|
||||
leftEdge, rightEdge, obstacles, dt, insideStreet
|
||||
);
|
||||
|
||||
if (cost < bestCost)
|
||||
{
|
||||
bestCost = cost;
|
||||
bestControl = control;
|
||||
}
|
||||
}
|
||||
|
||||
_ultimaAtualizacao = DateTime.Now;
|
||||
return bestControl;
|
||||
}
|
||||
|
||||
private List<(double steering, Enums.TipoMovimentoDirecional mode)> GenerateControlCandidates()
|
||||
{
|
||||
List<(double steering, Enums.TipoMovimentoDirecional mode)> candidates = new List<(double, Enums.TipoMovimentoDirecional)>();
|
||||
|
||||
List<Enums.TipoMovimentoDirecional> modes = new List<Enums.TipoMovimentoDirecional>()
|
||||
{
|
||||
Enums.TipoMovimentoDirecional.RodasDianteiras,
|
||||
Enums.TipoMovimentoDirecional.RodasTraseiras,
|
||||
Enums.TipoMovimentoDirecional.MovimentoArco,
|
||||
};
|
||||
|
||||
foreach (var mode in modes)
|
||||
{
|
||||
double maxAngle = (mode == Enums.TipoMovimentoDirecional.MovimentoArco || mode == Enums.TipoMovimentoDirecional.MovimentoDiagonal)
|
||||
? maxSteeringAngleFourWheels
|
||||
: maxSteeringAngle;
|
||||
|
||||
for (double s = -maxAngle; s <= maxAngle; s += 0.5)
|
||||
{
|
||||
candidates.Add((s, mode));
|
||||
}
|
||||
}
|
||||
|
||||
return candidates;
|
||||
}
|
||||
|
||||
private double SimulateTrajectory(
|
||||
double lat, double lon, double theta,
|
||||
double targetLat, double targetLon, double targetTheta,
|
||||
double currentVelocity, double steering,
|
||||
Enums.TipoMovimentoDirecional mode, double leftEdge, double rightEdge,
|
||||
List<Obstaculo> obstacles, double dt, bool insideStreet)
|
||||
{
|
||||
double cost = 0;
|
||||
|
||||
// Converte a posição atual e o alvo para coordenadas métricas
|
||||
var posicaoAtual = new GPSModel { Latitude = lat, Longitude = lon, OrientacaoReal = theta };
|
||||
var posicaoAlvo = new GPSModel { Latitude = targetLat, Longitude = targetLon };
|
||||
|
||||
// Converte para coordenadas métricas usando o método mais preciso
|
||||
(double targetX, double targetY) = GPSUtils.ConverterLatLongParaMetros(posicaoAlvo, posicaoAtual);
|
||||
|
||||
double lastSteering = steering; // Para suavizar mudanças bruscas de direção
|
||||
|
||||
// Simula a trajetória ao longo do horizonte de previsão
|
||||
for (int i = 0; i < horizon; i++)
|
||||
{
|
||||
// Simula a próxima posição usando o método SimularNovaPosicao
|
||||
var novaPosicao = VariaveisEquipamento.SimularNovaPosicao(mode, steering, currentVelocity, dt, theta, posicaoAtual);
|
||||
|
||||
// Atualiza a posição atual e a orientação
|
||||
posicaoAtual = novaPosicao;
|
||||
theta = novaPosicao.OrientacaoReal;
|
||||
|
||||
// Converte a nova posição para coordenadas métricas
|
||||
(double novaX, double novaY) = GPSUtils.ConverterLatLongParaMetros(novaPosicao, posicaoAlvo);
|
||||
|
||||
// Calcula o erro de posição e alinhamento
|
||||
// Calcula apenas o erro de posição
|
||||
double distanceError = Math.Sqrt(Math.Pow(targetX - novaX, 2) + Math.Pow(targetY - novaY, 2));
|
||||
|
||||
// Calcula o erro lateral (distância da esquerda e direita devem ser iguais)
|
||||
double laneError = insideStreet ? Math.Abs(leftEdge - rightEdge) : 0.0;
|
||||
|
||||
// Penaliza mudanças muito bruscas na direção (para suavizar a oscilação)
|
||||
double steeringChangePenalty = Math.Pow(Math.Abs(steering - lastSteering), 2) * 10.0;
|
||||
|
||||
// Atualiza o último valor de steering
|
||||
lastSteering = steering;
|
||||
|
||||
double centralLaneError = Math.Abs((leftEdge + rightEdge) / 2.0 - novaX);
|
||||
|
||||
// Função de custo ajustada
|
||||
cost +=
|
||||
(distanceError * 10.0) +
|
||||
(laneError * 20.0) +
|
||||
(steeringChangePenalty * 15.0) +
|
||||
(centralLaneError * 5.0);
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
using System.Collections.Generic;
|
||||
using static AgroBase.Models.Enums;
|
||||
|
||||
namespace AgroBase.Models
|
||||
{
|
||||
public class MPCComandoModel
|
||||
{
|
||||
public TipoMovimentoDirecional tipo { get; set; } = TipoMovimentoDirecional.RodasDianteiras;
|
||||
public double angulo { get; set; } = 0;
|
||||
public List<MPCSimulacaoModel> simulacao { get; set; } = new List<MPCSimulacaoModel>();
|
||||
|
||||
public MPCComandoModel Clone()
|
||||
{
|
||||
return new MPCComandoModel()
|
||||
{
|
||||
tipo = tipo,
|
||||
angulo = angulo,
|
||||
simulacao = new List<MPCSimulacaoModel>(simulacao)
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
public class MPCSimulacaoModel
|
||||
{
|
||||
public double latitude { get; set; }
|
||||
public double longitude { get; set; }
|
||||
public double orientacao { get; set; }
|
||||
}
|
||||
|
||||
public class MPCPesosModel
|
||||
{
|
||||
public double posicao { get; set; }
|
||||
public double orientacao { get; set; }
|
||||
public double suavidade_min { get; set; }
|
||||
public double suavidade_max { get; set; }
|
||||
public double fator_re { get; set; }
|
||||
public double ideal { get; set; }
|
||||
public double lateral_min { get; set; }
|
||||
public double lateral_max { get; set; }
|
||||
}
|
||||
}
|
||||
|
|
@ -488,6 +488,7 @@ namespace AgroBase.Models.Modules
|
|||
if (Comandar)
|
||||
{
|
||||
op.SimulacaoAnguloControle = op.Controle.Angulo;
|
||||
op.SimulacaoTipoMovimentoControle = op.Controle.TipoMovimento;
|
||||
//Console.WriteLine($"[{Mod_ID}] Angulo Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle}");
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -339,6 +339,14 @@ namespace AgroBase.Models
|
|||
public bool Treinando { get; set; } = false;
|
||||
public bool Simulando { get; set; } = false;
|
||||
public double SimulacaoAnguloControle { get; set; } = 0;
|
||||
public TipoMovimentoDirecional SimulacaoTipoMovimentoControle { get; set; } = TipoMovimentoDirecional.RodasDianteiras;
|
||||
|
||||
// Estado físico 4WS publicado pelo simulador para que o MPC comece
|
||||
// exatamente da mesma geometria que a planta virtual está executando.
|
||||
public double SimulacaoAnguloDianteiroFisico { get; set; } = 0;
|
||||
public double SimulacaoAnguloTraseiroFisico { get; set; } = 0;
|
||||
public bool SimulacaoAngulosFisicosDirecionaisValidos { get; set; } = false;
|
||||
|
||||
public double SimulacaoRpmControle { get; set; } = 0;
|
||||
|
||||
public int TempoIniciarOperacao { get; set; } = 10;
|
||||
|
|
@ -1585,6 +1593,12 @@ namespace AgroBase.Models
|
|||
|
||||
this.ReiniciarOperacao(false);
|
||||
|
||||
SimulacaoAnguloControle = 0;
|
||||
SimulacaoTipoMovimentoControle = TipoMovimentoDirecional.RodasDianteiras;
|
||||
SimulacaoAnguloDianteiroFisico = 0;
|
||||
SimulacaoAnguloTraseiroFisico = 0;
|
||||
SimulacaoAngulosFisicosDirecionaisValidos = false;
|
||||
|
||||
//var Topico = Variaveis.MqttServiceLocal.Topicos.FirstOrDefault(x => x.Topico == MapasVariaveisModel.TopicoSelecaoRuasMapa);
|
||||
//Topico.Mensagens.Add(new MqttService.MqttTopicosMensagensModel()
|
||||
//{
|
||||
|
|
|
|||
|
|
@ -151,6 +151,94 @@ namespace AgroBase.Services.Operadores
|
|||
}
|
||||
}
|
||||
|
||||
private static (
|
||||
double dianteiro,
|
||||
double traseiro,
|
||||
int rodasDianteirasValidas,
|
||||
int rodasTraseirasValidas,
|
||||
bool valido,
|
||||
string fonte
|
||||
) ObterAngulosFisicosDirecionais(OperacaoModel op)
|
||||
{
|
||||
if (op == null)
|
||||
return (0.0, 0.0, 0, 0, false, "indisponivel");
|
||||
|
||||
// No simulador, a autoridade é o estado físico integrado por eixo.
|
||||
if (op.Simulando)
|
||||
{
|
||||
if (op.SimulacaoAngulosFisicosDirecionaisValidos)
|
||||
{
|
||||
double df = op.SimulacaoAnguloDianteiroFisico;
|
||||
double dr = op.SimulacaoAnguloTraseiroFisico;
|
||||
|
||||
bool finitos =
|
||||
!double.IsNaN(df) && !double.IsInfinity(df) &&
|
||||
!double.IsNaN(dr) && !double.IsInfinity(dr);
|
||||
|
||||
if (finitos)
|
||||
return (df, dr, 2, 2, true, "simulador");
|
||||
}
|
||||
|
||||
// Nunca mistura hardware real/stale com uma operação simulada.
|
||||
// O Python reconstruirá df/dr pelo estado canônico se necessário.
|
||||
return (0.0, 0.0, 0, 0, false, "simulador_fallback_canonico");
|
||||
}
|
||||
|
||||
// Rover real: EF/ET têm sinal físico espelhado em relação a DF/DT.
|
||||
// Normalizamos todos para a convenção cinemática do MPC:
|
||||
// df > 0 e dr > 0 = esterço físico positivo do respectivo eixo.
|
||||
var dianteiros = new List<double>();
|
||||
var traseiros = new List<double>();
|
||||
|
||||
var modulos = op.DispMvd?.Dados?.Modulos;
|
||||
if (modulos != null)
|
||||
{
|
||||
foreach (var modulo in modulos)
|
||||
{
|
||||
var dir = modulo?.DirMotor;
|
||||
if (dir == null || !dir.Inicializado)
|
||||
continue;
|
||||
|
||||
double leitura = dir.AnguloLeitura;
|
||||
if (double.IsNaN(leitura) || double.IsInfinity(leitura))
|
||||
continue;
|
||||
|
||||
string id = (modulo.Modulo_ID ?? dir.Mod_ID ?? "")
|
||||
.Trim()
|
||||
.ToUpperInvariant();
|
||||
|
||||
switch (id)
|
||||
{
|
||||
case "EF":
|
||||
dianteiros.Add(-leitura);
|
||||
break;
|
||||
case "DF":
|
||||
dianteiros.Add(leitura);
|
||||
break;
|
||||
case "ET":
|
||||
traseiros.Add(-leitura);
|
||||
break;
|
||||
case "DT":
|
||||
traseiros.Add(leitura);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool valido = dianteiros.Count > 0 && traseiros.Count > 0;
|
||||
double dianteiro = dianteiros.Count > 0 ? dianteiros.Average() : 0.0;
|
||||
double traseiro = traseiros.Count > 0 ? traseiros.Average() : 0.0;
|
||||
|
||||
return (
|
||||
dianteiro,
|
||||
traseiro,
|
||||
dianteiros.Count,
|
||||
traseiros.Count,
|
||||
valido,
|
||||
valido ? "telemetria_mks" : "fallback_canonico"
|
||||
);
|
||||
}
|
||||
|
||||
public static void AtualizarDadosContexto()
|
||||
{
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
|
|
@ -200,6 +288,8 @@ namespace AgroBase.Services.Operadores
|
|||
op.Simulando
|
||||
);
|
||||
|
||||
var estadoEixosDirecionais = ObterAngulosFisicosDirecionais(op);
|
||||
|
||||
var bombaLinha = op?.DispAtu?.Dados?.BombaPressurizadora;
|
||||
double pressaoAlvo = bombaLinha?._PressaoSPControle > 0 ? bombaLinha._PressaoSPControle : op.Parametros?.Controle?.AtuPressaoLinha ?? 0;
|
||||
double pressaoLinha = bombaLinha?._PressaoAtual ?? _Sensoriamento?.Atuador?.PressaoLinha ?? 0;
|
||||
|
|
@ -265,6 +355,16 @@ namespace AgroBase.Services.Operadores
|
|||
angulo_comando = estadoDirecional.AnguloComando,
|
||||
angulo_sp = estadoDirecional.AnguloSP,
|
||||
angulo_fisico = estadoDirecional.AnguloFisico,
|
||||
|
||||
// Estado físico 4WS por eixo. É a fonte de verdade do
|
||||
// MPC durante transições de Dianteira/Traseira/Arco/Diagonal.
|
||||
angulo_fisico_dianteiro = estadoEixosDirecionais.dianteiro,
|
||||
angulo_fisico_traseiro = estadoEixosDirecionais.traseiro,
|
||||
rodas_dianteiras_validas = estadoEixosDirecionais.rodasDianteirasValidas,
|
||||
rodas_traseiras_validas = estadoEixosDirecionais.rodasTraseirasValidas,
|
||||
eixos_fisicos_validos = estadoEixosDirecionais.valido,
|
||||
fonte_eixos_fisicos = estadoEixosDirecionais.fonte,
|
||||
|
||||
dispersao = estadoDirecional.Dispersao,
|
||||
rodas_validas = estadoDirecional.RodasValidas,
|
||||
valido = estadoDirecional.Valido,
|
||||
|
|
|
|||
|
|
@ -143,7 +143,7 @@ def _comando_mapa_gps_mpc(contexto: dict):
|
|||
parada_necessaria=True,
|
||||
)
|
||||
|
||||
comando_anterior = _ler_comando_anterior_para_mpc(contexto)
|
||||
comando_anterior = _ler_comando_anterior_para_mpc()
|
||||
|
||||
inicio = time.perf_counter()
|
||||
comando = mpc.compute_receding(contexto, comando_anterior)
|
||||
|
|
@ -250,12 +250,6 @@ def _normalizar_comando_mpc(comando: dict, contexto: dict):
|
|||
comando.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value)
|
||||
)
|
||||
|
||||
parada_necessaria = _bool(comando.get("parada_necessaria", False))
|
||||
if parada_necessaria:
|
||||
# A direção deve ficar neutra no mesmo ciclo em que o MPC pede stop.
|
||||
angulo = 0.0
|
||||
tipo = TipoMovimentoDirecional.RodasDianteiras.value
|
||||
|
||||
debug_custo = comando.get("debug_custo", {})
|
||||
if not isinstance(debug_custo, dict):
|
||||
debug_custo = {}
|
||||
|
|
@ -267,7 +261,7 @@ def _normalizar_comando_mpc(comando: dict, contexto: dict):
|
|||
return {
|
||||
"comando_definido": True,
|
||||
"enviar_comando": _bool(comando.get("enviar_comando", True)),
|
||||
"parada_necessaria": parada_necessaria,
|
||||
"parada_necessaria": _bool(comando.get("parada_necessaria", False)),
|
||||
"erro": _bool(comando.get("erro", False)),
|
||||
"latencia": _float(comando.get("latencia", 0.0), 0.0),
|
||||
"angulo": round(angulo, 2),
|
||||
|
|
@ -515,35 +509,42 @@ def _montar_contexto_direcional(*, operacao, controle, contexto_global, equipame
|
|||
),
|
||||
},
|
||||
"Direcional": {
|
||||
"TipoMovimento": _int(
|
||||
"TipoMovimento": _normalizar_tipo_movimento(
|
||||
direcional_fisico.get(
|
||||
"tipo_movimento",
|
||||
controle.get(
|
||||
"tipo_movimento_direcional",
|
||||
TipoMovimentoDirecional.RodasDianteiras.value,
|
||||
"tipo_movimento_direcional_ant",
|
||||
controle.get(
|
||||
"tipo_movimento_direcional",
|
||||
TipoMovimentoDirecional.RodasDianteiras.value,
|
||||
),
|
||||
),
|
||||
),
|
||||
TipoMovimentoDirecional.RodasDianteiras.value,
|
||||
)
|
||||
),
|
||||
"AnguloComando": _float(
|
||||
direcional_fisico.get("angulo_comando", controle.get("angulo_sp", 0.0)),
|
||||
0.0,
|
||||
"AnguloComando": _float(direcional_fisico.get("angulo_comando", 0.0), 0.0),
|
||||
"AnguloSP": _float(direcional_fisico.get("angulo_sp", 0.0), 0.0),
|
||||
"AnguloFisico": _float(direcional_fisico.get("angulo_fisico", 0.0), 0.0),
|
||||
"AnguloFisicoDianteiro": _float(
|
||||
direcional_fisico.get("angulo_fisico_dianteiro", 0.0), 0.0
|
||||
),
|
||||
"AnguloSP": _float(
|
||||
direcional_fisico.get("angulo_sp", controle.get("angulo_sp", 0.0)),
|
||||
0.0,
|
||||
"AnguloFisicoTraseiro": _float(
|
||||
direcional_fisico.get("angulo_fisico_traseiro", 0.0), 0.0
|
||||
),
|
||||
"AnguloFisico": _float(
|
||||
direcional_fisico.get("angulo_fisico", controle.get("angulo_sp_ant", 0.0)),
|
||||
0.0,
|
||||
"EixosFisicosValidos": _bool(
|
||||
direcional_fisico.get("eixos_fisicos_validos", False)
|
||||
),
|
||||
"RodasDianteirasValidas": _int(
|
||||
direcional_fisico.get("rodas_dianteiras_validas", 0), 0
|
||||
),
|
||||
"RodasTraseirasValidas": _int(
|
||||
direcional_fisico.get("rodas_traseiras_validas", 0), 0
|
||||
),
|
||||
"Dispersao": _float(direcional_fisico.get("dispersao", 0.0), 0.0),
|
||||
"RodasValidas": _int(direcional_fisico.get("rodas_validas", 0), 0),
|
||||
"Valido": _bool(direcional_fisico.get("valido", False)),
|
||||
"EmTransicao": _bool(direcional_fisico.get("em_transicao", False)),
|
||||
"TimestampUnixMs": _float(
|
||||
direcional_fisico.get("timestamp_unix_ms", 0.0),
|
||||
0.0,
|
||||
"FonteEixosFisicos": str(
|
||||
direcional_fisico.get("fonte_eixos_fisicos", "indisponivel")
|
||||
),
|
||||
"TimestampUnixMs": _int(
|
||||
direcional_fisico.get("timestamp_unix_ms", 0), 0
|
||||
),
|
||||
},
|
||||
"VisualWorker": visual_worker,
|
||||
|
|
@ -551,7 +552,6 @@ def _montar_contexto_direcional(*, operacao, controle, contexto_global, equipame
|
|||
"Equipamento": {
|
||||
"largura": _float(equipamento.get("largura", 0.85), 0.85),
|
||||
"entre_eixos": _float(equipamento.get("distancia_entre_eixos", 0.92), 0.92),
|
||||
"ku_direcional": _float(equipamento.get("ku_direcional", 2.65), 2.65),
|
||||
"imu_roll_direita_sinal": _float(equipamento.get("imu_roll_direita_sinal", 1.0), 1.0),
|
||||
"dir_angulo_direita_sinal": _float(equipamento.get("dir_angulo_direita_sinal", 1.0), 1.0),
|
||||
"imu_dir_roll_min_aux": _float(equipamento.get("imu_dir_roll_min_aux", 6.0), 6.0),
|
||||
|
|
@ -1123,25 +1123,6 @@ def _montar_comando_retorno(comando, latencia=-1.0):
|
|||
comando["erro_lateral"] = _float(comando.get("erro_lateral", 0.0), 0.0)
|
||||
comando["erro_orientacao"] = _float(comando.get("erro_orientacao", 0.0), 0.0)
|
||||
|
||||
# V17 SAFETY:
|
||||
# "parada_necessaria" é um contrato forte. Nenhum chamador pode receber
|
||||
# esse flag junto com um esterçamento residual e continuar aplicando-o.
|
||||
if comando["parada_necessaria"]:
|
||||
comando["enviar_comando"] = True
|
||||
comando["angulo"] = 0.0
|
||||
comando["tipo"] = TipoMovimentoDirecional.RodasDianteiras.value
|
||||
comando["simulacao"] = []
|
||||
|
||||
debug_stop = comando.get("debug_custo", {})
|
||||
if not isinstance(debug_stop, dict):
|
||||
debug_stop = {}
|
||||
|
||||
debug_stop["parada_necessaria"] = {
|
||||
"ativo": True,
|
||||
"acao_direcional": "angulo_zero_rodas_dianteiras",
|
||||
}
|
||||
comando["debug_custo"] = debug_stop
|
||||
|
||||
if not isinstance(comando.get("simulacao", []), list):
|
||||
comando["simulacao"] = []
|
||||
|
||||
|
|
@ -1158,53 +1139,59 @@ def _montar_comando_retorno(comando, latencia=-1.0):
|
|||
return comando
|
||||
|
||||
|
||||
def _ler_comando_anterior_para_mpc(contexto=None):
|
||||
def _ler_comando_anterior_para_mpc():
|
||||
controle = _dict(ContextoGlobalRedis.get_controle())
|
||||
direcional = _dict(_dict(contexto).get("Direcional", {}))
|
||||
contexto_global = _dict(ContextoGlobalRedis.get_contexto())
|
||||
direcional = _dict(contexto_global.get("Direcional", {}))
|
||||
|
||||
telemetria_valida = _bool(direcional.get("Valido", False))
|
||||
timestamp_ms = _float(direcional.get("TimestampUnixMs", 0.0), 0.0)
|
||||
idade_ms = max(0.0, time.time() * 1000.0 - timestamp_ms) if timestamp_ms > 0 else 1e9
|
||||
telemetria_valida = telemetria_valida and idade_ms <= 1500.0
|
||||
|
||||
comando = {
|
||||
"angulo": _float(
|
||||
controle.get("angulo_sp_ant", controle.get("angulo_sp", 0.0)),
|
||||
0.0,
|
||||
),
|
||||
"tipo": _normalizar_tipo_movimento(
|
||||
angulo = _float(
|
||||
controle.get("angulo_sp_ant", controle.get("angulo_sp", 0.0)),
|
||||
0.0,
|
||||
)
|
||||
tipo = _normalizar_tipo_movimento(
|
||||
controle.get(
|
||||
"tipo_movimento_direcional_ant",
|
||||
controle.get(
|
||||
"tipo_movimento_direcional_ant",
|
||||
controle.get(
|
||||
"tipo_movimento_direcional",
|
||||
TipoMovimentoDirecional.RodasDianteiras.value,
|
||||
),
|
||||
)
|
||||
),
|
||||
"tipo_movimento_direcional",
|
||||
TipoMovimentoDirecional.RodasDianteiras.value,
|
||||
),
|
||||
)
|
||||
)
|
||||
|
||||
return {
|
||||
"angulo": angulo,
|
||||
"tipo": tipo,
|
||||
"velocidade": _float(
|
||||
controle.get("velocidade_sp_ant", controle.get("velocidade_sp", 0.0)),
|
||||
0.0,
|
||||
),
|
||||
}
|
||||
|
||||
comando["angulo_aplicado"] = (
|
||||
_float(direcional.get("AnguloFisico", comando["angulo"]), comando["angulo"])
|
||||
if telemetria_valida else comando["angulo"]
|
||||
)
|
||||
comando["angulo_sp_aplicado"] = (
|
||||
_float(direcional.get("AnguloSP", comando["angulo"]), comando["angulo"])
|
||||
if telemetria_valida else comando["angulo"]
|
||||
)
|
||||
comando["tipo_aplicado"] = (
|
||||
_normalizar_tipo_movimento(direcional.get("TipoMovimento", comando["tipo"]))
|
||||
if telemetria_valida else comando["tipo"]
|
||||
)
|
||||
comando["telemetria_direcional_valida"] = telemetria_valida
|
||||
comando["telemetria_direcional_idade_ms"] = idade_ms
|
||||
comando["telemetria_direcional_dispersao"] = _float(
|
||||
direcional.get("Dispersao", 0.0), 0.0
|
||||
)
|
||||
return comando
|
||||
# Estado realmente aplicado. Mantemos os campos canônicos para
|
||||
# compatibilidade e acrescentamos df/dr físicos como autoridade.
|
||||
# O SP/tipo aplicado vêm do ControleAnterior, atualizado somente quando
|
||||
# o scheduler C# realmente libera o comando. O bloco Direcional abaixo
|
||||
# fornece a POSIÇÃO física medida, não substitui a autoridade do scheduler.
|
||||
"angulo_sp_aplicado": angulo,
|
||||
"tipo_aplicado": tipo,
|
||||
"angulo_aplicado": _float(
|
||||
direcional.get("angulo_fisico", angulo), angulo
|
||||
),
|
||||
"angulo_dianteiro_aplicado": _float(
|
||||
direcional.get("angulo_fisico_dianteiro", 0.0), 0.0
|
||||
),
|
||||
"angulo_traseiro_aplicado": _float(
|
||||
direcional.get("angulo_fisico_traseiro", 0.0), 0.0
|
||||
),
|
||||
"eixos_fisicos_validos": _bool(
|
||||
direcional.get("eixos_fisicos_validos", False)
|
||||
),
|
||||
"direcional_em_transicao": _bool(
|
||||
direcional.get("em_transicao", False)
|
||||
),
|
||||
"fonte_eixos_fisicos": str(
|
||||
direcional.get("fonte_eixos_fisicos", "indisponivel")
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
# ============================================================
|
||||
|
|
|
|||
|
|
@ -1530,17 +1530,26 @@ class ControladorMPC:
|
|||
xs, ys, ths = float(x), float(y), float(theta)
|
||||
idx_sim = max(0, min(_safe_int(idx_base, 0), len(self.pontos_info) - 1))
|
||||
traj = []
|
||||
df_estado, dr_estado, _ = self._estado_eixos_fisicos_rad(
|
||||
contexto=contexto,
|
||||
tipo_fallback=TipoMovimentoDirecional.MovimentoArco,
|
||||
angulo_fallback_rad=0.0,
|
||||
)
|
||||
|
||||
for _ in range(passos):
|
||||
ang, dbg = self._referencia_seguidor_dubins(
|
||||
xs, ys, ths, idx_sim, v, contexto
|
||||
)
|
||||
omega = self._calcular_omega_4ws(
|
||||
v, ang, TipoMovimentoDirecional.MovimentoArco
|
||||
df_alvo, dr_alvo = self._angulos_eixos_4ws(
|
||||
TipoMovimentoDirecional.MovimentoArco, ang
|
||||
)
|
||||
df_estado = self._avancar_atuador_direcional(df_estado, df_alvo, dt)
|
||||
dr_estado = self._avancar_atuador_direcional(dr_estado, dr_alvo, dt)
|
||||
kin = self._cinematica_4ws_eixos(df_estado, dr_estado, v)
|
||||
xs, ys, ths = self._nova_posicao(
|
||||
xs, ys, ths, omega, v,
|
||||
TipoMovimentoDirecional.MovimentoArco, ang, dt=dt
|
||||
xs, ys, ths, kin["omega"], v,
|
||||
TipoMovimentoDirecional.MovimentoArco, ang, dt=dt,
|
||||
df_rad=df_estado, dr_rad=dr_estado,
|
||||
)
|
||||
traj.append((xs, ys, ths))
|
||||
|
||||
|
|
@ -2304,31 +2313,21 @@ class ControladorMPC:
|
|||
|
||||
return a, 0.0
|
||||
|
||||
def _cinematica_4ws(
|
||||
def _cinematica_4ws_eixos(
|
||||
self,
|
||||
tipo,
|
||||
angulo_comando_rad,
|
||||
angulo_dianteiro_rad,
|
||||
angulo_traseiro_rad,
|
||||
velocidade,
|
||||
*,
|
||||
entre_eixos=None,
|
||||
aplicar_dinamica=True,
|
||||
):
|
||||
"""Modelo cinemático único do centro do rover para direção 4WS.
|
||||
"""Cinemática 4WS a partir dos ângulos FÍSICOS atuais dos eixos.
|
||||
|
||||
Estado de referência = ponto médio entre os eixos.
|
||||
|
||||
Para l_f = l_r = L/2:
|
||||
beta = atan((tan(df) + tan(dr)) / 2)
|
||||
kappa_geom = cos(beta) * (tan(df) - tan(dr)) / L
|
||||
|
||||
`velocidade` é tratada como módulo da velocidade do centro do rover.
|
||||
A direção instantânea de translação é theta + beta.
|
||||
|
||||
Ku é uma correção empírica de subesterço já usada pelo simulador:
|
||||
kappa_ef = kappa_geom / (1 + Ku*v²)
|
||||
omega = v*kappa_ef
|
||||
Este é o núcleo físico usado durante transições de modo. `df` e `dr`
|
||||
podem estar entre duas geometrias nominais, exatamente como acontece
|
||||
enquanto os MKS ainda estão girando.
|
||||
"""
|
||||
tipo = _movimento_from_value(tipo)
|
||||
v = float(velocidade)
|
||||
L = (
|
||||
float(self._cinematica_entre_eixos_m)
|
||||
|
|
@ -2336,9 +2335,10 @@ class ControladorMPC:
|
|||
else max(0.20, float(entre_eixos))
|
||||
)
|
||||
|
||||
df, dr = self._angulos_eixos_4ws(tipo, angulo_comando_rad)
|
||||
tf = math.tan(float(df))
|
||||
tr = math.tan(float(dr))
|
||||
df = float(angulo_dianteiro_rad)
|
||||
dr = float(angulo_traseiro_rad)
|
||||
tf = math.tan(df)
|
||||
tr = math.tan(dr)
|
||||
|
||||
if abs(tf) < 1e-12:
|
||||
tf = 0.0
|
||||
|
|
@ -2360,8 +2360,8 @@ class ControladorMPC:
|
|||
omega = v * kappa_ef
|
||||
|
||||
return {
|
||||
"df": float(df),
|
||||
"dr": float(dr),
|
||||
"df": df,
|
||||
"dr": dr,
|
||||
"beta": float(beta),
|
||||
"kappa_geom": float(kappa_geom),
|
||||
"kappa_ef": float(kappa_ef),
|
||||
|
|
@ -2369,6 +2369,27 @@ class ControladorMPC:
|
|||
"entre_eixos": float(L),
|
||||
}
|
||||
|
||||
def _cinematica_4ws(
|
||||
self,
|
||||
tipo,
|
||||
angulo_comando_rad,
|
||||
velocidade,
|
||||
*,
|
||||
entre_eixos=None,
|
||||
aplicar_dinamica=True,
|
||||
):
|
||||
"""Cinemática nominal de um comando de alto nível.
|
||||
|
||||
Mantida para compatibilidade. Converte `tipo + ângulo` nos alvos físicos
|
||||
dos dois eixos e delega para o núcleo `_cinematica_4ws_eixos`.
|
||||
"""
|
||||
df, dr = self._angulos_eixos_4ws(tipo, angulo_comando_rad)
|
||||
return self._cinematica_4ws_eixos(
|
||||
df, dr, velocidade,
|
||||
entre_eixos=entre_eixos,
|
||||
aplicar_dinamica=aplicar_dinamica,
|
||||
)
|
||||
|
||||
def _calcular_omega_4ws(self, velocidade, angulo_rad, tipo):
|
||||
"""Compatível com a assinatura histórica calcular_omega(v, ang, tipo)."""
|
||||
return float(
|
||||
|
|
@ -2381,12 +2402,83 @@ class ControladorMPC:
|
|||
)
|
||||
|
||||
def _avancar_atuador_direcional(self, angulo_atual, angulo_alvo, dt):
|
||||
"""Aplica limite de velocidade ao estado físico da direção (radianos)."""
|
||||
"""Aplica limite de velocidade a UM eixo direcional (radianos)."""
|
||||
atual = float(angulo_atual)
|
||||
alvo = float(angulo_alvo)
|
||||
passo_max = np.radians(self._atuador_dir_taxa_graus_s) * max(0.0, float(dt))
|
||||
return float(atual + np.clip(alvo - atual, -passo_max, passo_max))
|
||||
|
||||
def _estado_eixos_fisicos_rad(
|
||||
self,
|
||||
contexto=None,
|
||||
comando_anterior=None,
|
||||
tipo_fallback=TipoMovimentoDirecional.RodasDianteiras,
|
||||
angulo_fallback_rad=0.0,
|
||||
):
|
||||
"""Resolve o estado físico atual (df, dr) em radianos.
|
||||
|
||||
Prioridade:
|
||||
1) estado interno já propagado pelo beam/latência;
|
||||
2) telemetria física por eixo entregue pelo C#/simulador;
|
||||
3) reconstrução conservadora a partir do estado canônico legado.
|
||||
"""
|
||||
cmd = _as_dict(comando_anterior)
|
||||
|
||||
try:
|
||||
df = float(cmd.get("_df_aplicado_rad"))
|
||||
dr = float(cmd.get("_dr_aplicado_rad"))
|
||||
if math.isfinite(df) and math.isfinite(dr):
|
||||
return df, dr, True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
if bool(cmd.get("eixos_fisicos_validos", False)):
|
||||
try:
|
||||
df = math.radians(float(cmd.get("angulo_dianteiro_aplicado")))
|
||||
dr = math.radians(float(cmd.get("angulo_traseiro_aplicado")))
|
||||
if math.isfinite(df) and math.isfinite(dr):
|
||||
return df, dr, True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
direcional = _as_dict(_as_dict(contexto).get("Direcional", {}))
|
||||
if bool(direcional.get("EixosFisicosValidos", False)):
|
||||
try:
|
||||
df = math.radians(float(direcional.get("AnguloFisicoDianteiro", 0.0)))
|
||||
dr = math.radians(float(direcional.get("AnguloFisicoTraseiro", 0.0)))
|
||||
if math.isfinite(df) and math.isfinite(dr):
|
||||
return df, dr, True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
tipo = cmd.get("tipo_aplicado", cmd.get("tipo", None))
|
||||
if tipo is None:
|
||||
tipo = direcional.get("TipoMovimento", tipo_fallback)
|
||||
tipo = _movimento_from_value(tipo, _movimento_from_value(tipo_fallback))
|
||||
|
||||
# Em comando_anterior os ângulos públicos são graus. O fallback explícito
|
||||
# já chega em radianos e evita ambiguidade nos estados internos do beam.
|
||||
angulo_rad = float(angulo_fallback_rad)
|
||||
if cmd:
|
||||
try:
|
||||
angulo_deg = cmd.get(
|
||||
"angulo_aplicado",
|
||||
cmd.get("angulo", math.degrees(angulo_rad)),
|
||||
)
|
||||
angulo_rad = math.radians(float(angulo_deg))
|
||||
except Exception:
|
||||
pass
|
||||
elif direcional:
|
||||
try:
|
||||
angulo_rad = math.radians(float(
|
||||
direcional.get("AnguloFisico", math.degrees(angulo_rad))
|
||||
))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
df, dr = self._angulos_eixos_4ws(tipo, angulo_rad)
|
||||
return float(df), float(dr), False
|
||||
|
||||
@staticmethod
|
||||
def _posicoes_eixos_tracking(x, y, theta, entre_eixos):
|
||||
"""Retorna centros dos eixos dianteiro/traseiro a partir do centro.
|
||||
|
|
@ -2425,23 +2517,28 @@ class ControladorMPC:
|
|||
theta,
|
||||
velocidade,
|
||||
dt,
|
||||
df_estado,
|
||||
dr_estado,
|
||||
):
|
||||
"""V16: micro-predição usa EXATAMENTE a mesma cinemática global.
|
||||
|
||||
Não existe mais um modelo especial para o seletor Dianteira/Traseira.
|
||||
Isso evita o MPC escolher um eixo por uma física e depois avaliar o
|
||||
candidato pesado por outra.
|
||||
"""
|
||||
"""Micro-predição axle-aware com dinâmica física dos dois eixos."""
|
||||
v = max(0.0, float(velocidade))
|
||||
ang = float(angulo_rad)
|
||||
tipo = _movimento_from_value(tipo)
|
||||
omega = self._calcular_omega_4ws(v, ang, tipo)
|
||||
dt_use = max(1e-4, float(dt))
|
||||
|
||||
return self._nova_posicao(
|
||||
df_alvo, dr_alvo = self._angulos_eixos_4ws(tipo, ang)
|
||||
df_estado = self._avancar_atuador_direcional(df_estado, df_alvo, dt_use)
|
||||
dr_estado = self._avancar_atuador_direcional(dr_estado, dr_alvo, dt_use)
|
||||
kin = self._cinematica_4ws_eixos(df_estado, dr_estado, v)
|
||||
|
||||
xp, yp, thp = self._nova_posicao(
|
||||
float(x), float(y), float(theta),
|
||||
omega, v, tipo, ang,
|
||||
dt=max(1e-4, float(dt)),
|
||||
kin["omega"], v, tipo, ang,
|
||||
dt=dt_use,
|
||||
df_rad=df_estado,
|
||||
dr_rad=dr_estado,
|
||||
)
|
||||
return xp, yp, thp, float(df_estado), float(dr_estado)
|
||||
|
||||
def _avaliar_eixo_yaw_tracking(
|
||||
self,
|
||||
|
|
@ -2476,6 +2573,11 @@ class ControladorMPC:
|
|||
entre_eixos = self._entre_eixos_tracking(contexto)
|
||||
|
||||
xp, yp, thp = float(x), float(y), float(theta)
|
||||
df_estado, dr_estado, _ = self._estado_eixos_fisicos_rad(
|
||||
contexto=contexto,
|
||||
tipo_fallback=tipo_prev,
|
||||
angulo_fallback_rad=0.0,
|
||||
)
|
||||
|
||||
try:
|
||||
# Estado inicial dos eixos: usado apenas para saber qual deles já
|
||||
|
|
@ -2491,11 +2593,12 @@ class ControladorMPC:
|
|||
)
|
||||
|
||||
for _ in range(n):
|
||||
xp, yp, thp = self._passo_micro_eixo_yaw(
|
||||
xp, yp, thp, df_estado, dr_estado = self._passo_micro_eixo_yaw(
|
||||
tipo,
|
||||
float(angulo_rad),
|
||||
xp, yp, thp,
|
||||
v, dt,
|
||||
df_estado, dr_estado,
|
||||
)
|
||||
|
||||
ref = self._referencia_caminho_lookahead(
|
||||
|
|
@ -3993,23 +4096,23 @@ class ControladorMPC:
|
|||
def corrigir_pose_por_latencia(
|
||||
self,
|
||||
x, y, theta, visitados,
|
||||
latency_s, # atraso a compensar (s)
|
||||
dt, # dt nominal do seu controle
|
||||
nova_posicao_fn, # self._nova_posicao
|
||||
u_hold, # {'v': m/s, 'omega': rad/s} (ou forneça 'tipo' e 'angulo' + calc_omega)
|
||||
cmd_seq=None, # opcional: [(dur_s, u_dict), ...] que cobre latency_s
|
||||
latency_s,
|
||||
dt,
|
||||
nova_posicao_fn,
|
||||
u_hold,
|
||||
cmd_seq=None,
|
||||
time_left_ms=lambda: 1e9,
|
||||
calc_omega_fn=None # opcional: fn(tipo, angulo_rad, v) -> omega
|
||||
calc_omega_fn=None,
|
||||
):
|
||||
"""
|
||||
Propaga (x, y, theta) por 'latency_s' usando 'nova_posicao_fn' que depende de self.dt.
|
||||
Retorna (x, y, theta) estimados no presente.
|
||||
"""
|
||||
"""Propaga pose + estados físicos df/dr até o instante presente."""
|
||||
latency_s = max(0.0, float(latency_s))
|
||||
if latency_s == 0.0:
|
||||
return x, y, theta, visitados
|
||||
|
||||
# --- constrói sequência efetiva de comandos ---
|
||||
df_estado = float(u_hold.get("df_inicial", 0.0))
|
||||
dr_estado = float(u_hold.get("dr_inicial", 0.0))
|
||||
|
||||
if latency_s == 0.0:
|
||||
return x, y, theta, visitados, df_estado, dr_estado
|
||||
|
||||
if not cmd_seq:
|
||||
cmd_seq = [(latency_s, u_hold)]
|
||||
else:
|
||||
|
|
@ -4017,66 +4120,55 @@ class ControladorMPC:
|
|||
if total < latency_s:
|
||||
cmd_seq = list(cmd_seq) + [(latency_s - total, cmd_seq[-1][1])]
|
||||
|
||||
# --- adaptador para usar dt_eff com fn que usa self.dt ---
|
||||
def aplicar_passo_dt(xk, yk, thetak, u, dt_eff):
|
||||
# garantir que temos omega e v
|
||||
v = u.get('v', 0.0)
|
||||
if 'omega' in u:
|
||||
omega = u['omega']
|
||||
else:
|
||||
if calc_omega_fn is not None and 'tipo' in u and 'angulo' in u:
|
||||
omega = calc_omega_fn(v, u['angulo'], u['tipo'])
|
||||
else:
|
||||
omega = 0.0 # fallback: reta
|
||||
old_dt = self.dt
|
||||
try:
|
||||
self.dt = dt_eff
|
||||
return nova_posicao_fn(xk, yk, thetak, omega, v, u.get('tipo'), u.get('angulo', 0.0))
|
||||
finally:
|
||||
self.dt = old_dt
|
||||
|
||||
# --- integra por tempo, quebrando cada trecho em subpassos ~dt ---
|
||||
angulo_estado = float(
|
||||
u_hold.get('angulo_inicial', u_hold.get('angulo', 0.0))
|
||||
)
|
||||
t_rem = latency_s
|
||||
for dur_s, u in cmd_seq:
|
||||
if t_rem <= 0.0:
|
||||
break
|
||||
seg = min(dur_s, t_rem)
|
||||
|
||||
seg = min(max(0.0, float(dur_s)), t_rem)
|
||||
if seg <= 0.0:
|
||||
continue
|
||||
|
||||
n = max(1, int(round(seg / dt)))
|
||||
n = max(1, int(round(seg / max(float(dt), 1e-4))))
|
||||
dt_eff = seg / n
|
||||
|
||||
if "df_alvo" in u and "dr_alvo" in u:
|
||||
df_alvo = float(u["df_alvo"])
|
||||
dr_alvo = float(u["dr_alvo"])
|
||||
else:
|
||||
df_alvo, dr_alvo = self._angulos_eixos_4ws(
|
||||
u.get("tipo", TipoMovimentoDirecional.RodasDianteiras),
|
||||
u.get("angulo", 0.0),
|
||||
)
|
||||
|
||||
for _ in range(n):
|
||||
if time_left_ms() <= 0.0:
|
||||
return x, y, theta, visitados
|
||||
angulo_estado = self._avancar_atuador_direcional(
|
||||
angulo_estado,
|
||||
u.get('angulo', angulo_estado),
|
||||
dt_eff,
|
||||
return x, y, theta, visitados, df_estado, dr_estado
|
||||
|
||||
df_estado = self._avancar_atuador_direcional(
|
||||
df_estado, df_alvo, dt_eff
|
||||
)
|
||||
u_aplicado = dict(u)
|
||||
u_aplicado['angulo'] = angulo_estado
|
||||
# omega precisa acompanhar o ângulo físico, não o alvo antigo.
|
||||
u_aplicado.pop('omega', None)
|
||||
x, y, theta = aplicar_passo_dt(
|
||||
x, y, theta, u_aplicado, dt_eff
|
||||
dr_estado = self._avancar_atuador_direcional(
|
||||
dr_estado, dr_alvo, dt_eff
|
||||
)
|
||||
idx_alvo = self._corrigir_pontos_visitados(
|
||||
x,
|
||||
y,
|
||||
visitados,
|
||||
theta=theta,
|
||||
|
||||
v = float(u.get("v", 0.0))
|
||||
kin = self._cinematica_4ws_eixos(df_estado, dr_estado, v)
|
||||
x, y, theta = nova_posicao_fn(
|
||||
x, y, theta,
|
||||
kin["omega"], v,
|
||||
u.get("tipo"), u.get("angulo", 0.0),
|
||||
dt=dt_eff,
|
||||
df_rad=df_estado,
|
||||
dr_rad=dr_estado,
|
||||
)
|
||||
self._corrigir_pontos_visitados(
|
||||
x, y, visitados, theta=theta
|
||||
)
|
||||
|
||||
t_rem -= seg
|
||||
|
||||
# normaliza theta se quiser
|
||||
# theta = (theta + np.pi) % (2*np.pi) - np.pi
|
||||
return x, y, theta, visitados
|
||||
return x, y, theta, visitados, float(df_estado), float(dr_estado)
|
||||
|
||||
|
||||
def _wrap_pi(self, a):
|
||||
|
|
@ -4317,23 +4409,38 @@ class ControladorMPC:
|
|||
#self.passos_horizonte_local = max(1, math.floor(1 / self.tempo_execucao_local))
|
||||
|
||||
# -------------------- Correção por latência (vida real) --------------------
|
||||
# Mantém correção com 'self.dt' e 'velocidade' REAIS
|
||||
u_hold = {
|
||||
"tipo": comando_anterior.get(
|
||||
"tipo_aplicado", comando_anterior.get("tipo")
|
||||
),
|
||||
"angulo": np.radians(
|
||||
comando_anterior.get(
|
||||
"angulo_sp_aplicado",
|
||||
comando_anterior.get("angulo", 0.0),
|
||||
)
|
||||
),
|
||||
"angulo_inicial": np.radians(
|
||||
# Estado inicial agora é 4WS físico: frente e traseira independentes.
|
||||
tipo_aplicado = comando_anterior.get(
|
||||
"tipo_aplicado", comando_anterior.get("tipo")
|
||||
)
|
||||
angulo_sp_aplicado_rad = np.radians(
|
||||
comando_anterior.get(
|
||||
"angulo_sp_aplicado",
|
||||
comando_anterior.get("angulo", 0.0),
|
||||
)
|
||||
)
|
||||
df_inicial, dr_inicial, eixos_reais_validos = self._estado_eixos_fisicos_rad(
|
||||
contexto=contexto,
|
||||
comando_anterior=comando_anterior,
|
||||
tipo_fallback=tipo_aplicado,
|
||||
angulo_fallback_rad=np.radians(
|
||||
comando_anterior.get(
|
||||
"angulo_aplicado",
|
||||
comando_anterior.get("angulo", 0.0),
|
||||
)
|
||||
),
|
||||
)
|
||||
df_alvo, dr_alvo = self._angulos_eixos_4ws(
|
||||
tipo_aplicado, angulo_sp_aplicado_rad
|
||||
)
|
||||
|
||||
u_hold = {
|
||||
"tipo": tipo_aplicado,
|
||||
"angulo": angulo_sp_aplicado_rad,
|
||||
"df_inicial": df_inicial,
|
||||
"dr_inicial": dr_inicial,
|
||||
"df_alvo": df_alvo,
|
||||
"dr_alvo": dr_alvo,
|
||||
"v": velocidade,
|
||||
}
|
||||
cmd_seq = None
|
||||
|
|
@ -4341,7 +4448,10 @@ class ControladorMPC:
|
|||
pos_latencia + self.tempo_execucao_local,
|
||||
LAT_MAX,
|
||||
)
|
||||
x, y, theta, self.visitados_execucao = self.corrigir_pose_por_latencia(
|
||||
(
|
||||
x, y, theta, self.visitados_execucao,
|
||||
df_aplicado_estimado, dr_aplicado_estimado,
|
||||
) = self.corrigir_pose_por_latencia(
|
||||
x, y, theta, self.visitados_execucao,
|
||||
latency_s=latencia_compensada_s,
|
||||
dt=self.dt,
|
||||
|
|
@ -4349,21 +4459,14 @@ class ControladorMPC:
|
|||
u_hold=u_hold,
|
||||
cmd_seq=cmd_seq,
|
||||
time_left_ms=lambda: (deadline - now()) * 1000.0,
|
||||
calc_omega_fn=self._calcular_omega_4ws
|
||||
calc_omega_fn=self._calcular_omega_4ws,
|
||||
)
|
||||
|
||||
# A pose acima já foi trazida até o presente. O estado inicial do
|
||||
# beam search precisa avançar pelo mesmo intervalo para não voltar
|
||||
# ao ângulo físico antigo recebido junto com a posição GNSS.
|
||||
angulo_aplicado_estimado = self._avancar_atuador_direcional(
|
||||
u_hold["angulo_inicial"],
|
||||
u_hold["angulo"],
|
||||
latencia_compensada_s,
|
||||
)
|
||||
# A pose e os dois eixos foram trazidos para o mesmo instante.
|
||||
comando_anterior = dict(comando_anterior)
|
||||
comando_anterior["angulo_aplicado"] = float(
|
||||
np.degrees(angulo_aplicado_estimado)
|
||||
)
|
||||
comando_anterior["_df_aplicado_rad"] = float(df_aplicado_estimado)
|
||||
comando_anterior["_dr_aplicado_rad"] = float(dr_aplicado_estimado)
|
||||
comando_anterior["eixos_fisicos_validos"] = bool(eixos_reais_validos)
|
||||
idx_alvo_correcao = self._corrigir_pontos_visitados(
|
||||
x,
|
||||
y,
|
||||
|
|
@ -4557,16 +4660,18 @@ class ControladorMPC:
|
|||
angulo_final, tipo_final = comando_parado()
|
||||
parada_necessaria = True
|
||||
else:
|
||||
df_beam_inicial, dr_beam_inicial, _ = self._estado_eixos_fisicos_rad(
|
||||
contexto=contexto,
|
||||
comando_anterior=comando_anterior,
|
||||
tipo_fallback=tipo_anterior,
|
||||
angulo_fallback_rad=angulo_anterior,
|
||||
)
|
||||
candidatos_ativos = [{
|
||||
"x": x, "y": y, "theta": theta,
|
||||
"custo": 0.0,
|
||||
"comandos": [(tipo_anterior, angulo_anterior)],
|
||||
"angulo_atuador": np.radians(
|
||||
comando_anterior.get(
|
||||
"angulo_aplicado",
|
||||
comando_anterior.get("angulo", 0.0),
|
||||
)
|
||||
),
|
||||
"angulo_dianteiro_atuador": float(df_beam_inicial),
|
||||
"angulo_traseiro_atuador": float(dr_beam_inicial),
|
||||
"trajetoria": [],
|
||||
"visitados": self.visitados_execucao,
|
||||
"inicial": True
|
||||
|
|
@ -4593,9 +4698,11 @@ class ControladorMPC:
|
|||
cmd_anterior_local = {
|
||||
"tipo": candidato["comandos"][-1][0],
|
||||
"angulo": candidato["comandos"][-1][1],
|
||||
"angulo_aplicado": candidato.get(
|
||||
"angulo_atuador",
|
||||
candidato["comandos"][-1][1],
|
||||
"_df_aplicado_rad": float(
|
||||
candidato["angulo_dianteiro_atuador"]
|
||||
),
|
||||
"_dr_aplicado_rad": float(
|
||||
candidato["angulo_traseiro_atuador"]
|
||||
),
|
||||
}
|
||||
|
||||
|
|
@ -4636,6 +4743,8 @@ class ControladorMPC:
|
|||
15.0,
|
||||
dados_costmap=dados_costmap_ciclo,
|
||||
tipo_preferido=cmd_anterior_local.get("tipo"),
|
||||
df_inicial_rad=cmd_anterior_local.get("_df_aplicado_rad"),
|
||||
dr_inicial_rad=cmd_anterior_local.get("_dr_aplicado_rad"),
|
||||
)
|
||||
|
||||
if ms_left() <= 0 or exp_budget <= 0:
|
||||
|
|
@ -4689,15 +4798,18 @@ class ControladorMPC:
|
|||
t_c_ini = now()
|
||||
try:
|
||||
# >>> CHANGED: passa dt_pred e v_sim para a simulação de um PASSO
|
||||
custo, sim, valido, visitados_sim, _debug_custo, angulo_atuador_final = self._simular_passo(
|
||||
(
|
||||
custo, sim, valido, visitados_sim, _debug_custo,
|
||||
df_atuador_final, dr_atuador_final,
|
||||
) = self._simular_passo(
|
||||
x_atual, y_atual, theta_atual,
|
||||
tipo_k, ang_k,
|
||||
custos_candidatos,
|
||||
cmd_anterior_local, contexto,
|
||||
visitados,
|
||||
dt_pred=dt_pred, # <-- NOVO
|
||||
v_planejado=v_sim, # <-- NOVO
|
||||
passo=passo
|
||||
dt_pred=dt_pred,
|
||||
v_planejado=v_sim,
|
||||
passo=passo,
|
||||
)
|
||||
if passo == 0:
|
||||
debug_custo[f"{tipo_k.value}_{np.degrees(ang_k):.2f}"] = _debug_custo
|
||||
|
|
@ -4707,7 +4819,8 @@ class ControladorMPC:
|
|||
"x": x_f, "y": y_f, "theta": theta_f,
|
||||
"custo": candidato["custo"] + float(custo),
|
||||
"comandos": candidato["comandos"] + [(tipo_k, ang_k)],
|
||||
"angulo_atuador": angulo_atuador_final,
|
||||
"angulo_dianteiro_atuador": df_atuador_final,
|
||||
"angulo_traseiro_atuador": dr_atuador_final,
|
||||
"trajetoria": candidato["trajetoria"] + sim,
|
||||
"visitados": visitados_sim,
|
||||
"inicial": False
|
||||
|
|
@ -4965,6 +5078,8 @@ class ControladorMPC:
|
|||
margem_ms,
|
||||
dados_costmap=None,
|
||||
tipo_preferido=None,
|
||||
df_inicial_rad=None,
|
||||
dr_inicial_rad=None,
|
||||
):
|
||||
"""
|
||||
Gera candidatos de (tipo, ângulo) respeitando o deadline do ciclo:
|
||||
|
|
@ -5277,7 +5392,9 @@ class ControladorMPC:
|
|||
margem_ms=float(margem_ms),
|
||||
permitidos=permitidos,
|
||||
e_lat=e_lat,
|
||||
e_ori=e_ori
|
||||
e_ori=e_ori,
|
||||
df_inicial_rad=df_inicial_rad,
|
||||
dr_inicial_rad=dr_inicial_rad,
|
||||
)
|
||||
|
||||
pares_final = self._uniformizar_pares_candidatos(
|
||||
|
|
@ -5355,7 +5472,9 @@ class ControladorMPC:
|
|||
margem_ms: float = 20.0,
|
||||
permitidos=None,
|
||||
e_lat=0.0,
|
||||
e_ori=0.0
|
||||
e_ori=0.0,
|
||||
df_inicial_rad=None,
|
||||
dr_inicial_rad=None,
|
||||
):
|
||||
"""
|
||||
Seleciona e avalia candidatos (tipo, ângulo) contra a matriz de custo.
|
||||
|
|
@ -5509,7 +5628,11 @@ class ControladorMPC:
|
|||
|
||||
t_c_ini = now()
|
||||
try:
|
||||
traj = self._simular_trajetoria_curta(p_atual, tipo, ang_rad, velocidade, distancia_sim_m)
|
||||
traj = self._simular_trajetoria_curta(
|
||||
p_atual, tipo, ang_rad, velocidade, distancia_sim_m,
|
||||
df_inicial_rad=df_inicial_rad,
|
||||
dr_inicial_rad=dr_inicial_rad,
|
||||
)
|
||||
custo, valido = self._avaliar_trajetoria_matriz_custo(tipo, ang_rad, traj, p_ref, dados_costmap)
|
||||
#print(f"avaliando {tipo} {np.degrees(ang_rad):.2f} - custo: {custo}, valido: {valido}")
|
||||
if valido:
|
||||
|
|
@ -5569,65 +5692,62 @@ class ControladorMPC:
|
|||
mostrar_log(f"❌ Erro ao filtrar candidatos por matriz: {e}")
|
||||
return [], {}
|
||||
|
||||
def _simular_trajetoria_curta(self, p_atual, tipo, angulo, velocidade, dist_min):
|
||||
"""V16: trajetória curta usando a cinemática 4WS global.
|
||||
def _simular_trajetoria_curta(
|
||||
self,
|
||||
p_atual,
|
||||
tipo,
|
||||
angulo,
|
||||
velocidade,
|
||||
dist_min,
|
||||
*,
|
||||
df_inicial_rad=None,
|
||||
dr_inicial_rad=None,
|
||||
):
|
||||
"""Trajetória curta com slew físico independente de df/dr.
|
||||
|
||||
A mesma combinação beta/omega usada pelo MPC pesado é integrada aqui
|
||||
de forma analítica para v, steering e omega constantes.
|
||||
Quando o chamador fornece o estado atual dos eixos, uma troca de modo
|
||||
só ganha a nova geometria conforme cada eixo realmente consegue girar.
|
||||
"""
|
||||
try:
|
||||
dt = float(self.dt)
|
||||
dt = max(1e-4, float(self.dt))
|
||||
v = float(velocidade)
|
||||
if v <= 1e-6 or dist_min <= 1e-6 or dt <= 0.0:
|
||||
if abs(v) <= 1e-6 or dist_min <= 1e-6:
|
||||
return []
|
||||
|
||||
dist_step = v * dt
|
||||
n = int(math.ceil(dist_min / dist_step))
|
||||
dist_step = abs(v) * dt
|
||||
n = int(math.ceil(dist_min / max(dist_step, 1e-9)))
|
||||
if n <= 0:
|
||||
return []
|
||||
n = min(n, int(getattr(self, "_traj_curta_max_steps", 64)))
|
||||
|
||||
x0, y0, th0 = (
|
||||
float(p_atual[0]),
|
||||
float(p_atual[1]),
|
||||
float(p_atual[2]),
|
||||
)
|
||||
x, y, th = map(float, p_atual[:3])
|
||||
df_alvo, dr_alvo = self._angulos_eixos_4ws(tipo, float(angulo))
|
||||
|
||||
kin = self._cinematica_4ws(
|
||||
tipo,
|
||||
float(angulo),
|
||||
v,
|
||||
aplicar_dinamica=True,
|
||||
)
|
||||
beta = float(kin["beta"])
|
||||
omega = float(kin["omega"])
|
||||
|
||||
t = np.arange(1, n + 1, dtype=np.float64) * dt
|
||||
th = th0 + omega * t
|
||||
|
||||
phi0 = th0 + beta
|
||||
|
||||
if abs(omega) <= 1e-9:
|
||||
x = x0 + v * t * math.sin(phi0)
|
||||
y = y0 + v * t * math.cos(phi0)
|
||||
if df_inicial_rad is None or dr_inicial_rad is None:
|
||||
# Compatibilidade para chamadas de debug sem estado físico.
|
||||
df_estado, dr_estado = float(df_alvo), float(dr_alvo)
|
||||
else:
|
||||
phi = th + beta
|
||||
x = x0 + (v / omega) * (
|
||||
math.cos(phi0) - np.cos(phi)
|
||||
)
|
||||
y = y0 + (v / omega) * (
|
||||
np.sin(phi) - math.sin(phi0)
|
||||
)
|
||||
df_estado = float(df_inicial_rad)
|
||||
dr_estado = float(dr_inicial_rad)
|
||||
|
||||
return list(
|
||||
zip(
|
||||
np.asarray(x, dtype=float).tolist(),
|
||||
np.asarray(y, dtype=float).tolist(),
|
||||
np.asarray(th, dtype=float).tolist(),
|
||||
traj = []
|
||||
for _ in range(n):
|
||||
df_estado = self._avancar_atuador_direcional(
|
||||
df_estado, df_alvo, dt
|
||||
)
|
||||
)
|
||||
dr_estado = self._avancar_atuador_direcional(
|
||||
dr_estado, dr_alvo, dt
|
||||
)
|
||||
kin = self._cinematica_4ws_eixos(df_estado, dr_estado, v)
|
||||
x, y, th = self._nova_posicao(
|
||||
x, y, th, kin["omega"], v, tipo, float(angulo),
|
||||
dt=dt, df_rad=df_estado, dr_rad=dr_estado,
|
||||
)
|
||||
traj.append((x, y, th))
|
||||
|
||||
return traj
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro na simulação curta: {e}")
|
||||
mostrar_log(f"Erro ao simular trajetória curta 4WS física: {e}")
|
||||
return []
|
||||
|
||||
def _avaliar_trajetoria_matriz_custo(self, tipo, ang, trajetoria, p_ref, dados_costmap):
|
||||
|
|
@ -5952,11 +6072,19 @@ class ControladorMPC:
|
|||
self.x_ant, self.y_ant = 0.0, 0.0
|
||||
|
||||
simulacoes = []
|
||||
prev_ang = float(comando_anterior.get("angulo", 0.0))
|
||||
prev_tipo = comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value)
|
||||
angulo_atuador = float(
|
||||
comando_anterior.get("angulo_aplicado", prev_ang)
|
||||
prev_ang = float(comando_anterior.get("angulo", 0.0))
|
||||
prev_tipo = comando_anterior.get(
|
||||
"tipo", TipoMovimentoDirecional.RodasDianteiras.value
|
||||
)
|
||||
df_atuador, dr_atuador, _ = self._estado_eixos_fisicos_rad(
|
||||
contexto=contexto,
|
||||
comando_anterior=comando_anterior,
|
||||
tipo_fallback=prev_tipo,
|
||||
angulo_fallback_rad=prev_ang,
|
||||
)
|
||||
df_inicial_passo = float(df_atuador)
|
||||
dr_inicial_passo = float(dr_atuador)
|
||||
df_alvo, dr_alvo = self._angulos_eixos_4ws(tipo, angulo_testado)
|
||||
pontos_visitados = visitados.copy()
|
||||
|
||||
# chave para custo heurístico vindo do VisualWorker (por ângulo/tipo)
|
||||
|
|
@ -6012,22 +6140,23 @@ class ControladorMPC:
|
|||
side_mem_local = 0
|
||||
|
||||
for n_sub in range(n_subs):
|
||||
angulo_atuador = self._avancar_atuador_direcional(
|
||||
angulo_atuador,
|
||||
angulo_testado,
|
||||
sub_dt,
|
||||
df_atuador = self._avancar_atuador_direcional(
|
||||
df_atuador, df_alvo, sub_dt
|
||||
)
|
||||
omega_aplicado = self._calcular_omega_4ws(
|
||||
v_planejado,
|
||||
angulo_atuador,
|
||||
tipo,
|
||||
dr_atuador = self._avancar_atuador_direcional(
|
||||
dr_atuador, dr_alvo, sub_dt
|
||||
)
|
||||
# IMPORTANTE: _nova_posicao precisa aceitar dt opcional
|
||||
kin_aplicada = self._cinematica_4ws_eixos(
|
||||
df_atuador, dr_atuador, v_planejado
|
||||
)
|
||||
omega_aplicado = float(kin_aplicada["omega"])
|
||||
x_sim, y_sim, theta_sim = self._nova_posicao(
|
||||
x_sim, y_sim, theta_sim,
|
||||
omega_aplicado, v_planejado,
|
||||
tipo, angulo_atuador,
|
||||
dt=sub_dt
|
||||
tipo, angulo_testado,
|
||||
dt=sub_dt,
|
||||
df_rad=df_atuador,
|
||||
dr_rad=dr_atuador,
|
||||
)
|
||||
|
||||
simulacoes.append((x_sim, y_sim, theta_sim))
|
||||
|
|
@ -6234,22 +6363,42 @@ class ControladorMPC:
|
|||
#print(custo_visual_worker)
|
||||
|
||||
debug_custo["atuador_direcional"] = {
|
||||
"angulo_inicial_deg": float(np.degrees(
|
||||
comando_anterior.get("angulo_aplicado", prev_ang)
|
||||
)),
|
||||
"angulo_alvo_deg": float(np.degrees(angulo_testado)),
|
||||
"angulo_final_deg": float(np.degrees(angulo_atuador)),
|
||||
"comando_alvo_deg": float(np.degrees(angulo_testado)),
|
||||
"df_inicial_deg": float(np.degrees(df_inicial_passo)),
|
||||
"dr_inicial_deg": float(np.degrees(dr_inicial_passo)),
|
||||
"df_alvo_deg": float(np.degrees(df_alvo)),
|
||||
"dr_alvo_deg": float(np.degrees(dr_alvo)),
|
||||
"df_final_deg": float(np.degrees(df_atuador)),
|
||||
"dr_final_deg": float(np.degrees(dr_atuador)),
|
||||
"taxa_graus_s": float(self._atuador_dir_taxa_graus_s),
|
||||
}
|
||||
return float(custo_total), simulacoes, True, pontos_visitados, debug_custo, angulo_atuador
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"Erro ao simular passo para {tipo.name} | angulo: {np.degrees(angulo_testado):.2f}: {e}")
|
||||
return float('inf'), [(0.0, 0.0, 0.0)], False, visitados, {}, float(
|
||||
comando_anterior.get("angulo_aplicado", comando_anterior.get("angulo", 0.0))
|
||||
return (
|
||||
float(custo_total), simulacoes, True, pontos_visitados,
|
||||
debug_custo, float(df_atuador), float(dr_atuador),
|
||||
)
|
||||
|
||||
def _nova_posicao(self, x, y, theta, omega, velocidade, tipo, angulo_rad, dt=None):
|
||||
except Exception as e:
|
||||
mostrar_log(
|
||||
f"Erro ao simular passo para {tipo.name} | "
|
||||
f"angulo: {np.degrees(angulo_testado):.2f}: {e}"
|
||||
)
|
||||
df_fallback, dr_fallback, _ = self._estado_eixos_fisicos_rad(
|
||||
contexto=contexto,
|
||||
comando_anterior=comando_anterior,
|
||||
tipo_fallback=comando_anterior.get(
|
||||
"tipo", TipoMovimentoDirecional.RodasDianteiras.value
|
||||
),
|
||||
angulo_fallback_rad=float(comando_anterior.get("angulo", 0.0)),
|
||||
)
|
||||
return (
|
||||
float('inf'), [(0.0, 0.0, 0.0)], False, visitados, {},
|
||||
float(df_fallback), float(dr_fallback),
|
||||
)
|
||||
|
||||
def _nova_posicao(
|
||||
self, x, y, theta, omega, velocidade, tipo, angulo_rad, dt=None,
|
||||
df_rad=None, dr_rad=None,
|
||||
):
|
||||
"""V16: integra UM passo com a cinemática 4WS unificada.
|
||||
|
||||
`omega` permanece na assinatura por compatibilidade com callbacks antigos,
|
||||
|
|
@ -6270,7 +6419,13 @@ class ControladorMPC:
|
|||
except Exception:
|
||||
tipo_enum = None
|
||||
|
||||
if tipo_enum is not None:
|
||||
if df_rad is not None and dr_rad is not None:
|
||||
kin = self._cinematica_4ws_eixos(
|
||||
float(df_rad), float(dr_rad), v, aplicar_dinamica=True
|
||||
)
|
||||
beta = float(kin["beta"])
|
||||
omega_use = float(kin["omega"])
|
||||
elif tipo_enum is not None:
|
||||
kin = self._cinematica_4ws(
|
||||
tipo_enum,
|
||||
float(angulo_rad),
|
||||
|
|
|
|||
Loading…
Reference in New Issue