Ajustes no MPC e finalizacao de abertura da curva
This commit is contained in:
Diego Freitas
parent
f4e1a2fe2e
commit
4425e03168
|
|
@ -132,10 +132,15 @@ namespace AgroBase.Forms
|
|||
if (_Trajetoria == null)
|
||||
{
|
||||
Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas();
|
||||
AtualizandoTela = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (_Sensoriamento.Trajetoria == null) return;
|
||||
if (_Sensoriamento.Trajetoria == null)
|
||||
{
|
||||
AtualizandoTela = false;
|
||||
return;
|
||||
}
|
||||
|
||||
lblStatus.Text = $"Carro: {_Sensoriamento.Trajetoria.StatusCarro} ({_Sensoriamento.OperadorVisual.Analises.segmentacao.status_corredor})";
|
||||
lblDirecao.Text = $"Direção: {_Sensoriamento.Trajetoria.DirecaoCaminho}";
|
||||
|
|
|
|||
|
|
@ -156,7 +156,7 @@ namespace AgroBase.Models
|
|||
|
||||
//public List<OperacaoModulosMandatoriosModel> ModulosMandatorios { get; set; } = new List<OperacaoModulosMandatoriosModel>();
|
||||
//public List<OperacaoParametrosMandatoriosModel> ParametrosMandatorios { get; set; } = new List<OperacaoParametrosMandatoriosModel>();
|
||||
public int TempoIniciarOperacao { get; set; } = 30;
|
||||
public int TempoIniciarOperacao { get; set; } = 10;
|
||||
public string ID { get; set; }
|
||||
public double tmrLogsInterval
|
||||
{
|
||||
|
|
@ -1517,7 +1517,7 @@ namespace AgroBase.Models
|
|||
Variaveis.OperacaoEmAndamento.DefinirComponentesEmUso();
|
||||
}
|
||||
|
||||
HealthWorkerService.AtualizarDadosOperacao();
|
||||
HealthWorkerService.AtualizarDadosOperacao(true);
|
||||
break;
|
||||
case BotoesJoystick.L1: // Inicia o modo simulador
|
||||
if (!Variaveis.OperacaoEmAndamento.Iniciado)
|
||||
|
|
@ -1896,8 +1896,9 @@ namespace AgroBase.Models
|
|||
}
|
||||
|
||||
string nome = Variaveis.OperacaoEmAndamento.idxLog.ToString();
|
||||
|
||||
Camera.SaveFrames(new List<TipoFrameCamera>() { TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao }, nome, Caminho); // TipoFrameCamera.Raw4
|
||||
var frames_salvar = new List<TipoFrameCamera>() { TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao };
|
||||
if (Variaveis.OperacaoEmAndamento.Parametros.Controle.RegistrarDadosPosProcessamento) frames_salvar.Add(TipoFrameCamera.Raw4);
|
||||
Camera.SaveFrames(frames_salvar, nome, Caminho);
|
||||
|
||||
var cameras_conectadas = JsonConvert.DeserializeObject<Dictionary<string, object>>(RedisService.Get(CtxKey.DadosCameras));
|
||||
bool conectada = cameras_conectadas.ContainsKey(Camera?.Id ?? "");
|
||||
|
|
@ -1920,8 +1921,9 @@ namespace AgroBase.Models
|
|||
}
|
||||
|
||||
string nome = Variaveis.OperacaoEmAndamento.idxLog.ToString();
|
||||
|
||||
Camera.SaveFrames(new List<TipoFrameCamera>() { TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao }, nome, Caminho); // CameraFrameType.Heatmap, CameraFrameType.RadarTopDown
|
||||
var frames_salvar = new List<TipoFrameCamera>() { TipoFrameCamera.Rgb, TipoFrameCamera.Segmentacao };
|
||||
if (Variaveis.OperacaoEmAndamento.Parametros.Controle.RegistrarDadosPosProcessamento) frames_salvar.Add(TipoFrameCamera.Heatmap);
|
||||
Camera.SaveFrames(frames_salvar, nome, Caminho);
|
||||
|
||||
var cameras_conectadas = JsonConvert.DeserializeObject<Dictionary<string, object>>(RedisService.Get(CtxKey.DadosCameras));
|
||||
bool conectada = cameras_conectadas.ContainsKey(Camera?.Id ?? "");
|
||||
|
|
|
|||
|
|
@ -39,6 +39,8 @@ namespace AgroBase.Models.Operacoes
|
|||
public bool ImuParadaPorInclinacao { get; set; }
|
||||
public bool FrenagemAutomaticaAoParar { get; set; }
|
||||
|
||||
public bool RegistrarDadosPosProcessamento { get; set; } = false;
|
||||
|
||||
public int MovVelocidadeSErvasPercent { get; set; }
|
||||
public int MovRpmMax
|
||||
{
|
||||
|
|
@ -64,6 +66,10 @@ namespace AgroBase.Models.Operacoes
|
|||
public double AtuAlturaAreaPulverizacao { get; set; }
|
||||
public double AtuPercentualErvasBicoOn { get; set; }
|
||||
public double AtuPercentualErvasBicoOff { get; set; }
|
||||
|
||||
public bool MpcMatrizCusto { get; set; } = true;
|
||||
public double MpcHorizonteMin { get; set; } = 1.0;
|
||||
public double MpcHorizonteMax { get; set; } = 2.0;
|
||||
}
|
||||
|
||||
public class OperacaoParametrosDadosModel
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@ namespace AgroBase.Models
|
|||
|
||||
#region PARAMETROS
|
||||
|
||||
public double AnguloAberturaCurva { get; set; } = 45; // Angulo usado para deslocar o ponto de curva
|
||||
public double DistanciaProjecaoRua { get; set; } = 1.5; // Distancia para projetar o primeiro ponto para fora do corredor
|
||||
public static double DistanciaEntrePontos { get; set; } = 0.8; // Distancia entre os pontos dentro do corredor
|
||||
public static double DistanciaEntrePontosCurva { get; set; } = 0.25; // Distancia entre os pontos durante a curva entre corredores
|
||||
|
|
@ -430,11 +431,12 @@ namespace AgroBase.Models
|
|||
{
|
||||
if ((RetornandoBase && CorredorAtual.Dentro) || (!RetornandoBase && Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.EmAndamento))
|
||||
{
|
||||
PontoAtual.AtualizarPropriedades();
|
||||
if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas)
|
||||
{
|
||||
StatusAtual = StatusCarroMapa.Direcionando;
|
||||
}
|
||||
else if (!CorredorAtual.Dentro && (PontoAtual.PontoBorda || PontoAtual.PontoLigacao || ProximoPonto.Tipo == TipoPontoRua.LigacaoEntrada || PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores))
|
||||
else if (!CorredorAtual.Dentro && (PontoAtual.PontoBorda || PontoAtual.PontoLigacao || PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores || PontoAtual.Tipo == TipoPontoRua.Desvio || ProximoPonto.Tipo == TipoPontoRua.LigacaoEntrada))
|
||||
{
|
||||
StatusAtual = StatusCarroMapa.Manobrando;
|
||||
}
|
||||
|
|
@ -772,8 +774,9 @@ namespace AgroBase.Models
|
|||
private void VerificaFimCorredorSegmentacao()
|
||||
{
|
||||
if (CorredorAtual == null) return;
|
||||
if (CorredorAtual.DadosVisuais.JaAntecipado) return;
|
||||
|
||||
bool modoSimulador = true && Variaveis.OperacaoEmAndamento.Simulando;
|
||||
bool modoSimulador = false && Variaveis.OperacaoEmAndamento.Simulando;
|
||||
|
||||
// Limiar de "margem" para considerar proximidade do fim
|
||||
double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 5.0;
|
||||
|
|
@ -880,11 +883,11 @@ namespace AgroBase.Models
|
|||
|
||||
bool podeUsarSegmentacao = modoSimulador || (dadoFresco && dv.CorredorConfirmado);
|
||||
|
||||
bool fimAntecipado = podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente;
|
||||
CorredorAtual.DadosVisuais.JaAntecipado = podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente;
|
||||
|
||||
Console.WriteLine($"podeUsarSegmentacao: {podeUsarSegmentacao}, pertoDoFim: {pertoDoFim}, foraDoCorredorAgora: {foraDoCorredorAgora}, foraContinuoSuficiente: {foraContinuoSuficiente}");
|
||||
|
||||
if (fimAntecipado)
|
||||
if (CorredorAtual.DadosVisuais.JaAntecipado)
|
||||
{
|
||||
List<PontoTrajetoriaModel> pontosRemover = CorredorAtual.Pontos.Where(x => !x.Visitado).ToList();
|
||||
pontosRemover.ForEach(x => x.Visitado = true);
|
||||
|
|
@ -897,9 +900,9 @@ namespace AgroBase.Models
|
|||
_TrajetoriaFixa.Remove(ponto);
|
||||
}
|
||||
|
||||
var pls = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 1];
|
||||
if (CorredorAtual.Pontos.Count >= 2)
|
||||
{
|
||||
var pls = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 1];
|
||||
var pbs = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 2];
|
||||
pls.AlterarTipo(TipoPontoRua.LigacaoSaida);
|
||||
pbs.AlterarTipo(TipoPontoRua.BordaSaida);
|
||||
|
|
@ -907,17 +910,29 @@ namespace AgroBase.Models
|
|||
_TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pbs))?.AlterarTipo(pbs.Tipo);
|
||||
}
|
||||
|
||||
double anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda ? -Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo : Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo;
|
||||
double anguloDesvio = PontoAtual.Orientacao + anguloControle;
|
||||
var pontoDesvio = GPSUtils.GerarPontoDeslocado(PontoAtual.Posicao, anguloDesvio, 1.5);
|
||||
IncluirDesvioNaTrajetoria(new List<GPSModel>() { pontoDesvio }, PontoAtual.idxPonto);
|
||||
|
||||
// Garante que os primeiros pontos do corredor atual estão como visitados
|
||||
// acha o ponto mais próximo do corredor seguinte
|
||||
var pc = _Corredores[CorredorAtual.Idx + 1];
|
||||
|
||||
// acha o ponto mais próximo
|
||||
(GPSModel pp, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, pc.Pontos.Select(x => x.Posicao).ToList());
|
||||
var _pp = pc.Pontos.FirstOrDefault(x => Math.Abs(x.Posicao.Latitude - pp.Latitude) < 1e-8 && Math.Abs(x.Posicao.Longitude - pp.Longitude) < 1e-8);
|
||||
|
||||
double anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda ? -AnguloAberturaCurva : AnguloAberturaCurva;
|
||||
double anguloDesvio = PontoAtual.Orientacao + anguloControle;
|
||||
double fd = FuncoesMatematicas.Map(Variaveis.OperacaoEmAndamento.Sensoriamento.Controle.PercentualVelocidadeSP, 10, 100, 0.7, 2.5);
|
||||
|
||||
var pontoDesvio = GPSUtils.GerarPontoDeslocado(pls.Posicao, anguloDesvio, DistanciaProjecaoRua * fd);
|
||||
|
||||
|
||||
double _FatorLarguraCorredor = CorredorAtual.Largura / LarguraCorredorPadrao;
|
||||
var proximoPontoProximoCorredor = pc.Pontos[_pp.idxPontoCorredor].Posicao;
|
||||
List<GPSModel> CurvaConexao = CriarCurvaEntrePontos(pontoDesvio, proximoPontoProximoCorredor, DistanciaProjecaoRua * 1.5, pls.Orientacao, 6, false);
|
||||
CurvaConexao.Insert(0, pontoDesvio);
|
||||
|
||||
|
||||
IncluirDesvioNaTrajetoria(CurvaConexao, pls.idxPonto, pls.LarguraCorredor * 0.8);
|
||||
|
||||
|
||||
|
||||
// Garante que os primeiros pontos do corredor atual estão como visitados
|
||||
if (_pp != null)
|
||||
{
|
||||
int idxProx = _pp.idxPontoCorredor;
|
||||
|
|
@ -946,8 +961,8 @@ namespace AgroBase.Models
|
|||
|
||||
ReindexarTrajetoriaFixa();
|
||||
|
||||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||||
Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados();
|
||||
HealthWorkerService.AtualizarDadosOperacao(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1350,7 +1365,7 @@ namespace AgroBase.Models
|
|||
int idxEsq = 0;
|
||||
int idxDir = 0;
|
||||
|
||||
double anguloAcrescentar = direcaoAtual == DirecaoCarroRua.Ida ? 45 : -45;
|
||||
double anguloAcrescentar = direcaoAtual == DirecaoCarroRua.Ida ? AnguloAberturaCurva : -AnguloAberturaCurva;
|
||||
|
||||
for (int idx = 0; idx < Corredores.Count(); idx++)
|
||||
{
|
||||
|
|
@ -1416,13 +1431,10 @@ namespace AgroBase.Models
|
|||
if (distanciaMinima && false)
|
||||
{
|
||||
int pontosAdicionr = Convert.ToInt32(CorredoresLarguras[idx] / (DistanciaEntrePontosCurva * _FatorLarguraCorredor));
|
||||
|
||||
var _penultimoPonto = _trajetoriaFixa[_trajetoriaFixa.Count() - 2].Posicao;
|
||||
double anguloProjecao = GPSUtils.CalcularOrientacao(Corredores[idx - 1].First(), Corredores[idx - 1].Last());
|
||||
GPSModel _ultimoPonto = GPSUtils.ProjetarPontoDeslocado(Corredores[idx - 1].Last(), DistanciaProjecaoRua, anguloProjecao);
|
||||
|
||||
GPSModel P0 = CalcularPontoControle(_penultimoPonto, _ultimoPonto, PrimeiroPonto, DistanciaProjecaoRua * 2.0);
|
||||
List<GPSModel> CurvaConexao = GerarCurvaConexao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, P0, pontosAdicionr);
|
||||
List<GPSModel> CurvaConexao = CriarCurvaEntrePontos(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, DistanciaProjecaoRua * 2.0, anguloProjecao, pontosAdicionr, false);
|
||||
|
||||
for (int i = 0; i < CurvaConexao.Count; i++)
|
||||
{
|
||||
|
|
@ -1556,6 +1568,15 @@ namespace AgroBase.Models
|
|||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||||
}
|
||||
|
||||
private List<GPSModel> CriarCurvaEntrePontos(GPSModel _p1, GPSModel _p2, double distanciaCurva, double anguloCurva, int qtdPontos, bool manterPontos)
|
||||
{
|
||||
GPSModel P0 = CalcularPontoControle(anguloCurva, _p1, _p2, distanciaCurva);
|
||||
List<GPSModel> curva = GerarCurvaConexao(_p1, _p2, P0, qtdPontos);
|
||||
if (manterPontos) curva.Insert(0, _p1);
|
||||
else curva.RemoveAt(curva.Count - 1);
|
||||
return curva;
|
||||
}
|
||||
|
||||
private (int, int) AtualizaIndiceRuaLateralCorredor(int i, int idxDir, int incDir, int incEsq)
|
||||
{
|
||||
bool par = (i % 2 == 0);
|
||||
|
|
@ -1575,9 +1596,8 @@ namespace AgroBase.Models
|
|||
return (incDir, incEsq);
|
||||
}
|
||||
|
||||
public static GPSModel CalcularPontoControle(GPSModel penultimoPontoRuaAtual, GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
|
||||
public static GPSModel CalcularPontoControle(double anguloCurva, GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
|
||||
{
|
||||
double anguloCurva = GPSUtils.CalcularOrientacao(penultimoPontoRuaAtual, ultimoPontoRuaAtual);
|
||||
double angulo = GPSUtils.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
|
||||
|
||||
// Calcular a diferença de ângulo corretamente no espaço de 360 graus
|
||||
|
|
@ -2306,7 +2326,7 @@ namespace AgroBase.Models
|
|||
|
||||
|
||||
|
||||
private void IncluirDesvioNaTrajetoria(List<GPSModel> pontos, int idxPontoApartir)
|
||||
private void IncluirDesvioNaTrajetoria(List<GPSModel> pontos, int idxPontoApartir, double larguraCorredor)
|
||||
{
|
||||
if (pontos == null || pontos.Count == 0)
|
||||
return;
|
||||
|
|
@ -2331,7 +2351,7 @@ namespace AgroBase.Models
|
|||
idxCorredor = pa.idxCorredor,
|
||||
Posicao = pontoGps,
|
||||
Direcao = pa.Direcao,
|
||||
LarguraCorredor = pa.LarguraCorredor,
|
||||
LarguraCorredor = larguraCorredor,
|
||||
Orientacao = GPSUtils.CalcularOrientacao(pontoGps, pa.Posicao),
|
||||
Visitado = false
|
||||
};
|
||||
|
|
@ -2472,6 +2492,7 @@ namespace AgroBase.Models
|
|||
|
||||
public class DadosVisuaisCorredor
|
||||
{
|
||||
public bool JaAntecipado { get; set; } = false;
|
||||
// Quanto já andou DENTRO do corredor (EntrandoRua, CaminhandoRua, SaindoRua)
|
||||
public double TempoDentroRuaTotal { get; set; } = 0;
|
||||
public double DistanciaDentroRuaTotal { get; set; } = 0;
|
||||
|
|
@ -2539,6 +2560,8 @@ namespace AgroBase.Models
|
|||
public void AlterarTipo(Enums.TipoPontoRua novo)
|
||||
{
|
||||
Tipo = novo;
|
||||
PontoBorda = Tipo == Enums.TipoPontoRua.BordaEntrada || Tipo == Enums.TipoPontoRua.BordaSaida;
|
||||
PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida;
|
||||
}
|
||||
|
||||
public void AtualizarPropriedades()
|
||||
|
|
|
|||
|
|
@ -195,10 +195,13 @@ namespace AgroBase.Services.Operadores
|
|||
);
|
||||
}
|
||||
|
||||
public static void AtualizarDadosOperacao()
|
||||
public static void AtualizarDadosOperacao(bool forcar = false)
|
||||
{
|
||||
if (AtualizandoRedis)
|
||||
if (AtualizandoRedis && !forcar)
|
||||
return;
|
||||
|
||||
while (AtualizandoRedis && forcar) {}
|
||||
|
||||
AtualizandoRedis = true;
|
||||
|
||||
AtualizarDadosEquipamento();
|
||||
|
|
@ -221,6 +224,7 @@ namespace AgroBase.Services.Operadores
|
|||
("modulos_mandatorios", _Parametros.ModulosMandatorios.Where(x => x.Mandatorio && x.Utilizar).Select(x => (int)x.Dispositivo).ToArray()),
|
||||
("modulos_opcionais", _Parametros.ModulosMandatorios.Where(x => !x.Mandatorio && x.Utilizar).Select(x => (int)x.Dispositivo).ToArray()),
|
||||
("parametros_mandatorios", _Parametros.ParametrosMandatorios.Where(x => x.Mandatorio && x.Utilizar).Select(x => (int)x.Parametro).ToArray()),
|
||||
("ponto_mapa_ref", Variaveis.OperacaoEmAndamento.Trajetoria?._TrajetoriaFixa?.Select(p => new double[] { p.Posicao.Latitude, p.Posicao.Longitude }).FirstOrDefault()),
|
||||
("Mov", new
|
||||
{
|
||||
percent_vel_max = pControle.MovVelocidadeSErvasPercent,
|
||||
|
|
@ -243,15 +247,14 @@ namespace AgroBase.Services.Operadores
|
|||
distanciaMargem = p.LarguraCorredor * 0.8
|
||||
})
|
||||
.ToList(),
|
||||
horizonte = 2.0,
|
||||
horizonte_min = 1.2,
|
||||
horizonte_max = 2.8,
|
||||
horizonte_parado = 1.2,
|
||||
horizonte_min = pControle.MpcHorizonteMin,
|
||||
horizonte_max = pControle.MpcHorizonteMax,
|
||||
angulo_max_graus = pControle.DirAnguloMaximo,
|
||||
velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeCErvasPercent),
|
||||
velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeSErvasPercent),
|
||||
velocidade_min = Math.Round(FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeCErvasPercent), 4),
|
||||
velocidade_max = Math.Round(FuncoesMatematicas.CalculaVelocidadeMsPercentual(pControle.MovVelocidadeSErvasPercent), 4),
|
||||
tempo_entre_comandos = (Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Enums.T_Code.Dir)?.DelayEnvioComando ?? 500) / 1000.0,
|
||||
passos_atraso = 1
|
||||
passos_atraso = 1,
|
||||
usa_matriz_custo = pControle.MpcMatrizCusto
|
||||
}
|
||||
}
|
||||
),
|
||||
|
|
@ -285,6 +288,8 @@ namespace AgroBase.Services.Operadores
|
|||
RedisService.Publish(CmdKey.HealthWorkerRx, JsonConvert.SerializeObject(comando));
|
||||
}
|
||||
|
||||
//Console.WriteLine("Dados de operacao atualizados com sucesso!");
|
||||
|
||||
AtualizandoRedis = false;
|
||||
}
|
||||
|
||||
|
|
@ -298,7 +303,9 @@ namespace AgroBase.Services.Operadores
|
|||
("direcional_automatico", pControle.DirecionalAutomatico),
|
||||
("frenagem_automatica", pControle.FrenagemAutomaticaAoParar),
|
||||
("oak_parada_por_bloqueio", pControle.OakParadaPorObstaculo),
|
||||
("imu_parada_por_inclinacao", pControle.ImuParadaPorInclinacao)
|
||||
("imu_parada_por_inclinacao", pControle.ImuParadaPorInclinacao),
|
||||
|
||||
("registrar_dados_pos_processamento", pControle.RegistrarDadosPosProcessamento)
|
||||
);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -132,7 +132,7 @@ namespace AgroBase.Services.Operadores
|
|||
TempoDesconectado = 0;
|
||||
if (!Variaveis.OperacaoEmAndamento.OperacaoConfigurada)
|
||||
{
|
||||
HealthWorkerService.AtualizarDadosOperacao();
|
||||
HealthWorkerService.AtualizarDadosOperacao(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -83,7 +83,10 @@ def main():
|
|||
acao = HealthWorkerCommandType(dados.get("cmd", 0))
|
||||
|
||||
if acao == HealthWorkerCommandType.AtualizarPontosMapa:
|
||||
ContextoGlobalRedis._atualizar_pontos_mapa()
|
||||
try:
|
||||
ContextoGlobalRedis._atualizar_pontos_mapa()
|
||||
except Exception as e:
|
||||
mostrar_log(f"Erro ao atualizar pontos do mapa: {e}")
|
||||
mostrar_log("Dados de mapa da Operacao atualizados com sucesso!")
|
||||
|
||||
def inicializar():
|
||||
|
|
|
|||
|
|
@ -10,7 +10,6 @@ def main():
|
|||
from manager_worker.gerenciador import ManagerWorker
|
||||
from manager_worker.config import mostrar_log
|
||||
from shared.enums import ManagerWorkerCommandType
|
||||
from manager_worker.modulos.mpc import inicializar as iniciar_mpc
|
||||
from shared.contexto_global_redis import ContextoGlobalRedis, CmdKey, CtxKey
|
||||
from shared.mensagem_worker import WorkerFilaMensagens
|
||||
|
||||
|
|
@ -58,12 +57,19 @@ def main():
|
|||
acao = ManagerWorkerCommandType(dados.get("cmd", 0))
|
||||
|
||||
if acao == ManagerWorkerCommandType.IniciarMPC:
|
||||
parametros = dados.get("params", {}).get("parametros")
|
||||
mapa = dados.get("params", {}).get("mapa")
|
||||
iniciar_operacao = dados.get("params", {}).get("iniciar_operacao", False)
|
||||
if parametros is not None:
|
||||
p_ref = ContextoGlobalRedis.get_operacao().get("ponto_mapa_ref", [])
|
||||
iniciar_mpc(parametros, mapa, p_ref, iniciar_operacao)
|
||||
try:
|
||||
_p = dados.get("params")
|
||||
if _p is not None:
|
||||
parametros = _p.get("parametros")
|
||||
mapa = _p.get("mapa")
|
||||
iniciar_operacao = _p.get("iniciar_operacao", False)
|
||||
p_ref = _p.get("p_ref", [])
|
||||
from manager_worker.modulos.mpc import inicializar as iniciar_mpc
|
||||
iniciar_mpc(parametros, mapa, p_ref, iniciar_operacao)
|
||||
else:
|
||||
ContextoGlobalRedis()._atualizar_mpc(do_manager=True, iniciar_operacao=False)
|
||||
except Exception as e:
|
||||
mostrar_log(f"Erro ao Iniciar MPC: {e}")
|
||||
elif acao == ManagerWorkerCommandType.AtualizarDadosControle:
|
||||
#mostrar_log("Adicionado na fila")
|
||||
fila.adicionar(acao)
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
|
|||
|
||||
_snr = ContextoGlobalRedis.get_modulo(T_Code.Snr)
|
||||
visual_worker_ativado = _controle.get("sonar_ativado", False)
|
||||
usa_matriz_custo = _controle.get("usa_matriz_custo", False)
|
||||
visual_worker_operante = (_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) == StatusModulo.OPERANTE.value
|
||||
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
|
||||
_snapshot_vw = _dados_vw.get("matriz_confianca")
|
||||
|
|
@ -56,10 +57,6 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
|
|||
Vmin = 0.1 # if _trajetoria.get("status", StatusCarroMapa.Parado.value) == StatusCarroMapa.CaminhandoRua.value else 0.8
|
||||
|
||||
contexto = {
|
||||
"Operacao": {
|
||||
"Status": _operacao.get("status"),
|
||||
"Finalizando": _operacao.get("finalizando")
|
||||
},
|
||||
"GPS": {
|
||||
"Latitude": _gps.get("lat", 0),
|
||||
"Longitude": _gps.get("lon", 0),
|
||||
|
|
@ -88,6 +85,7 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
|
|||
"Operante": visual_worker_operante,
|
||||
"Atualizado": visual_worker_atualizado,
|
||||
"MatrizCusto": {
|
||||
"EmUso": usa_matriz_custo,
|
||||
"Valida": visual_worker_ativado and visual_worker_operante and visual_worker_atualizado,
|
||||
"Custo": custo,
|
||||
"Navegavel": nav,
|
||||
|
|
@ -95,21 +93,10 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
|
|||
"EscalasX": escalas,
|
||||
"Block": block
|
||||
},
|
||||
"Camera": {
|
||||
"FovH": ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}).get("parametros", {}).get("fov_h", 0.7),
|
||||
"DistanciaMax": _operacao.get("Snr", {}).get("distancia_maxima", 5000) / 1000.0,
|
||||
"DistanciaMin": _operacao.get("Snr", {}).get("distancia_minima", 500) / 1000.0,
|
||||
}
|
||||
},
|
||||
"RegrasAtivas": {
|
||||
"matriz_custo": False,
|
||||
"deteccao_obstaculos": False,
|
||||
},
|
||||
"Equipamento": {
|
||||
"largura": _equipamento.get("largura", 0.85),
|
||||
"entre_eixos": _equipamento.get("distancia_entre_eixos", 0.92),
|
||||
"angulo_roll_max": _equipamento.get("angulo_roll_max", 15.0),
|
||||
"angulo_pitch_max": _equipamento.get("angulo_pitch_max", 30.0),
|
||||
"entre_eixos": _equipamento.get("distancia_entre_eixos", 0.92)
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -37,13 +37,11 @@ class ControladorMPC:
|
|||
self.dt = 0
|
||||
|
||||
#print(parametros_mpc)
|
||||
self.angulo_max_graus = parametros_mpc.get("angulo_max_graus", 30.0)
|
||||
self.velocidade_min = parametros_mpc.get("velocidade_min", 0.4)
|
||||
self.velocidade_max = parametros_mpc.get("velocidade_max", 1.9)
|
||||
self.horizonte = parametros_mpc.get("horizonte", 2.5)
|
||||
self.horizonte_min = parametros_mpc.get("horizonte_min", 0.6)
|
||||
self.horizonte_max = parametros_mpc.get("horizonte_max", 2.8)
|
||||
self.horizonte_parado = parametros_mpc.get("horizonte_parado", 0.5)
|
||||
self.angulo_max_graus = parametros_mpc.get("angulo_max_graus")
|
||||
self.velocidade_min = parametros_mpc.get("velocidade_min")
|
||||
self.velocidade_max = parametros_mpc.get("velocidade_max")
|
||||
self.horizonte_min = parametros_mpc.get("horizonte_min")
|
||||
self.horizonte_max = parametros_mpc.get("horizonte_max")
|
||||
self._beam_traj_min = parametros_mpc.get("beam_traj_min", 3) # nº mínimo de trajetórias completas que queremos
|
||||
self._beam_topN_max = parametros_mpc.get("beam_topN_max", 5) # limite superior de candidatos ativos
|
||||
self._beam_topN_min = parametros_mpc.get("beam_topN_min", 2) # limite inferior
|
||||
|
|
@ -357,64 +355,6 @@ class ControladorMPC:
|
|||
|
||||
# UTILS
|
||||
|
||||
def _corrigir_pontos_visitados_new(self, x, y, pontos_visitados: np.ndarray, idx_atual: int = -1, limite_max_avanco: float = 5.0, limite_max_pontos: int = 10, velocidade: float = -1, dt: float = -1, look_ahead: bool = True):
|
||||
try:
|
||||
p_xy, p_mg, p_s = self._p_xy, self._p_margem, self._p_s
|
||||
N = p_xy.shape[0]
|
||||
if N == 0:
|
||||
return -1
|
||||
|
||||
# --- (1) Igual ao antigo: marcar EXCLUSIVO quando idx_atual vier do C# ---
|
||||
if idx_atual > -1:
|
||||
pontos_visitados[:idx_atual+1] = True # <<< EXCLUSIVO (antes era :idx_atual+1)
|
||||
return idx_atual
|
||||
|
||||
if pontos_visitados.all():
|
||||
return N - 1
|
||||
|
||||
start_idx = int(np.argmax(~pontos_visitados))
|
||||
|
||||
# janela por metros + clamp por nº de pontos (compatível com o antigo)
|
||||
s_start = p_s[start_idx]
|
||||
s_lim = s_start + float(limite_max_avanco)
|
||||
end_idx = int(np.searchsorted(p_s, s_lim, side="right") - 1)
|
||||
if end_idx < start_idx:
|
||||
end_idx = start_idx
|
||||
end_idx = min(end_idx, start_idx + int(limite_max_pontos))
|
||||
|
||||
sl = slice(start_idx, end_idx + 1)
|
||||
dx = x - p_xy[sl, 0]
|
||||
dy = y - p_xy[sl, 1]
|
||||
dist2 = dx*dx + dy*dy
|
||||
margem2 = p_mg[sl] * p_mg[sl]
|
||||
|
||||
# --- (2) Opcional para fidelidade ao antigo: usar '<' em vez de '<=' ---
|
||||
hits = dist2 < margem2 # era <=
|
||||
|
||||
if np.any(hits):
|
||||
off = int(np.argmax(hits))
|
||||
idx = start_idx + off
|
||||
pontos_visitados[:idx] = True
|
||||
if velocidade > -1 and look_ahead:
|
||||
s_hit = self._p_s[idx]
|
||||
s_goal = s_hit + self.d_look_ahead_m(velocidade, dt)
|
||||
idx_alvo = int(np.searchsorted(self._p_s, s_goal, side="left"))
|
||||
idx_alvo = min(idx_alvo, len(self._p_s)-1)
|
||||
return idx_alvo
|
||||
return idx
|
||||
|
||||
# --- (3) Fallback robusto para “próximo não visitado” ---
|
||||
nv = np.flatnonzero(~pontos_visitados)
|
||||
return int(nv[0]) if nv.size else (N - 1)
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"Erro ao corrigir pontos visitados: {e}")
|
||||
try:
|
||||
nv = np.flatnonzero(~pontos_visitados)
|
||||
return int(nv[0]) if nv.size else (N - 1)
|
||||
except Exception:
|
||||
return -1
|
||||
|
||||
def _proximo_nao_visitado(self, visitados):
|
||||
try:
|
||||
for i, v in enumerate(visitados):
|
||||
|
|
@ -590,64 +530,6 @@ class ControladorMPC:
|
|||
# theta = (theta + np.pi) % (2*np.pi) - np.pi
|
||||
return x, y, theta, visitados
|
||||
|
||||
def d_look_ahead_m(self, velocidade, dt: float = None):
|
||||
"""
|
||||
Retorna look-ahead em METROS.
|
||||
- velocidade: pode vir em m/s ou normalizada (0-1). Detecto automaticamente.
|
||||
- dt: passo real do MPC (s) para suavização adaptativa (opcional).
|
||||
"""
|
||||
|
||||
# --- 1) Normalização de velocidade ---------------------------------------
|
||||
# tente usar self.vel_max_m_s se já existir; senão, usa 6.8 km/h (~1.889 m/s)
|
||||
vel_max_ms = getattr(self, "vel_max_m_s", 6.8/3.6)
|
||||
v = float(velocidade)
|
||||
#if v <= 1.05: # heurística: trata <=1.0 como v_norm
|
||||
# v = max(0.0, min(1.0, v)) * vel_max_ms
|
||||
#else:
|
||||
# v = max(0.0, v) # já está em m/s
|
||||
v = max(0.0, v) # já está em m/s
|
||||
|
||||
# --- 2) Base polinomial (suave e previsível) ------------------------------
|
||||
# L_base = L0 + a*v + b*v^2 (clamp em [Lmin, Lmax])
|
||||
Lmin, Lmax = self.horizonte_min, self.horizonte_max # bom para teu range (1.3–6.8 km/h)
|
||||
L0, a, b = self.horizonte_parado, 0.70, 0.20
|
||||
L = L0 + a*v + b*(v*v)
|
||||
if L < Lmin: L = Lmin
|
||||
if L > Lmax: L = Lmax
|
||||
|
||||
# --- 3) Redução por curvatura e erro de orientação ------------------------
|
||||
# Se você já calcula curvatura/erro, exponha em self; senão, assumo 0.
|
||||
kappa = abs(getattr(self, "curvatura_atual", 0.0)) # 1/m
|
||||
err_yaw = abs(getattr(self, "erro_angular_atual", 0.0)) # rad
|
||||
|
||||
# Fatores <=1.0: reduzem o L em curva forte e/ou erro de yaw grande.
|
||||
# Ajuste c_k, c_e conforme seu corredor (mais alto = reduz mais).
|
||||
c_k, c_e = 3.0, 1.2
|
||||
f_k = 1.0 / (1.0 + c_k * kappa)
|
||||
f_e = 1.0 / (1.0 + c_e * err_yaw)
|
||||
L *= min(f_k, f_e)
|
||||
|
||||
# --- 4) Compensação de latência de controle/planta ------------------------
|
||||
# Se você tem latência medida, exponha em self.tau_ctrl (s). Senão: ~0.15 s.
|
||||
tau = float(getattr(self, "tau_ctrl", 0.15))
|
||||
L += v * tau # “empurra” o alvo à frente para cobrir atraso
|
||||
|
||||
# Clamp final para garantir limites mesmo após fatores
|
||||
if L < Lmin: L = Lmin
|
||||
if L > Lmax: L = Lmax
|
||||
|
||||
# --- 5) Suavização temporal (anti-chatter) --------------------------------
|
||||
# EMA com constante de tempo ~0.4 s (ajuste T_ema). Usa dt se informado.
|
||||
T_ema = 0.40
|
||||
if dt is None:
|
||||
dt = float(getattr(self, "dt", 0.15)) # teu MPC já guarda dt
|
||||
alpha = max(0.0, min(1.0, 1.0 - pow(2.718281828, -dt / T_ema)))
|
||||
L_suave = getattr(self, "_L_ema", L)
|
||||
L_suave = (1.0 - alpha) * L_suave + alpha * L
|
||||
self._L_ema = L_suave
|
||||
|
||||
return L_suave
|
||||
|
||||
|
||||
def _wrap_pi(self, a):
|
||||
try:
|
||||
|
|
@ -792,6 +674,7 @@ class ControladorMPC:
|
|||
|
||||
self.tempo_execucao_local = contexto.get("Carro", {}).get("TempoEntreComandos", 0.5)
|
||||
self.passos_horizonte_local = max(1, math.floor(1 / self.tempo_execucao_local))
|
||||
self.passos_horizonte_local = 1
|
||||
|
||||
# -------------------- Correção por latência (vida real) --------------------
|
||||
# Mantém correção com 'self.dt' e 'velocidade' REAIS
|
||||
|
|
@ -819,8 +702,8 @@ class ControladorMPC:
|
|||
idx_alvo_correcao = self._corrigir_pontos_visitados(x, y, self.visitados_execucao, idx_proximo_ponto_real)
|
||||
|
||||
# -------------------- Planejamento: horizonte ESPACIAL fixo --------------------
|
||||
S_ALVO = float(getattr(self, "horizonte", 2.5))
|
||||
V_FLOOR = float(getattr(self, "_vmin_planejamento", 0.40))
|
||||
S_ALVO = self.horizonte_max
|
||||
V_FLOOR = float(getattr(self, "_vmin_planejamento", 0.30))
|
||||
DT_MIN = float(getattr(self, "_dt_pred_min", 0.05))
|
||||
DT_MAX = float(getattr(self, "_dt_pred_max", 0.25))
|
||||
K_ALVO = int(getattr(self, "_k_alvo", 16))
|
||||
|
|
@ -864,6 +747,8 @@ class ControladorMPC:
|
|||
|
||||
self.qtd_comandos_sucessivos = K
|
||||
|
||||
#print(f"S_ALVO: {S_ALVO}, v_sim: {v_sim}, passos_local: {self.passos_horizonte_local}, K: {K}, dt_pred: {dt_pred}")
|
||||
|
||||
# -------------------- Seleção do comando --------------------
|
||||
angulo_anterior = np.radians(comando_anterior.get("angulo", 0.0))
|
||||
tipo_anterior = TipoMovimentoDirecional(comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value))
|
||||
|
|
@ -1079,7 +964,7 @@ class ControladorMPC:
|
|||
|
||||
# -------------------- Debug opcional da matriz de custo --------------------
|
||||
self._tick_id = getattr(self, "_tick_id", 0) + 1
|
||||
if (getattr(self, "debug_cost_vis", False) and contexto.get("RegrasAtivas", {}).get("matriz_custo", False) and (self._tick_id % 10 == 0)):
|
||||
if (getattr(self, "debug_cost_vis", False) and contexto.get("VisualWorker", {}).get("MatrizCusto", {}).get("EmUso", False) and (self._tick_id % 10 == 0)):
|
||||
dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {})
|
||||
deadline_dbg = time.perf_counter() + 0.03
|
||||
tipos_dbg, angs_dbg, _ = self._gerar_angulos_candidatos_receding(
|
||||
|
|
@ -1184,9 +1069,9 @@ class ControladorMPC:
|
|||
|
||||
# 3) Flags de matriz
|
||||
dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {})
|
||||
regra_matriz_custo = bool(dados_matriz_custo.get("EmUso", False))
|
||||
matriz_custo_valida = bool(dados_matriz_custo.get("Valida", False))
|
||||
regra_matriz_custo = bool(contexto.get("RegrasAtivas", {}).get("matriz_custo", False))
|
||||
filtrar_matriz = matriz_custo_valida and regra_matriz_custo
|
||||
filtrar_matriz = regra_matriz_custo and matriz_custo_valida
|
||||
|
||||
# 4) Geração bruta (graus)
|
||||
angulos_raw = self._gerar_candidatos_brutos(angulo_ideal_deg, filtrar_matriz)
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -132,7 +132,7 @@ class ContextoGlobalRedis:
|
|||
dados = json.loads(mensagem["data"])
|
||||
funcao_callback(dados)
|
||||
except Exception as e:
|
||||
mostrar_log(f"Erro ao processar comando recebido: {e}")
|
||||
mostrar_log(f"Erro ao processar comando recebido: {mensagem['data']} | {e}")
|
||||
|
||||
# Roda o ouvinte em uma thread separada (não bloqueia)
|
||||
threading.Thread(target=_ouvinte, daemon=True).start()
|
||||
|
|
@ -553,24 +553,23 @@ class ContextoGlobalRedis:
|
|||
if (dados_dir.get("tipo_controle", TiposControladorDirecional.Manual.value) == TiposControladorDirecional.MPC.value):
|
||||
dados_mpc = dados_dir.get("mpc", {})
|
||||
mapa = cls.get_operacao().get("pontos_mapa", [])
|
||||
p_ref = cls.get_operacao().get("ponto_mapa_ref", [])
|
||||
if do_manager:
|
||||
p_ref = cls.get_operacao().get("ponto_mapa_ref", [])
|
||||
from manager_worker.modulos.mpc import inicializar as iniciar_mpc
|
||||
iniciar_mpc(dados_mpc, mapa, p_ref, iniciar_operacao)
|
||||
else:
|
||||
cls.publicar_comando(CmdKey.ManagerWorkerRx, { "cmd": ManagerWorkerCommandType.IniciarMPC.value, "params": { "parametros": dados_mpc, "mapa": mapa, "iniciar_operacao": iniciar_operacao } })
|
||||
cls.publicar_comando(CmdKey.ManagerWorkerRx, { "cmd": ManagerWorkerCommandType.IniciarMPC.value, "params": { "parametros": dados_mpc, "mapa": mapa, "p_ref": p_ref, "iniciar_operacao": iniciar_operacao } })
|
||||
|
||||
@classmethod
|
||||
def _atualizar_pontos_mapa(cls):
|
||||
pontos_info = []
|
||||
p_ref = (0, 0)
|
||||
_operacao = cls.get_operacao()
|
||||
dados_dir = _operacao.get("Dir", {})
|
||||
if (dados_dir.get("tipo_controle", TiposControladorDirecional.Manual.value) == TiposControladorDirecional.MPC.value):
|
||||
dados_mpc = dados_dir.get("mpc", {})
|
||||
pontos = dados_mpc.get("pontos", [])
|
||||
if pontos is not None and len(pontos) > 0:
|
||||
p_ref = pontos[0]["lat"], pontos[0]["lon"]
|
||||
p_ref = cls.get_operacao().get("ponto_mapa_ref")
|
||||
from shared.gps_handler import GPSHandler
|
||||
handler = GPSHandler(p_ref[0], p_ref[1])
|
||||
for p in pontos:
|
||||
|
|
@ -584,7 +583,6 @@ class ContextoGlobalRedis:
|
|||
cls.atualizar_ctx_dict(
|
||||
CtxKey.DadosOperacao,
|
||||
pontos_mapa=pontos_info,
|
||||
ponto_mapa_ref=p_ref
|
||||
)
|
||||
|
||||
if len(pontos_info) > 0:
|
||||
|
|
|
|||
|
|
@ -846,7 +846,7 @@ namespace OperationControl.Controls
|
|||
{
|
||||
foreach (var rua in RuasMapaCarregado)
|
||||
{
|
||||
var dado = Mapa.features.FirstOrDefault(x => x.properties.Id == rua.Id);
|
||||
var dado = Mapa?.features?.FirstOrDefault(x => x.properties.Id == rua.Id);
|
||||
if (dado == null) continue;
|
||||
rua.Selected = RuasSelecionadas.Contains(dado.properties.Id);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,37 @@
|
|||
import matplotlib.pyplot as plt
|
||||
|
||||
# Substitua pelas suas coordenadas (lat, lon)
|
||||
coords = [
|
||||
[-22.172709676583331,-47.395223974666663],
|
||||
[-22.172716850972222,-47.395224935138884],
|
||||
[-22.172725134386194,-47.39523641178512],
|
||||
[-22.172728586980455,-47.395222881810845],[-22.172727347766877,-47.395217842486744],[-22.172724131546165,-47.395213953604689],[-22.172718938318319,-47.395211215164665],[-22.172711768083332,-47.395209627166665]
|
||||
#[-22.172711768083332,-47.395209627166665]
|
||||
]
|
||||
|
||||
# Separar latitude e longitude
|
||||
lats = [c[0] for c in coords]
|
||||
lons = [c[1] for c in coords]
|
||||
|
||||
plt.figure(figsize=(8, 6))
|
||||
|
||||
# Linha conectando os pontos
|
||||
plt.plot(lons, lats, linestyle='-', marker='o')
|
||||
|
||||
# Primeiro ponto (verde)
|
||||
plt.scatter(lons[0], lats[0], color='green', s=100, label='Primeiro')
|
||||
|
||||
# Último ponto (vermelho)
|
||||
plt.scatter(lons[-1], lats[-1], color='red', s=100, label='Último')
|
||||
|
||||
# Numerar os pontos
|
||||
for i, (lat, lon) in enumerate(coords):
|
||||
plt.text(lon, lat, f' {i}', fontsize=10)
|
||||
|
||||
plt.xlabel("Longitude")
|
||||
plt.ylabel("Latitude")
|
||||
plt.title("Plot de Coordenadas")
|
||||
plt.legend()
|
||||
plt.grid(True)
|
||||
|
||||
plt.show()
|
||||
Loading…
Reference in New Issue