Ajustes horizonte MPC, status carro segmentacao
This commit is contained in:
Diego Freitas
parent
4425e03168
commit
c8861877fa
|
|
@ -142,11 +142,14 @@ namespace AgroBase.Forms
|
|||
return;
|
||||
}
|
||||
|
||||
var h = GPSUtils.DistanciaDoTrecho(simulacao.Select(x => new GPSModel() { Latitude = x.latitude, Longitude = x.longitude, OrientacaoReal = x.orientacao }).ToList());
|
||||
|
||||
lblStatus.Text = $"Carro: {_Sensoriamento.Trajetoria.StatusCarro} ({_Sensoriamento.OperadorVisual.Analises.segmentacao.status_corredor})";
|
||||
lblDirecao.Text = $"Direção: {_Sensoriamento.Trajetoria.DirecaoCaminho}";
|
||||
lblDistanciaOperacao.Text = $"Distância Operação: {_Sensoriamento.Trajetoria.DistanciaPercorrida:F2} m de {_Sensoriamento.Trajetoria.DistanciaTotal:F2} m";
|
||||
lblDistanciaFinal.Text = $"Distância Restante: {_Sensoriamento.Trajetoria.DistanciaRestante:F2} m";
|
||||
lblDistanciaLateral.Text = $"Esq: {_Sensoriamento.Trajetoria.DistanciaEsquerda:F2} m - Dir: {_Sensoriamento.Trajetoria.DistanciaDireita:F2} m {Variaveis.OperacaoEmAndamento.Controle.ErroLateral:F2} m";
|
||||
//lblDistanciaLateral.Text = $"Esq: {_Sensoriamento.Trajetoria.DistanciaEsquerda:F2} m - Dir: {_Sensoriamento.Trajetoria.DistanciaDireita:F2} m {Variaveis.OperacaoEmAndamento.Controle.ErroLateral:F2} m";
|
||||
lblDistanciaLateral.Text = $"h: {h:F2} m, p: {_Sensoriamento.Controle.SimulacaoMPC.Count}, s: {_Sensoriamento.Controle.DebugCustoMpc.Count}";
|
||||
lblRua.Text = $"Corredor: " + (_Sensoriamento.Trajetoria.CorredorAtual.Dentro ? "Dentro" : "Fora");
|
||||
lblMargem.Text = $"Margem: " + (_Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não");
|
||||
lblProximoPonto.Text = $"Próximo Ponto: {_Sensoriamento.Trajetoria.ProximoPonto.idxPonto}";
|
||||
|
|
@ -260,6 +263,7 @@ namespace AgroBase.Forms
|
|||
{
|
||||
GPSModel novaPosicao = null;
|
||||
velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle);
|
||||
Console.WriteLine($"RPM SP: {Variaveis.OperacaoEmAndamento.SimulacaoRpmControle} ({velocidadeCarroMs:F2} ms)");
|
||||
|
||||
Variaveis.OperacaoEmAndamento.DispMvd.Dados.VelocidadeMedia = velocidadeCarroMs;
|
||||
|
||||
|
|
|
|||
|
|
@ -199,7 +199,8 @@
|
|||
SaindoRua = 3,
|
||||
Manobrando = 4,
|
||||
Direcionando = 5,
|
||||
RetornandoBase = 6
|
||||
RetornandoBase = 6,
|
||||
Indefinido = 7
|
||||
}
|
||||
|
||||
public enum DirecaoCarroRua
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ namespace AgroBase.Models.Operacoes
|
|||
public bool ImuParadaPorInclinacao { get; set; }
|
||||
public bool FrenagemAutomaticaAoParar { get; set; }
|
||||
|
||||
public bool RegistrarDadosPosProcessamento { get; set; } = false;
|
||||
public bool RegistrarDadosPosProcessamento { get; set; }
|
||||
|
||||
public int MovVelocidadeSErvasPercent { get; set; }
|
||||
public int MovRpmMax
|
||||
|
|
@ -67,9 +67,8 @@ namespace AgroBase.Models.Operacoes
|
|||
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 bool MpcMatrizCusto { get; set; }
|
||||
public double MpcHorizonte { get; set; } = 4.0;
|
||||
}
|
||||
|
||||
public class OperacaoParametrosDadosModel
|
||||
|
|
|
|||
|
|
@ -548,6 +548,7 @@ namespace AgroBase.Models.Operadores
|
|||
public double erro_lateral_pct { get; set; }
|
||||
public StatusCarroMapa status_corredor { get; set; }
|
||||
public StatusCarroMapa status_corredor_anterior { get; set; }
|
||||
public VisualWorkerMessageSegmentacaoSemanticaDebugCorredorModel status_corredor_debug { get; set; }
|
||||
public List<double[]> centros_corredor { get; set; }
|
||||
|
||||
public VisualWorkerMessageSegmentacaoSemanticaModel Clone()
|
||||
|
|
@ -561,11 +562,20 @@ namespace AgroBase.Models.Operadores
|
|||
erro_lateral_pct = erro_lateral_pct,
|
||||
status_corredor = status_corredor,
|
||||
status_corredor_anterior = status_corredor_anterior,
|
||||
status_corredor_debug = status_corredor_debug,
|
||||
centros_corredor = new List<double[]>(centros_corredor ?? new List<double[]>())
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
public class VisualWorkerMessageSegmentacaoSemanticaDebugCorredorModel
|
||||
{
|
||||
public double nav_global { get; set; }
|
||||
public double nav_near { get; set; }
|
||||
public double nav_mid { get; set; }
|
||||
public double nav_far { get; set; }
|
||||
}
|
||||
|
||||
public class VisualWorkerMessageMatrizConfiancaModel
|
||||
{
|
||||
public float ts { get; set; }
|
||||
|
|
|
|||
|
|
@ -184,6 +184,7 @@ namespace AgroBase.Services.Operadores
|
|||
("camera_solo_id", Variaveis.OperacaoEmAndamento.DispSen?.Dados?.CamerasSolo?.FirstOrDefault()?.Id ?? ""),
|
||||
("path_ia_model_ruas_seg", VersionamentoService.ArquivoModeloSegStreetDetector.CaminhoCompleto),
|
||||
("path_ia_labelmap_ruas_seg", VersionamentoService.ArquivoLabelmapSegStreetDetector.CaminhoCompleto),
|
||||
("path_ia_norm_stats_ruas_seg", VersionamentoService.ArquivoModeloNormstatsWeedDetector.CaminhoCompleto),
|
||||
("ia_backbone_ruas_seg", "nvidia/segformer-b0-finetuned-ade-512-512"),
|
||||
("path_ia_model_ruas_det", VersionamentoService.ArquivoModeloDetStreetDetector.CaminhoCompleto),
|
||||
("path_ia_model_ervas", VersionamentoService.ArquivoModeloWeedDetector.CaminhoCompleto),
|
||||
|
|
@ -247,14 +248,13 @@ namespace AgroBase.Services.Operadores
|
|||
distanciaMargem = p.LarguraCorredor * 0.8
|
||||
})
|
||||
.ToList(),
|
||||
horizonte_min = pControle.MpcHorizonteMin,
|
||||
horizonte_max = pControle.MpcHorizonteMax,
|
||||
angulo_max_graus = pControle.DirAnguloMaximo,
|
||||
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,
|
||||
horizonte = pControle.MpcHorizonte,
|
||||
usa_matriz_custo = pControle.MpcMatrizCusto,
|
||||
passos_atraso = 1,
|
||||
usa_matriz_custo = pControle.MpcMatrizCusto
|
||||
}
|
||||
}
|
||||
),
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ namespace AgroBase.Services
|
|||
}
|
||||
}
|
||||
}
|
||||
public static VersaoArquivoModel ArquivoModeloDetStreetDetector
|
||||
public static VersaoArquivoModel ArquivoModeloNormstatsStreetDetector
|
||||
{
|
||||
get
|
||||
{
|
||||
|
|
@ -52,6 +52,16 @@ namespace AgroBase.Services
|
|||
}
|
||||
}
|
||||
}
|
||||
public static VersaoArquivoModel ArquivoModeloDetStreetDetector
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_ArquivoLock)
|
||||
{
|
||||
return _ArquivosVersionados.Where(x => x.TipoArquivo == TipoArquivoVersionado.ModeloIA_StreetDetector).Skip(3).FirstOrDefault();
|
||||
}
|
||||
}
|
||||
}
|
||||
public static VersaoArquivoModel ArquivoModeloWeedDetector
|
||||
{
|
||||
get
|
||||
|
|
|
|||
|
|
@ -1,3 +1,5 @@
|
|||
import json
|
||||
import os
|
||||
import time
|
||||
from PIL import Image
|
||||
import cv2
|
||||
|
|
@ -6,8 +8,8 @@ import torch
|
|||
from transformers import SegformerForSemanticSegmentation
|
||||
|
||||
class SegformerNavRunner:
|
||||
IMAGENET_MEAN = torch.tensor([0.485, 0.456, 0.406]).view(3, 1, 1)
|
||||
IMAGENET_STD = torch.tensor([0.229, 0.224, 0.225]).view(3, 1, 1)
|
||||
IMAGENET_MEAN = [0.485, 0.456, 0.406]
|
||||
IMAGENET_STD = [0.229, 0.224, 0.225]
|
||||
|
||||
def __init__(self, seg_config, device="cuda"):
|
||||
from shared.utils import carregar_labelmap_completo
|
||||
|
|
@ -20,6 +22,50 @@ class SegformerNavRunner:
|
|||
self.roi_tamanho = seg_config["ia_roi_size"]
|
||||
self.last_infer = None
|
||||
|
||||
# ==========================
|
||||
# Normalização fixa (igual treino)
|
||||
# ==========================
|
||||
norm_mean = self.IMAGENET_MEAN
|
||||
norm_std = self.IMAGENET_STD
|
||||
|
||||
norm_stats_path = seg_config.get("ia_norm_stats_path")
|
||||
if norm_stats_path and os.path.isfile(norm_stats_path):
|
||||
with open(norm_stats_path, "r", encoding="utf-8") as f:
|
||||
norm_stats = json.load(f)
|
||||
|
||||
stats_channels = norm_stats.get("channels", [])
|
||||
stats_mean = norm_stats.get("mean", [])
|
||||
stats_std = norm_stats.get("std", [])
|
||||
|
||||
print(f"[NORM] usando stats fixos de: {norm_stats_path}")
|
||||
print(f"[NORM] channels={stats_channels}")
|
||||
print(f"[NORM] mean={stats_mean}")
|
||||
print(f"[NORM] std ={stats_std}")
|
||||
|
||||
idx_by_name = {name: i for i, name in enumerate(stats_channels)}
|
||||
|
||||
m_R = stats_mean[idx_by_name["R"]]
|
||||
m_G = stats_mean[idx_by_name["G"]]
|
||||
m_B = stats_mean[idx_by_name["B"]]
|
||||
|
||||
s_R = stats_std[idx_by_name["R"]]
|
||||
s_G = stats_std[idx_by_name["G"]]
|
||||
s_B = stats_std[idx_by_name["B"]]
|
||||
|
||||
norm_mean = [m_R, m_G, m_B]
|
||||
norm_std = [s_R, s_G, s_B]
|
||||
else:
|
||||
print(f"[NORM] norm_stats.json não encontrado em {norm_stats_path}. "
|
||||
f"Usando normalização dinâmica por frame.")
|
||||
|
||||
self.set_norm_stats(norm_mean, norm_std)
|
||||
|
||||
def set_norm_stats(self, mean, std):
|
||||
mean = torch.tensor(mean, dtype=torch.float32).view(3, 1, 1)
|
||||
std = torch.tensor(std, dtype=torch.float32).view(3, 1, 1).clamp_min(1e-6)
|
||||
self._norm_mean = mean
|
||||
self._norm_std = std
|
||||
|
||||
def _extract_state_dict(self, ckpt):
|
||||
"""
|
||||
Aceita:
|
||||
|
|
@ -86,7 +132,7 @@ class SegformerNavRunner:
|
|||
return model
|
||||
|
||||
def normalize_img(self, img: torch.Tensor) -> torch.Tensor:
|
||||
return (img - self.IMAGENET_MEAN.to(img.device)) / self.IMAGENET_STD.to(img.device)
|
||||
return (img - self._norm_mean.to(img.device)) / self._norm_std.to(img.device)
|
||||
|
||||
def compute_roi_indices(self, H: int, zona_inicio: float, faixa_atuacao: float):
|
||||
y_inicio = int((1.0 - zona_inicio) * H)
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
|
|||
ang_max = _operacao.get("Dir", {}).get("angulo_max", 30)
|
||||
|
||||
status_ipb = StatusModulo(ContextoGlobalRedis.get_modulo(T_Code.Ipb).get("saude", {}).get("status", StatusModulo.DESCONECTADO.value))
|
||||
debug_mode = _operacao.get("debug_mode", False)
|
||||
|
||||
status_carro = StatusCarroMapa.Parado
|
||||
reduzir_para_pulverizar = False
|
||||
|
|
@ -47,7 +48,7 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
|
|||
|
||||
velocidade_sp = vel_com_ervas
|
||||
|
||||
if status_ipb != StatusModulo.OPERANTE:
|
||||
if status_ipb != StatusModulo.OPERANTE and not debug_mode:
|
||||
velocidade_sp = vel_min
|
||||
elif status_carro == StatusCarroMapa.Parado:
|
||||
velocidade_sp = 0
|
||||
|
|
|
|||
|
|
@ -40,11 +40,11 @@ class ControladorMPC:
|
|||
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.horizonte = parametros_mpc.get("horizonte")
|
||||
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
|
||||
self._n_subporpasso = 1
|
||||
|
||||
# otimizacoes
|
||||
self._np_bool = np.bool_
|
||||
|
|
@ -670,11 +670,10 @@ class ControladorMPC:
|
|||
status_carro = contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value)
|
||||
idx_proximo_ponto_real = contexto.get("Carro", {}).get("IdxProximoPonto", 0)
|
||||
idx_ponto_alvo = contexto.get("Carro", {}).get("IdxPontoAlvo", 0)
|
||||
Nmax = 20 if status_carro == StatusCarroMapa.CaminhandoRua.value else 60
|
||||
Nmax = 30 if status_carro == StatusCarroMapa.CaminhandoRua.value else 60
|
||||
|
||||
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
|
||||
#self.passos_horizonte_local = max(1, math.floor(1 / self.tempo_execucao_local))
|
||||
|
||||
# -------------------- Correção por latência (vida real) --------------------
|
||||
# Mantém correção com 'self.dt' e 'velocidade' REAIS
|
||||
|
|
@ -702,7 +701,7 @@ class ControladorMPC:
|
|||
idx_alvo_correcao = self._corrigir_pontos_visitados(x, y, self.visitados_execucao, idx_proximo_ponto_real)
|
||||
|
||||
# -------------------- Planejamento: horizonte ESPACIAL fixo --------------------
|
||||
S_ALVO = self.horizonte_max
|
||||
S_ALVO = float(self.horizonte)
|
||||
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))
|
||||
|
|
@ -711,25 +710,32 @@ class ControladorMPC:
|
|||
|
||||
v_sim = max(velocidade, V_FLOOR)
|
||||
|
||||
# dt de controle real: tempo minimo entre comandos executados no robo
|
||||
dt_ctrl = float(self.tempo_execucao_local)
|
||||
dt_ctrl = max(DT_MIN, dt_ctrl)
|
||||
|
||||
# orçamento de avaliações (continua igual, para pacing)
|
||||
est_ms = max(1.0, float(getattr(self, "_t_est_passo_ms", 6.0)))
|
||||
exp_budget = max(4, int(ms_left() // est_ms))
|
||||
|
||||
# >>> NOVO: limites teóricos de K impostos por DT_MAX/DT_MIN
|
||||
K_lb = max(1, int(math.ceil(S_ALVO / max(1e-6, v_sim * DT_MAX)))) # mínimo p/ cobrir S
|
||||
K_ub = max(K_lb, int(math.floor(S_ALVO / max(1e-6, v_sim * DT_MIN)))) # cima (granularidade)
|
||||
# Horizonte em numero de comandos K, coerente com S_ALVO, v_sim e dt_ctrol
|
||||
K_float = S_ALVO / max(1e-6, v_sim * dt_ctrl)
|
||||
K_nom = int(max(1, math.ceil(K_float)))
|
||||
K = min(Nmax, K_nom)
|
||||
|
||||
# >>> NOVO: NÃO deixe Nmax te travar abaixo do necessário
|
||||
# se quiser ainda respeitar Nmax, aumente-o dinamicamente:
|
||||
Nmax = max(Nmax, K_lb)
|
||||
|
||||
# teto “de intenção” (não crítico, só para não explodir absurdamente)
|
||||
K_cap = min(Nmax, max(K_ALVO, K_lb))
|
||||
## >>> NOVO: limites teóricos de K impostos por DT_MAX/DT_MIN
|
||||
#K_lb = max(1, int(math.ceil(S_ALVO / max(1e-6, v_sim * DT_MAX)))) # mínimo p/ cobrir S
|
||||
#K_ub = max(K_lb, int(math.floor(S_ALVO / max(1e-6, v_sim * DT_MIN)))) # cima (granularidade)
|
||||
## >>> NOVO: NÃO deixe Nmax te travar abaixo do necessário
|
||||
## se quiser ainda respeitar Nmax, aumente-o dinamicamente:
|
||||
#Nmax = max(Nmax, K_lb)
|
||||
## teto “de intenção” (não crítico, só para não explodir absurdamente)
|
||||
#K_cap = min(Nmax, max(K_ALVO, K_lb))
|
||||
## >>> escolha K dentro do [K_lb, K_cap]; pacing do orçamento vem depois
|
||||
#K = max(K_lb, min(K_cap, K_ALVO))
|
||||
|
||||
# >>> escolha K dentro do [K_lb, K_cap]; pacing do orçamento vem depois
|
||||
K = max(K_lb, min(K_cap, K_ALVO))
|
||||
|
||||
beam_traj_min = int(getattr(self, "_beam_traj_min", 3))
|
||||
beam_traj_min = int(self._beam_traj_min)
|
||||
# custo bruto para levar beam_traj_min trajetórias até K passos
|
||||
exp_min_full = beam_traj_min * K
|
||||
if exp_budget < exp_min_full:
|
||||
|
|
@ -742,8 +748,9 @@ class ControladorMPC:
|
|||
return comando_anterior
|
||||
|
||||
# dt para que K passos cubram S (agora garantido dentro de [DT_MIN, DT_MAX])
|
||||
dt_pred = (S_ALVO / (v_sim * K))
|
||||
dt_pred = max(DT_MIN, min(dt_pred, DT_MAX))
|
||||
#dt_pred = (S_ALVO / (v_sim * K))
|
||||
#dt_pred = max(DT_MIN, min(dt_pred, DT_MAX))
|
||||
dt_pred = dt_ctrl
|
||||
|
||||
self.qtd_comandos_sucessivos = K
|
||||
|
||||
|
|
@ -1815,11 +1822,13 @@ class ControladorMPC:
|
|||
# omega e sub-stepping
|
||||
omega_const = self.gps_handler.calcular_omega(v_planejado, angulo_testado, tipo)
|
||||
|
||||
passos_local = max(1, int(getattr(self, "passos_horizonte_local", 1)))
|
||||
dt_total = float(dt_pred) * passos_local
|
||||
|
||||
SUB_MAX = float(getattr(self, "_dt_sub_max", 0.15)) # ~150 ms por subpasso (ajuste teu comentário)
|
||||
n_subs = max(1, int(math.ceil(dt_total / SUB_MAX)))
|
||||
#passos_local = max(1, int(getattr(self, "passos_horizonte_local", 1)))
|
||||
#dt_total = float(dt_pred) * passos_local
|
||||
#SUB_MAX = float(getattr(self, "_dt_sub_max", 0.15)) # ~150 ms por subpasso (ajuste teu comentário)
|
||||
#n_subs = max(1, int(math.ceil(dt_total / SUB_MAX)))
|
||||
dt_total = dt_pred
|
||||
n_subs = max(1, self._n_subporpasso)
|
||||
|
||||
sub_dt = dt_total / n_subs
|
||||
dist_sub = v_planejado * sub_dt # custo por metro
|
||||
|
||||
|
|
|
|||
|
|
@ -57,6 +57,7 @@ class StatusCarroMapa(IntEnum):
|
|||
Manobrando = 4
|
||||
Direcionando = 5
|
||||
RetornandoBase = 6
|
||||
Indefinido = 7
|
||||
|
||||
class T_Code(IntEnum):
|
||||
Vzo = -1
|
||||
|
|
|
|||
|
|
@ -81,6 +81,7 @@ def load_seg_config(force_reload=False):
|
|||
}
|
||||
_CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ruas_seg")
|
||||
_CONFIG_CACHE["ia_labelmap_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_labelmap_ruas_seg")
|
||||
_CONFIG_CACHE["ia_norm_stats_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_norm_stats_ruas_seg")
|
||||
_CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ruas_seg")
|
||||
|
||||
return _CONFIG_CACHE
|
||||
|
|
|
|||
|
|
@ -191,7 +191,7 @@ class SegmentacaoManager:
|
|||
lat_out = round(float(self._ema_lat), 3)
|
||||
|
||||
mask_nav = (self.predictions == ClassesSegmentacao.NAVEGAVEL.value)
|
||||
status_now, status_final, status_before, prob, debug = self.classificar_status_corredor(mask_nav)
|
||||
status_now, status_final, status_before, debug = self.classificar_status_corredor(mask_nav)
|
||||
|
||||
return {
|
||||
"timestamp": time.time(),
|
||||
|
|
@ -201,6 +201,7 @@ class SegmentacaoManager:
|
|||
"erro_lateral_pct": lat_out,
|
||||
"status_corredor": status_final.value,
|
||||
"status_corredor_anterior": status_before.value,
|
||||
"status_corredor_debug": debug,
|
||||
"centros_corredor": centros,
|
||||
"larguras_px": larguras,
|
||||
"confianca": round(float(score), 3),
|
||||
|
|
@ -355,103 +356,300 @@ class SegmentacaoManager:
|
|||
def classificar_status_corredor(self, mask_nav: np.ndarray):
|
||||
"""
|
||||
mask_nav: (H,W) com 1 = navegável, 0 = não-navegável
|
||||
|
||||
Retorna:
|
||||
status_now : StatusCarroMapa
|
||||
status_final : StatusCarroMapa (igual ao now, sem histerese por enquanto)
|
||||
probs : dict[StatusCarroMapa, float] (aqui só 1.0 pro escolhido)
|
||||
debug : métricas pra log
|
||||
status_now : StatusCarroMapa (instantâneo deste frame)
|
||||
status_final : StatusCarroMapa (com histerese)
|
||||
probs : dict[StatusCarroMapa, float] (one-hot)
|
||||
debug : métricas pra log/diagnóstico
|
||||
"""
|
||||
if mask_nav is None or mask_nav.size == 0:
|
||||
status_now = StatusCarroMapa.Indefinido
|
||||
self._status_hist.append((status_now, self._now()))
|
||||
status_final = self._maioria_ultimos()
|
||||
self._status_final_hist.append(status_final)
|
||||
status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final
|
||||
return status_now, status_final, status_before, {
|
||||
"nav_global": 0.0,
|
||||
"nav_near": 0.0,
|
||||
"nav_mid": 0.0,
|
||||
"nav_far": 0.0,
|
||||
}
|
||||
|
||||
H, W = mask_nav.shape
|
||||
nav = (mask_nav > 0).astype(np.float32)
|
||||
|
||||
def faixa_mean(y0, y1):
|
||||
def faixa_mean(y0: int, y1: int) -> float:
|
||||
fatia = nav[y0:y1, :]
|
||||
if fatia.size == 0:
|
||||
return 0.0
|
||||
return float(fatia.mean())
|
||||
|
||||
# corta em 3 faixas: far (topo), mid (meio), near (embaixo)
|
||||
# 3 faixas verticais: far (topo), mid (meio), near (embaixo)
|
||||
y_far_top = 0
|
||||
y_far_bot = int(0.2 * H)
|
||||
y_far_bot = int(0.25 * H)
|
||||
y_mid_top = y_far_bot
|
||||
y_mid_bot = int(0.4 * H)
|
||||
y_mid_bot = int(0.5 * H)
|
||||
y_near_top = y_mid_bot
|
||||
y_near_bot = H
|
||||
|
||||
nav_far = faixa_mean(y_far_top, y_far_bot)
|
||||
nav_mid = faixa_mean(y_mid_top, y_mid_bot)
|
||||
nav_near = faixa_mean(y_near_top, y_near_bot)
|
||||
nav_global = float(nav.mean()) if nav.size > 0 else 0.0
|
||||
nav_far = faixa_mean(y_far_top, y_far_bot)
|
||||
nav_mid = faixa_mean(y_mid_top, y_mid_bot)
|
||||
nav_near = faixa_mean(y_near_top, y_near_bot)
|
||||
nav_global = float(nav.mean())
|
||||
|
||||
# limiares & delta
|
||||
THR_NAV_ALTO = 0.95 # "quase tudo navegável"
|
||||
THR_NAV_BAIXO = 0.30 # "quase nada navegável"
|
||||
THR_NEAR_ALTO = 1.00 # near "100%"
|
||||
DELTA = 0.05 # diferença mínima pra considerar > de verdade
|
||||
# diferenças entre faixas (pra medir quão "desbalanceado" está)
|
||||
d_nm = abs(nav_near - nav_mid)
|
||||
d_mf = abs(nav_mid - nav_far)
|
||||
d_nf = abs(nav_near - nav_far)
|
||||
max_delta = max(d_nm, d_mf, d_nf)
|
||||
|
||||
def maior_que(a, b):
|
||||
return a > min(b - DELTA, 1.0)
|
||||
|
||||
def maior_igual_que(a, b):
|
||||
return a >= min(b - DELTA, 1.0)
|
||||
# ---- Blobs 2D na região distante (parede esquerda x direita) ----
|
||||
y_blob_top = 0
|
||||
y_blob_bot = int(0.35 * H) # um pouco mais profundo que o nav_far
|
||||
|
||||
status = None
|
||||
faixa_obs = (mask_nav[y_blob_top:y_blob_bot, :] == 0).astype(np.uint8) # 1 = obstáculo
|
||||
|
||||
# 1) PARADO: quase todo frame não navegável
|
||||
if nav_global < THR_NAV_BAIXO:
|
||||
H_blob, W_blob = faixa_obs.shape
|
||||
num_blobs_far = 0
|
||||
corridor_nav_far = 0.0
|
||||
corridor_width_frac = 0.0
|
||||
|
||||
if H_blob > 0 and W_blob > 0 and faixa_obs.max() > 0:
|
||||
# connectedComponents espera 0/255
|
||||
faixa_obs_bin = (faixa_obs * 255).astype(np.uint8)
|
||||
|
||||
num_labels, labels = cv2.connectedComponents(faixa_obs_bin)
|
||||
|
||||
# ignora blobs muito pequenos (ruído)
|
||||
MIN_AREA = 0.005 * H_blob * W_blob # 0.5% da área da faixa
|
||||
blobs = []
|
||||
|
||||
for label in range(1, num_labels): # 0 é o fundo
|
||||
ys, xs = np.where(labels == label)
|
||||
area = len(xs)
|
||||
if area < MIN_AREA:
|
||||
continue
|
||||
|
||||
x_min, x_max = xs.min(), xs.max()
|
||||
y_min, y_max = ys.min(), ys.max()
|
||||
blobs.append({
|
||||
"area": area,
|
||||
"bbox": (x_min, y_min, x_max, y_max),
|
||||
"x_center": float(xs.mean()),
|
||||
})
|
||||
|
||||
num_blobs_far = len(blobs)
|
||||
|
||||
if num_blobs_far >= 2:
|
||||
# ordena da parede mais à esquerda pra mais à direita
|
||||
blobs_sorted = sorted(blobs, key=lambda b: b["x_center"])
|
||||
left_blob = blobs_sorted[0]
|
||||
right_blob = blobs_sorted[-1]
|
||||
|
||||
# corredor é a região entre x_max da esquerda e x_min da direita
|
||||
x_left = left_blob["bbox"][2] + 1 # x_max (inclusivo) -> +1 pra slice
|
||||
x_right = right_blob["bbox"][0] # x_min
|
||||
|
||||
if x_right > x_left:
|
||||
corridor_width = x_right - x_left
|
||||
corridor_width_frac = corridor_width / float(W)
|
||||
|
||||
faixa_nav_corridor = (mask_nav[y_blob_top:y_blob_bot, x_left:x_right] > 0).astype(np.float32)
|
||||
if faixa_nav_corridor.size > 0:
|
||||
corridor_nav_far = float(faixa_nav_corridor.mean())
|
||||
|
||||
# ===== Limiares (podemos tunar depois) =====
|
||||
THR_BLOCKED = 0.30 # global bem baixo -> quase sem caminho
|
||||
THR_OPEN = 0.90 # global bem alto -> mundo aberto
|
||||
THR_DELTA_COR = 0.15 # diferença "significativa" entre faixas
|
||||
THR_COR_FAR_LARGO = 0.65 # far ainda relativamente alto em corredor largo
|
||||
THR_OPEN_MUITO_LIMPO = 0.90 # quase tudo navegável
|
||||
THR_DELTA_OBST_PEQ = 0.20 # desbalance vertical máximo para considerar "campo aberto com obstáculo pequeno"
|
||||
MIN_CORRIDOR_WIDTH_FRAC = 0.10 # corredor tem que ter pelo menos ~10% da largura
|
||||
MAX_CORRIDOR_WIDTH_FRAC = 0.70 # evita chamar de corredor quando é um "campo" gigante
|
||||
|
||||
# 1) Parado: quase não há caminho à frente (meio+frente mortos)
|
||||
cond_parado = (
|
||||
nav_global <= THR_BLOCKED
|
||||
and nav_mid < 0.20
|
||||
and nav_far < 0.10
|
||||
)
|
||||
|
||||
# 2) Direcionando: mundo aberto, sem corredor marcado
|
||||
# 2.1 base: tudo alto e muito homogêneo
|
||||
cond_direcionando_base = (
|
||||
nav_global >= THR_OPEN
|
||||
and max_delta < 0.10
|
||||
)
|
||||
|
||||
# 2.2 modo "campo aberto com obstáculo pequeno":
|
||||
# quase tudo navegável, e até o far é bem alto
|
||||
cond_direcionando_obst_peq = (
|
||||
nav_global >= THR_OPEN_MUITO_LIMPO and # >= 0.90
|
||||
nav_near >= 0.95 and
|
||||
nav_mid >= 0.80 and # um pouco mais permissivo
|
||||
nav_far >= 0.50 and # aceita far um pouco mais fechado
|
||||
num_blobs_far <= 1 # no máximo UMA parede grande
|
||||
)
|
||||
|
||||
# 2.3 modo "campo aberto com parede na frente":
|
||||
# chão bem navegável perto, sem corredor definido, e FAR quase todo bloqueado
|
||||
cond_direcionando_frente_fe_chada = (
|
||||
nav_near >= 0.80 and # perto bem aberto
|
||||
nav_mid >= 0.30 and # meio ainda razoável
|
||||
nav_far <= 0.10 and # topo praticamente bloqueado (parede)
|
||||
nav_global >= 0.50 and # ainda tem bastante área navegável no frame
|
||||
num_blobs_far <= 1 # no máximo uma "parede", nada de corredor
|
||||
)
|
||||
|
||||
# 2.3 modo "campo aberto com borda lateral":
|
||||
# cena razoavelmente aberta, uma parede forte de um lado, mas sem corredor fechado
|
||||
cond_direcionando_borda_lateral = (
|
||||
nav_global >= 0.60 and # já tem boa área navegável
|
||||
nav_near >= 0.70 and
|
||||
nav_mid >= 0.50 and
|
||||
nav_far >= 0.40 and # far não está "morrendo", só mais sujo
|
||||
nav_far <= 0.80 and # não é mundão 100% limpo
|
||||
num_blobs_far == 1 # exatamente UMA parede grande
|
||||
)
|
||||
|
||||
cond_direcionando_aberto = (
|
||||
nav_global >= 0.75 and
|
||||
nav_near >= 0.70 and
|
||||
nav_mid >= 0.70 and
|
||||
nav_far >= 0.70 and
|
||||
max_delta <= 0.12 and
|
||||
num_blobs_far <= 1
|
||||
)
|
||||
|
||||
cond_direcionando = (
|
||||
cond_direcionando_base
|
||||
or cond_direcionando_obst_peq
|
||||
or cond_direcionando_frente_fe_chada
|
||||
or cond_direcionando_borda_lateral
|
||||
or cond_direcionando_aberto
|
||||
)
|
||||
|
||||
# 3) CaminhandoRua: dentro do corredor "clássico"
|
||||
cond_caminhando_base = (
|
||||
nav_near >= 0.55 and
|
||||
nav_mid >= 0.25 and
|
||||
nav_far <= 0.50 and
|
||||
(nav_near - nav_far) >= 0.20 and
|
||||
num_blobs_far >= 2 # precisa de DUAS paredes
|
||||
)
|
||||
|
||||
# 3.1 CaminhandoRua em corredor mais largo, com parede só de um lado
|
||||
# enquadra bem os casos:
|
||||
# nav_global ~0.66–0.77, near ~0.75–0.88, mid ~0.57–0.70, far ~0.56–0.63
|
||||
cond_caminhando_largo = (
|
||||
nav_global >= 0.60 and
|
||||
nav_near >= 0.75 and
|
||||
nav_mid >= 0.50 and
|
||||
nav_far >= 0.50 and
|
||||
nav_far <= THR_COR_FAR_LARGO and
|
||||
(nav_near - nav_far) >= THR_DELTA_COR and
|
||||
num_blobs_far >= 2 # corredor largo, mas ainda corredor
|
||||
)
|
||||
|
||||
cond_caminhando_multi_corredores = (
|
||||
nav_global >= 0.50 and # tem chão suficiente
|
||||
nav_mid >= 0.55 and # meio bem limpo
|
||||
nav_near >= 0.50 and # perto também ok
|
||||
num_blobs_far >= 2 and # pelo menos duas "paredes"
|
||||
corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and
|
||||
corridor_width_frac <= MAX_CORRIDOR_WIDTH_FRAC and
|
||||
corridor_nav_far >= 0.55 # corredor entre paredes bem navegável
|
||||
)
|
||||
|
||||
cond_caminhando = (
|
||||
cond_caminhando_base
|
||||
or cond_caminhando_largo
|
||||
or cond_caminhando_multi_corredores
|
||||
)
|
||||
|
||||
cond_entrando = (
|
||||
nav_global >= 0.75 and
|
||||
nav_near >= 0.90 and
|
||||
nav_mid >= 0.60 and
|
||||
nav_far >= 0.40 and
|
||||
nav_far <= 0.85 and
|
||||
(nav_near - nav_far) >= 0.10 and # far mais fechado que near
|
||||
num_blobs_far >= 2 and # duas paredes detectadas
|
||||
corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and
|
||||
corridor_nav_far >= 0.60 # corredor entre as paredes bem navegável
|
||||
)
|
||||
|
||||
# 5) SaindoRua
|
||||
cond_saindo_1 = (
|
||||
nav_near >= 0.50 and
|
||||
nav_mid >= 0.25 and
|
||||
nav_far >= 0.55 and
|
||||
(nav_far - nav_mid) >= 0.10 and
|
||||
nav_far >= nav_near - 0.15
|
||||
)
|
||||
|
||||
cond_saindo_2 = (
|
||||
nav_global >= 0.75 and
|
||||
nav_near >= 0.70 and
|
||||
nav_mid >= 0.60 and
|
||||
nav_far >= 0.75 and
|
||||
nav_far >= nav_mid
|
||||
)
|
||||
|
||||
cond_saindo = cond_saindo_1 or cond_saindo_2
|
||||
|
||||
# ===== Decisão (ordem importa!) =====
|
||||
if cond_parado:
|
||||
status = StatusCarroMapa.Parado
|
||||
|
||||
# 2) DIRECIONANDO: quase todo frame navegável
|
||||
elif nav_global > THR_NAV_ALTO:
|
||||
elif cond_direcionando:
|
||||
status = StatusCarroMapa.Direcionando
|
||||
|
||||
elif cond_saindo:
|
||||
status = StatusCarroMapa.SaindoRua
|
||||
elif cond_entrando:
|
||||
status = StatusCarroMapa.EntrandoRua
|
||||
elif cond_caminhando:
|
||||
status = StatusCarroMapa.CaminhandoRua
|
||||
else:
|
||||
# 3) ENTRANDO RUA
|
||||
cond_near_alto = nav_near >= THR_NEAR_ALTO
|
||||
cond_near_gt_mid = maior_igual_que(nav_near, nav_mid)
|
||||
cond_mid_gt_far = maior_que(nav_mid, nav_far)
|
||||
|
||||
# 4) SAINDO RUA
|
||||
cond_near_gt_mid2 = True or maior_que(nav_near, nav_mid)
|
||||
cond_far_gt_mid = maior_que(nav_far, nav_mid)
|
||||
|
||||
if cond_near_alto and cond_near_gt_mid and cond_mid_gt_far:
|
||||
status = StatusCarroMapa.EntrandoRua
|
||||
|
||||
elif cond_near_gt_mid2 and cond_far_gt_mid:
|
||||
status = StatusCarroMapa.SaindoRua
|
||||
|
||||
# 5) CAMINHANDO RUA (cone "normal" NEAR > MID > FAR)
|
||||
elif cond_near_gt_mid and cond_mid_gt_far:
|
||||
status = StatusCarroMapa.CaminhandoRua
|
||||
|
||||
else:
|
||||
# fallback: se ficar numa zona cinza, chama de Direcionando
|
||||
status = StatusCarroMapa.Manobrando
|
||||
|
||||
# monta probs "one-hot"
|
||||
probs = {s: 0.0 for s in StatusCarroMapa}
|
||||
probs[status] = 1.0
|
||||
status = StatusCarroMapa.Indefinido
|
||||
|
||||
debug = {
|
||||
"nav_global": nav_global,
|
||||
"nav_near": nav_near,
|
||||
"nav_mid": nav_mid,
|
||||
"nav_far": nav_far,
|
||||
"nav_global": nav_global,
|
||||
"THR_NAV_ALTO": THR_NAV_ALTO,
|
||||
"THR_NAV_BAIXO": THR_NAV_BAIXO,
|
||||
"THR_NEAR_ALTO": THR_NEAR_ALTO,
|
||||
"d_nm": d_nm,
|
||||
"d_mf": d_mf,
|
||||
"d_nf": d_nf,
|
||||
"max_delta": max_delta,
|
||||
"num_blobs_far": num_blobs_far,
|
||||
"corridor_nav_far": corridor_nav_far,
|
||||
"corridor_width_frac": corridor_width_frac,
|
||||
"cond_parado": cond_parado,
|
||||
"cond_direcionando_base": cond_direcionando_base,
|
||||
"cond_direcionando_obst_peq": cond_direcionando_obst_peq,
|
||||
"cond_caminhando_base": cond_caminhando_base,
|
||||
"cond_caminhando_largo": cond_caminhando_largo,
|
||||
"cond_entrando": cond_entrando,
|
||||
"cond_saindo": cond_saindo,
|
||||
"THR_BLOCKED": THR_BLOCKED,
|
||||
"THR_OPEN": THR_OPEN,
|
||||
"THR_DELTA_COR": THR_DELTA_COR,
|
||||
"THR_COR_FAR_LARGO": THR_COR_FAR_LARGO,
|
||||
"THR_OPEN_MUITO_LIMPO": THR_OPEN_MUITO_LIMPO,
|
||||
}
|
||||
|
||||
status_now = status
|
||||
|
||||
#print(status_now.name, debug)
|
||||
|
||||
# Histerese temporal: mantém teu esquema de histórico
|
||||
self._status_hist.append((status_now, self._now()))
|
||||
status_final = self._maioria_ultimos()
|
||||
self._status_final_hist.append(status_final)
|
||||
status_before = self._status_final_hist[0] if len(self._status_final_hist) > 1 else status_final
|
||||
|
||||
return status_now, status_final, status_before, probs, debug
|
||||
return status_now, status_final, status_before, debug
|
||||
|
||||
@staticmethod
|
||||
def _now():
|
||||
|
|
|
|||
|
|
@ -112,8 +112,8 @@ def load_seg_config(force_reload=False):
|
|||
|
||||
_CONFIG_CACHE["ia_model_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_model_ervas")
|
||||
_CONFIG_CACHE["ia_labelmap_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_labelmap_ervas")
|
||||
_CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ervas")
|
||||
_CONFIG_CACHE["ia_norm_stats_path"] = ContextoGlobalRedis.get_equipamento().get("path_ia_norm_stats_ervas")
|
||||
_CONFIG_CACHE["ia_backbone"] = ContextoGlobalRedis.get_equipamento().get("ia_backbone_ervas")
|
||||
_CONFIG_CACHE["faixa_atuacao_bicos"] = dadosAtu.get("percent_vertical_deteccao", 0.7)
|
||||
|
||||
_CONFIG_CACHE["area_atuacao_bicos"] = dadosAtu.get("height_area_deteccao", 0.1)
|
||||
|
|
|
|||
|
|
@ -1016,11 +1016,11 @@ namespace OperationControl.Services
|
|||
LoopRTK_Ntrip = true;
|
||||
|
||||
// Configurações do NTRIP caster para RTK2Go
|
||||
string host = "gps-ntrip.ibge.gov.br";
|
||||
int port = 2101;
|
||||
string host = "gps-ntrip.ibge.gov.br";
|
||||
int port = 2101;
|
||||
string mountpoint = "EESC0";
|
||||
string username = "Zendion"; // Geralmente vazio para RTK2Go
|
||||
string password = "vyNEF$5*"; // Geralmente vazio para RTK2Go
|
||||
string username = "Zendion"; // Geralmente vazio para RTK2Go
|
||||
string password = "QD&m1p60"; // Geralmente vazio para RTK2Go
|
||||
|
||||
while (CorrecaoRTK_Ntrip && APIService.HasInternet) // Loop para reconectar em caso de falha
|
||||
{
|
||||
|
|
@ -1265,12 +1265,20 @@ namespace OperationControl.Services
|
|||
|
||||
amostras_pos = new List<GgaFix>(1000);
|
||||
inicioProcesso = DateTime.UtcNow;
|
||||
DateTime? inicioTentativaFix = DateTime.UtcNow;
|
||||
inicioFix = null;
|
||||
|
||||
// Você já deve ter um leitor da COM que devolve linhas NMEA.
|
||||
// Abaixo, vamos supor um método async que lê GGA parseado.
|
||||
while (ProgressoGeral < 100)
|
||||
{
|
||||
// Verifica se o timeout tentando aplicar fix ja excedeu
|
||||
if (inicioTentativaFix.Value.AddMinutes(2) < DateTime.UtcNow && inicioFix is null)
|
||||
{
|
||||
Models.Variaveis.MostrarLog("Timeout ao tentar receber fix atraves do Ntrip...");
|
||||
break;
|
||||
}
|
||||
|
||||
// Lê próxima sentença (bloqueante/assíncrono)
|
||||
var gga = await LerProximoGgaAsync(); // implemente no seu stack
|
||||
|
||||
|
|
@ -1280,6 +1288,7 @@ namespace OperationControl.Services
|
|||
if (!FixLiberado || !new List<TiposCorrecaoGPS>() { TiposCorrecaoGPS.RTKFixo }.Contains(gga.FixQuality))
|
||||
{
|
||||
inicioFix = null; // reset
|
||||
inicioTentativaFix = DateTime.UtcNow;
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -1302,7 +1311,7 @@ namespace OperationControl.Services
|
|||
}
|
||||
|
||||
// ===== 2.1) Ajuste a assinatura se quiser passar timeout e CT de fora
|
||||
private async Task<GgaFix> LerProximoGgaAsync(int timeoutMs = 5000, CancellationToken ct = default)
|
||||
private async Task<GgaFix?> LerProximoGgaAsync(int timeoutMs = 5000, CancellationToken ct = default)
|
||||
{
|
||||
var sw = System.Diagnostics.Stopwatch.StartNew();
|
||||
int startHb = System.Threading.Volatile.Read(ref _lastReadHeartbeat);
|
||||
|
|
@ -1315,7 +1324,7 @@ namespace OperationControl.Services
|
|||
|
||||
if (sw.ElapsedMilliseconds >= timeoutMs)
|
||||
//throw new TimeoutException("Timeout aguardando nova leitura GGA.");
|
||||
return new GgaFix(DateTime.UtcNow, 0, 0, 0, TiposCorrecaoGPS.SemCorrecao);
|
||||
return null;
|
||||
|
||||
await Task.Delay(75, ct).ConfigureAwait(false);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue