endurecido a trajetoria e revisado comportamento final de recuperação do barramento i2c no sensoriamento
This commit is contained in:
Diego Freitas
parent
9410933079
commit
024eceef60
|
|
@ -436,77 +436,105 @@ namespace AgroBase.Models
|
|||
public void PopularTrajetoriaMapa(MapaFeatureCollectionModel dados = null)
|
||||
{
|
||||
if (dados != null)
|
||||
{
|
||||
mapaService.DefinirMapa(dados, mapaService.NomeArquivos ?? "Mapa");
|
||||
}
|
||||
|
||||
if (mapaService.DadosMapa == null || mapaService.DadosMapa.features == null)
|
||||
if (mapaService.DadosMapa?.features == null)
|
||||
throw new InvalidOperationException("Mapa sem features para montar a trajetória.");
|
||||
|
||||
var ids = (RuasPercorrer ?? new List<string>())
|
||||
.Select(x => x?.Trim())
|
||||
.Where(x => !string.IsNullOrWhiteSpace(x))
|
||||
.ToList();
|
||||
|
||||
if (ids.Count < 2)
|
||||
throw new InvalidOperationException("Selecione ao menos duas ruas, na ordem de percurso.");
|
||||
|
||||
if (ids.Distinct(StringComparer.Ordinal).Count() != ids.Count)
|
||||
throw new InvalidOperationException("A seleção contém IDs de rua repetidos.");
|
||||
|
||||
var ruasMontadas = new List<List<GPSModel>>();
|
||||
|
||||
foreach (string idSelecionado in ids)
|
||||
{
|
||||
return;
|
||||
}
|
||||
var correspondencias = mapaService.DadosMapa.features
|
||||
.Where(x =>
|
||||
x?.properties != null &&
|
||||
string.Equals(
|
||||
x.properties.Id?.Trim(),
|
||||
idSelecionado,
|
||||
StringComparison.Ordinal
|
||||
)
|
||||
)
|
||||
.ToList();
|
||||
|
||||
TrajetoriaMapa = new List<List<GPSModel>>();
|
||||
|
||||
double orientacaoReferencia = double.NaN;
|
||||
|
||||
foreach (string idSelecionado in RuasPercorrer)
|
||||
{
|
||||
MapaFeatureModel feature = mapaService.DadosMapa.features
|
||||
.FirstOrDefault(x =>
|
||||
x != null &&
|
||||
x.properties != null &&
|
||||
string.Equals(x.properties?.Id?.Trim(), idSelecionado?.Trim(), StringComparison.Ordinal)
|
||||
if (correspondencias.Count != 1)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"A rua '{idSelecionado}' possui {correspondencias.Count} correspondências no mapa; esperado=1."
|
||||
);
|
||||
|
||||
if (feature?.geometry?.coordinates == null || feature.geometry.coordinates.Count < 2)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
List<GPSModel> rua = new List<GPSModel>();
|
||||
var coordenadas = correspondencias[0].geometry?.coordinates;
|
||||
|
||||
foreach (List<double> ponto in feature.geometry.coordinates)
|
||||
if (coordenadas == null || coordenadas.Count < 2)
|
||||
throw new InvalidOperationException($"A rua '{idSelecionado}' não possui geometria navegável.");
|
||||
|
||||
var rua = new List<GPSModel>();
|
||||
|
||||
for (int i = 0; i < coordenadas.Count; i++)
|
||||
{
|
||||
List<double> ponto = coordenadas[i];
|
||||
|
||||
if (ponto == null || ponto.Count < 2)
|
||||
throw new InvalidOperationException($"Coordenada incompleta na rua '{idSelecionado}', ponto {i + 1}.");
|
||||
|
||||
double longitude = ponto[0];
|
||||
double latitude = ponto[1];
|
||||
|
||||
bool valida =
|
||||
!double.IsNaN(latitude) &&
|
||||
!double.IsInfinity(latitude) &&
|
||||
!double.IsNaN(longitude) &&
|
||||
!double.IsInfinity(longitude) &&
|
||||
latitude >= -90 && latitude <= 90 &&
|
||||
longitude >= -180 && longitude <= 180;
|
||||
|
||||
if (!valida)
|
||||
throw new InvalidOperationException($"Coordenada inválida na rua '{idSelecionado}', ponto {i + 1}.");
|
||||
|
||||
rua.Add(new GPSModel
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
rua.Add(new GPSModel { Longitude = ponto[0], Latitude = ponto[1] });
|
||||
Latitude = latitude,
|
||||
Longitude = longitude
|
||||
});
|
||||
}
|
||||
|
||||
if (rua.Count < 2)
|
||||
continue;
|
||||
|
||||
double orientacaoRua = GPSUtils.CalcularOrientacao(rua[rua.Count - 1], rua[rua.Count - 2]);
|
||||
|
||||
if (double.IsNaN(orientacaoReferencia))
|
||||
{
|
||||
orientacaoReferencia = orientacaoRua;
|
||||
}
|
||||
else
|
||||
{
|
||||
double anguloOposto = (orientacaoRua + 180.0) % 360.0;
|
||||
double erro = Math.Abs(CalcularErroAngular(anguloOposto, orientacaoReferencia));
|
||||
if (erro <= 45.0)
|
||||
{
|
||||
rua.Reverse();
|
||||
}
|
||||
}
|
||||
|
||||
TrajetoriaMapa.Add(rua);
|
||||
ruasMontadas.Add(rua);
|
||||
}
|
||||
|
||||
if (!TrajetoriaMapa.Any())
|
||||
return;
|
||||
|
||||
OperacaoModel op = Variaveis.OperacaoEmAndamento;
|
||||
|
||||
if (op == null)
|
||||
return;
|
||||
throw new InvalidOperationException("Operação indisponível para instalar a trajetória.");
|
||||
|
||||
op.Trajetoria = new TrajetoriaMapaOperacaoModel(TrajetoriaMapa);
|
||||
op.Trajetoria.ProjetarTrajetoriaFixa();
|
||||
var trajetoriaAnterior = op.Trajetoria;
|
||||
var mapaAnterior = TrajetoriaMapa;
|
||||
var novaTrajetoria = new TrajetoriaMapaOperacaoModel(ruasMontadas);
|
||||
|
||||
try
|
||||
{
|
||||
// A projeção consulta op.Trajetoria internamente; por isso a nova
|
||||
// instância é instalada dentro da transação e restaurada na falha.
|
||||
op.Trajetoria = novaTrajetoria;
|
||||
novaTrajetoria.ProjetarTrajetoriaFixa();
|
||||
TrajetoriaMapa = ruasMontadas;
|
||||
}
|
||||
catch
|
||||
{
|
||||
op.Trajetoria = trajetoriaAnterior;
|
||||
TrajetoriaMapa = mapaAnterior;
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
private static double CalcularErroAngular(double atual, double referencia)
|
||||
|
|
|
|||
|
|
@ -497,23 +497,38 @@ namespace AgroBase.Models.Modules
|
|||
FreioAlterado ||
|
||||
ReafirmacaoNecessaria;
|
||||
|
||||
OidFuncCode modoControle =
|
||||
Freado ? OidFuncCode.ComandoCorrenteFreio :
|
||||
ComandoLigado ? OidFuncCode.ComandoVelocidade :
|
||||
OidFuncCode.ComandoCorrente;
|
||||
OidFuncCode modoControle;
|
||||
|
||||
if (Freado)
|
||||
{
|
||||
// TESTE DIAGNÓSTICO:
|
||||
// mantém toda a lógica EmFreio ativa,
|
||||
// mas NÃO aplica frenagem elétrica nos OID.
|
||||
modoControle = OidFuncCode.ComandoCorrente;
|
||||
}
|
||||
else
|
||||
{
|
||||
modoControle =
|
||||
ComandoLigado
|
||||
? OidFuncCode.ComandoVelocidade
|
||||
: OidFuncCode.ComandoCorrente;
|
||||
}
|
||||
|
||||
if (EnviaComando)
|
||||
{
|
||||
float setPoint = 0;
|
||||
|
||||
if (modoControle == OidFuncCode.ComandoCorrenteFreio)
|
||||
{
|
||||
setPoint = (float)CorrenteFreio;
|
||||
}
|
||||
else {
|
||||
else if (modoControle == OidFuncCode.ComandoVelocidade)
|
||||
{
|
||||
int mx =
|
||||
Sentido_SP == Sentido.Horario ? 1 :
|
||||
Sentido_SP == Sentido.Antihorario ? -1 :
|
||||
0;
|
||||
Sentido_SP == Sentido.Horario ? 1 :
|
||||
Sentido_SP == Sentido.Antihorario ? -1 :
|
||||
0;
|
||||
|
||||
setPoint = RPM_SP * mx;
|
||||
}
|
||||
|
||||
|
|
@ -584,6 +599,7 @@ namespace AgroBase.Models.Modules
|
|||
public SensorServoModel Servo => Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Servos?.FirstOrDefault(x => x.Inicializado && x.ID.Contains(Mod_ID));
|
||||
private double VelocidadeAtual => FuncoesMatematicas.CalculaVelocidadePercentualMs(Variaveis.OperacaoEmAndamento.Sensoriamento?.Movimentacao?.VelocidadeMediaMs ?? 0);
|
||||
private bool StatusControleEmFreio => Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.Modulo_ID == Mod_ID)?.MovMotor?.Leitura?.Freado ?? false;
|
||||
private bool StatusComandoFreio => Variaveis.OperacaoEmAndamento.Controle?.EmFreio ?? false;
|
||||
private int? AnguloLido => (int?)Servo?.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.ServoAnguloLeitura)?.atual?.valor;
|
||||
|
||||
// histerese de velocidade
|
||||
|
|
@ -622,7 +638,8 @@ namespace AgroBase.Models.Modules
|
|||
return;
|
||||
}
|
||||
|
||||
bool emFreio = StatusControleEmFreio;
|
||||
//bool emFreio = StatusControleEmFreio;
|
||||
bool emFreio = StatusComandoFreio;
|
||||
bool transicao = (UltimoControle != emFreio);
|
||||
|
||||
if (anguloInicial == -1) anguloInicial = AnguloInicial;
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -2071,6 +2071,17 @@ namespace AgroBase.Services
|
|||
return PenultimaLeitura.Clone();
|
||||
}
|
||||
|
||||
public static (GPSModel atual, GPSModel anterior) GetSnapshotPair()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return (
|
||||
UltimaLeitura.Clone(),
|
||||
PenultimaLeitura.Clone()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
private static bool PossuiHeadingValido(GPSModel leitura)
|
||||
{
|
||||
if (leitura == null)
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
using AgroBase.Models;
|
||||
using AgroBase.Models;
|
||||
using AgroBase.Models.Modules;
|
||||
using AgroBase.Models.Operadores;
|
||||
using Newtonsoft.Json;
|
||||
|
|
@ -344,7 +344,29 @@ namespace AgroBase.Services.Operadores
|
|||
lat = p.Posicao.Latitude,
|
||||
lon = p.Posicao.Longitude,
|
||||
tipo = (int)p.Tipo,
|
||||
distanciaMargem = p.LarguraCorredor * 0.8
|
||||
distanciaMargem = Math.Max(
|
||||
0.25,
|
||||
p.LarguraCorredor * 0.8
|
||||
),
|
||||
|
||||
/*
|
||||
* Identidade global preservada até o MPC.
|
||||
* Não renumerar após recortes/conversões.
|
||||
*/
|
||||
idxPonto = p.idxPonto,
|
||||
idxCorredor = p.idxCorredor,
|
||||
|
||||
/*
|
||||
* O MPC usa um limiar de heading mais
|
||||
* conservador para bordas e ligações.
|
||||
*/
|
||||
estrutural =
|
||||
p.PontoBorda ||
|
||||
p.PontoLigacao ||
|
||||
p.Tipo == TipoPontoRua.BordaEntrada ||
|
||||
p.Tipo == TipoPontoRua.BordaSaida ||
|
||||
p.Tipo == TipoPontoRua.LigacaoEntrada ||
|
||||
p.Tipo == TipoPontoRua.LigacaoSaida
|
||||
})
|
||||
.ToList(),
|
||||
angulo_max_graus = pControle.DirAnguloMaximo,
|
||||
|
|
|
|||
|
|
@ -216,6 +216,20 @@ def _assinatura_mapa(mapa, p_ref):
|
|||
round(_safe_float(
|
||||
ponto.get("distanciaMargem", 0.7)
|
||||
), 3),
|
||||
_safe_int(
|
||||
ponto.get("idxPonto", ponto.get("IdxPonto", -1)),
|
||||
-1,
|
||||
),
|
||||
_safe_int(
|
||||
ponto.get("idxCorredor", ponto.get("IdxCorredor", -1)),
|
||||
-1,
|
||||
),
|
||||
bool(
|
||||
ponto.get(
|
||||
"estrutural",
|
||||
ponto.get("PontoEstrutural", False),
|
||||
)
|
||||
),
|
||||
))
|
||||
|
||||
referencia = (
|
||||
|
|
@ -273,6 +287,81 @@ class ControladorMPC:
|
|||
self.velocidade_max = _safe_float(parametros_mpc.get("velocidade_max", 2.0), 2.0, min_value=max(self.velocidade_min, 1e-6))
|
||||
self.horizonte = _safe_float(parametros_mpc.get("horizonte", 2.0), 2.0, min_value=0.1)
|
||||
|
||||
# O progresso continua sendo autoridade exclusiva do C#. A direcao,
|
||||
# porem, nao deve perseguir o waypoint discreto mais proximo: com
|
||||
# pontos a cada ~0,8 m isso inverte o erro a cada passagem e produz
|
||||
# zigue-zague. Estes parametros criam um alvo continuo adiante da
|
||||
# projecao do rover sobre a polilinha, sem marcar nenhum ponto.
|
||||
self._lookahead_reta_base_m = _safe_float(
|
||||
parametros_mpc.get("lookahead_reta_base_m", 1.80),
|
||||
1.80,
|
||||
min_value=0.50,
|
||||
max_value=8.0,
|
||||
)
|
||||
self._lookahead_reta_velocidade_s = _safe_float(
|
||||
parametros_mpc.get("lookahead_reta_velocidade_s", 0.70),
|
||||
0.70,
|
||||
min_value=0.0,
|
||||
max_value=4.0,
|
||||
)
|
||||
self._lookahead_reta_min_m = _safe_float(
|
||||
parametros_mpc.get("lookahead_reta_min_m", 1.60),
|
||||
1.60,
|
||||
min_value=0.30,
|
||||
max_value=8.0,
|
||||
)
|
||||
self._lookahead_reta_max_m = _safe_float(
|
||||
parametros_mpc.get("lookahead_reta_max_m", 3.00),
|
||||
3.00,
|
||||
min_value=self._lookahead_reta_min_m,
|
||||
max_value=12.0,
|
||||
)
|
||||
self._lookahead_manobra_base_m = _safe_float(
|
||||
parametros_mpc.get("lookahead_manobra_base_m", 0.80),
|
||||
0.80,
|
||||
min_value=0.25,
|
||||
max_value=4.0,
|
||||
)
|
||||
self._lookahead_manobra_velocidade_s = _safe_float(
|
||||
parametros_mpc.get("lookahead_manobra_velocidade_s", 0.35),
|
||||
0.35,
|
||||
min_value=0.0,
|
||||
max_value=2.0,
|
||||
)
|
||||
self._lookahead_manobra_max_m = _safe_float(
|
||||
parametros_mpc.get("lookahead_manobra_max_m", 1.40),
|
||||
1.40,
|
||||
min_value=self._lookahead_manobra_base_m,
|
||||
max_value=6.0,
|
||||
)
|
||||
self._janela_projecao_alvo_m = _safe_float(
|
||||
parametros_mpc.get("janela_projecao_alvo_m", 5.0),
|
||||
5.0,
|
||||
min_value=self._lookahead_reta_max_m,
|
||||
max_value=20.0,
|
||||
)
|
||||
|
||||
# Amortecimento aplicado somente durante CaminhandoRua. Nas curvas o
|
||||
# MPC preserva toda a autoridade angular necessaria para a manobra.
|
||||
self._filtro_angulo_reta_tau_s = _safe_float(
|
||||
parametros_mpc.get("filtro_angulo_reta_tau_s", 0.25),
|
||||
0.25,
|
||||
min_value=0.0,
|
||||
max_value=2.0,
|
||||
)
|
||||
self._taxa_angulo_reta_graus_s = _safe_float(
|
||||
parametros_mpc.get("taxa_angulo_reta_graus_s", 18.0),
|
||||
18.0,
|
||||
min_value=1.0,
|
||||
max_value=120.0,
|
||||
)
|
||||
self._deadband_angulo_reta_graus = _safe_float(
|
||||
parametros_mpc.get("deadband_angulo_reta_graus", 0.60),
|
||||
0.60,
|
||||
min_value=0.0,
|
||||
max_value=5.0,
|
||||
)
|
||||
|
||||
self._beam_traj_min = _safe_int(parametros_mpc.get("beam_traj_min", 3), 3, min_value=1, max_value=20)
|
||||
self._beam_topN_max = _safe_int(parametros_mpc.get("beam_topN_max", 5), 5, min_value=1, max_value=50)
|
||||
self._beam_topN_min = _safe_int(parametros_mpc.get("beam_topN_min", 2), 2, min_value=1, max_value=self._beam_topN_max)
|
||||
|
|
@ -287,6 +376,22 @@ class ControladorMPC:
|
|||
|
||||
self.visitados_execucao = np.zeros(len(self.pontos_info), dtype=self._np_bool) if self.pontos_info else np.zeros(0, dtype=self._np_bool)
|
||||
|
||||
# Índices globais são opcionais para manter compatibilidade. Quando
|
||||
# presentes, eliminam a ambiguidade entre o índice da trajetória C#
|
||||
# e o índice local de uma lista dinâmica enviada ao MPC.
|
||||
self._idx_global_pontos = np.array([
|
||||
_safe_int(
|
||||
p.get("idxPonto", p.get("IdxPonto", i)),
|
||||
i,
|
||||
)
|
||||
for i, p in enumerate(self.pontos_info)
|
||||
], dtype=np.int64)
|
||||
self._idx_global_para_local = {
|
||||
int(idx_global): int(i)
|
||||
for i, idx_global in enumerate(self._idx_global_pontos)
|
||||
}
|
||||
self._ultima_divergencia_visitados = False
|
||||
|
||||
try:
|
||||
if self.pontos_info:
|
||||
P = np.array([p.get("xy", (0.0, 0.0)) for p in self.pontos_info], dtype=np.float32)
|
||||
|
|
@ -356,7 +461,69 @@ class ControladorMPC:
|
|||
self._s_nodes = s_nodes
|
||||
self._mg_nodes = mg_nodes
|
||||
|
||||
def cross_track_error_point(self, x, y):
|
||||
def _projetar_na_trajetoria_local(self, x, y, idx_referencia=None):
|
||||
"""Projeta a pose apenas na vizinhanca do progresso corrente.
|
||||
|
||||
A busca global e ambigua em mapas agricolas: duas passadas paralelas
|
||||
podem estar a apenas 1,5 m. Ancorar a busca no proximo indice da rota
|
||||
impede o erro lateral de trocar silenciosamente para outra rua.
|
||||
"""
|
||||
if self._seg_p0 is None or self._seg_p0.shape[0] == 0:
|
||||
return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7
|
||||
|
||||
total_segmentos = int(self._seg_p0.shape[0])
|
||||
|
||||
if idx_referencia is None:
|
||||
i0 = 0
|
||||
i1 = total_segmentos - 1
|
||||
else:
|
||||
idx = max(0, min(_safe_int(idx_referencia, 0), len(self.pontos_info) - 1))
|
||||
s_base = float(self._s_nodes[idx])
|
||||
s_min = max(0.0, s_base - 1.25)
|
||||
s_max = min(
|
||||
float(self._s_nodes[-1]),
|
||||
s_base + float(self._janela_projecao_alvo_m),
|
||||
)
|
||||
|
||||
i0 = max(0, int(np.searchsorted(self._s_nodes, s_min, side="right")) - 1)
|
||||
i1 = min(
|
||||
total_segmentos - 1,
|
||||
int(np.searchsorted(self._s_nodes, s_max, side="left")),
|
||||
)
|
||||
|
||||
# Nunca exclui o segmento que termina no proximo ponto
|
||||
# autoritativo, mesmo diante de pequenos segmentos degenerados.
|
||||
i0 = min(i0, max(0, idx - 1))
|
||||
i1 = max(i1, min(total_segmentos - 1, idx))
|
||||
|
||||
P = np.array([x, y], dtype=float)
|
||||
p0 = self._seg_p0[i0:i1 + 1]
|
||||
vetores = self._seg_v[i0:i1 + 1]
|
||||
comprimentos = self._seg_L[i0:i1 + 1]
|
||||
l2 = np.maximum(comprimentos ** 2, 1e-12)
|
||||
|
||||
w = P - p0
|
||||
t = np.clip(np.sum(w * vetores, axis=1) / l2, 0.0, 1.0)
|
||||
projecoes = p0 + t[:, None] * vetores
|
||||
d2 = np.sum((projecoes - P) ** 2, axis=1)
|
||||
indice_relativo = int(np.argmin(d2))
|
||||
i = i0 + indice_relativo
|
||||
|
||||
proj_i = projecoes[indice_relativo]
|
||||
t_i = float(t[indice_relativo])
|
||||
e_lat = float(np.dot(P - proj_i, self._seg_n_hat[i]))
|
||||
s_proj = float(self._s_nodes[i] + t_i * self._seg_L[i])
|
||||
theta_path = float(
|
||||
np.arctan2(self._seg_t_hat[i, 0], self._seg_t_hat[i, 1])
|
||||
)
|
||||
|
||||
mg0 = self._mg_nodes[i]
|
||||
mg1 = self._mg_nodes[i + 1] if (i + 1) < self._mg_nodes.size else mg0
|
||||
margem = float((1.0 - t_i) * mg0 + t_i * mg1)
|
||||
|
||||
return e_lat, s_proj, i, proj_i, theta_path, margem
|
||||
|
||||
def cross_track_error_point(self, x, y, idx_referencia=None):
|
||||
try:
|
||||
"""
|
||||
Retorna:
|
||||
|
|
@ -367,33 +534,124 @@ class ControladorMPC:
|
|||
theta_path -> orientação do caminho (rad) naquele segmento
|
||||
margem_interp -> margem interpolada (m) naquele s
|
||||
"""
|
||||
if self._seg_p0 is None or self._seg_p0.shape[0] == 0:
|
||||
return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7
|
||||
|
||||
P = np.array([x, y], float)
|
||||
w = P - self._seg_p0 # (M,2)
|
||||
L2 = (self._seg_L ** 2) # (M,)
|
||||
t = np.clip(np.sum(w * self._seg_v, axis=1) / L2, 0.0, 1.0) # (M,)
|
||||
proj = self._seg_p0 + t[:, None] * self._seg_v # (M,2)
|
||||
d2 = np.sum((proj - P)**2, axis=1) # (M,)
|
||||
i = int(np.argmin(d2))
|
||||
|
||||
# projeção escolhida
|
||||
proj_i = proj[i]
|
||||
e_lat = float(np.dot(P - proj_i, self._seg_n_hat[i])) # sinal pela normal esquerda
|
||||
s_proj = float(self._s_nodes[i] + t[i] * self._seg_L[i])
|
||||
theta_path = float(np.arctan2(self._seg_t_hat[i,0], self._seg_t_hat[i,1])) # 0=Norte se (x=E,y=N)
|
||||
|
||||
# margem interpolada entre nós i e i+1
|
||||
mg0 = self._mg_nodes[i]
|
||||
mg1 = self._mg_nodes[i+1] if (i+1) < self._mg_nodes.size else mg0
|
||||
margem = float((1.0 - t[i]) * mg0 + t[i] * mg1)
|
||||
|
||||
return e_lat, s_proj, i, proj_i, theta_path, margem
|
||||
return self._projetar_na_trajetoria_local(
|
||||
x,
|
||||
y,
|
||||
idx_referencia=idx_referencia,
|
||||
)
|
||||
except Exception as e:
|
||||
_log(f"Erro ao calcular erro lateral: {e}")
|
||||
return 0.0, 0.0, 0, np.array([x, y], float), 0.0, 0.7
|
||||
|
||||
def _xy_na_abscissa(self, s_alvo):
|
||||
"""Interpola um ponto continuo na polilinha pela distancia acumulada."""
|
||||
if self._cl_xy is None or self._cl_xy.shape[0] == 0:
|
||||
return (0.0, 0.0), 0
|
||||
|
||||
if self._cl_xy.shape[0] == 1 or self._seg_p0 is None:
|
||||
return tuple(self._cl_xy[0]), 0
|
||||
|
||||
s = float(np.clip(s_alvo, 0.0, float(self._s_nodes[-1])))
|
||||
i = int(np.searchsorted(self._s_nodes, s, side="right") - 1)
|
||||
i = max(0, min(i, self._seg_p0.shape[0] - 1))
|
||||
|
||||
comprimento = float(self._seg_L[i])
|
||||
t = 0.0 if comprimento <= 1e-9 else (s - float(self._s_nodes[i])) / comprimento
|
||||
t = float(np.clip(t, 0.0, 1.0))
|
||||
xy = self._seg_p0[i] + t * self._seg_v[i]
|
||||
return (float(xy[0]), float(xy[1])), i
|
||||
|
||||
def _distancia_lookahead(self, status_carro, velocidade):
|
||||
v = max(0.0, _safe_float(velocidade, 0.0))
|
||||
|
||||
if _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]):
|
||||
desejado = self._lookahead_reta_base_m + self._lookahead_reta_velocidade_s * v
|
||||
return float(np.clip(
|
||||
desejado,
|
||||
self._lookahead_reta_min_m,
|
||||
self._lookahead_reta_max_m,
|
||||
))
|
||||
|
||||
desejado = self._lookahead_manobra_base_m + self._lookahead_manobra_velocidade_s * v
|
||||
return float(np.clip(
|
||||
desejado,
|
||||
self._lookahead_manobra_base_m,
|
||||
self._lookahead_manobra_max_m,
|
||||
))
|
||||
|
||||
def _construir_alvo_direcional(self, x, y, idx_base, status_carro, velocidade):
|
||||
"""Cria alvo pure-pursuit continuo sem alterar o progresso visitado."""
|
||||
n = len(self.pontos_info)
|
||||
if n <= 0:
|
||||
return {"xy": (float(x), float(y)), "_idx_base": 0}
|
||||
|
||||
idx = max(0, min(_safe_int(idx_base, 0), n - 1))
|
||||
_, s_proj, _, _, _, margem = self._projetar_na_trajetoria_local(
|
||||
x,
|
||||
y,
|
||||
idx_referencia=idx,
|
||||
)
|
||||
|
||||
lookahead = self._distancia_lookahead(status_carro, velocidade)
|
||||
s_alvo = min(float(self._s_nodes[-1]), float(s_proj) + lookahead)
|
||||
xy_alvo, i_seg = self._xy_na_abscissa(s_alvo)
|
||||
|
||||
idx_meta = min(n - 1, i_seg + 1)
|
||||
alvo = dict(self.pontos_info[idx_meta])
|
||||
alvo["xy"] = xy_alvo
|
||||
alvo["distanciaMargem"] = margem
|
||||
alvo["_idx_base"] = idx
|
||||
alvo["_s_projecao"] = float(s_proj)
|
||||
alvo["_s_alvo"] = float(s_alvo)
|
||||
alvo["_lookahead_m"] = float(max(0.0, s_alvo - s_proj))
|
||||
return alvo
|
||||
|
||||
def _estabilizar_angulo_saida(
|
||||
self,
|
||||
angulo_desejado_rad,
|
||||
status_carro,
|
||||
comando_anterior,
|
||||
dt_controle,
|
||||
):
|
||||
"""Filtra/rate-limita esterco somente na passada reta."""
|
||||
desejado = float(np.clip(
|
||||
angulo_desejado_rad,
|
||||
-np.radians(self.angulo_max_graus),
|
||||
np.radians(self.angulo_max_graus),
|
||||
))
|
||||
|
||||
if not _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]):
|
||||
return desejado
|
||||
|
||||
anterior = np.radians(_safe_float(
|
||||
_as_dict(comando_anterior).get("angulo", 0.0),
|
||||
0.0,
|
||||
min_value=-self.angulo_max_graus,
|
||||
max_value=self.angulo_max_graus,
|
||||
))
|
||||
dt = _safe_float(dt_controle, 0.5, min_value=0.05, max_value=1.5)
|
||||
|
||||
tau = float(self._filtro_angulo_reta_tau_s)
|
||||
alpha = 1.0 if tau <= 1e-6 else dt / (tau + dt)
|
||||
filtrado = anterior + alpha * (desejado - anterior)
|
||||
|
||||
max_delta = np.radians(self._taxa_angulo_reta_graus_s) * dt
|
||||
filtrado = anterior + float(np.clip(
|
||||
filtrado - anterior,
|
||||
-max_delta,
|
||||
max_delta,
|
||||
))
|
||||
|
||||
dead = np.radians(self._deadband_angulo_reta_graus)
|
||||
if abs(desejado) <= dead and abs(anterior) <= 2.0 * dead:
|
||||
filtrado = 0.0
|
||||
|
||||
return float(np.clip(
|
||||
filtrado,
|
||||
-np.radians(self.angulo_max_graus),
|
||||
np.radians(self.angulo_max_graus),
|
||||
))
|
||||
|
||||
def _get_costmap_direcional(self, contexto):
|
||||
"""
|
||||
Lê o contrato VisualWorker.CostMapDirecional e devolve uma estrutura interna
|
||||
|
|
@ -775,27 +1033,162 @@ class ControladorMPC:
|
|||
_log(f"Erro ao consultar proximo ponto nao visitado: {e}")
|
||||
return 0
|
||||
|
||||
def _corrigir_pontos_visitados(self, x, y, pontos_visitados, idx_atual=-1, limite_max_avanco=5.0, limite_max_pontos: int = 10, velocidade: float = -1, dt: float = -1, look_ahead: bool = True):
|
||||
if (idx_atual > -1):
|
||||
for idx, ponto in enumerate(self.pontos_info):
|
||||
if (idx < idx_atual):
|
||||
pontos_visitados[idx] = True
|
||||
else:
|
||||
break
|
||||
return idx_atual
|
||||
def _resolver_indice_local_autoritativo(self, idx_atual: int) -> int:
|
||||
"""Converte o índice publicado pelo C# para o índice local do mapa."""
|
||||
n = len(self.pontos_info)
|
||||
if n <= 0:
|
||||
return 0
|
||||
|
||||
for idx, ponto in enumerate(self.pontos_info):
|
||||
if pontos_visitados[idx]:
|
||||
continue
|
||||
pos = ponto["xy"]
|
||||
margem = ponto.get("distanciaMargem", 0.7)
|
||||
dist = np.linalg.norm([x - pos[0], y - pos[1]])
|
||||
if dist < margem:
|
||||
for i in range(idx + 1):
|
||||
pontos_visitados[i] = True
|
||||
return idx
|
||||
if idx > 0 and not pontos_visitados[idx - 1] and idx >= limite_max_avanco:
|
||||
idx_bruto = _safe_int(idx_atual, 0)
|
||||
|
||||
if idx_bruto in self._idx_global_para_local:
|
||||
return self._idx_global_para_local[idx_bruto]
|
||||
|
||||
# Compatibilidade com o contrato antigo, no qual o C# já publicava
|
||||
# o índice na mesma base do array entregue ao MPC.
|
||||
return max(0, min(idx_bruto, n))
|
||||
|
||||
def _sincronizar_visitados_com_csharp(self, pontos_visitados, idx_atual: int) -> int:
|
||||
"""Aplica exatamente o prefixo autoritativo publicado pelo C#.
|
||||
|
||||
O MPC pode avançar pontos em cópias usadas nas simulações, mas esse
|
||||
avanço é especulativo e nunca pode sobreviver no estado da execução
|
||||
real se o C# ainda não confirmou a mesma transição.
|
||||
"""
|
||||
n = len(self.pontos_info)
|
||||
if n <= 0:
|
||||
return 0
|
||||
|
||||
idx_local = self._resolver_indice_local_autoritativo(idx_atual)
|
||||
idx_local = max(0, min(idx_local, n))
|
||||
|
||||
havia_avanco_especulativo = bool(
|
||||
np.any(pontos_visitados[idx_local:])
|
||||
) if idx_local < n else False
|
||||
|
||||
pontos_visitados[:] = False
|
||||
if idx_local > 0:
|
||||
pontos_visitados[:idx_local] = True
|
||||
|
||||
if havia_avanco_especulativo and not self._ultima_divergencia_visitados:
|
||||
_log(
|
||||
f"[MPC] Progresso especulativo descartado; "
|
||||
f"C# autoritativo no idx_local={idx_local}."
|
||||
)
|
||||
|
||||
self._ultima_divergencia_visitados = havia_avanco_especulativo
|
||||
|
||||
# Quando o C# sinaliza conclusão (idx == n), o chamador já ordena a
|
||||
# parada. Retornamos n-1 apenas para manter acessos de debug seguros.
|
||||
return min(idx_local, n - 1)
|
||||
|
||||
@staticmethod
|
||||
def _progresso_segmento_xy(inicio, fim, robo):
|
||||
a = np.asarray(inicio, dtype=float)
|
||||
b = np.asarray(fim, dtype=float)
|
||||
p = np.asarray(robo, dtype=float)
|
||||
v = b - a
|
||||
w = p - a
|
||||
l2 = float(np.dot(v, v))
|
||||
|
||||
if not math.isfinite(l2) or l2 < 1e-8:
|
||||
return float("-inf"), float("inf"), 0.0
|
||||
|
||||
comprimento = math.sqrt(l2)
|
||||
produto = float(np.dot(w, v))
|
||||
avanco = produto / comprimento
|
||||
lateral = abs(float(v[0] * w[1] - v[1] * w[0])) / comprimento
|
||||
parametro = produto / l2
|
||||
return avanco, lateral, parametro
|
||||
|
||||
def _corrigir_pontos_visitados(
|
||||
self,
|
||||
x,
|
||||
y,
|
||||
pontos_visitados,
|
||||
idx_atual=-1,
|
||||
limite_max_avanco=5.0,
|
||||
limite_max_pontos: int = 10,
|
||||
velocidade: float = -1,
|
||||
dt: float = -1,
|
||||
look_ahead: bool = True,
|
||||
theta=None,
|
||||
):
|
||||
if idx_atual > -1:
|
||||
return self._sincronizar_visitados_com_csharp(
|
||||
pontos_visitados,
|
||||
idx_atual,
|
||||
)
|
||||
|
||||
n = len(self.pontos_info)
|
||||
if n <= 0:
|
||||
return 0
|
||||
|
||||
idx_primeiro = self._proximo_nao_visitado(pontos_visitados)
|
||||
idx_primeiro = max(0, min(idx_primeiro, n - 1))
|
||||
idx_limite = min(
|
||||
n - 2,
|
||||
idx_primeiro + max(1, int(limite_max_pontos)) - 1,
|
||||
)
|
||||
|
||||
# Sem heading confiável, a simulação mantém a regra antiga de alvo
|
||||
# sequencial e não tenta recuperar pontos pulados.
|
||||
theta_valido = theta is not None and math.isfinite(float(theta))
|
||||
|
||||
if not theta_valido or idx_primeiro > idx_limite:
|
||||
return idx_primeiro
|
||||
|
||||
robo = (float(x), float(y))
|
||||
theta_robo = float(theta)
|
||||
|
||||
for idx in range(idx_primeiro, idx_limite + 1):
|
||||
ponto = self.pontos_info[idx]
|
||||
proximo = self.pontos_info[idx + 1]
|
||||
inicio = ponto.get("xy", (0.0, 0.0))
|
||||
fim = proximo.get("xy", inicio)
|
||||
|
||||
avanco, lateral, parametro = self._progresso_segmento_xy(
|
||||
inicio,
|
||||
fim,
|
||||
robo,
|
||||
)
|
||||
|
||||
margem_ponto = max(
|
||||
0.30,
|
||||
_safe_float(ponto.get("distanciaMargem", 0.7), 0.7),
|
||||
)
|
||||
tolerancia_lateral = min(1.50, margem_ponto + 0.35)
|
||||
estrutural = bool(
|
||||
ponto.get(
|
||||
"estrutural",
|
||||
ponto.get("PontoEstrutural", False),
|
||||
)
|
||||
)
|
||||
avanco_minimo = 0.0 if estrutural else -0.10
|
||||
|
||||
vetor = np.asarray(fim, dtype=float) - np.asarray(inicio, dtype=float)
|
||||
theta_trecho = math.atan2(float(vetor[0]), float(vetor[1]))
|
||||
erro_heading = abs(
|
||||
(theta_trecho - theta_robo + math.pi) % (2.0 * math.pi)
|
||||
- math.pi
|
||||
)
|
||||
erro_maximo = math.radians(35.0 if estrutural else 45.0)
|
||||
|
||||
ponto_ficou_para_tras = (
|
||||
math.isfinite(avanco)
|
||||
and math.isfinite(lateral)
|
||||
and math.isfinite(parametro)
|
||||
and avanco >= avanco_minimo
|
||||
and parametro >= (0.0 if estrutural else -0.10)
|
||||
and lateral <= tolerancia_lateral
|
||||
and erro_heading <= erro_maximo
|
||||
)
|
||||
|
||||
if not ponto_ficou_para_tras:
|
||||
break
|
||||
|
||||
pontos_visitados[idx] = True
|
||||
|
||||
return self._proximo_nao_visitado(pontos_visitados)
|
||||
|
||||
def _calcular_pesos_movimento(self, contexto, erro_ori_graus, erro_lat_m):
|
||||
|
|
@ -859,14 +1252,17 @@ class ControladorMPC:
|
|||
TipoMovimentoDirecional.MovimentoDiagonal: 0.0
|
||||
}
|
||||
|
||||
erro_ori_abs = abs(float(erro_ori_graus))
|
||||
erro_lat_abs = abs(float(erro_lat_m))
|
||||
|
||||
if status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]:
|
||||
custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0
|
||||
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.0
|
||||
custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 2.0
|
||||
else:
|
||||
custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 if erro_ori_graus >= 10.0 else 1.0
|
||||
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.2 if erro_ori_graus >= 10.0 else 0.0
|
||||
custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.0 if erro_ori_graus <= 2.0 and erro_lat_m <= 0.4 else 2.0
|
||||
custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 if erro_ori_abs >= 10.0 else 1.0
|
||||
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.2 if erro_ori_abs >= 10.0 else 0.0
|
||||
custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.0 if erro_ori_abs <= 2.0 and erro_lat_abs <= 0.4 else 2.0
|
||||
|
||||
return (peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, custo_movimento)
|
||||
except Exception as e:
|
||||
|
|
@ -938,7 +1334,12 @@ class ControladorMPC:
|
|||
if time_left_ms() <= 0.0:
|
||||
return x, y, theta, visitados
|
||||
x, y, theta = aplicar_passo_dt(x, y, theta, u, dt_eff)
|
||||
idx_alvo = self._corrigir_pontos_visitados(x, y, visitados)
|
||||
idx_alvo = self._corrigir_pontos_visitados(
|
||||
x,
|
||||
y,
|
||||
visitados,
|
||||
theta=theta,
|
||||
)
|
||||
|
||||
t_rem -= seg
|
||||
|
||||
|
|
@ -1120,7 +1521,20 @@ class ControladorMPC:
|
|||
time_left_ms=lambda: (deadline - now()) * 1000.0,
|
||||
calc_omega_fn=self.gps_handler.calcular_omega
|
||||
)
|
||||
idx_alvo_correcao = self._corrigir_pontos_visitados(x, y, self.visitados_execucao, idx_proximo_ponto_real)
|
||||
idx_alvo_correcao = self._corrigir_pontos_visitados(
|
||||
x,
|
||||
y,
|
||||
self.visitados_execucao,
|
||||
idx_proximo_ponto_real,
|
||||
theta=theta,
|
||||
)
|
||||
ponto_alvo_real = self._construir_alvo_direcional(
|
||||
x,
|
||||
y,
|
||||
idx_alvo_correcao,
|
||||
status_carro,
|
||||
velocidade,
|
||||
)
|
||||
|
||||
# -------------------- Planejamento: horizonte ESPACIAL fixo --------------------
|
||||
S_ALVO = float(self.horizonte)
|
||||
|
|
@ -1222,11 +1636,32 @@ class ControladorMPC:
|
|||
}
|
||||
|
||||
#look_ahead = passo > 2 and status_carro not in [StatusCarroMapa.Manobrando.value, StatusCarroMapa.Direcionando.value]
|
||||
idx_alvo = self._corrigir_pontos_visitados(x_atual, y_atual, visitados, velocidade=v_sim, dt=dt_pred)
|
||||
#if passo == 0:
|
||||
# print(f"idx_alvo: {idx_alvo}")
|
||||
#if idx_alvo < len(self.pontos_info) - 1: idx_alvo += 1
|
||||
ponto_alvo = self.pontos_info[idx_alvo]
|
||||
if passo == 0:
|
||||
# O primeiro comando nasce obrigatoriamente do
|
||||
# próximo ponto confirmado pelo C#. O MPC não
|
||||
# pode usar a pose real para promover um ponto
|
||||
# especulativamente e continuar andando enquanto
|
||||
# a trajetória autoritativa permanece parada.
|
||||
idx_alvo = idx_alvo_correcao
|
||||
else:
|
||||
idx_alvo = self._corrigir_pontos_visitados(
|
||||
x_atual,
|
||||
y_atual,
|
||||
visitados,
|
||||
velocidade=v_sim,
|
||||
dt=dt_pred,
|
||||
theta=theta_atual,
|
||||
)
|
||||
# A autoridade de visitados continua em idx_alvo. O
|
||||
# ponto usado para dirigir e continuo e fica adiante
|
||||
# da projecao local do candidato sobre a rota.
|
||||
ponto_alvo = self._construir_alvo_direcional(
|
||||
x_atual,
|
||||
y_atual,
|
||||
idx_alvo,
|
||||
status_carro,
|
||||
v_sim,
|
||||
)
|
||||
|
||||
pares_candidatos, custos_candidatos = self._gerar_angulos_candidatos_receding(
|
||||
ponto_alvo,
|
||||
|
|
@ -1235,7 +1670,8 @@ class ControladorMPC:
|
|||
contexto,
|
||||
deadline,
|
||||
15.0,
|
||||
dados_costmap=dados_costmap_ciclo
|
||||
dados_costmap=dados_costmap_ciclo,
|
||||
tipo_preferido=cmd_anterior_local.get("tipo"),
|
||||
)
|
||||
|
||||
if ms_left() <= 0 or exp_budget <= 0:
|
||||
|
|
@ -1371,8 +1807,20 @@ class ControladorMPC:
|
|||
tipo_final = TipoMovimentoDirecional.RodasDianteiras
|
||||
simulacao_latlon = []
|
||||
|
||||
ponto_alvo_primario = self.pontos_info[idx_alvo_correcao]["xy"]
|
||||
erro_lateral, _, _, _, theta_path, _ = self.cross_track_error_point(x, y)
|
||||
if not parada_necessaria:
|
||||
angulo_final = self._estabilizar_angulo_saida(
|
||||
angulo_final,
|
||||
status_carro,
|
||||
comando_anterior,
|
||||
self.tempo_execucao_local,
|
||||
)
|
||||
|
||||
ponto_alvo_primario = ponto_alvo_real["xy"]
|
||||
erro_lateral, _, _, _, theta_path, _ = self.cross_track_error_point(
|
||||
x,
|
||||
y,
|
||||
idx_referencia=idx_alvo_correcao,
|
||||
)
|
||||
_, erro_orientacao = self._calcula_erro_orientacao(x, y, theta, ponto_alvo_primario, theta_path, 0.5)
|
||||
|
||||
_cmd = {
|
||||
|
|
@ -1385,6 +1833,9 @@ class ControladorMPC:
|
|||
"simulacao": simulacao_latlon,
|
||||
"erro_lateral": erro_lateral,
|
||||
"erro_orientacao": erro_orientacao,
|
||||
"idx_alvo_autoritativo": int(idx_alvo_correcao),
|
||||
"alvo_direcional_xy": ponto_alvo_primario,
|
||||
"lookahead_direcional_m": float(ponto_alvo_real.get("_lookahead_m", 0.0)),
|
||||
"debug_custo": debug_custo,
|
||||
"candidatos_testados": K,
|
||||
"motivos": []
|
||||
|
|
@ -1400,13 +1851,14 @@ class ControladorMPC:
|
|||
):
|
||||
deadline_dbg = time.perf_counter() + 0.03
|
||||
pares_dbg, _ = self._gerar_angulos_candidatos_receding(
|
||||
ponto_alvo=self.pontos_info[idx_alvo_correcao],
|
||||
ponto_alvo=ponto_alvo_real,
|
||||
ponto_atual=(x, y, theta),
|
||||
ponto_ref=(x, y, theta),
|
||||
contexto=contexto,
|
||||
deadline=deadline_dbg,
|
||||
margem_ms=8.0,
|
||||
dados_costmap=dados_costmap_dbg
|
||||
dados_costmap=dados_costmap_dbg,
|
||||
tipo_preferido=comando_anterior.get("tipo"),
|
||||
)
|
||||
|
||||
tipos_dbg = []
|
||||
|
|
@ -1460,7 +1912,17 @@ class ControladorMPC:
|
|||
"motivos": _safe_motivos(motivos),
|
||||
}
|
||||
|
||||
def _gerar_angulos_candidatos_receding(self, ponto_alvo, ponto_atual, ponto_ref, contexto, deadline, margem_ms, dados_costmap=None):
|
||||
def _gerar_angulos_candidatos_receding(
|
||||
self,
|
||||
ponto_alvo,
|
||||
ponto_atual,
|
||||
ponto_ref,
|
||||
contexto,
|
||||
deadline,
|
||||
margem_ms,
|
||||
dados_costmap=None,
|
||||
tipo_preferido=None,
|
||||
):
|
||||
"""
|
||||
Gera candidatos de (tipo, ângulo) respeitando o deadline do ciclo:
|
||||
1) Ângulo ideal
|
||||
|
|
@ -1506,7 +1968,11 @@ class ControladorMPC:
|
|||
angulo_ideal_deg = float(np.degrees(angulo_ideal_rad))
|
||||
#print(f"Orient: {np.degrees(orient)}, delta theta: {np.degrees(delta_theta)}, angulo ideal: {angulo_ideal_deg}")
|
||||
|
||||
e_lat, _, _, _, theta_path, _ = self.cross_track_error_point(ponto_atual[0], ponto_atual[1])
|
||||
e_lat, _, _, _, theta_path, _ = self.cross_track_error_point(
|
||||
ponto_atual[0],
|
||||
ponto_atual[1],
|
||||
idx_referencia=ponto_alvo.get("_idx_base"),
|
||||
)
|
||||
(_, e_ori) = self._calcula_erro_orientacao(ponto_atual[0], ponto_atual[1], ponto_atual[2], ponto_alvo["xy"], theta_path, 0.5)
|
||||
|
||||
# 2) Tipos válidos conforme contexto
|
||||
|
|
@ -1516,10 +1982,23 @@ class ControladorMPC:
|
|||
tipos_validos = [TipoMovimentoDirecional.MovimentoArco]
|
||||
elif abs(e_ori) > 15.0:
|
||||
tipos_validos = [TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.MovimentoArco]
|
||||
elif abs(e_ori) <= 3.0 and abs(e_lat) <= 0.2:
|
||||
tipos_validos = [TipoMovimentoDirecional.MovimentoDiagonal]
|
||||
else:
|
||||
tipos_validos = [TipoMovimentoDirecional.RodasDianteiras]
|
||||
# Histerese de modo na passada. Antes, 0,20 m/3 graus era
|
||||
# uma fronteira seca: qualquer ruido alternava Diagonal e
|
||||
# RodasDianteiras. Entrar exige uma faixa estreita; depois de
|
||||
# entrar, a faixa de permanencia e um pouco maior.
|
||||
tipo_preferido_enum = _movimento_from_value(tipo_preferido)
|
||||
manter_diagonal = (
|
||||
tipo_preferido_enum == TipoMovimentoDirecional.MovimentoDiagonal
|
||||
and abs(e_ori) <= 4.0
|
||||
and abs(e_lat) <= 0.30
|
||||
)
|
||||
entrar_diagonal = abs(e_ori) <= 1.5 and abs(e_lat) <= 0.12
|
||||
|
||||
if manter_diagonal or entrar_diagonal:
|
||||
tipos_validos = [TipoMovimentoDirecional.MovimentoDiagonal]
|
||||
else:
|
||||
tipos_validos = [TipoMovimentoDirecional.RodasDianteiras]
|
||||
|
||||
# 3) Flags de matriz
|
||||
if dados_costmap is None:
|
||||
|
|
@ -2379,9 +2858,18 @@ class ControladorMPC:
|
|||
# alvo mais próximo após avançar (mantido)
|
||||
idx_alvo_sim = self._corrigir_pontos_visitados(
|
||||
x_sim, y_sim, pontos_visitados,
|
||||
velocidade=v_planejado, dt=sub_dt
|
||||
velocidade=v_planejado,
|
||||
dt=sub_dt,
|
||||
theta=theta_sim,
|
||||
)
|
||||
ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"]
|
||||
alvo_direcional_sim = self._construir_alvo_direcional(
|
||||
x_sim,
|
||||
y_sim,
|
||||
idx_alvo_sim,
|
||||
contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value),
|
||||
v_planejado,
|
||||
)
|
||||
ponto_alvo_sim = alvo_direcional_sim["xy"]
|
||||
|
||||
# --------------------------
|
||||
# ERROS / CUSTOS
|
||||
|
|
@ -2391,7 +2879,11 @@ class ControladorMPC:
|
|||
|
||||
# --- PATCH A: usa theta do CAMINHO (segmento projetado) como referência
|
||||
# e usa também margem local pra normalização do custo lateral
|
||||
e_lat, _, _, _, theta_path, margem = self.cross_track_error_point(x_sim, y_sim)
|
||||
e_lat, _, _, _, theta_path, margem = self.cross_track_error_point(
|
||||
x_sim,
|
||||
y_sim,
|
||||
idx_referencia=idx_alvo_sim,
|
||||
)
|
||||
|
||||
# erro de orientação
|
||||
(o_norm, erro_ori_sim) = self._calcula_erro_orientacao(x_sim, y_sim, theta_sim, ponto_alvo_sim, theta_path, 0.5)
|
||||
|
|
@ -2403,7 +2895,9 @@ class ControladorMPC:
|
|||
# ângulo relativo ao alvo e suavidade (parcialmente mantido)
|
||||
vetor_alvo = np.array(ponto_alvo_sim) - np.array([x_sim, y_sim])
|
||||
vetor_alvo_norm = vetor_alvo / (np.linalg.norm(vetor_alvo) + 1e-9)
|
||||
vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
|
||||
# Mesmo referencial de _nova_posicao: x=Leste usa sin(theta)
|
||||
# e y=Norte usa cos(theta).
|
||||
vetor_movel = np.array([np.sin(theta_sim), np.cos(theta_sim)])
|
||||
cos_delta = float(np.dot(vetor_movel, vetor_alvo_norm))
|
||||
delta_angulo = abs(angulo_testado - prev_ang)
|
||||
|
||||
|
|
|
|||
|
|
@ -1511,22 +1511,56 @@ class ContextoGlobalRedis:
|
|||
|
||||
handler = GPSHandler(float(p_ref[0]), float(p_ref[1]))
|
||||
|
||||
for p in points_iter(pontos):
|
||||
indices_globais = set()
|
||||
identidade_ambigua = False
|
||||
|
||||
for indice_local, p in enumerate(points_iter(pontos)):
|
||||
try:
|
||||
lat = cls._float(p.get("lat", 0.0), 0.0)
|
||||
lon = cls._float(p.get("lon", 0.0), 0.0)
|
||||
|
||||
idx_ponto = cls._int(
|
||||
p.get("idxPonto", p.get("IdxPonto", indice_local)),
|
||||
indice_local,
|
||||
)
|
||||
idx_corredor = cls._int(
|
||||
p.get("idxCorredor", p.get("IdxCorredor", -1)),
|
||||
-1,
|
||||
)
|
||||
|
||||
if idx_ponto in indices_globais:
|
||||
identidade_ambigua = True
|
||||
raise ValueError(
|
||||
f"idxPonto global duplicado: {idx_ponto}"
|
||||
)
|
||||
|
||||
x, y = handler.converter_latlon_para_xz(lat, lon)
|
||||
|
||||
pontos_info.append({
|
||||
"xy": (x, y),
|
||||
"tipo": cls._int(p.get("tipo", 3), 3),
|
||||
"distanciaMargem": cls._float(p.get("distanciaMargem", 0.7), 0.7),
|
||||
"idxPonto": idx_ponto,
|
||||
"idxCorredor": idx_corredor,
|
||||
"estrutural": cls._bool(
|
||||
p.get(
|
||||
"estrutural",
|
||||
p.get("PontoEstrutural", False),
|
||||
)
|
||||
),
|
||||
})
|
||||
|
||||
indices_globais.add(idx_ponto)
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"Erro ao converter ponto MPC: {p} | {e}")
|
||||
|
||||
if identidade_ambigua:
|
||||
mostrar_log(
|
||||
"Mapa MPC rejeitado: identidade global dos pontos é ambígua."
|
||||
)
|
||||
return []
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"Erro ao atualizar pontos do mapa: {e}")
|
||||
cls.atualizar_ctx_dict(CtxKey.DadosOperacao, pontos_mapa=pontos_info)
|
||||
|
|
|
|||
|
|
@ -3213,7 +3213,7 @@ int I2CService::_pinoTcaEn = -1;
|
|||
int I2CService::_pinoMuxRst = -1;
|
||||
int I2CService::_pinoAdsPwrEn = -1;
|
||||
|
||||
unsigned long I2CService::LimiteTempoI2C = 2000;
|
||||
unsigned long I2CService::LimiteTempoI2C = 4000;
|
||||
|
||||
bool I2CService::I2CIniciado = false;
|
||||
bool I2CService::MuxIniciado = false;
|
||||
|
|
|
|||
|
|
@ -514,7 +514,7 @@ class SensorTemperaturaNtc : public ComponenteCAN {
|
|||
* 4 NTC * 4 conversões * 2 Hz = 32 conversões/s.
|
||||
*/
|
||||
static constexpr int
|
||||
QuantidadeAmostras = 4;
|
||||
QuantidadeAmostras = 3;
|
||||
|
||||
/*
|
||||
* Filtro final leve.
|
||||
|
|
|
|||
Loading…
Reference in New Issue