melhoria do watchdog da trajetoria, usar direcional para buscar centro ao entrar no corredor, PP assume o controle antes da curva começar, recuperação de curva de manobra mais robusto
This commit is contained in:
Diego Freitas
parent
1decf784c6
commit
8af660c9b5
File diff suppressed because it is too large
Load Diff
|
|
@ -2,6 +2,7 @@ using AgroBase.Models.Modules;
|
|||
using AgroBase.Services;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Drawing.Imaging;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
|
|
@ -35,6 +36,168 @@ namespace AgroBase.Models
|
|||
public static bool DebugMode { get; set; } = false;
|
||||
public static bool Fechando { get; set; } = false;
|
||||
|
||||
// ============================================================================
|
||||
// LOG GLOBAL DE SESSÃO DO AGROBASE
|
||||
// ============================================================================
|
||||
// Toda chamada a Variaveis.MostrarLog(...) continua indo ao console e é
|
||||
// enfileirada para persistência em um arquivo único por execução do processo.
|
||||
// O thread que chamou MostrarLog NUNCA faz escrita normal de disco.
|
||||
// Assim, logging não entra no caminho crítico do controle.
|
||||
private static readonly object _logSessaoLock = new object();
|
||||
private static readonly ConcurrentQueue<string> _logSessaoFila = new ConcurrentQueue<string>();
|
||||
private static readonly AutoResetEvent _logSessaoSignal = new AutoResetEvent(false);
|
||||
private static StreamWriter _logSessaoWriter;
|
||||
private static Thread _logSessaoThread;
|
||||
private static string _caminhoLogSessao;
|
||||
private static bool _logSessaoFalhou;
|
||||
private static bool _logSessaoHookRegistrado;
|
||||
private static volatile bool _logSessaoEncerrando;
|
||||
|
||||
public static string CaminhoLogSessao
|
||||
{
|
||||
get { return _caminhoLogSessao ?? string.Empty; }
|
||||
}
|
||||
|
||||
private static void GarantirLogSessao()
|
||||
{
|
||||
if (_logSessaoThread != null || _logSessaoFalhou)
|
||||
return;
|
||||
|
||||
lock (_logSessaoLock)
|
||||
{
|
||||
if (_logSessaoThread != null || _logSessaoFalhou)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
string pasta = Path.Combine(CaminhoLogsDispositivos, "Sessoes");
|
||||
Directory.CreateDirectory(pasta);
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
int pid = Process.GetCurrentProcess().Id;
|
||||
string nome = $"AgroBase_{agora:yyyyMMdd_HHmmss_fff}_PID{pid}.log";
|
||||
|
||||
_caminhoLogSessao = Path.Combine(pasta, nome);
|
||||
_logSessaoWriter = new StreamWriter(_caminhoLogSessao, true);
|
||||
_logSessaoWriter.AutoFlush = false;
|
||||
_logSessaoEncerrando = false;
|
||||
|
||||
_logSessaoWriter.WriteLine("============================================================");
|
||||
_logSessaoWriter.WriteLine("AGROBASE SESSION LOG");
|
||||
_logSessaoWriter.WriteLine($"Inicio: {agora:yyyy-MM-dd HH:mm:ss.fff}");
|
||||
_logSessaoWriter.WriteLine($"PID: {pid}");
|
||||
_logSessaoWriter.WriteLine($"Aplicacao: {NomeAplicacao}");
|
||||
_logSessaoWriter.WriteLine($"Versao: {Versao}");
|
||||
_logSessaoWriter.WriteLine($"Machine: {Environment.MachineName}");
|
||||
_logSessaoWriter.WriteLine("============================================================");
|
||||
_logSessaoWriter.Flush();
|
||||
|
||||
_logSessaoThread = new Thread(ProcessarFilaLogSessao);
|
||||
_logSessaoThread.IsBackground = true;
|
||||
_logSessaoThread.Name = "AgroBase.SessionLogger";
|
||||
_logSessaoThread.Start();
|
||||
|
||||
if (!_logSessaoHookRegistrado)
|
||||
{
|
||||
_logSessaoHookRegistrado = true;
|
||||
AppDomain.CurrentDomain.ProcessExit += (sender, args) => EncerrarLogSessao();
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logSessaoFalhou = true;
|
||||
try
|
||||
{
|
||||
_logSessaoWriter?.Dispose();
|
||||
_logSessaoWriter = null;
|
||||
Debug.WriteLine("[AGROBASE/LOGGER] Falha ao iniciar log de sessão: " + ex.Message);
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void ProcessarFilaLogSessao()
|
||||
{
|
||||
try
|
||||
{
|
||||
while (!_logSessaoEncerrando || !_logSessaoFila.IsEmpty)
|
||||
{
|
||||
bool escreveu = false;
|
||||
string linha;
|
||||
|
||||
while (_logSessaoFila.TryDequeue(out linha))
|
||||
{
|
||||
_logSessaoWriter?.WriteLine(linha);
|
||||
escreveu = true;
|
||||
}
|
||||
|
||||
if (escreveu)
|
||||
_logSessaoWriter?.Flush();
|
||||
|
||||
if (!_logSessaoEncerrando)
|
||||
_logSessaoSignal.WaitOne(500);
|
||||
}
|
||||
|
||||
_logSessaoWriter?.Flush();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logSessaoFalhou = true;
|
||||
try
|
||||
{
|
||||
Debug.WriteLine("[AGROBASE/LOGGER] Falha no writer de sessão: " + ex.Message);
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
}
|
||||
|
||||
public static void EncerrarLogSessao()
|
||||
{
|
||||
Thread thread;
|
||||
|
||||
lock (_logSessaoLock)
|
||||
{
|
||||
if (_logSessaoThread == null && _logSessaoWriter == null)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
_logSessaoFila.Enqueue(
|
||||
$"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff} " +
|
||||
$"[T{Thread.CurrentThread.ManagedThreadId}] [AGROBASE] [SESSION] Encerrando log."
|
||||
);
|
||||
}
|
||||
catch { }
|
||||
|
||||
_logSessaoEncerrando = true;
|
||||
_logSessaoSignal.Set();
|
||||
thread = _logSessaoThread;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (thread != null && thread != Thread.CurrentThread)
|
||||
thread.Join(1500);
|
||||
}
|
||||
catch { }
|
||||
|
||||
lock (_logSessaoLock)
|
||||
{
|
||||
try
|
||||
{
|
||||
_logSessaoWriter?.Flush();
|
||||
_logSessaoWriter?.Dispose();
|
||||
}
|
||||
catch { }
|
||||
finally
|
||||
{
|
||||
_logSessaoWriter = null;
|
||||
_logSessaoThread = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static string CaminhoSistema { get; set; } = AppDomain.CurrentDomain.BaseDirectory;
|
||||
public static string NomeAplicacao
|
||||
{
|
||||
|
|
@ -784,7 +947,37 @@ namespace AgroBase.Models
|
|||
|
||||
public static void MostrarLog(string Mensagem)
|
||||
{
|
||||
Console.WriteLine($"[AGROBASE] {Mensagem}");
|
||||
string mensagem = Mensagem ?? string.Empty;
|
||||
string linhaConsole = $"[AGROBASE] {mensagem}";
|
||||
|
||||
// Mantém o comportamento histórico do console.
|
||||
Console.WriteLine(linhaConsole);
|
||||
|
||||
// A thread chamadora apenas enfileira. Escrita/flush ficam no logger.
|
||||
try
|
||||
{
|
||||
GarantirLogSessao();
|
||||
|
||||
if (_logSessaoFalhou || _logSessaoThread == null)
|
||||
return;
|
||||
|
||||
string linhaArquivo =
|
||||
$"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff} " +
|
||||
$"[T{Thread.CurrentThread.ManagedThreadId}] " +
|
||||
linhaConsole;
|
||||
|
||||
_logSessaoFila.Enqueue(linhaArquivo);
|
||||
_logSessaoSignal.Set();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// Logging nunca pode impactar controle/movimento.
|
||||
try
|
||||
{
|
||||
Debug.WriteLine("[AGROBASE/LOGGER] Falha ao enfileirar log: " + ex.Message);
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -825,8 +1018,9 @@ namespace AgroBase.Models
|
|||
public static double LarguraDireitaCm { get; } = 53.0; // 22
|
||||
public static double ComprimentoFrenteCm { get; } = 90.0; // 7
|
||||
public static double ComprimentoTrasCm { get; } = 40.0; // 107
|
||||
public static double DistanciaEntreEixosCm { get; set; } = 92.0;
|
||||
public static double BitolaRodasCm { get; set; } = 75.0;
|
||||
public static double DistanciaEntreEixosCm { get; } = 92.0;
|
||||
public static double BitolaRodasCm { get; } = 75.0;
|
||||
public static double KuDirecional { get; } = 2.65;
|
||||
public static double LeverArmFrontalCm { get; set; } = 37.0; // cm
|
||||
public static double LeverArmLateralCm { get; set; } = 0.0; // cm
|
||||
public static double LarguraEquipamentoMm
|
||||
|
|
@ -1179,7 +1373,7 @@ namespace AgroBase.Models
|
|||
// ------------------------------------------------------------
|
||||
// Deve ser o MESMO valor usado em:
|
||||
// mpc_v16 -> cinematica_4ws_ku
|
||||
double Ku = 2.5;
|
||||
double Ku = KuDirecional;
|
||||
|
||||
double kappaEf =
|
||||
kappaGeom /
|
||||
|
|
|
|||
|
|
@ -252,6 +252,7 @@ namespace AgroBase.Services.Operadores
|
|||
("serial_number", VariaveisEquipamento.Parametros.serial_number),
|
||||
("pct_vel_min", VariaveisEquipamento.PercentualVelMin),
|
||||
("largura", VariaveisEquipamento.LarguraEquipamentoMm / 1000.0),
|
||||
("ku_direcional", VariaveisEquipamento.KuDirecional),
|
||||
("distancia_entre_eixos", VariaveisEquipamento.DistanciaEntreEixosCm / 100.0),
|
||||
("qtd_bicos", VariaveisEquipamento.QuantidadeBicosPulverizadores),
|
||||
("percentual_reservatorio_min", VariaveisEquipamento.PercentualReservatorioMin),
|
||||
|
|
@ -389,6 +390,7 @@ namespace AgroBase.Services.Operadores
|
|||
tempo_entre_comandos = (op.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Enums.T_Code.Dir)?.DelayEnvioComando ?? 500) / 1000.0,
|
||||
horizonte = pControle.MpcHorizonte,
|
||||
passos_atraso = 1,
|
||||
ku_direcional = VariaveisEquipamento.KuDirecional,
|
||||
}
|
||||
}
|
||||
),
|
||||
|
|
|
|||
|
|
@ -518,6 +518,7 @@ 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),
|
||||
|
|
|
|||
|
|
@ -450,6 +450,21 @@ class ControladorMPC:
|
|||
1.0, min_value=0.25, max_value=2.0,
|
||||
)
|
||||
|
||||
# V18 - handoff antecipado para o Pure Pursuit.
|
||||
#
|
||||
# SaindoRua ja nasce alguns metros antes da BordaSaida para reduzir a
|
||||
# velocidade. Nessa fase o MPC pesado nao deve mais antecipar a curva:
|
||||
# o mesmo Pure Pursuit que executa a Dubins assume cedo, segue reto
|
||||
# enquanto a centerline ainda e reta e comeca a esterçar naturalmente
|
||||
# quando o look-ahead passa a enxergar a ferradura.
|
||||
#
|
||||
# A busca por mudanca de corredor evita ativar esse handoff no fim do
|
||||
# ultimo corredor, onde nao existe uma manobra Dubins seguinte.
|
||||
self._dubins_handoff_busca_frente_m = _safe_float(
|
||||
parametros_mpc.get("dubins_handoff_busca_frente_m", 8.0),
|
||||
8.0, min_value=2.0, max_value=20.0,
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# PATH TRACKING V2 - referência geométrica da passada
|
||||
# ------------------------------------------------------------------
|
||||
|
|
@ -527,6 +542,39 @@ class ControladorMPC:
|
|||
3.0, min_value=0.5, max_value=8.0,
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# V19 - ENTRY CAPTURE pós-Dubins
|
||||
# ------------------------------------------------------------------
|
||||
# O campo mostrou que, ao terminar a ferradura, o rover pode entrar na
|
||||
# nova passada com 20-70 cm de cross-track e ainda 5-12 graus de yaw.
|
||||
# A política antiga priorizava yaw e só depois liberava o crab, fazendo
|
||||
# o rover consumir alguns metros dentro da rua antes de centralizar.
|
||||
#
|
||||
# Agora o fim do Pure Pursuit arma uma janela curta "centro primeiro":
|
||||
# - se o erro lateral ainda é relevante e |heading| <= 15 graus,
|
||||
# MovimentoDiagonal recebe prioridade mesmo sem heading perfeito;
|
||||
# - heading muito grande continua com Arco/Dianteira normalmente;
|
||||
# - assim que |e_lat| <= 10 cm, a política normal volta a mandar;
|
||||
# - existe limite de distância para o latch nunca permanecer indefinido.
|
||||
self._entry_capture_enabled = bool(
|
||||
parametros_mpc.get("entry_capture_enabled", True)
|
||||
)
|
||||
self._entry_capture_heading_max_graus = _safe_float(
|
||||
parametros_mpc.get("entry_capture_heading_max_graus", 15.0),
|
||||
15.0, min_value=3.0, max_value=25.0,
|
||||
)
|
||||
self._entry_capture_lateral_release_m = _safe_float(
|
||||
parametros_mpc.get("entry_capture_lateral_release_m", 0.10),
|
||||
0.10, min_value=0.03, max_value=0.30,
|
||||
)
|
||||
self._entry_capture_max_dist_m = _safe_float(
|
||||
parametros_mpc.get("entry_capture_max_dist_m", 4.0),
|
||||
4.0, min_value=1.0, max_value=10.0,
|
||||
)
|
||||
self._entry_capture_active = False
|
||||
self._entry_capture_dist_m = 0.0
|
||||
self._dubins_pp_ativo_ciclo_anterior = False
|
||||
|
||||
self._lateral_deadband_m = _safe_float(
|
||||
parametros_mpc.get("lateral_deadband_m", 0.05),
|
||||
0.05, min_value=0.0, max_value=0.30,
|
||||
|
|
@ -699,9 +747,14 @@ class ControladorMPC:
|
|||
# é o oposto do comando de alto nível (efeito empilhadeira).
|
||||
#
|
||||
# O fator Ku preserva a mesma correção dinâmica usada pelo simulador C#.
|
||||
#equipamento = _as_dict(_as_dict(contexto).get("Equipamento", {}))
|
||||
#self._cinematica_4ws_ku = _safe_float(
|
||||
# equipamento.get("ku_direcional", 2.65),
|
||||
# 2.65, min_value=0.0, max_value=20.0,
|
||||
#)
|
||||
self._cinematica_4ws_ku = _safe_float(
|
||||
parametros_mpc.get("cinematica_4ws_ku", 2.5),
|
||||
2.5, min_value=0.0, max_value=20.0,
|
||||
parametros_mpc.get("ku_direcional", 2.65),
|
||||
2.65, min_value=0.0, max_value=20.0,
|
||||
)
|
||||
self._cinematica_entre_eixos_m = _safe_float(
|
||||
parametros_mpc.get("cinematica_entre_eixos_m", 0.94),
|
||||
|
|
@ -1366,17 +1419,53 @@ class ControladorMPC:
|
|||
|
||||
return traj
|
||||
|
||||
def _usar_seguidor_dubins(self, status_carro, idx_base):
|
||||
"""V12: cabeceira estrutural usa Pure Pursuit dedicado em vez da arvore MPC.
|
||||
def _ha_transicao_corredor_adiante(self, idx_base):
|
||||
"""Confirma que existe uma troca de corredor logo adiante na centerline.
|
||||
|
||||
O C# ja coloca Status=Manobrando quando o ponto atual/proximo pertence
|
||||
a geometria de manobra. Para o primeiro A/B de campo/simulador usamos
|
||||
esse contrato diretamente, evitando depender do valor numerico privado
|
||||
de TipoPontoRua no Python.
|
||||
Usado somente para o handoff antecipado SaindoRua -> Pure Pursuit.
|
||||
A verificacao e geometrica/estrutural, nao altera progresso visitado.
|
||||
"""
|
||||
if not self.pontos_info or self._s_nodes is None or len(self._s_nodes) == 0:
|
||||
return False
|
||||
|
||||
n = len(self.pontos_info)
|
||||
idx = max(0, min(_safe_int(idx_base, 0), n - 1))
|
||||
corredor_base = self._idx_corredor(idx)
|
||||
if corredor_base < 0:
|
||||
return False
|
||||
|
||||
s_base = float(self._s_nodes[idx])
|
||||
limite_m = max(2.0, float(self._dubins_handoff_busca_frente_m))
|
||||
|
||||
for j in range(idx + 1, n):
|
||||
if float(self._s_nodes[j]) - s_base > limite_m:
|
||||
break
|
||||
|
||||
corredor_j = self._idx_corredor(j)
|
||||
if corredor_j >= 0 and corredor_j != corredor_base:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def _usar_seguidor_dubins(self, status_carro, idx_base):
|
||||
"""V18: Pure Pursuit assume a cabeceira antes da BordaSaida.
|
||||
|
||||
- Manobrando: contrato ja validado em campo.
|
||||
- SaindoRua: assume antecipadamente SOMENTE se ha uma troca real de
|
||||
corredor logo adiante na centerline. Assim o MPC pesado deixa de
|
||||
"prometer" uma curva antes do handoff e o PP chega contextualizado
|
||||
desde o trecho reto de aproximacao.
|
||||
"""
|
||||
if not bool(self._seguidor_dubins_enabled):
|
||||
return False
|
||||
return _status_in(status_carro, [StatusCarroMapa.Manobrando])
|
||||
|
||||
if _status_in(status_carro, [StatusCarroMapa.Manobrando]):
|
||||
return True
|
||||
|
||||
if _status_in(status_carro, [StatusCarroMapa.SaindoRua]):
|
||||
return self._ha_transicao_corredor_adiante(idx_base)
|
||||
|
||||
return False
|
||||
|
||||
def _comando_seguidor_dubins(
|
||||
self, contexto, comando_anterior, x, y, theta, velocidade, status_carro,
|
||||
|
|
@ -1425,7 +1514,11 @@ class ControladorMPC:
|
|||
"erro_heading_caminho_graus": abs(float(math.degrees(dbg["erro_heading"]))),
|
||||
"debug_custo": {},
|
||||
"debug_seguidor_dubins": {
|
||||
"modo": "pure_pursuit_4ws",
|
||||
"modo": (
|
||||
"pure_pursuit_4ws_antecipado"
|
||||
if _status_in(status_carro, [StatusCarroMapa.SaindoRua])
|
||||
else "pure_pursuit_4ws"
|
||||
),
|
||||
"lookahead_m": float(dbg["lookahead_m"]),
|
||||
"distancia_alvo_m": float(dbg["distancia_alvo_m"]),
|
||||
"alpha_graus": float(math.degrees(dbg["alpha"])),
|
||||
|
|
@ -2489,6 +2582,31 @@ class ControladorMPC:
|
|||
e_head_deg = abs(math.degrees(erro_heading_rad))
|
||||
tipo_prev = _movimento_from_value(tipo_preferido)
|
||||
|
||||
# V19 - captura lateral imediatamente após a ferradura.
|
||||
#
|
||||
# Este override é propositalmente ANTES da política normal de
|
||||
# CaminhandoRua/EntrandoRua. O estado é armado apenas pela saída real do
|
||||
# seguidor Dubins, portanto não altera a centralização nominal de uma rua
|
||||
# comum. Enquanto ativo, o crab pode começar com heading moderadamente
|
||||
# errado, preservando a prioridade de manter o rover entre as linhas.
|
||||
if (
|
||||
bool(getattr(self, "_entry_capture_active", False))
|
||||
and _status_in(
|
||||
status_carro,
|
||||
[
|
||||
StatusCarroMapa.EntrandoRua,
|
||||
StatusCarroMapa.CaminhandoRua,
|
||||
StatusCarroMapa.Direcionando,
|
||||
],
|
||||
)
|
||||
and abs(e_lat) > float(self._entry_capture_lateral_release_m)
|
||||
and e_head_deg <= float(self._entry_capture_heading_max_graus)
|
||||
):
|
||||
return (
|
||||
TipoMovimentoDirecional.MovimentoDiagonal,
|
||||
self._angulo_diagonal_tracking(e_lat, v),
|
||||
)
|
||||
|
||||
if _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]):
|
||||
return self._referencia_supervisor_corredor(
|
||||
e_lat,
|
||||
|
|
@ -3774,11 +3892,21 @@ class ControladorMPC:
|
|||
velocidade,
|
||||
)
|
||||
|
||||
# V12 - cabeceira Dubins: a rota ja esta pronta. Em Manobrando nao
|
||||
# abrimos a arvore MPC; usamos Pure Pursuit 4WS mirando um ponto
|
||||
# fisico adiante na propria curva. O heading/tangente fica apenas
|
||||
# como telemetria e nao alivia o comando antes da hora.
|
||||
if self._usar_seguidor_dubins(status_carro, idx_alvo_correcao):
|
||||
# V18 - handoff antecipado da cabeceira. SaindoRua ja opera lento e,
|
||||
# quando existe troca de corredor logo adiante, entrega o volante ao
|
||||
# mesmo Pure Pursuit 4WS usado em Manobrando. Enquanto a centerline
|
||||
# ainda e reta o comando permanece praticamente reto; quando o alvo
|
||||
# entra na Dubins a curva nasce sem a previsao concorrente do MPC.
|
||||
usando_pp_dubins = self._usar_seguidor_dubins(
|
||||
status_carro, idx_alvo_correcao
|
||||
)
|
||||
if usando_pp_dubins:
|
||||
# V19: enquanto PP possui o volante apenas memorizamos que a
|
||||
# próxima saída dele deve armar o Entry Capture.
|
||||
self._dubins_pp_ativo_ciclo_anterior = True
|
||||
self._entry_capture_active = False
|
||||
self._entry_capture_dist_m = 0.0
|
||||
|
||||
return self._comando_seguidor_dubins(
|
||||
contexto=contexto,
|
||||
comando_anterior=comando_anterior,
|
||||
|
|
@ -3791,6 +3919,77 @@ class ControladorMPC:
|
|||
ponto_alvo_real=ponto_alvo_real,
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------
|
||||
# V19 - arma/atualiza ENTRY CAPTURE somente no ciclo REAL.
|
||||
# ------------------------------------------------------------
|
||||
status_entry_ok = _status_in(
|
||||
status_carro,
|
||||
[
|
||||
StatusCarroMapa.EntrandoRua,
|
||||
StatusCarroMapa.CaminhandoRua,
|
||||
StatusCarroMapa.Direcionando,
|
||||
],
|
||||
)
|
||||
|
||||
if bool(getattr(self, "_dubins_pp_ativo_ciclo_anterior", False)):
|
||||
self._dubins_pp_ativo_ciclo_anterior = False
|
||||
if bool(self._entry_capture_enabled) and status_entry_ok:
|
||||
self._entry_capture_active = True
|
||||
self._entry_capture_dist_m = 0.0
|
||||
_log(
|
||||
"[MPC/V19 ENTRY] Capture armado após Dubins | "
|
||||
f"status={_enum_name(status_carro, StatusCarroMapa)}"
|
||||
)
|
||||
|
||||
if bool(getattr(self, "_entry_capture_active", False)):
|
||||
if not status_entry_ok:
|
||||
self._entry_capture_active = False
|
||||
self._entry_capture_dist_m = 0.0
|
||||
_log(
|
||||
"[MPC/V19 ENTRY] Capture encerrado por mudança de estado | "
|
||||
f"status={_enum_name(status_carro, StatusCarroMapa)}"
|
||||
)
|
||||
else:
|
||||
dt_entry = _safe_float(
|
||||
self.tempo_execucao_local,
|
||||
0.5,
|
||||
min_value=0.05,
|
||||
max_value=1.5,
|
||||
)
|
||||
self._entry_capture_dist_m += (
|
||||
abs(float(velocidade)) * dt_entry
|
||||
)
|
||||
|
||||
try:
|
||||
ref_entry = self._referencia_caminho_lookahead(
|
||||
x, y, idx_alvo_correcao, velocidade, status_carro
|
||||
)
|
||||
e_lat_entry = abs(float(ref_entry["e_lat"]))
|
||||
theta_entry = float(ref_entry["theta_ref"])
|
||||
e_head_entry = abs(math.degrees(
|
||||
self._erro_heading_caminho(theta, theta_entry)
|
||||
))
|
||||
except Exception:
|
||||
e_lat_entry = float("inf")
|
||||
e_head_entry = float("inf")
|
||||
|
||||
if e_lat_entry <= float(self._entry_capture_lateral_release_m):
|
||||
self._entry_capture_active = False
|
||||
_log(
|
||||
"[MPC/V19 ENTRY] Capture concluído | "
|
||||
f"e_lat={e_lat_entry:.2f}m | "
|
||||
f"e_head={e_head_entry:.1f}deg | "
|
||||
f"dist={self._entry_capture_dist_m:.2f}m"
|
||||
)
|
||||
elif self._entry_capture_dist_m >= float(self._entry_capture_max_dist_m):
|
||||
self._entry_capture_active = False
|
||||
_log(
|
||||
"[MPC/V19 ENTRY] Capture expirado por distância | "
|
||||
f"e_lat={e_lat_entry:.2f}m | "
|
||||
f"e_head={e_head_entry:.1f}deg | "
|
||||
f"dist={self._entry_capture_dist_m:.2f}m"
|
||||
)
|
||||
|
||||
# -------------------- Planejamento: horizonte ESPACIAL fixo --------------------
|
||||
S_ALVO = float(self.horizonte)
|
||||
V_FLOOR = float(getattr(self, "_vmin_planejamento", 0.30))
|
||||
|
|
|
|||
Loading…
Reference in New Issue