Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ajuste no mpc, alvo eh o caminho e nao o proximo ponto, regra de reduzir para pulverizar, novo modelo de treinamento para oak-d lite, implementacao no visual worker do novo modelo

This commit is contained in:
Diego Freitas 2026-09-15 08:21:02 -03:00
parent 437acdb56a
commit 679f344e39
23 changed files with 29501 additions and 690 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/camera_worker/camera_oak.py
View File

@ -419,6 +419,7 @@ class CameraOak:
t0 = time.time()
frame = pkt.getCvFrame()
dur = time.time() - t0
frame_ts_host = time.time()
resultado = {
"erro": None,
@ -450,7 +451,7 @@ class CameraOak:
self.perf.tick(
"camera_rgb",
latencia_ms=dur * 1000.0,
frame_ts_host=self._rgb_cache_ts,
frame_ts_host=frame_ts_host,
frame_ts_device=pkt_ts_device,
seq=pkt_seq,
seq_delta=seq_delta,
@ -461,7 +462,7 @@ class CameraOak:
with self._cache_lock:
self._rgb_cache = frame
self._rgb_cache_ts = time.time()
self._rgb_cache_ts = frame_ts_host
self._rgb_cache_resultado = resultado
self.timestamp_ultimo_frame_rgb = self._rgb_cache_ts
self._rgb_seq = pkt_seq
@ -478,6 +479,7 @@ class CameraOak:
t0 = time.time()
frame = pkt.getFrame()
dur = time.time() - t0
frame_ts_host = time.time()
resultado = {
"erro": None,
@ -509,7 +511,7 @@ class CameraOak:
self.perf.tick(
"camera_depth",
latencia_ms=dur * 1000.0,
frame_ts_host=self._depth_cache_ts,
frame_ts_host=frame_ts_host,
frame_ts_device=pkt_ts_device,
seq=pkt_seq,
seq_delta=seq_delta,
@ -520,7 +522,7 @@ class CameraOak:
with self._cache_lock:
self._depth_cache = frame
self._depth_cache_ts = time.time()
self._depth_cache_ts = frame_ts_host
self._depth_cache_resultado = resultado
self.timestamp_ultimo_frame_depth = self._depth_cache_ts
self.ultimo_resultado_depth = resultado
@ -1062,6 +1064,50 @@ class CameraOak:
self._definir_heartbeat()
return frame, resultado
def requisitar_frame_rgb_ref(self):
"""
Retorna referência READ-ONLY ao latest frame RGB cacheado.
Diferente de requisitar_frame_rgb(), não copia ~6 MB por frame 1080p.
O cache da câmera apenas substitui a referência por um ndarray novo; não
modifica o ndarray anterior. Portanto o consumidor pode manter a referência
durante resize/inferência sem segurar o lock.
REGRA: o consumidor não deve escrever no ndarray retornado.
"""
with self._cache_lock:
resultado = dict(self._rgb_cache_resultado)
erro = resultado.get("erro") if self._is_erro_fatal_depthai(resultado.get("erro")) else None
frame = self._rgb_cache
if erro:
self._falha_fatal_depthai(erro)
return None, resultado
self._definir_heartbeat()
return frame, resultado
def requisitar_frame_depth_ref(self):
"""Mesmo contrato read-only do RGB, aplicado ao depth latest-frame."""
if not self.tem_depth:
return None, {
"erro": "Camera nao possui sensor de profundidade",
"duracao": 0,
"frame_valido": False,
}
with self._cache_lock:
resultado = dict(self._depth_cache_resultado)
erro = resultado.get("erro") if self._is_erro_fatal_depthai(resultado.get("erro")) else None
frame = self._depth_cache
if erro:
self._falha_fatal_depthai(erro)
return None, resultado
self._definir_heartbeat()
return frame, resultado
def requisitar_frame_depth(self):
if not self.tem_depth:
return None, {

398
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/movimentacao.py
View File

@ -110,6 +110,25 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
debug["status_carro"] = status_carro.name
debug["erro_orientacao"] = erro_angular
debug["erro_orientacao_fonte"] = estado.get(
"erro_orientacao_fonte",
"nao_informada"
)
debug["erros_orientacao"] = {
"selecionado": round(float(erro_angular), 4),
"combinado": round(
float(estado.get("erro_angular_combinado", erro_angular)),
4
),
"caminho": round(
float(estado.get("erro_angular_caminho", erro_angular)),
4
),
"proximo_ponto": round(
float(estado.get("erro_angular_proximo_ponto", erro_angular)),
4
),
}
debug["pontos_fim_corredor"] = pontos_fim_corredor
if not estado_valido:
@ -133,6 +152,8 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
resetar_filtro = False
reduzir_para_pulverizar = False
reduzir_por_weed_indisponivel = False
limitar_velocidade_por_weed = False
reduzir_para_fim_corredor = False
reduzir_por_curva = False
reduzir_por_ipb = False
@ -166,18 +187,45 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
curva=0.80,
dead=10.0
)
debug["motivos"].append("direcionando/retornando com redução por erro angular")
debug["motivos"].append(
"direcionando/retornando: velocidade por erro angular combinado"
)
elif status_carro == StatusCarroMapa.CaminhandoRua:
info_ervas = _ler_weed_worker()
ervas_no_radar = (
pulverizador_automatico and
info_ervas["atualizado"] and
info_ervas["ervas_no_radar"]
# O WeedWorker é a autoridade para dizer se há ervas no radar.
#
# Contrato atual:
# DadosWeedWorker.analise = envelope
# envelope.analise = dados_visuais reais
#
# _ler_weed_worker() normaliza esse contrato e também aceita o
# formato legado (campos diretamente em DadosWeedWorker.analise).
#
# Se o campo booleano não existir em uma versão legada, usamos
# ema_global apenas como fallback.
if info_ervas["campo_ervas_no_radar_presente"]:
ervas_detectadas = bool(info_ervas["ervas_no_radar"])
else:
ervas_detectadas = (
info_ervas["percentual_ervas"] >= ervas_min_percent
)
reduzir_para_pulverizar = bool(ervas_no_radar)
if pulverizador_automatico:
if info_ervas["atualizado"]:
reduzir_para_pulverizar = bool(ervas_detectadas)
else:
# Falha conservadora de qualidade de aplicação:
# percepção desconhecida NÃO significa "sem ervas".
# Mantém a velocidade de pulverização até a análise voltar,
# sem parar a operação por si só.
reduzir_por_weed_indisponivel = True
limitar_velocidade_por_weed = bool(
reduzir_para_pulverizar
or reduzir_por_weed_indisponivel
)
reduzir_para_fim_corredor = (
pontos_fim_corredor >= 0 and
@ -185,6 +233,15 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
)
debug["weed"] = info_ervas
debug["weed"]["pulverizador_automatico"] = bool(
pulverizador_automatico
)
debug["weed"]["ervas_detectadas_efetivo"] = bool(
ervas_detectadas
)
debug["weed"]["limitar_velocidade"] = bool(
limitar_velocidade_por_weed
)
velocidade_livre = calcular_velocidade_relativa(
vel_min=vel_com_ervas,
@ -196,9 +253,22 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
dead=5.0
)
debug["motivos"].append(
"caminhando rua: velocidade por erro angular do caminho"
)
if reduzir_para_pulverizar:
velocidade_sp = min(velocidade_livre, vel_com_ervas)
debug["motivos"].append("ervas no radar: reduzindo para pulverizar")
debug["motivos"].append(
"ervas no radar: reduzindo para pulverizar"
)
elif reduzir_por_weed_indisponivel:
velocidade_sp = min(velocidade_livre, vel_com_ervas)
debug["motivos"].append(
"WeedWorker ausente/desatualizado: "
"mantendo velocidade conservadora de pulverização"
)
elif reduzir_para_fim_corredor:
velocidade_sp = min(velocidade_livre, vel_com_ervas)
@ -284,7 +354,8 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
if usar_imu_movimento:
vel_imu, imu_resetar, imu_stop, imu_debug = _aplicar_limitador_imu(
velocidade_atual=velocidade_sp
velocidade_atual=velocidade_sp,
vel_min_equip=vel_min_equip
)
debug["imu"] = imu_debug
@ -329,7 +400,7 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
# Aceleração normal é filtrada para não dar tranco.
reducao_seguranca = bool(
reduzir_para_pulverizar
limitar_velocidade_por_weed
or reduzir_para_fim_corredor
or reduzir_por_curva
or reduzir_por_ipb
@ -351,23 +422,36 @@ def definir_comando(filtro_vel: FiltroVelocidade, erro_orientacao: float = None)
debug=debug,
)
# Correção crítica:
# havendo erva no radar, a saída nunca pode superar a mínima definida.
if reduzir_para_pulverizar:
# Teto rígido do WeedWorker:
#
# - ervas detectadas: nunca ultrapassa vel_com_ervas;
# - WeedWorker desatualizado com pulverizador automático ligado:
# também não assume que a rua está livre.
#
# Esse clamp acontece DEPOIS da rampa para não existir overshoot
# transitório acima da velocidade de aplicação.
if limitar_velocidade_por_weed:
velocidade_filtrada = min(
velocidade_filtrada,
vel_com_ervas
)
# Sincroniza o estado da rampa com a saída realmente aplicada.
# Assim, quando a condição desaparecer, a aceleração volta a
# acontecer pela rampa normal em vez de saltar para o SP antigo.
try:
filtro_vel._ultimo_sp_campo = velocidade_filtrada
filtro_vel._ultimo_ts_campo = time.monotonic()
except Exception:
pass
if reduzir_para_pulverizar:
motivo_teto = "ervas no radar"
else:
motivo_teto = "WeedWorker indisponível/desatualizado"
debug["limitadores"].append(
f"ervas no radar: teto rígido em {vel_com_ervas:.1f}%"
f"{motivo_teto}: teto rígido em {vel_com_ervas:.1f}%"
)
velocidade_filtrada = round(
@ -415,6 +499,33 @@ def _resolver_estado_operacional(
contexto,
erro_orientacao
):
"""
Resolve o estado operacional e escolhe a referência angular usada
EXCLUSIVAMENTE para a política de velocidade.
Política alinhada ao MPC Path Tracking V2:
- CaminhandoRua:
usa erro_angular_caminho.
O que importa para liberar velocidade é o corpo do rover estar
alinhado com a orientação local da passada. Deslocamento lateral
pode ser corrigido pelo MovimentoDiagonal sem penalizar a velocidade
como se o rover estivesse "torto".
- Direcionando / RetornandoBase:
usa erro_angular combinado.
Fora da passada, a aquisição geométrica do alvo continua relevante.
- EntrandoRua / SaindoRua / Manobrando:
a velocidade base é vel_curva, portanto o erro selecionado não
altera a velocidade nesses estados. Mantemos o combinado para
diagnóstico/coerência.
- erro_orientacao explícito:
continua tendo prioridade para preservar compatibilidade com
chamadas externas/legadas.
"""
if op_modo in [ModoOperacao.MapaGPS, ModoOperacao.RetornoBase]:
trajetoria = _dict(contexto.get("Trajetoria", {}))
@ -424,6 +535,10 @@ def _resolver_estado_operacional(
"motivo": "Trajetoria ausente no contexto",
"status_carro": StatusCarroMapa.Parado,
"erro_orientacao": 0.0,
"erro_orientacao_fonte": "trajetoria_ausente",
"erro_angular_combinado": 0.0,
"erro_angular_caminho": 0.0,
"erro_angular_proximo_ponto": 0.0,
"pontos_fim_corredor": -1,
}
@ -433,33 +548,86 @@ def _resolver_estado_operacional(
StatusCarroMapa.Parado
)
if erro_orientacao is None:
erro_orientacao = _float(trajetoria.get("erro_angular", 0.0), 0.0)
erro_combinado = _float(
trajetoria.get("erro_angular", 0.0),
0.0
)
erro_caminho = _float(
trajetoria.get(
"erro_angular_caminho",
erro_combinado
),
erro_combinado
)
erro_proximo_ponto = _float(
trajetoria.get(
"erro_angular_proximo_ponto",
erro_combinado
),
erro_combinado
)
if erro_orientacao is not None:
erro_selecionado = _float(
erro_orientacao,
erro_combinado
)
fonte_erro = "override_explicito"
elif status_carro == StatusCarroMapa.CaminhandoRua:
# Na passada, o heading local da rua é a referência correta.
# O bearing para waypoint não deve reduzir velocidade só porque
# o MPC está corrigindo offset lateral em MovimentoDiagonal.
erro_selecionado = erro_caminho
fonte_erro = "erro_angular_caminho"
else:
# Aquisição, retorno e demais estados mantêm a referência
# combinada que incorpora a geometria do alvo.
erro_selecionado = erro_combinado
fonte_erro = "erro_angular_combinado"
corredor = _dict(trajetoria.get("CorredorAtual", {}))
pontos_fim_corredor = _int(corredor.get("pontos_restantes", -1), -1)
pontos_fim_corredor = _int(
corredor.get("pontos_restantes", -1),
-1
)
return {
"valido": True,
"motivo": "",
"status_carro": status_carro,
"erro_orientacao": _float(erro_orientacao, 0.0),
"erro_orientacao": float(erro_selecionado),
"erro_orientacao_fonte": fonte_erro,
"erro_angular_combinado": float(erro_combinado),
"erro_angular_caminho": float(erro_caminho),
"erro_angular_proximo_ponto": float(erro_proximo_ponto),
"pontos_fim_corredor": pontos_fim_corredor,
}
if op_modo == ModoOperacao.MapeamentoVisual:
dados_vw = _dict(ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}))
segmentacao = _dict(dados_vw.get("segmentacao", {}))
dados_vw = _dict(
ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
)
segmentacao = _dict(
dados_vw.get("segmentacao", {})
)
seg_ts = _float(
segmentacao.get(
"ts",
dados_vw.get("ts_segmentacao", dados_vw.get("momento", 0.0))
dados_vw.get(
"ts_segmentacao",
dados_vw.get("momento", 0.0)
)
),
0.0
)
seg_atualizada, idade_ms = _timestamp_atualizado(seg_ts, max_idade_s=1.0)
seg_atualizada, idade_ms = _timestamp_atualizado(
seg_ts,
max_idade_s=1.0
)
if not segmentacao or not seg_atualizada:
motivo = "segmentacao visual ausente/desatualizada"
@ -472,31 +640,61 @@ def _resolver_estado_operacional(
"motivo": motivo,
"status_carro": StatusCarroMapa.Parado,
"erro_orientacao": 0.0,
"erro_orientacao_fonte": "segmentacao_invalida",
"erro_angular_combinado": 0.0,
"erro_angular_caminho": 0.0,
"erro_angular_proximo_ponto": 0.0,
"pontos_fim_corredor": -1,
}
status_carro = _enum_or(
StatusCarroMapa,
segmentacao.get("status_corredor", StatusCarroMapa.Parado.value),
segmentacao.get(
"status_corredor",
StatusCarroMapa.Parado.value
),
StatusCarroMapa.Parado
)
if erro_orientacao is None:
erro_orientacao = _float(segmentacao.get("erro_angular", 0.0), 0.0)
erro_visual = _float(
segmentacao.get("erro_angular", 0.0),
0.0
)
if erro_orientacao is not None:
erro_selecionado = _float(
erro_orientacao,
erro_visual
)
fonte_erro = "override_explicito"
else:
erro_selecionado = erro_visual
fonte_erro = "segmentacao_visual"
return {
"valido": True,
"motivo": "",
"status_carro": status_carro,
"erro_orientacao": _float(erro_orientacao, 0.0),
"erro_orientacao": float(erro_selecionado),
"erro_orientacao_fonte": fonte_erro,
"erro_angular_combinado": float(erro_visual),
"erro_angular_caminho": float(erro_visual),
"erro_angular_proximo_ponto": float(erro_visual),
"pontos_fim_corredor": -1,
}
return {
"valido": False,
"motivo": f"Modo de operação sem estratégia de movimento: {op_modo.name}",
"motivo": (
"Modo de operação sem estratégia de movimento: "
f"{op_modo.name}"
),
"status_carro": StatusCarroMapa.Parado,
"erro_orientacao": 0.0,
"erro_orientacao_fonte": "modo_sem_estrategia",
"erro_angular_combinado": 0.0,
"erro_angular_caminho": 0.0,
"erro_angular_proximo_ponto": 0.0,
"pontos_fim_corredor": -1,
}
@ -506,34 +704,119 @@ def _resolver_estado_operacional(
# ============================================================
def _ler_weed_worker():
try:
dados = _dict(ContextoGlobalRedis.get(CtxKey.DadosWeedWorker, {}))
analise = _dict(dados.get("analise", {}))
"""
Lê e normaliza o contrato do WeedWorker.
Contrato atual publicado pelo CameraManager:
DadosWeedWorker = {
"ts_publicacao": ...,
"ts_analise": ...,
"analise": {
"ts_analise": ...,
"fps_model": ...,
"fps_inferencia": ...,
"infer_ms": ...,
"infer_gpu_ms": ...,
"analise": {
"ervas_no_radar": ...,
"estatisticas": {
"erva": {
"ema_global": ...
}
},
...
}
}
}
Também aceita o formato legado, no qual ervas_no_radar e estatisticas
ficam diretamente em DadosWeedWorker["analise"].
"""
try:
dados = _dict(
ContextoGlobalRedis.get(CtxKey.DadosWeedWorker, {})
)
envelope = _dict(dados.get("analise", {}))
# Contrato atual possui envelope["analise"].
# No contrato legado, o próprio envelope já é a análise.
analise_interna = _dict(envelope.get("analise", {}))
if analise_interna:
analise = analise_interna
esquema = "nested_v2"
else:
analise = envelope
esquema = "flat_legacy"
# Preferência de timestamp:
# 1) timestamp do envelope da análise atual;
# 2) timestamp dentro da análise legada;
# 3) timestamp raiz do DadosWeedWorker.
ts = _float(
envelope.get(
"ts_analise",
envelope.get(
"ts",
envelope.get(
"timestamp",
analise.get(
"ts",
analise.get(
"timestamp",
dados.get("ts_analise", dados.get("momento", 0.0))
dados.get(
"ts_analise",
dados.get(
"ts_publicacao",
dados.get("momento", 0.0)
)
)
)
)
)
)
),
0.0
)
atualizado, idade_ms = _timestamp_atualizado(ts, max_idade_s=1.5)
atualizado, idade_ms = _timestamp_atualizado(
ts,
max_idade_s=1.5
)
estatisticas = _dict(analise.get("estatisticas", {}))
erva = _dict(estatisticas.get("erva", {}))
estatisticas = _dict(
analise.get("estatisticas", {})
)
erva = _dict(
estatisticas.get("erva", {})
)
ervas_no_radar = _bool(analise.get("ervas_no_radar", False))
percentual_ervas = _float(erva.get("ema_global", 0.0), 0.0)
campo_ervas_presente = "ervas_no_radar" in analise
ervas_no_radar = _bool(
analise.get("ervas_no_radar", False)
)
percentual_ervas = _float(
erva.get(
"ema_global",
analise.get("percentual_ervas_no_radar", 0.0)
),
0.0
)
return {
"atualizado": atualizado,
"atualizado": bool(atualizado),
"idade_ms": idade_ms,
"ervas_no_radar": ervas_no_radar,
"ervas_no_radar": bool(ervas_no_radar),
"campo_ervas_no_radar_presente": bool(
campo_ervas_presente
),
"percentual_ervas": percentual_ervas,
"timestamp_analise": ts,
"esquema": esquema,
}
except Exception as e:
@ -541,7 +824,10 @@ def _ler_weed_worker():
"atualizado": False,
"idade_ms": None,
"ervas_no_radar": False,
"campo_ervas_no_radar_presente": False,
"percentual_ervas": 0.0,
"timestamp_analise": 0.0,
"esquema": "erro",
"erro": str(e),
}
@ -643,7 +929,7 @@ def _aplicar_limitador_visual(
# Limitador IMU
# ============================================================
def _aplicar_limitador_imu(*, velocidade_atual):
def _aplicar_limitador_imu(*, velocidade_atual, vel_min_equip):
debug = {
"habilitado": True,
"aplicado": False,
@ -693,7 +979,41 @@ def _aplicar_limitador_imu(*, velocidade_atual):
if risk_level_num <= 0:
return velocidade_atual, False, False, debug
vel_nova = float(velocidade_atual) * velocidade_factor
velocidade_atual = max(0.0, float(velocidade_atual))
vel_min_equip = max(0.0, float(vel_min_equip))
# Attention/risk são REDUÇÕES, não ordens de parada.
#
# Se a IMU, sozinha, levar o SP abaixo da velocidade mínima útil do
# equipamento, limita no piso operacional em vez de transformar um
# nível attention/risk em parada.
#
# O min(..., velocidade_atual) é proposital: a IMU nunca pode elevar
# uma velocidade que já tenha sido reduzida por outro limitador
# (Visual Worker, IPB, fim de corredor, pulverização etc.).
vel_calculada = velocidade_atual * velocidade_factor
if velocidade_atual >= vel_min_equip and vel_calculada > 0.0:
vel_nova = min(
velocidade_atual,
max(vel_min_equip, vel_calculada)
)
else:
# Se outro limitador já entregou algo abaixo do piso, a IMU não
# "ressuscita" a velocidade. A regra geral posterior decide se
# esse valor deve virar parada.
vel_nova = min(velocidade_atual, vel_calculada)
debug["velocidade_entrada"] = round(velocidade_atual, 3)
debug["velocidade_calculada"] = round(vel_calculada, 3)
debug["velocidade_saida"] = round(vel_nova, 3)
debug["vel_min_equip"] = round(vel_min_equip, 3)
debug["piso_operacional_aplicado"] = bool(
velocidade_atual >= vel_min_equip
and 0.0 < vel_calculada < vel_min_equip
and abs(vel_nova - vel_min_equip) < 1e-9
)
resetar_filtro = False
return vel_nova, resetar_filtro, False, debug

689
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py
View File

@ -341,7 +341,97 @@ class ControladorMPC:
max_value=20.0,
)
# Amortecimento aplicado somente durante CaminhandoRua. Nas curvas o
# ------------------------------------------------------------------
# PATH TRACKING V2 - referência geométrica da passada
# ------------------------------------------------------------------
# Na passada o rover NÃO mira um ponto. O ponto/progresso continua
# sendo autoridade do C#, mas a direção segue: (1) heading médio do
# caminho nos próximos metros + (2) cross-track lateral. O alvo
# pure-pursuit fica reservado para entrada/saída/manobra.
self._heading_lookahead_base_m = _safe_float(
parametros_mpc.get("heading_lookahead_base_m", 1.60),
1.60, min_value=0.40, max_value=8.0,
)
self._heading_lookahead_velocidade_s = _safe_float(
parametros_mpc.get("heading_lookahead_velocidade_s", 0.55),
0.55, min_value=0.0, max_value=4.0,
)
self._heading_lookahead_min_m = _safe_float(
parametros_mpc.get("heading_lookahead_min_m", 1.20),
1.20, min_value=0.25, max_value=8.0,
)
self._heading_lookahead_max_m = _safe_float(
parametros_mpc.get("heading_lookahead_max_m", 3.00),
3.00, min_value=self._heading_lookahead_min_m, max_value=12.0,
)
# Heading primeiro, lateral depois. A diagonal só pode existir quando
# o corpo já está praticamente paralelo ao caminho; assim ela
# recentraliza sem congelar um erro de heading de 5-10 graus.
self._heading_deadband_graus = _safe_float(
parametros_mpc.get("heading_deadband_graus", 0.45),
0.45, min_value=0.0, max_value=3.0,
)
self._diagonal_entrada_heading_graus = _safe_float(
parametros_mpc.get("diagonal_entrada_heading_graus", 1.25),
1.25, min_value=0.2, max_value=8.0,
)
self._diagonal_saida_heading_graus = _safe_float(
parametros_mpc.get("diagonal_saida_heading_graus", 2.50),
2.50, min_value=self._diagonal_entrada_heading_graus, max_value=12.0,
)
self._lateral_deadband_m = _safe_float(
parametros_mpc.get("lateral_deadband_m", 0.05),
0.05, min_value=0.0, max_value=0.30,
)
self._ganho_heading_reta = _safe_float(
parametros_mpc.get("ganho_heading_reta", 1.00),
1.00, min_value=0.1, max_value=3.0,
)
self._ganho_lateral_dianteira = _safe_float(
parametros_mpc.get("ganho_lateral_dianteira", 0.32),
0.32, min_value=0.0, max_value=3.0,
)
self._ganho_lateral_diagonal = _safe_float(
parametros_mpc.get("ganho_lateral_diagonal", 0.85),
0.85, min_value=0.0, max_value=4.0,
)
self._velocidade_offset_lateral = _safe_float(
parametros_mpc.get("velocidade_offset_lateral", 0.35),
0.35, min_value=0.05, max_value=2.0,
)
self._angulo_diagonal_max_graus = _safe_float(
parametros_mpc.get("angulo_diagonal_max_graus", 16.0),
16.0, min_value=1.0, max_value=self.angulo_max_graus,
)
self._heading_reta_aquisicao_max_graus = _safe_float(
parametros_mpc.get("heading_reta_aquisicao_max_graus", 15.0),
15.0, min_value=3.0, max_value=35.0,
)
self._lateral_reta_aquisicao_max_m = _safe_float(
parametros_mpc.get("lateral_reta_aquisicao_max_m", 0.80),
0.80, min_value=0.15, max_value=2.0,
)
self._penalidade_troca_tipo = _safe_float(
parametros_mpc.get("penalidade_troca_tipo", 0.18),
0.18, min_value=0.0, max_value=3.0,
)
# Direcionando fora da rua: usa Arco para a correção grosseira de
# heading e volta para RodasDianteiras somente quando já estiver bem
# alinhado. Os dois limiares criam histerese e evitam troca de modo
# perto da fronteira.
self._direcionando_arco_entrada_graus = _safe_float(
parametros_mpc.get("direcionando_arco_entrada_graus", 15.0),
15.0, min_value=3.0, max_value=60.0,
)
self._direcionando_arco_saida_graus = _safe_float(
parametros_mpc.get("direcionando_arco_saida_graus", 8.0),
8.0, min_value=1.0, max_value=self._direcionando_arco_entrada_graus,
)
# Amortecimento aplicado ao tracking de passada. MovimentoArco em
# manobra preserva autoridade; RodasDianteiras/Diagonal são limitados.
# 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),
@ -562,6 +652,11 @@ class ControladorMPC:
return (float(xy[0]), float(xy[1])), i
def _distancia_lookahead(self, status_carro, velocidade):
"""Look-ahead do alvo de manobra/pure-pursuit.
Em CaminhandoRua esse alvo continua existindo para debug/progresso,
mas NÃO define o heading desejado do rover.
"""
v = max(0.0, _safe_float(velocidade, 0.0))
if _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]):
@ -580,7 +675,7 @@ class ControladorMPC:
))
def _construir_alvo_direcional(self, x, y, idx_base, status_carro, velocidade):
"""Cria alvo pure-pursuit continuo sem alterar o progresso visitado."""
"""Cria alvo contínuo para manobra/debug sem alterar progresso visitado."""
n = len(self.pontos_info)
if n <= 0:
return {"xy": (float(x), float(y)), "_idx_base": 0}
@ -606,21 +701,212 @@ class ControladorMPC:
alvo["_lookahead_m"] = float(max(0.0, s_alvo - s_proj))
return alvo
def _ponto_estrutural(self, idx):
if not self.pontos_info:
return True
i = max(0, min(_safe_int(idx, 0), len(self.pontos_info) - 1))
p = self.pontos_info[i]
return bool(p.get("estrutural", p.get("PontoEstrutural", False)))
def _idx_corredor(self, idx):
if not self.pontos_info:
return -1
i = max(0, min(_safe_int(idx, 0), len(self.pontos_info) - 1))
p = self.pontos_info[i]
return _safe_int(p.get("idxCorredor", p.get("IdxCorredor", -1)), -1)
def _segmento_operacional_mesmo_corredor(self, idx_base):
"""True quando há um segmento de passada válido junto ao índice base.
Aceita a borda de entrada como início da aquisição se o segmento à
frente já pertence ao mesmo corredor e o próximo ponto é operacional.
Isso evita obrigar Arco só porque o estado C# ainda diz Manobrando.
"""
n = len(self.pontos_info)
if n < 2:
return False
idx = max(0, min(_safe_int(idx_base, 0), n - 1))
pares = []
if idx + 1 < n:
pares.append((idx, idx + 1))
if idx - 1 >= 0:
pares.append((idx - 1, idx))
for a, b in pares:
ca, cb = self._idx_corredor(a), self._idx_corredor(b)
if ca < 0 or cb < 0 or ca != cb:
continue
# Um ponto estrutural isolado na entrada pode iniciar a reta, mas
# dois pontos estruturais seguidos caracterizam ligação/manobra.
if self._ponto_estrutural(a) and self._ponto_estrutural(b):
continue
return True
return False
def _limitar_s_ao_corredor(self, s_proj, s_desejado, idx_base):
"""Não deixa o heading look-ahead enxergar a rua seguinte pela cabeceira."""
if self._s_nodes is None or len(self._s_nodes) == 0:
return float(s_desejado)
n = len(self.pontos_info)
idx = max(0, min(_safe_int(idx_base, 0), n - 1))
corredor = self._idx_corredor(idx)
if corredor < 0:
return float(s_desejado)
s_lim = float(self._s_nodes[-1])
for j in range(idx + 1, n):
if self._idx_corredor(j) != corredor or self._ponto_estrutural(j):
s_lim = float(self._s_nodes[j])
break
# precisa sobrar um pequeno vetor para formar direção; se já estamos
# no fim, a tangente local será usada pelo chamador.
return float(min(float(s_desejado), s_lim))
def _referencia_caminho_lookahead(self, x, y, idx_referencia, velocidade):
"""Retorna heading médio dos próximos metros + cross-track.
A orientação é a corda da centerline entre a projeção atual e um ponto
adiante. Em reta ela é constante; em curva antecipa suavemente o arco.
Nunca aponta do ROBÔ para o alvo, portanto deslocamento lateral não
contamina o erro de heading.
"""
e_lat, s_proj, i_seg, proj, theta_local, margem = self._projetar_na_trajetoria_local(
x, y, idx_referencia=idx_referencia,
)
v = max(0.0, _safe_float(velocidade, 0.0))
look = self._heading_lookahead_base_m + self._heading_lookahead_velocidade_s * v
look = float(np.clip(look, self._heading_lookahead_min_m, self._heading_lookahead_max_m))
s_fim = min(float(self._s_nodes[-1]), float(s_proj) + look)
s_fim = self._limitar_s_ao_corredor(s_proj, s_fim, idx_referencia)
p0, _ = self._xy_na_abscissa(s_proj)
p1, _ = self._xy_na_abscissa(s_fim)
dx = float(p1[0] - p0[0])
dy = float(p1[1] - p0[1])
dist = math.hypot(dx, dy)
if dist >= 0.20:
theta_ref = float(math.atan2(dx, dy))
look_real = float(s_fim - s_proj)
else:
theta_ref = float(theta_local)
look_real = 0.0
return {
"e_lat": float(e_lat),
"s_proj": float(s_proj),
"i_seg": int(i_seg),
"projecao": proj,
"theta_local": float(theta_local),
"theta_ref": float(theta_ref),
"margem": float(margem),
"lookahead_heading_m": float(max(0.0, look_real)),
}
def _erro_heading_caminho(self, theta_robo, theta_ref):
return float(self._wrap_pi(float(theta_ref) - float(theta_robo)))
def _tracking_reta_permitido(self, contexto, idx_base, e_lat, erro_heading_rad):
"""Decide se a política de passada pode assumir o controle.
CaminhandoRua sempre usa path tracking. Entrando/Manobrando só migra
cedo para a política de reta quando a referência ainda pertence ao
mesmo corredor e a pose já está perto o bastante da passada. Isso
evita o caso real em que o rover inicia alinhado mas recebe Arco por
burocracia de estado.
"""
carro = _as_dict(_as_dict(contexto).get("Carro", {}))
status = carro.get("Status", StatusCarroMapa.Parado.value)
if _status_in(status, [StatusCarroMapa.CaminhandoRua]):
return True
if not _status_in(status, [StatusCarroMapa.EntrandoRua, StatusCarroMapa.Manobrando]):
return False
if not self._segmento_operacional_mesmo_corredor(idx_base):
return False
return bool(
abs(float(e_lat)) <= self._lateral_reta_aquisicao_max_m
and abs(math.degrees(float(erro_heading_rad))) <= self._heading_reta_aquisicao_max_graus
)
def _referencia_direcional_reta(self, e_lat, erro_heading_rad, velocidade, tipo_preferido=None):
"""Escolhe modo e centro angular da busca para uma passada.
- heading fora do alinhamento -> RodasDianteiras e corrige heading;
- heading alinhado -> MovimentoDiagonal para remover cross-track sem
girar o corpo;
- histerese estreita impede ficar 5-10 graus torto em diagonal.
"""
e_lat = float(e_lat)
v = max(0.0, float(velocidade))
e_head_deg = abs(math.degrees(float(erro_heading_rad)))
tipo_prev = _movimento_from_value(tipo_preferido)
precisa_recentralizar = abs(e_lat) > self._lateral_deadband_m
manter_diag = (
tipo_prev == TipoMovimentoDirecional.MovimentoDiagonal
and e_head_deg <= self._diagonal_saida_heading_graus
)
entrar_diag = (
precisa_recentralizar
and e_head_deg <= self._diagonal_entrada_heading_graus
)
usar_diag = bool(manter_diag or entrar_diag)
if usar_diag:
if abs(e_lat) <= self._lateral_deadband_m:
delta = 0.0
else:
delta = math.atan2(
self._ganho_lateral_diagonal * e_lat,
v + self._velocidade_offset_lateral,
)
lim = math.radians(min(self._angulo_diagonal_max_graus, self.angulo_max_graus))
return TipoMovimentoDirecional.MovimentoDiagonal, float(np.clip(delta, -lim, lim))
# Fora da janela diagonal, prioridade absoluta é voltar a ficar
# paralelo. A parcela lateral perde força conforme o erro de heading
# cresce, para não transformar a correção em perseguição oscilante.
if abs(math.degrees(erro_heading_rad)) <= self._heading_deadband_graus:
e_head_eff = 0.0
else:
e_head_eff = float(erro_heading_rad)
fator_lat = 1.0 - min(1.0, e_head_deg / max(3.0, self._diagonal_saida_heading_graus * 3.0))
lat_eff = 0.0 if abs(e_lat) <= self._lateral_deadband_m else e_lat
delta_lat = math.atan2(
self._ganho_lateral_dianteira * lat_eff,
v + self._velocidade_offset_lateral,
) * fator_lat
delta = self._ganho_heading_reta * e_head_eff + delta_lat
lim = math.radians(self.angulo_max_graus)
return TipoMovimentoDirecional.RodasDianteiras, float(np.clip(delta, -lim, lim))
def _estabilizar_angulo_saida(
self,
angulo_desejado_rad,
status_carro,
comando_anterior,
dt_controle,
tipo_movimento=None,
):
"""Filtra/rate-limita esterco somente na passada reta."""
"""Rate-limit de saída para modos de tracking; Arco mantém autoridade."""
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]):
tipo = _movimento_from_value(tipo_movimento) if tipo_movimento is not None else None
if tipo == TipoMovimentoDirecional.MovimentoArco:
return desejado
if tipo is None and not _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]):
return desejado
anterior = np.radians(_safe_float(
@ -980,23 +1266,37 @@ class ControladorMPC:
def _kappa_from_angle(self, tipo, a_rad, L, k_r=0.5, crab_in_phase=False, beta_crab=0.0):
"""
Mapeia ângulo de direção 'a_rad' -> curvatura κ (1/m) de acordo com o tipo.
- crab_in_phase=True: movimento 'crab' (quase κ=0), usa desloc. lateral beta_crab (rad).
Curvatura GEOMÉTRICA usada pela LUT do costmap.
Importante: não usa calcular_omega(v=1). O GPSHandler aplica um fator
dinâmico dependente da velocidade (Ku), então omega(1)/1 não representa
uma curvatura geométrica independente da velocidade. A LUT é deliberadamente
geométrica/conservadora; a simulação pesada do MPC usa calcular_omega com
a velocidade real do ciclo.
"""
if crab_in_phase:
return 0.0 # tratamos crab fora (x ≈ y * tan(beta_crab))
if tipo == TipoMovimentoDirecional.RodasDianteiras:
return math.tan(a_rad) / L
if tipo == TipoMovimentoDirecional.RodasTraseiras:
return math.tan(a_rad) / L
elif tipo == TipoMovimentoDirecional.MovimentoArco:
return math.tan((1.0 - k_r) * a_rad) / L
elif tipo == TipoMovimentoDirecional.MovimentoDiagonal:
return 0.0
elif tipo == TipoMovimentoDirecional.MovimentoLateral:
tipo_enum = _movimento_from_value(tipo)
L_eff = max(float(L), 1e-6)
t = math.tan(float(a_rad))
if abs(t) < 1e-9:
return 0.0
else:
return math.tan(a_rad) / L # default
if tipo_enum == TipoMovimentoDirecional.RodasDianteiras:
return t / L_eff
if tipo_enum == TipoMovimentoDirecional.RodasTraseiras:
# Mantém a convenção atualmente usada pelo GPSHandler/C#.
return t / L_eff
if tipo_enum == TipoMovimentoDirecional.MovimentoArco:
# df=+delta, dr=-delta => (tan(df)-tan(dr))/L = 2*tan(delta)/L
return (2.0 * t) / L_eff
if tipo_enum in [
TipoMovimentoDirecional.MovimentoDiagonal,
TipoMovimentoDirecional.MovimentoLateral,
]:
return 0.0
return t / L_eff
def _calcula_erro_posicao(self, x, y, ponto_alvo, d_sat=3.0):
try:
@ -1007,16 +1307,20 @@ class ControladorMPC:
_log(f"Erro ao calcular erro de posicao: {e}")
return 1.0, float("inf")
def _calcula_erro_orientacao(self, x, y, theta, ponto_alvo, angulo_caminho, peso_proximo_ponto=0.5):
def _calcula_erro_orientacao(self, x, y, theta, ponto_alvo, angulo_caminho, peso_proximo_ponto=0.5, usar_apenas_caminho=False):
try:
if usar_apenas_caminho:
erro = abs(self._wrap_pi(float(angulo_caminho) - float(theta)))
else:
# Compatibilidade para manobras: mantém o blend histórico
# entre bearing do alvo e direção local do caminho.
orient_sim = self.gps_handler.calcular_orientacao((x, y), ponto_alvo)
orient_sim = (orient_sim + np.pi) % (2 * np.pi)
erro_ori_sim = self.gps_handler.erro_angular(orient_sim, theta, angulo_caminho, peso_proximo_ponto)
#erro_ori_sim = abs(self._wrap_pi(orient_sim - theta_sim))
#erro_ori_sim = abs(self._wrap_pi(theta_path - theta_sim))
o_norm = (erro_ori_sim / np.pi)
erro_ori_sim = np.degrees(erro_ori_sim)
return (o_norm, erro_ori_sim)
erro = self.gps_handler.erro_angular(
orient_sim, theta, angulo_caminho, peso_proximo_ponto
)
o_norm = abs(float(erro)) / np.pi
return (o_norm, abs(float(np.degrees(erro))))
except Exception as e:
_log(f"Erro ao calcular erro de orientacao: {e}")
return 1.0, 180.0
@ -1191,7 +1495,7 @@ class ControladorMPC:
return self._proximo_nao_visitado(pontos_visitados)
def _calcular_pesos_movimento(self, contexto, erro_ori_graus, erro_lat_m):
def _calcular_pesos_movimento(self, contexto, erro_ori_graus, erro_lat_m, tracking_reta=False):
# helpers simples
def _clamp(x, lo, hi):
return lo if x < lo else hi if x > hi else x
@ -1241,7 +1545,11 @@ class ControladorMPC:
peso_suavidade = _w_from_err_inv(erro_lat_m, w_min=pesos.get("suavidade_min", 0.5), w_max=pesos.get("suavidade_max", 3.6), deadband=0.05, tol=0.25, power=1.3, smooth=True)
peso_fator_re = pesos.get("fator_re", 0.05)
peso_ideal = pesos.get("ideal", 1.4)
peso_lateral = _w_from_err(erro_lat_m, w_min=pesos.get("lateral_min", 1.8), w_max=pesos.get("lateral_max", 8.5), deadband=0.05, tol=0.7, power=1.3, smooth=True) if dentro else 0.0
peso_lateral = _w_from_err(erro_lat_m, w_min=pesos.get("lateral_min", 1.8), w_max=pesos.get("lateral_max", 8.5), deadband=self._lateral_deadband_m, tol=0.7, power=1.3, smooth=True)
if not dentro:
# Se saiu da faixa, recuperar a centerline fica MAIS importante,
# nunca menos. A versão anterior zerava este custo.
peso_lateral *= 1.20
# Penalidade proporcional à velocidade
penalidade_por_velocidade = (velocidade / self.velocidade_max) / 10.0
@ -1255,14 +1563,25 @@ class ControladorMPC:
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 ]:
if tracking_reta:
# Na passada Arco é indesejado. Diagonal é o modo de
# recentralização quando heading está alinhado; dianteira
# recupera heading quando ele sai da janela estreita.
custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 3.0
if erro_ori_abs <= self._diagonal_saida_heading_graus:
custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.0
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 0.25
else:
custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 3.0
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 0.0
elif 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_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
custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 1.5
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 0.0
custo_movimento[TipoMovimentoDirecional.MovimentoDiagonal] += 0.5
return (peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, custo_movimento)
except Exception as e:
@ -1769,7 +2088,11 @@ class ControladorMPC:
#candidatos_ativos = self._selecionar_melhores(candidatos_ativos, N=N_top)
beam_topN_max = int(getattr(self, "_beam_topN_max", 5))
beam_topN_min = int(getattr(self, "_beam_topN_min", 2))
candidatos_ativos = self._filtrar_por_margem_angular(novos_candidatos, margem_graus=2.0)
candidatos_ativos = self._filtrar_por_margem_angular(
novos_candidatos,
margem_graus=2.0,
por_tipo=True,
)
# número alvo de trajetórias ativas
#N_alvo = min(beam_topN_max, max(beam_topN_min, beam_traj_min))
beam_traj_min = max(1, min(beam_traj_min, beam_topN_max)) # sanity
@ -1814,15 +2137,24 @@ class ControladorMPC:
status_carro,
comando_anterior,
self.tempo_execucao_local,
tipo_movimento=tipo_final,
)
ponto_alvo_primario = ponto_alvo_real["xy"]
erro_lateral, _, _, _, theta_path, _ = self.cross_track_error_point(
x,
y,
idx_referencia=idx_alvo_correcao,
ref_saida = self._referencia_caminho_lookahead(
x, y, idx_alvo_correcao, velocidade
)
erro_lateral = float(ref_saida["e_lat"])
erro_heading_saida = self._erro_heading_caminho(theta, ref_saida["theta_ref"])
tracking_reta_saida = self._tracking_reta_permitido(
contexto, idx_alvo_correcao, erro_lateral, erro_heading_saida
)
if tracking_reta_saida:
erro_orientacao = abs(float(np.degrees(erro_heading_saida)))
else:
_, erro_orientacao = self._calcula_erro_orientacao(
x, y, theta, ponto_alvo_primario, ref_saida["theta_local"], 0.5
)
_, erro_orientacao = self._calcula_erro_orientacao(x, y, theta, ponto_alvo_primario, theta_path, 0.5)
_cmd = {
"enviar_comando": True,
@ -1837,6 +2169,11 @@ class ControladorMPC:
"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)),
"tracking_reta": bool(tracking_reta_saida),
"heading_caminho_lookahead_graus": float(np.degrees(ref_saida["theta_ref"])),
"heading_caminho_local_graus": float(np.degrees(ref_saida["theta_local"])),
"heading_lookahead_m": float(ref_saida["lookahead_heading_m"]),
"erro_heading_caminho_graus": abs(float(np.degrees(erro_heading_saida))),
"debug_custo": debug_custo,
"candidatos_testados": K,
"motivos": []
@ -1946,61 +2283,96 @@ class ControladorMPC:
"""
try:
now = time.perf_counter
CARRO = _as_dict(contexto.get("Carro", {}))
status = CARRO.get("Status", StatusCarroMapa.Parado.value)
velocidade = max(0.0, _safe_float(CARRO.get("Velocidade", 0.0), 0.0))
idx_base = _safe_int(ponto_alvo.get("_idx_base", 0), 0)
# 1) Ângulo ideal (wrap [-pi,pi] e clamp ao máx)
orient = self.gps_handler.calcular_orientacao(ponto_alvo["xy"], ponto_atual)
delta_theta = (orient - ponto_atual[2] + np.pi) % (2*np.pi) - np.pi
#delta_theta, theta_ref, dbg = self.delta_por_dist_esq_dir(
# estado_atual=ponto_atual,
# ponto_alvo_xy=ponto_alvo["xy"],
# orient_corredor=np.radians(contexto["Carro"]["AnguloCaminho"]), # em rad
# dist_esq=contexto["Carro"]["DistanciaEsquerda"],
# dist_dir=contexto["Carro"]["DistanciaDireita"],
# angulo_max_graus=self.angulo_max_graus,
# sigma_factor=0.6,
# w_min=0.15,
# usar_nudge_centro=True,
# k_e=1.0, v=contexto["Carro"].get("Velocidade", 0.0), v0=0.3
#)
ang_max_rad = np.radians(self.angulo_max_graus)
angulo_ideal_rad = float(np.clip(delta_theta, -ang_max_rad, ang_max_rad))
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],
idx_referencia=ponto_alvo.get("_idx_base"),
# --------------------------------------------------------------
# RETA: heading do CAMINHO + cross-track, sem mirar ponto.
# MANOBRA: mantém pure-pursuit para contornar cabeceira/ligação.
# --------------------------------------------------------------
ref_path = self._referencia_caminho_lookahead(
ponto_atual[0], ponto_atual[1], idx_base, velocidade
)
(_, e_ori) = self._calcula_erro_orientacao(ponto_atual[0], ponto_atual[1], ponto_atual[2], ponto_alvo["xy"], theta_path, 0.5)
e_lat = float(ref_path["e_lat"])
theta_ref = float(ref_path["theta_ref"])
erro_heading = self._erro_heading_caminho(ponto_atual[2], theta_ref)
e_ori = abs(math.degrees(erro_heading))
# 2) Tipos válidos conforme contexto
CARRO = contexto.get("Carro", {})
status = CARRO.get("Status", 0)
if _status_in(status, [StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando]):
tipos_validos = [TipoMovimentoDirecional.MovimentoArco]
elif abs(e_ori) > 15.0:
tipos_validos = [TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.MovimentoArco]
tracking_reta = self._tracking_reta_permitido(
contexto, idx_base, e_lat, erro_heading
)
if tracking_reta:
tipo_ref, angulo_ideal_rad = self._referencia_direcional_reta(
e_lat,
erro_heading,
velocidade,
tipo_preferido=tipo_preferido,
)
tipos_validos = [tipo_ref]
else:
# 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
orient = self.gps_handler.calcular_orientacao(ponto_alvo["xy"], ponto_atual)
delta_theta = self._wrap_pi(orient - ponto_atual[2])
angulo_ideal_rad = float(np.clip(
delta_theta,
-np.radians(self.angulo_max_graus),
np.radians(self.angulo_max_graus),
))
if manter_diagonal or entrar_diagonal:
tipos_validos = [TipoMovimentoDirecional.MovimentoDiagonal]
tipo_preferido_enum = _movimento_from_value(tipo_preferido)
erro_heading_alvo_deg = abs(float(math.degrees(delta_theta)))
if _status_in(
status,
[
StatusCarroMapa.EntrandoRua,
StatusCarroMapa.SaindoRua,
StatusCarroMapa.Manobrando,
],
):
# Manobras estruturais continuam com Arco obrigatório.
tipos_validos = [TipoMovimentoDirecional.MovimentoArco]
elif _status_in(status, [StatusCarroMapa.Direcionando]):
# Fora da rua, um heading muito errado deve ser corrigido
# com Arco, que possui raio de giro menor. Quando o rover
# já estiver alinhado, RodasDianteiras finaliza de forma
# mais suave. A faixa 8°..15° é a histerese: se já entrou
# em Arco, permanece nele até cruzar o limiar de saída.
manter_arco = (
tipo_preferido_enum == TipoMovimentoDirecional.MovimentoArco
and erro_heading_alvo_deg > self._direcionando_arco_saida_graus
)
if (
erro_heading_alvo_deg >= self._direcionando_arco_entrada_graus
or manter_arco
):
tipos_validos = [TipoMovimentoDirecional.MovimentoArco]
else:
tipos_validos = [TipoMovimentoDirecional.RodasDianteiras]
else:
tipos_validos = [TipoMovimentoDirecional.RodasDianteiras]
# Para ranking/telemetria de manobra, preserva erro histórico.
_, _, _, _, theta_local, _ = self.cross_track_error_point(
ponto_atual[0], ponto_atual[1], idx_referencia=idx_base
)
_, e_ori = self._calcula_erro_orientacao(
ponto_atual[0], ponto_atual[1], ponto_atual[2],
ponto_alvo["xy"], theta_local, 0.5, usar_apenas_caminho=False
)
angulo_ideal_rad = float(np.clip(
angulo_ideal_rad,
-np.radians(self.angulo_max_graus),
np.radians(self.angulo_max_graus),
))
angulo_ideal_deg = float(np.degrees(angulo_ideal_rad))
# 3) Flags de matriz
if dados_costmap is None:
dados_costmap = self._get_costmap_direcional(contexto)
@ -2008,6 +2380,11 @@ class ControladorMPC:
# 4) Geração bruta (graus)
angulos_raw = self._gerar_candidatos_brutos(angulo_ideal_deg, filtrar_matriz)
if tipos_validos == [TipoMovimentoDirecional.MovimentoDiagonal]:
lim_diag = float(min(self._angulo_diagonal_max_graus, self.angulo_max_graus))
angulos_raw = [a for a in angulos_raw if abs(float(a)) <= lim_diag + 1e-9]
if not angulos_raw:
angulos_raw = [float(np.clip(angulo_ideal_deg, -lim_diag, lim_diag))]
if not angulos_raw:
# fallback mínimo em torno do ideal
base = [angulo_ideal_deg + d for d in (-2.0, -1.0, 0.0, 1.0, 2.0)]
@ -2449,21 +2826,25 @@ class ControladorMPC:
omega = float(self.gps_handler.calcular_omega(v, angulo, tipo))
k = np.arange(1, n + 1, dtype=np.float32)
if abs(omega) < 1e-6:
# reta: mesmo esquema do seu _nova_posicao
dx = dist_step * k * math.sin(th0)
dy = dist_step * k * math.cos(th0)
if tipo in [TipoMovimentoDirecional.MovimentoDiagonal, TipoMovimentoDirecional.MovimentoLateral]:
# 4WS em fase: heading do corpo permanece, mas a velocidade
# translacional aponta para theta + angulo.
direcao = th0 + float(angulo)
dx = dist_step * k * math.sin(direcao)
dy = dist_step * k * math.cos(direcao)
th = th0 + np.zeros_like(k, dtype=np.float32)
x = x0 + dx
y = y0 + dy
else:
R = v / omega
# Mesma integração semi-implícita usada por _nova_posicao e
# pelo simulador C#: primeiro atualiza theta, depois translada.
# O cumsum reproduz exatamente a aplicação passo a passo.
dth = omega * dt
th = th0 + dth * k
s0, c0 = math.sin(th0), math.cos(th0)
# solução fechada consistente com x+=sin(θ)*d, y+=cos(θ)*d
x = x0 + R * (c0 - np.cos(th))
y = y0 + R * (np.sin(th) - s0)
dx_step = dist_step * np.sin(th)
dy_step = dist_step * np.cos(th)
x = x0 + np.cumsum(dx_step)
y = y0 + np.cumsum(dy_step)
return list(zip(x.astype(float).tolist(),
y.astype(float).tolist(),
@ -2841,9 +3222,16 @@ class ControladorMPC:
sub_dt = dt_total / n_subs
dist_sub = v_planejado * sub_dt # custo por metro
# --- PATCH: estado local p/ amortecimento e anti-chatter
# Estado inicial real do candidato para derivada/anti-chatter.
try:
e_lat_inicial, _, _, _, _, _ = self.cross_track_error_point(
x_sim, y_sim, idx_referencia=self._proximo_nao_visitado(pontos_visitados)
)
e_lat_prev_local = float(e_lat_inicial)
side_mem_local = 0 if abs(e_lat_inicial) < max(0.08, self._lateral_deadband_m * 2.0) else (1 if e_lat_inicial > 0 else -1)
except Exception:
e_lat_prev_local = None
side_mem_local = 0 # -1 direita, +1 esquerda, 0 centro
side_mem_local = 0
for n_sub in range(n_subs):
# IMPORTANTE: _nova_posicao precisa aceitar dt opcional
@ -2875,89 +3263,102 @@ class ControladorMPC:
# --------------------------
# ERROS / CUSTOS
# --------------------------
# posição (mantido)
(d_norm, erro_pos) = self._calcula_erro_posicao(x_sim, y_sim, ponto_alvo_sim)
# --- 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,
idx_referencia=idx_alvo_sim,
ref_path_sim = self._referencia_caminho_lookahead(
x_sim, y_sim, idx_alvo_sim, v_planejado
)
e_lat = float(ref_path_sim["e_lat"])
theta_path = float(ref_path_sim["theta_ref"])
margem = float(ref_path_sim["margem"])
erro_heading_sim = self._erro_heading_caminho(theta_sim, theta_path)
tracking_reta_sim = self._tracking_reta_permitido(
contexto, idx_alvo_sim, e_lat, erro_heading_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)
if tracking_reta_sim:
# Passada: NÃO existe atração para ponto. O custo é path
# tracking puro: heading da centerline + cross-track.
erro_pos = abs(e_lat)
d_norm = 0.0
erro_ori_sim = abs(float(np.degrees(erro_heading_sim)))
o_norm = abs(float(erro_heading_sim)) / np.pi
else:
(d_norm, erro_pos) = self._calcula_erro_posicao(
x_sim, y_sim, ponto_alvo_sim
)
(o_norm, erro_ori_sim) = self._calcula_erro_orientacao(
x_sim, y_sim, theta_sim,
ponto_alvo_sim, ref_path_sim["theta_local"], 0.5,
usar_apenas_caminho=False,
)
# pesos dinâmicos (mantido)
pesos = self._calcular_pesos_movimento(contexto, erro_ori_sim, e_lat)
(peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_lateral, peso_movimento) = pesos
pesos = self._calcular_pesos_movimento(
contexto, erro_ori_sim, e_lat, tracking_reta=tracking_reta_sim
)
(peso_erro_pos, peso_erro_ori, peso_suavidade,
peso_fator_re, peso_ideal, peso_lateral,
peso_movimento) = pesos
# â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)
# 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)
delta_angulo = abs(self._wrap_pi(float(angulo_testado) - float(prev_ang)))
# --------------------------
# COMPONENTES DE CUSTO
# --------------------------
# posição (mantido)
#custo_pos = erro_pos * peso_erro_pos
custo_pos = d_norm * peso_erro_pos
# --- PATCH B: custo de orientação desacoplado de erro_pos e quadrático
#custo_ori = erro_ori_sim * peso_erro_ori
custo_pos = 0.0 if tracking_reta_sim else d_norm * peso_erro_pos
custo_ori = o_norm * peso_erro_ori
# --- PATCH C: suavidade SEM depender de erro_pos + piso
base_suav = 0.25 # piso de penalização (0.25–0.5)
erro_suavidade = ((base_suav + 1.0) * (abs(delta_angulo) / np.pi) * erro_pos)
# Suavidade realmente independente do erro de posição.
ang_norm = delta_angulo / max(np.radians(self.angulo_max_graus), 1e-6)
erro_suavidade = float(ang_norm * ang_norm)
custo_suavidade = erro_suavidade * peso_suavidade
# tipo de movimento (mantido)
custo_tipo_movimento = float(peso_movimento[tipo])
if prev_tipo != tipo:
custo_tipo_movimento += float(self._penalidade_troca_tipo)
# fator "re" (mantido como estava)
erro_re = -cos_delta # já está em [-1, +1]
custo_re = erro_re * peso_fator_re # aplica peso normalmente
if tracking_reta_sim:
# Pequena recompensa por manter o corpo paralelo à linha;
# não há vetor carro->ponto neste termo.
cos_delta = math.cos(float(erro_heading_sim))
else:
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.sin(theta_sim), np.cos(theta_sim)])
cos_delta = float(np.dot(vetor_movel, vetor_alvo_norm))
erro_re = -float(cos_delta)
custo_re = erro_re * peso_fator_re
# --- PATCH D: custo lateral com deadband + quadrático + normalização
dead = 0.0 # 5–12 cm
half = max(float(margem) / 2.0, 0.5) # meia-largura mínima
# Cross-track sempre ativo, inclusive fora do corredor.
dead = self._lateral_deadband_m
half = max(float(margem) / 2.0, 0.5)
e_eff = max(0.0, abs(e_lat) - dead)
r = e_eff / half
if r <= 1.0:
alpha = 0.25 # 0 = só quadrático; 1 = só linear
alpha = 0.25
l_norm = alpha*r + (1-alpha)*r**2
else:
gamma = 1.0
l_norm = 1.0 + gamma*(r - 1.0) # ex.: gamma = 1.0 (ajuste)
l_norm = 1.0 + (r - 1.0)
custo_lateral = l_norm * peso_lateral
# --- PATCH E: amortecimento por derivada do erro lateral (m/s)
# Derivada funciona mesmo com n_subporpasso=1 porque o estado
# inicial do candidato é usado como memória.
if e_lat_prev_local is not None:
de = (e_lat - e_lat_prev_local) / max(sub_dt, 1e-6) # m/s
peso_lat_der = 0.1 * float(peso_lateral) # ganho pequeno
de = (e_lat - e_lat_prev_local) / max(sub_dt, 1e-6)
peso_lat_der = 0.08 * float(peso_lateral)
custo_lat_der = (de ** 2) * peso_lat_der
dlat = (abs(e_lat) - abs(e_lat_prev_local)) / max(sub_dt, 1e-6)
pen_away = max(0.0, dlat/half) * (0.2 * peso_lateral)
pen_away = max(0.0, dlat / half) * (0.15 * peso_lateral)
else:
custo_lat_der = 0.0
pen_away = 0.0
custo_lateral += pen_away
e_lat_prev_local = e_lat
# --- PATCH F: anti-chatter (histerese) perto do centro
band = 0.10 # 8–12 cm
band = max(0.08, self._lateral_deadband_m * 2.0)
side_now = 0 if abs(e_lat) < band else (1 if e_lat > 0 else -1)
flip_pen = 0.0
if abs(e_lat) < band and side_mem_local != 0 and side_now != side_mem_local:
flip_pen = 0.12 * float(peso_suavidade) # 0.08–0.20
if side_mem_local != 0 and side_now != 0 and side_now != side_mem_local:
flip_pen = 0.12 * float(peso_suavidade)
if side_now != 0:
side_mem_local = side_now
# soma local (heurística do visual + demais) e NORMALIZA por metro

795
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/camera_manager.py
View File

File diff suppressed because it is too large Load Diff

184
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/config.py
View File

@ -50,7 +50,7 @@ _CONFIG_LOCK = threading.Lock()
# ============================================================
# CONFIG V1 - VISUAL WORKER
# CONFIG V2 - VISUAL WORKER / ONNX FIELD CONTRACT
# ============================================================
@ -117,12 +117,39 @@ VISUAL_DEFAULT_CONFIG = {
"debug_visual": False,
"posproc_intervalo_min_s": 5.0,
# Publica logs de performance no console.
"debug_perf": False,
# Tamanho dos frames de preview/debug enviados ao C#.
"preview_size": (1280, 720),
# Preview producer lateral: só trabalha quando há consumidor.
"preview_fps": 2.0,
"preview_request_timeout_s": 0.35,
"preview_cache_fresh_s": 0.75,
# Replay/debug usa JPEG leve. PosProcessamento continua em PNG/RGB original.
"replay_jpeg_quality": 70,
# Telemetria técnica. Tudo que custa payload/log extra fica atrás deste bloco.
"telemetry": {
# Publica performance_visual no Redis.
"enabled": True,
# Imprime resumo periódico no console.
"console_performance": True,
# Abre o timing da inferência em crop/resize/cor/session/decode.
"runner_detailed_timing": True,
# Inclui o vetor completo de 8 probabilidades do status no debug.
# Desligado por padrão porque top1/top2/margin/entropy já bastam no campo.
"include_status_probs": False,
# Anexa contrato/runtime do ONNX em performance_visual.
"publish_runtime_info": True,
# Loga contrato/provider/cache na inicialização.
"log_runtime_startup": True,
},
# ============================================================
# 2) Frequências dos loops
@ -144,100 +171,81 @@ VISUAL_DEFAULT_CONFIG = {
# ============================================================
# 3) Modelo ONNX/TensorRT - segmentação de ruas/corredor
# 3) Runtime oficial do modelo de corredor - ONNX FIELD V2
# ============================================================
# Runtime oficial da v1.
"runtime_backend": "onnx",
"onnx_provider": "tensorrt",
# O caminho do ONNX vem do Redis/equipamento: path_ia_model_ruas_seg.
# Resolução, ROI, mean/std, layout, nomes de I/O e classes NÃO ficam aqui.
# Tudo isso é lido da metadata embutida no próprio ONNX.
"model_runtime": {
"provider": "tensorrt",
# Resolução esperada pelo ONNX: [W, H].
"ia_resolution": [1024, 576],
# Se True: TensorRT -> CUDA -> CPU, sempre expondo degradação na telemetria.
# Se False: falha a inicialização quando o provider solicitado não estiver disponível.
"allow_provider_fallback": True,
# ROI vertical da segmentação.
# 0.0 + 1.0 = frame inteiro.
"ia_roi_begin": 0.0,
"ia_roi_size": 1.0,
# CameraOak configura ColorCamera.ColorOrder.RGB. O ONNX também exige RGB,
# então o hot path evita conversão de cor. Se a fonte mudar no futuro,
# BGR continua suportado explicitamente pelo runner.
"camera_frame_color": "RGB",
# Contrato validado do modelo modelseg-2_0.onnx:
# entrada: float32 [1, 3, 576, 1024], RGB 0..1, NCHW
# saída 1: semantic_logits
# saída 2: label_probs
"onnx_has_preprocess": False,
"onnx_input_layout": "nchw_float",
"onnx_input_scale": 1.0 / 255.0,
"onnx_seg_output_format": "logits",
# Nomes das entradas/saídas.
# input_name None deixa o runner detectar automaticamente.
"onnx_input_name": None,
"onnx_seg_output_name": "semantic_logits",
"onnx_aux_output_name": "label_probs",
"onnx_aux_output_format": "probs",
# Auxiliar do status de corredor.
# True = retorna label_id, label_name, label_conf.
# False = inclui também vetor de probabilidades.
"use_compact_aux": True,
# Logs internos do runner ONNX.
"debug_timing": False,
"debug_session": True,
# ============================================================
# 4) TensorRT
# ============================================================
# TensorRT. O cache real vira <root>/<sha-do-onnx>/ automaticamente.
"trt_fp16_enable": True,
"trt_engine_cache_enable": True,
"trt_engine_cache_path": "./trt_cache_visual_worker",
# Deixe None salvo se não quiser fixar workspace.
"trt_engine_cache_root": "./trt_cache_visual_worker",
"trt_max_workspace_size": None,
# Threads do ONNX Runtime.
# Para TensorRT/CUDA, 1 costuma ser suficiente e evita ruído.
"onnx_intra_op_num_threads": 1,
"onnx_inter_op_num_threads": 1,
# ONNX Runtime.
"ort_intra_op_num_threads": 1,
"ort_inter_op_num_threads": 1,
# Se existir .contract.json, valida o SHA256 do ONNX no startup.
"verify_sidecar_sha256": True,
# Checagem de IDs únicos a cada inferência. Útil em laboratório, desnecessária no campo.
"validate_outputs_each_inference": False,
},
# ============================================================
# 5) SegmentacaoManager v1
# 4) Análise geométrica + resolução temporal do status
# ============================================================
# Este bloco controla apenas a análise da máscara:
# pred_ids + label_probs -> dados_visuais.
# IDs semânticos e nomes de status vêm do contrato ONNX.
"segmentacao": {
# Inclui debug textual no payload de segmentação.
# Não gera imagem.
"include_debug": False,
# Inclui timing interno do SegmentacaoManager.
"debug_timing": False,
# ID das classes no modelo de segmentação.
"id_nao_navegavel": 0,
"id_navegavel": 1,
# Frações verticais usadas para estimar centro/ângulo do corredor.
# 0.0 = topo, 1.0 = base.
# Frações verticais para centro/largura/ângulo. 0=topo, 1=base.
"scanline_fracs": (0.96, 0.86, 0.74, 0.62, 0.50, 0.38, 0.26),
# A região próxima ao robô está na base da imagem.
"near_is_bottom": True,
# Suavização das saídas usadas pelo controle.
# Suavização geométrica.
"ema_alpha_ang": 0.25,
"ema_alpha_lat": 0.25,
"ema_alpha_conf": 0.20,
# Histerese temporal do status do corredor.
"status_window_s": 1.5,
# Status v2: voto temporal ponderado + troca rápida quando a cabeça está muito segura.
"status_window_s": 0.90,
"status_decay_tau_s": 0.35,
"status_expected_fps": 10.0,
# Confiança da cabeça auxiliar ONNX.
# >= accept: modelo manda.
# >= soft: modelo ajuda quando heurística está indefinida.
"model_conf_accept": 0.70,
"model_conf_soft": 0.45,
# Modelo manda quando confiança E margem são boas.
"model_conf_accept": 0.72,
"model_margin_accept": 0.15,
# Zona intermediária: só ajuda quando concorda com heurística ou ela está indefinida.
"model_conf_soft": 0.55,
"model_margin_soft": 0.06,
# Transições muito fortes podem furar a inércia após N frames consecutivos.
"model_conf_fast": 0.90,
"model_margin_fast": 0.30,
"fast_switch_min_frames": 2,
# Pesos base do voto temporal.
"status_weight_model": 1.35,
"status_weight_model_soft": 1.00,
"status_weight_heuristic": 0.75,
"status_weight_agreement_bonus": 0.40,
# Grid leve interna para centro/fallback do corredor.
"corridor_grid_rows": 6,
@ -250,7 +258,7 @@ VISUAL_DEFAULT_CONFIG = {
"prefer_prev_weight": 0.65,
"prefer_width_weight": 1.00,
# Heurística de status quando o modelo auxiliar não está confiante.
# Heurística fica como fallback/segundo sensor até termos evidência de campo para removê-la.
"thr_parado_global": 0.30,
"thr_direcionando_global": 0.82,
"thr_caminhando_score": 0.48,
@ -260,7 +268,7 @@ VISUAL_DEFAULT_CONFIG = {
# ============================================================
# 6) Grid de confiança/custo
# 5) Grid de confiança/custo
# ============================================================
"grid": {
# Formato global da grid: (cols, rows).
@ -408,7 +416,7 @@ VISUAL_DEFAULT_CONFIG = {
# ============================================================
# 7) GPU Priority Controller
# 6) GPU Priority Controller
# ============================================================
# Controla FPS do Visual Worker conforme saúde do Weed Worker.
"gpu_priority": {
@ -510,24 +518,16 @@ DET_DEFAULT_CONFIG = {
def aplicar_overrides_redis_seg(cfg: dict) -> dict:
equipamento = ContextoGlobalRedis.get_equipamento()
# Modelo ONNX oficial de ruas/corredor.
cfg["ia_onnx_path"] = equipamento.get("path_ia_model_ruas_seg")
# Labelmap da segmentação.
cfg["ia_labelmap_path"] = equipamento.get("path_ia_labelmap_ruas_seg")
# Único artefato de IA exigido pelo Visual Worker v2.
# Classes, normalização, ROI e I/O vivem dentro do contrato do ONNX.
cfg["model_path"] = equipamento.get("path_ia_model_ruas_seg")
return cfg
def normalizar_config_runtime_seg(cfg: dict) -> dict:
# Alias único interno, para logs e validações.
cfg["onnx_model_path"] = cfg.get("ia_onnx_path")
if not cfg.get("ia_onnx_path"):
mostrar_log("[WARN] path do modelo ONNX de ruas não definido no Redis/equipamento.")
if not cfg.get("ia_labelmap_path"):
mostrar_log("[WARN] path do labelmap de ruas não definido no Redis/equipamento.")
if not cfg.get("model_path"):
mostrar_log("[WARN] path do ONNX de corredor não definido no Redis/equipamento.")
# Garante tuplas onde as dataclasses esperam tuplas.
cfg["preview_size"] = tuple(cfg.get("preview_size", (1280, 720)))
@ -556,10 +556,6 @@ def normalizar_config_runtime_seg(cfg: dict) -> dict:
if "veto_labels" in det and not isinstance(det["veto_labels"], set):
det["veto_labels"] = set(det["veto_labels"])
fuser = grid.get("fuser", {}) or {}
if "y_range_m" in fuser:
fuser["y_range_m"] = tuple(fuser["y_range_m"])
cfg["grid"] = grid
segmentacao = cfg.get("segmentacao", {}) or {}
@ -615,6 +611,8 @@ def load_seg_config():
# Cópia profunda simples dos blocos aninhados.
# Evita compartilhar dict interno entre chamadas.
cfg["telemetry"] = dict(VISUAL_DEFAULT_CONFIG["telemetry"])
cfg["model_runtime"] = dict(VISUAL_DEFAULT_CONFIG["model_runtime"])
cfg["segmentacao"] = dict(VISUAL_DEFAULT_CONFIG["segmentacao"])
cfg["grid"] = {
"grid_shape": VISUAL_DEFAULT_CONFIG["grid"]["grid_shape"],

23
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/main.py
View File

@ -20,8 +20,13 @@ def main():
# Importar e configurar OpenCV antes dos módulos do Visual Worker,
# pois eles podem carregar OpenCV internamente.
import cv2
cv2.setNumThreads(2)
opencv_threads = max(1, int(os.environ.get("VISUAL_OPENCV_THREADS", "4")))
cv2.setNumThreads(opencv_threads)
cv2.ocl.setUseOpenCL(False)
print(
f"[visual][PERF] OpenCV threads requested={opencv_threads} "
f"effective={cv2.getNumThreads()} preview_cache=True"
)
from shared.enums import VisualWorkerCommandType, TipoFrameCamera
from shared.utils import encode_image_base64
@ -53,6 +58,7 @@ def main():
time.sleep(0.1)
def redis_callback(dados):
acao = None
try:
acao = VisualWorkerCommandType(dados.get("cmd", 0))
@ -60,14 +66,14 @@ def main():
mx_id = dados.get("params")
if mx_id is not None:
iniciar_camera_manager(mx_id)
elif acao == VisualWorkerCommandType.CalibrarProfundidade:
n_frames = dados.get("params", 50)
get_camera_manager().gerar_grid_ref(n_frames)
elif acao == VisualWorkerCommandType.AtualizarSaudeCamera:
get_camera_manager().atualizar_saude_camera()
elif acao == VisualWorkerCommandType.GetCameraFrame:
tipo = TipoFrameCamera(dados.get("params", TipoFrameCamera.Rgb.value))
frame = get_camera_manager().get_selected_frame(tipo)
frame = get_camera_manager().get_selected_frame(
tipo,
aguardar_atualizacao=True,
)
if frame is not None:
base64_img = encode_image_base64(frame)
if base64_img is not None:
@ -82,14 +88,11 @@ def main():
pasta = dados.get("params", {}).get("caminho", "frames_salvos")
tipos = dados.get("params", {}).get("tipos", [])
get_camera_manager().salvar_frames(tipos, nome, pasta)
elif acao == VisualWorkerCommandType.EnviarImagemMock:
caminho = dados.get("params", "")
get_camera_manager().segmentacao_manager.use_mock = caminho != ""
get_camera_manager().segmentacao_manager.img_mock = caminho
else:
mostrar_log(f"⚠️ Comando desconhecido: {acao.name}")
except Exception as e:
mostrar_log(f"Erro ao processar comando {acao.name}: {e}")
acao_nome = getattr(acao, "name", str(dados.get("cmd", "desconhecido")))
mostrar_log(f"Erro ao processar comando {acao_nome}: {e}")
def inicializar():
mostrar_log("🚀 Iniciando Visual Worker...")

267
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/segmentacao_semantica.py
View File

@ -2,7 +2,6 @@ from __future__ import annotations
import time
from dataclasses import dataclass, field
from enum import IntEnum
from collections import deque
from typing import Any, Dict, List, Optional, Sequence, Tuple
@ -12,23 +11,19 @@ import numpy as np
from shared.enums import StatusCarroMapa
class ClassesSegmentacao(IntEnum):
NAONAVEGAVEL = 0
NAVEGAVEL = 1
@dataclass
class SegmentacaoConfig:
"""
Configuração v1 do analisador semântico do Visual Worker.
Configuração v2 do analisador semântico do Visual Worker.
Esta classe controla apenas análise de máscara.
Não controla ONNX, câmera, render, preview, stream nem Redis.
"""
# IDs esperados na máscara de segmentação.
id_nao_navegavel: int = int(ClassesSegmentacao.NAONAVEGAVEL)
id_navegavel: int = int(ClassesSegmentacao.NAVEGAVEL)
# IDs são OBRIGATORIAMENTE injetados do contrato ONNX v2.
# -1 faz o componente falhar fechado se alguém tentar inicializá-lo isolado.
id_nao_navegavel: int = -1
id_navegavel: int = -1
# Scanlines usadas para estimar centro, largura e ângulo do corredor.
# Frações verticais do frame: 0.0 = topo, 1.0 = base.
@ -42,13 +37,27 @@ class SegmentacaoConfig:
ema_alpha_lat: float = 0.25
ema_alpha_conf: float = 0.20
# Histórico para estabilizar status do corredor.
status_window_s: float = 1.5
# Histórico ponderado para estabilizar status do corredor.
status_window_s: float = 0.90
status_decay_tau_s: float = 0.35
status_expected_fps: float = 10.0
# Prioridade do status vindo da cabeça auxiliar ONNX.
model_conf_accept: float = 0.70
model_conf_soft: float = 0.45
# Cabeça de status ONNX v2: confiança + margem top1-top2.
model_conf_accept: float = 0.72
model_margin_accept: float = 0.15
model_conf_soft: float = 0.55
model_margin_soft: float = 0.06
# Troca rápida para transições muito seguras.
model_conf_fast: float = 0.90
model_margin_fast: float = 0.30
fast_switch_min_frames: int = 2
# Pesos do voto temporal.
status_weight_model: float = 1.35
status_weight_model_soft: float = 1.00
status_weight_heuristic: float = 0.75
status_weight_agreement_bonus: float = 0.40
# Grid leve usada para pontuar corredor e fallback de centro.
corridor_grid_rows: int = 6
@ -70,15 +79,16 @@ class SegmentacaoConfig:
# Debug leve: inclui métricas extras no retorno, sem criar imagens.
include_debug: bool = True
include_model_probs: bool = False
debug_timing: bool = False
class SegmentacaoManager:
"""
Analisador de segmentação semântica v1.
Analisador de segmentação semântica v2.
Responsabilidade única:
pred_ids + aux_result -> dados_visuais
pred_ids + status_result -> dados_visuais
Não renderiza.
Não colore máscara.
@ -94,6 +104,14 @@ class SegmentacaoManager:
):
self.config = self._normalizar_config(config)
if int(self.config.id_navegavel) < 0 or int(self.config.id_nao_navegavel) < 0:
raise RuntimeError(
"IDs semânticos não foram injetados pelo contrato ONNX v2: "
f"nav={self.config.id_navegavel} non_nav={self.config.id_nao_navegavel}"
)
if int(self.config.id_navegavel) == int(self.config.id_nao_navegavel):
raise RuntimeError("IDs navegável/não-navegável são iguais no contrato.")
# Mantidos como metadados úteis, mas não usados no caminho quente.
self.color_map = color_map
self.classes = classes
@ -105,6 +123,8 @@ class SegmentacaoManager:
self._status_hist = deque(maxlen=max_len)
self._status_final_hist = deque(maxlen=2)
self._strong_candidate: Optional[StatusCarroMapa] = None
self._strong_candidate_count: int = 0
self._ema_ang: Optional[float] = None
self._ema_lat: Optional[float] = None
@ -118,22 +138,22 @@ class SegmentacaoManager:
# API principal
# ---------------------------------------------------------------------
def segmentar(self, predictions: np.ndarray, aux_result: Optional[Dict[str, Any]] = None):
def segmentar(self, predictions: np.ndarray, status_result: Optional[Dict[str, Any]] = None):
"""
Compatibilidade nominal para o CameraManager atual.
Para a v1, o método preferido é analisar().
No contrato v2, o método preferido é analisar().
Retorna:
resultado, erro
"""
try:
resultado = self.analisar(predictions, aux_result=aux_result)
resultado = self.analisar(predictions, status_result=status_result)
return resultado, None
except Exception as e:
self._ultimo_erro = f"Erro na análise de segmentação: {e}"
return None, self._ultimo_erro
def analisar(self, predictions: np.ndarray, aux_result: Optional[Dict[str, Any]] = None) -> Dict[str, Any]:
def analisar(self, predictions: np.ndarray, status_result: Optional[Dict[str, Any]] = None) -> Dict[str, Any]:
t0 = time.perf_counter()
pred = self._validar_predictions(predictions)
@ -161,7 +181,7 @@ class SegmentacaoManager:
pred=pred,
mask_nav=mask_nav,
corridor_score=corridor_score,
aux_result=aux_result,
status_result=status_result,
)
t_status = time.perf_counter()
@ -182,10 +202,26 @@ class SegmentacaoManager:
"status_corredor_anterior": int(status_before.value),
"status_corredor_anterior_nome": str(status_before.name),
# Telemetria compacta da segunda cabeça. Não depende do debug pesado.
"status_fonte": str(status_debug.get("origem_status", "desconhecida")),
"status_modelo_id": (status_debug.get("modelo") or {}).get("label_id"),
"status_modelo_nome": (status_debug.get("modelo") or {}).get("status_nome"),
"status_modelo_conf": (status_debug.get("modelo") or {}).get("conf"),
"status_modelo_margin": (status_debug.get("modelo") or {}).get("margin"),
"status_modelo_entropy": (status_debug.get("modelo") or {}).get("entropy"),
"status_modelo_second_nome": (status_debug.get("modelo") or {}).get("second_name"),
"status_modelo_second_conf": (status_debug.get("modelo") or {}).get("second_conf"),
"status_fast_switch": bool(status_debug.get("fast_switch", False)),
"centros_corredor": centros,
"larguras_px": larguras_px,
}
if self.config.include_model_probs:
probs = (status_debug.get("modelo") or {}).get("probs")
if probs is not None:
dados_visuais["status_modelo_probs"] = probs
if self.config.include_debug:
dados_visuais["status_corredor_debug"] = status_debug
dados_visuais["debug_corredor"] = {
@ -670,29 +706,100 @@ class SegmentacaoManager:
pred: np.ndarray,
mask_nav: np.ndarray,
corridor_score: float,
aux_result: Optional[Dict[str, Any]],
status_result: Optional[Dict[str, Any]],
):
model_status, model_conf, model_debug = self._status_from_aux(aux_result)
model_status, model_conf, model_margin, model_debug = self._status_from_model(
status_result
)
heur_status, heur_debug = self._status_heuristico(pred, mask_nav, corridor_score)
heur_status, heur_debug = self._status_heuristico(
pred, mask_nav, corridor_score
)
origem = "heuristica"
status_now = heur_status
weight = float(self.config.status_weight_heuristic)
if model_status is not None and model_conf >= self.config.model_conf_accept:
model_fast = (
model_status is not None
and model_conf >= self.config.model_conf_fast
and model_margin >= self.config.model_margin_fast
)
model_accept = (
model_status is not None
and model_conf >= self.config.model_conf_accept
and model_margin >= self.config.model_margin_accept
)
model_soft = (
model_status is not None
and model_conf >= self.config.model_conf_soft
and model_margin >= self.config.model_margin_soft
)
if model_accept:
status_now = model_status
origem = "modelo"
elif model_status is not None and model_conf >= self.config.model_conf_soft:
# Modelo com média confiança só vence quando a heurística está indefinida.
origem = "modelo_fast" if model_fast else "modelo"
weight = float(self.config.status_weight_model)
# Confiança e margem entram suavemente no peso, sem explodir a janela.
weight *= 0.75 + 0.25 * float(np.clip(model_conf, 0.0, 1.0))
weight *= 0.85 + 0.15 * float(np.clip(model_margin, 0.0, 1.0))
if heur_status == model_status:
weight += float(self.config.status_weight_agreement_bonus)
origem += "_agree"
elif model_soft:
if heur_status == StatusCarroMapa.Indefinido:
status_now = model_status
origem = "modelo_soft"
weight = float(self.config.status_weight_model_soft)
elif heur_status == model_status:
status_now = model_status
origem = "modelo_soft_agree"
weight = (
float(self.config.status_weight_model_soft)
+ float(self.config.status_weight_agreement_bonus)
)
else:
status_now = heur_status
origem = "heuristica_com_modelo_soft"
origem = "heuristica_modelo_soft_discorda"
now = self._now()
self._status_hist.append((status_now, now, float(weight), origem))
# Transições realmente fortes não precisam aguardar toda a janela temporal.
fast_switch = False
if model_fast and model_status is not None:
if self._strong_candidate == model_status:
self._strong_candidate_count += 1
else:
self._strong_candidate = model_status
self._strong_candidate_count = 1
if self._strong_candidate_count >= max(1, int(self.config.fast_switch_min_frames)):
status_final = model_status
fast_switch = True
# Re-semeia a janela com o estado forte para não voltar no frame seguinte.
self._status_hist.clear()
self._status_hist.append((
status_final,
now,
float(self.config.status_weight_model)
+ float(self.config.status_weight_agreement_bonus),
"fast_switch_seed",
))
else:
status_final, weighted_scores = self._weighted_status()
else:
self._strong_candidate = None
self._strong_candidate_count = 0
status_final, weighted_scores = self._weighted_status()
if fast_switch:
weighted_scores = {status_final.name: 1.0}
self._status_hist.append((status_now, self._now()))
status_final = self._majority_status()
self._status_final_hist.append(status_final)
status_before = (
@ -705,34 +812,64 @@ class SegmentacaoManager:
"origem_status": origem,
"modelo": model_debug,
"heuristica": heur_debug,
"status_now": status_now.name,
"status_final": status_final.name,
"candidate_weight": round(float(weight), 4),
"weighted_scores": weighted_scores,
"fast_switch": bool(fast_switch),
"fast_candidate": (
self._strong_candidate.name
if self._strong_candidate is not None
else None
),
"fast_candidate_count": int(self._strong_candidate_count),
}
return status_now, status_final, status_before, debug
def _status_from_aux(self, aux_result: Optional[Dict[str, Any]]):
if not aux_result or aux_result.get("type") != "label":
return None, 0.0, {
def _status_from_model(self, status_result: Optional[Dict[str, Any]]):
if not status_result:
return None, 0.0, 0.0, {
"disponivel": False,
"motivo": "sem_aux_result",
"motivo": "sem_status_result",
}
try:
label_id = int(aux_result.get("label_id", StatusCarroMapa.Indefinido.value))
conf = float(aux_result.get("label_conf", 0.0))
status = StatusCarroMapa(label_id)
label_name = str(status_result.get("label_name", "")).strip()
conf = float(status_result.get("confidence", 0.0))
margin = float(status_result.get("margin", 0.0))
entropy = float(status_result.get("entropy", 1.0))
return status, conf, {
if not label_name:
raise ValueError("label_name vazio")
# Mapeamento oficial é por NOME. A ordem dos IDs do treino pode mudar
# sem embaralhar a enum do controle.
status = StatusCarroMapa[label_name]
debug = {
"disponivel": True,
"status": int(status.value),
"status_nome": str(status.name),
"label_name": aux_result.get("label_name"),
"label_id": int(status_result.get("label_id", -1)),
"label_name": label_name,
"conf": round(conf, 4),
"margin": round(margin, 4),
"entropy": round(entropy, 4),
"second_id": int(status_result.get("second_id", -1)),
"second_name": status_result.get("second_name"),
"second_conf": round(float(status_result.get("second_confidence", 0.0)), 4),
}
except Exception as e:
return None, 0.0, {
if "probs" in status_result:
debug["probs"] = list(status_result["probs"])
return status, conf, margin, debug
except Exception as exc:
return None, 0.0, 0.0, {
"disponivel": False,
"motivo": f"aux_invalido: {e}",
"motivo": f"status_modelo_invalido: {exc}",
}
def _status_heuristico(
@ -827,33 +964,39 @@ class SegmentacaoManager:
def _now() -> float:
return time.monotonic()
def _majority_status(self, janela_s: Optional[float] = None) -> StatusCarroMapa:
J = self.config.status_window_s if janela_s is None else float(janela_s)
def _weighted_status(self) -> Tuple[StatusCarroMapa, Dict[str, float]]:
window_s = max(0.05, float(self.config.status_window_s))
tau_s = max(0.05, float(self.config.status_decay_tau_s))
now = self._now()
while self._status_hist and (now - self._status_hist[0][1] > J):
while self._status_hist and (now - self._status_hist[0][1] > window_s):
self._status_hist.popleft()
if not self._status_hist:
return StatusCarroMapa.Direcionando
return StatusCarroMapa.Direcionando, {}
counts: Dict[StatusCarroMapa, int] = {}
scores: Dict[StatusCarroMapa, float] = {}
latest_ts: Dict[StatusCarroMapa, float] = {}
for status, _t in self._status_hist:
counts[status] = counts.get(status, 0) + 1
top = max(counts.values())
tied = [status for status, count in counts.items() if count == top]
for status, ts, base_weight, _source in self._status_hist:
age = max(0.0, now - float(ts))
decay = float(np.exp(-age / tau_s))
score = max(0.01, float(base_weight)) * decay
scores[status] = scores.get(status, 0.0) + score
latest_ts[status] = max(latest_ts.get(status, 0.0), float(ts))
top_score = max(scores.values())
tied = [s for s, score in scores.items() if abs(score - top_score) <= 1e-9]
if len(tied) == 1:
return tied[0]
winner = tied[0]
else:
winner = max(tied, key=lambda st: latest_ts.get(st, 0.0))
# Desempate: status mais recente.
for status, _t in reversed(self._status_hist):
if status in tied:
return status
return StatusCarroMapa.Direcionando
debug_scores = {
status.name: round(float(score), 5)
for status, score in sorted(scores.items(), key=lambda item: item[1], reverse=True)
}
return winner, debug_scores
# ---------------------------------------------------------------------
# Suavização
@ -879,6 +1022,8 @@ class SegmentacaoManager:
def reset_estado_temporal(self):
self._status_hist.clear()
self._status_final_hist.clear()
self._strong_candidate = None
self._strong_candidate_count = 0
self._ema_ang = None
self._ema_lat = None

17
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_debug_renderer.py
View File

@ -57,9 +57,12 @@ class VisualDebugRenderer:
return self.build_detection_overlay(rgb_frame, detections)
if frame_type == TipoFrameCamera.Debug:
overlay = self.build_overlay(rgb_frame, pred_ids, alpha=alpha)
# Debug prefere costmap. Só monta overlay como fallback, evitando
# construir duas imagens completas para descartar uma delas.
grid = self.build_costmap_debug(rgb_frame, snapshot)
return grid if grid is not None else overlay
if grid is not None:
return grid
return self.build_overlay(rgb_frame, pred_ids, alpha=alpha)
return None
@ -67,12 +70,12 @@ class VisualDebugRenderer:
if rgb_frame is None:
return None
img = rgb_frame.copy()
if rgb_frame.shape[1::-1] != self.preview_size:
# cv2.resize já cria um novo buffer; copiar antes seria tráfego de
# memória inútil no caminho de preview.
return cv2.resize(rgb_frame, self.preview_size, interpolation=cv2.INTER_AREA)
if img.shape[1::-1] != self.preview_size:
img = cv2.resize(img, self.preview_size, interpolation=cv2.INTER_AREA)
return img
return rgb_frame.copy()
def build_segmentation_preview(self, pred_ids):
if pred_ids is None or self.color_map is None:

18
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/visual_grid_builder.py
View File

@ -6,8 +6,6 @@ from typing import Any, Dict, Optional, Sequence, Tuple
import cv2
import numpy as np
from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao
@dataclass
class GridGeometryConfig:
@ -41,6 +39,10 @@ class DetectionGridConfig:
@dataclass
class GridConfidenceConfig:
# IDs vêm OBRIGATORIAMENTE do contrato ONNX e são injetados pelo CameraManager.
id_nao_navegavel: int = -1
id_navegavel: int = -1
valid_mm: Tuple[int, int] = (300, 10000)
min_valid_frac: float = 0.30
conf_params: Tuple[float, float] = (0.30, 0.80)
@ -144,6 +146,14 @@ class VisualGridBuilder:
def __init__(self, config: Optional[GridConfidenceConfig | Dict[str, Any]] = None):
self.config = self._normalizar_config(config)
if int(self.config.id_navegavel) < 0 or int(self.config.id_nao_navegavel) < 0:
raise RuntimeError(
"VisualGridBuilder exige IDs do contrato ONNX v2: "
f"nav={self.config.id_navegavel} non_nav={self.config.id_nao_navegavel}"
)
if int(self.config.id_navegavel) == int(self.config.id_nao_navegavel):
raise RuntimeError("Grid recebeu IDs navegável/não-navegável iguais.")
@staticmethod
def _normalizar_config(config):
if config is None:
@ -236,8 +246,8 @@ class VisualGridBuilder:
if n == 0:
continue
n_nav = np.count_nonzero(seg_block == ClassesSegmentacao.NAVEGAVEL.value)
n_naonav = np.count_nonzero(seg_block == ClassesSegmentacao.NAONAVEGAVEL.value)
n_nav = np.count_nonzero(seg_block == cfg.id_navegavel)
n_naonav = np.count_nonzero(seg_block == cfg.id_nao_navegavel)
pct_navegavel[j, i] = n_nav / n
pct_nao_navegavel[j, i] = n_naonav / n

133
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/utils.py
View File

@ -1,144 +1,31 @@
import cv2
import numpy as np
import cupy as cp
import math
from scipy import stats
from shared.enums import T_Code
def gerar_heatmap(depth_frame, dist_max_mm=3000.0):
mask_valid = depth_frame <= dist_max_mm
depth_normalized = np.zeros(depth_frame.shape, dtype=np.uint8)
depth_normalized[mask_valid] = (255 - ((depth_frame[mask_valid] / dist_max_mm) * 255)).astype(np.uint8)
depth_normalized[mask_valid] = (
255 - ((depth_frame[mask_valid] / dist_max_mm) * 255)
).astype(np.uint8)
heatmap = cv2.applyColorMap(depth_normalized, cv2.COLORMAP_JET)
heatmap[~mask_valid] = [0, 0, 0]
return heatmap
def calcular_threshold_anomalias(velocidade_ms=None, threshold_base=0.15, fator_sensibilidade=100):
"""
Calcula o threshold dinamicamente baseado na velocidade do robô.
- velocidade_ms: Velocidade em metros por segundo (m/s).
"""
try:
if velocidade_ms is None:
from shared.contexto_global_redis import ContextoGlobalRedis
velocidade_ms = ContextoGlobalRedis.get_contexto().get("Gerais", {}).get("velocidade_ms", 0.0)
threshold = threshold_base + (velocidade_ms * fator_sensibilidade)
return round(threshold, 3)
except Exception as e:
print(f"❌ Erro ao calcular thresold de anomalias: {e}")
return threshold_base
def obter_regiao_solo(depth_frame, percentual_altura):
depth_frame = cp.asarray(depth_frame) # 🔥 Garante que está na GPU
altura, largura = depth_frame.shape
linhas_solo = int((percentual_altura / 100) * altura)
y_inicio = altura - linhas_solo
depth_solo = depth_frame[y_inicio:altura, :] # 🔥 Slice direto na GPU
return depth_solo, y_inicio, linhas_solo, altura, largura
def calcular_referencia(frames, metodo="moda"):
stack = np.stack(frames)
if metodo == "moda":
ref = stats.mode(stack, axis=0, keepdims=True)[0][0]
elif metodo == "media":
ref = np.mean(stack, axis=0)
elif metodo == "mediana":
ref = np.median(stack, axis=0)
else:
raise ValueError("Método inválido. Use 'moda', 'media' ou 'mediana'.")
return ref
def calcular_inclinacao_solo(perfil, fov_h):
"""
Calcula o ângulo de inclinação do solo em graus.
:param perfil: Lista de profundidades dos setores [z1, z2, ..., zn]
:param largura_total_metros: Largura total em metros do campo de visão
:return: Ângulo de inclinação em graus
"""
if len(perfil) < 2:
return 0 # Não dá pra calcular
distancia_media = np.mean(perfil)
largura = 2 * math.tan(fov_h / 2) * distancia_media
delta_z = perfil[-1] - perfil[0]
delta_x = largura
angulo_rad = math.atan2(delta_z, delta_x)
angulo_graus = math.degrees(angulo_rad)
return round(angulo_graus, 3)
def calcular_profundidades_setores(depth_solo, n_setores, largura, distancia_max_mm):
depth_solo = cp.asarray(depth_solo) # 🔥 Garante que está na GPU
largura_setor = largura // n_setores
profundidades = []
for i in range(n_setores):
x_inicio = i * largura_setor
x_fim = (i + 1) * largura_setor if (i < n_setores - 1) else largura
setor = depth_solo[:, x_inicio:x_fim]
# 🔥 Filtra profundidade válida
setor_valido = setor[(setor > 0) & (setor < distancia_max_mm)]
z = cp.median(setor_valido).item() if setor_valido.size > 0 else 0 # 🔥 .item() p/ float
profundidades.append(z)
return cp.asnumpy(cp.array(profundidades)), largura_setor
def histogram2d_gpu(x, y, bins, range):
try:
if x.size == 0 or y.size == 0:
return cp.zeros(bins, dtype=cp.int32)
x_bins, y_bins = bins
(x_min, x_max), (y_min, y_max) = range
x_idx = cp.floor((x - x_min) / (x_max - x_min) * x_bins).astype(cp.int32)
y_idx = cp.floor((y - y_min) / (y_max - y_min) * y_bins).astype(cp.int32)
mask = (x_idx >= 0) & (x_idx < x_bins) & (y_idx >= 0) & (y_idx < y_bins)
x_idx = x_idx[mask]
y_idx = y_idx[mask]
if x_idx.size == 0:
return cp.zeros((x_bins, y_bins), dtype=cp.int32)
linear_idx = x_idx * y_bins + y_idx
if linear_idx.size == 0:
return cp.zeros((x_bins, y_bins), dtype=cp.int32)
hist_flat = cp.bincount(linear_idx, minlength=x_bins * y_bins)
hist = hist_flat.reshape((x_bins, y_bins))
return hist
except Exception as e:
print(f"Erro ao gerar histograma 2d: {e}")
return cp.zeros((x_bins, y_bins), dtype=cp.int32)
def converter_valores_numpy(obj):
if isinstance(obj, dict):
return {k: converter_valores_numpy(v) for k, v in obj.items()}
elif isinstance(obj, list):
if isinstance(obj, list):
return [converter_valores_numpy(v) for v in obj]
elif isinstance(obj, tuple):
if isinstance(obj, tuple):
return tuple(converter_valores_numpy(v) for v in obj)
elif isinstance(obj, (np.integer, np.int32, np.int64)):
if isinstance(obj, (np.integer, np.int32, np.int64)):
return int(obj)
elif isinstance(obj, (np.floating, np.float32, np.float64)):
if isinstance(obj, (np.floating, np.float32, np.float64)):
return float(obj)
elif isinstance(obj, (np.bool_)):
if isinstance(obj, np.bool_):
return bool(obj)
elif isinstance(obj, np.ndarray):
if isinstance(obj, np.ndarray):
return obj.tolist()
else:
return obj

4
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/weed_worker/config.py
View File

@ -170,8 +170,8 @@ WEED_DEFAULT_CONFIG = {
# 1) Observabilidade / salvamento
# ========================================================
"debug_visual": False,
"debug_perf": False,
"detector_debug_perf": False,
"debug_perf": True,
"detector_debug_perf": True,
# RAW científico para pós-processamento.
"posproc_intervalo_min_s": 5.0,

851
Python/OAK/datasets/oak-d/_0_capture_corridor.py Normal file
View File

@ -0,0 +1,851 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Capture OAK-D Lite - Agri Corridor Dataset
==========================================
Captura RGB para o dataset da camera frontal.
Contrato desta etapa:
- roda de dentro da pasta oak-d/
- captura o mesmo dominio de video usado no runtime:
OAK-D Lite -> ColorCamera 1080p -> cam.video -> host
- salva SOMENTE a imagem RGB original em PNG, sem resize e sem normalizacao
- destino padrao:
dataset/brutas/
- resize existe apenas para PREVIEW da interface e nunca toca a imagem salva
Teclas:
SPACE / S : salvar frame atual
A : ligar/desligar auto-save
E : alternar exposicao AUTO/MANUAL
+ / = : aumentar ISO no modo manual
- : diminuir ISO no modo manual
M : aumentar exposicao no modo manual
N : diminuir exposicao no modo manual
F : alternar foco AUTO/MANUAL
] : aumentar foco manual
[ : diminuir foco manual
V : alternar fullscreen
Q / ESC : sair
Exemplo:
python _0_capture.py
Auto-save a cada 1 segundo:
python _0_capture.py --auto --interval 1.0
Outra pasta:
python _0_capture.py --out dataset/brutas
"""
from __future__ import annotations
import argparse
import time
from concurrent.futures import ThreadPoolExecutor
from datetime import datetime
from pathlib import Path
from typing import Optional, Tuple
import cv2
import depthai as dai
import numpy as np
CAPTURE_W = 1920
CAPTURE_H = 1080
CAPTURE_FPS = 30.0
DEFAULT_OUT = Path("dataset") / "brutas"
DEFAULT_EXPOSURE_US = 6000
DEFAULT_ISO = 400
DEFAULT_FOCUS = 130
EXPOSURE_MIN_US = 100
EXPOSURE_MAX_US = 30000
EXPOSURE_STEP_US = 500
ISO_MIN = 100
ISO_MAX = 1600
ISO_STEP = 50
FOCUS_MIN = 0
FOCUS_MAX = 255
FOCUS_STEP = 5
WINDOW_NAME = "OAK-D Lite | Dataset Corredor Agricola"
CAPTURE_SESSION_ID = datetime.now().strftime("%Y%m%d_%H%M%S")
def timestamp_name() -> str:
"""
Nome novo:
img_sYYYYMMDD_HHMMSS__YYYYMMDD_HHMMSS_micro.png
O primeiro timestamp identifica a SESSAO de captura (uma execução do script).
O segundo identifica o frame.
Isso permite ao split manter uma mesma sessão inteira em train OU val.
"""
frame_ts = datetime.now().strftime("%Y%m%d_%H%M%S_%f")
return f"img_s{CAPTURE_SESSION_ID}__{frame_ts}"
def save_png(path: Path, frame_bgr: np.ndarray) -> Tuple[bool, str]:
path.parent.mkdir(parents=True, exist_ok=True)
ok = cv2.imwrite(
str(path),
frame_bgr,
[cv2.IMWRITE_PNG_COMPRESSION, 3],
)
return bool(ok), str(path)
def fit_inside(image: np.ndarray, max_w: int, max_h: int) -> np.ndarray:
h, w = image.shape[:2]
if w <= 0 or h <= 0:
return image
scale = min(
float(max_w) / float(w),
float(max_h) / float(h),
)
scale = max(scale, 1e-6)
new_w = max(1, int(round(w * scale)))
new_h = max(1, int(round(h * scale)))
interp = cv2.INTER_AREA if scale < 1.0 else cv2.INTER_LINEAR
return cv2.resize(image, (new_w, new_h), interpolation=interp)
def live_quality(frame_bgr: np.ndarray) -> dict:
"""
QA apenas visual.
Nunca rejeita nem altera a imagem salva.
"""
small = fit_inside(frame_bgr, 640, 360)
gray = cv2.cvtColor(small, cv2.COLOR_BGR2GRAY)
mean = float(gray.mean())
dark_pct = float((gray <= 5).mean() * 100.0)
bright_pct = float((gray >= 250).mean() * 100.0)
sharpness = float(cv2.Laplacian(gray, cv2.CV_64F).var())
warnings = []
if mean < 35:
warnings.append("MUITO ESCURA")
elif mean > 225:
warnings.append("MUITO CLARA")
if dark_pct > 20.0:
warnings.append("CLIP PRETO")
if bright_pct > 8.0:
warnings.append("CLIP BRANCO")
if sharpness < 35.0:
warnings.append("FOCO/BLUR?")
return {
"mean": mean,
"dark_pct": dark_pct,
"bright_pct": bright_pct,
"sharpness": sharpness,
"warnings": warnings,
}
def put_line(
canvas: np.ndarray,
text: str,
xy: Tuple[int, int],
scale: float = 0.65,
thickness: int = 1,
color=(235, 235, 235),
):
cv2.putText(
canvas,
text,
xy,
cv2.FONT_HERSHEY_SIMPLEX,
scale,
color,
thickness,
cv2.LINE_AA,
)
class CameraControls:
def __init__(
self,
queue,
exposure_us: int,
iso: int,
focus: int,
):
self.queue = queue
self.exposure_auto = True
self.focus_auto = True
self.exposure_us = int(exposure_us)
self.iso = int(iso)
self.focus = int(focus)
def send_initial_auto(self):
ctrl = dai.CameraControl()
try:
ctrl.setAutoExposureEnable()
except Exception:
pass
try:
ctrl.setAutoWhiteBalanceLock(False)
except Exception:
pass
try:
ctrl.setAutoFocusMode(
dai.CameraControl.AutoFocusMode.CONTINUOUS_VIDEO
)
ctrl.setAutoFocusTrigger()
except Exception:
pass
self.queue.send(ctrl)
def set_exposure_auto(self):
ctrl = dai.CameraControl()
try:
ctrl.setAutoExposureEnable()
except Exception as exc:
print(f"[WARN] Nao consegui ativar AE: {exc}")
return
self.queue.send(ctrl)
self.exposure_auto = True
print("[CAM] Exposicao: AUTO")
def set_exposure_manual(self):
self.exposure_us = int(
np.clip(
self.exposure_us,
EXPOSURE_MIN_US,
EXPOSURE_MAX_US,
)
)
self.iso = int(
np.clip(
self.iso,
ISO_MIN,
ISO_MAX,
)
)
ctrl = dai.CameraControl()
ctrl.setManualExposure(
self.exposure_us,
self.iso,
)
self.queue.send(ctrl)
self.exposure_auto = False
print(
f"[CAM] Exposicao: MANUAL | "
f"{self.exposure_us} us | ISO {self.iso}"
)
def toggle_exposure(self):
if self.exposure_auto:
self.set_exposure_manual()
else:
self.set_exposure_auto()
def adjust_exposure(self, delta_us: int):
if self.exposure_auto:
print("[CAM] Ajuste de exposicao ignorado: pressione E para modo MANUAL.")
return
self.exposure_us += int(delta_us)
self.set_exposure_manual()
def adjust_iso(self, delta_iso: int):
if self.exposure_auto:
print("[CAM] Ajuste de ISO ignorado: pressione E para modo MANUAL.")
return
self.iso += int(delta_iso)
self.set_exposure_manual()
def set_focus_auto(self):
ctrl = dai.CameraControl()
try:
ctrl.setAutoFocusMode(
dai.CameraControl.AutoFocusMode.CONTINUOUS_VIDEO
)
ctrl.setAutoFocusTrigger()
except Exception as exc:
print(f"[WARN] Autofocus nao disponivel: {exc}")
return
self.queue.send(ctrl)
self.focus_auto = True
print("[CAM] Foco: AUTO CONTINUOUS")
def set_focus_manual(self):
self.focus = int(
np.clip(
self.focus,
FOCUS_MIN,
FOCUS_MAX,
)
)
ctrl = dai.CameraControl()
try:
ctrl.setManualFocus(self.focus)
except Exception as exc:
print(f"[WARN] Foco manual nao disponivel: {exc}")
return
self.queue.send(ctrl)
self.focus_auto = False
print(f"[CAM] Foco: MANUAL | lens={self.focus}")
def toggle_focus(self):
if self.focus_auto:
self.set_focus_manual()
else:
self.set_focus_auto()
def adjust_focus(self, delta: int):
if self.focus_auto:
print("[CAM] Ajuste de foco ignorado: pressione F para modo MANUAL.")
return
self.focus += int(delta)
self.set_focus_manual()
def create_pipeline(fps: float) -> dai.Pipeline:
pipeline = dai.Pipeline()
cam_rgb = pipeline.createColorCamera()
cam_rgb.setBoardSocket(dai.CameraBoardSocket.CAM_A)
cam_rgb.setResolution(
dai.ColorCameraProperties.SensorResolution.THE_1080_P
)
cam_rgb.setFps(float(fps))
cam_rgb.setInterleaved(False)
# Mesmo contrato do runtime atual.
cam_rgb.setColorOrder(
dai.ColorCameraProperties.ColorOrder.RGB
)
xout = pipeline.createXLinkOut()
xout.setStreamName("rgb")
# IMPORTANTE: VIDEO, nao preview.
cam_rgb.video.link(xout.input)
control_in = pipeline.createXLinkIn()
control_in.setStreamName("control")
control_in.out.link(cam_rgb.inputControl)
return pipeline
def build_canvas(
frame_bgr: np.ndarray,
last_saved: Optional[np.ndarray],
*,
controls: CameraControls,
auto_save: bool,
interval_s: float,
saved_count: int,
out_dir: Path,
fps_view: float,
quality: dict,
) -> np.ndarray:
canvas_w = 1600
canvas_h = 900
canvas = np.zeros(
(canvas_h, canvas_w, 3),
dtype=np.uint8,
)
live = fit_inside(frame_bgr, 1150, 780)
lh, lw = live.shape[:2]
live_x = 20
live_y = 60
canvas[
live_y:live_y + lh,
live_x:live_x + lw,
] = live
panel_x = 1200
put_line(
canvas,
"OAK-D LITE / CORREDOR",
(panel_x, 60),
scale=0.78,
thickness=2,
)
put_line(
canvas,
"Fonte: VIDEO 1920x1080",
(panel_x, 100),
)
put_line(
canvas,
f"View FPS: {fps_view:.1f}",
(panel_x, 130),
)
put_line(
canvas,
f"Salvas: {saved_count}",
(panel_x, 160),
)
exp_text = (
"AUTO"
if controls.exposure_auto
else f"MANUAL {controls.exposure_us}us ISO{controls.iso}"
)
focus_text = (
"AUTO"
if controls.focus_auto
else f"MANUAL {controls.focus}"
)
put_line(
canvas,
f"Exposure: {exp_text}",
(panel_x, 205),
)
put_line(
canvas,
f"Focus: {focus_text}",
(panel_x, 235),
)
auto_text = (
f"ON ({interval_s:.2f}s)"
if auto_save
else "OFF"
)
put_line(
canvas,
f"Auto-save: {auto_text}",
(panel_x, 265),
)
put_line(
canvas,
f"Luma mean: {quality['mean']:.1f}",
(panel_x, 315),
)
put_line(
canvas,
f"Dark clip: {quality['dark_pct']:.1f}%",
(panel_x, 345),
)
put_line(
canvas,
f"White clip: {quality['bright_pct']:.1f}%",
(panel_x, 375),
)
put_line(
canvas,
f"Sharpness: {quality['sharpness']:.1f}",
(panel_x, 405),
)
if quality["warnings"]:
put_line(
canvas,
"QA: " + " | ".join(quality["warnings"]),
(panel_x, 440),
scale=0.55,
thickness=2,
color=(0, 180, 255),
)
else:
put_line(
canvas,
"QA: OK",
(panel_x, 440),
scale=0.62,
thickness=2,
color=(80, 230, 80),
)
put_line(canvas, "SPACE/S salvar", (panel_x, 510))
put_line(canvas, "A auto-save", (panel_x, 540))
put_line(canvas, "E auto/manual exp", (panel_x, 570))
put_line(canvas, "+/- ISO manual", (panel_x, 600))
put_line(canvas, "M/N exposicao manual", (panel_x, 630))
put_line(canvas, "F auto/manual foco", (panel_x, 660))
put_line(canvas, "[/] foco manual", (panel_x, 690))
put_line(canvas, "V fullscreen", (panel_x, 720))
put_line(canvas, "Q/ESC sair", (panel_x, 750))
if last_saved is not None:
thumb = fit_inside(last_saved, 360, 100)
th, tw = thumb.shape[:2]
tx = panel_x
ty = 780
if ty + th <= canvas_h and tx + tw <= canvas_w:
canvas[
ty:ty + th,
tx:tx + tw,
] = thumb
put_line(
canvas,
f"Saida: {out_dir}",
(20, 30),
scale=0.62,
color=(200, 220, 255),
)
return canvas
def main():
parser = argparse.ArgumentParser(
description=(
"Captura PNG lossless da OAK-D Lite para dataset "
"de corredor agricola."
)
)
parser.add_argument(
"--out",
default=str(DEFAULT_OUT),
help="Fila de imagens brutas (somente PNGs).",
)
parser.add_argument(
"--fps",
type=float,
default=CAPTURE_FPS,
help="FPS da camera.",
)
parser.add_argument(
"--auto",
action="store_true",
help="Inicia com auto-save ligado.",
)
parser.add_argument(
"--interval",
type=float,
default=1.0,
help="Intervalo do auto-save em segundos.",
)
parser.add_argument(
"--exposure_us",
type=int,
default=DEFAULT_EXPOSURE_US,
help="Exposicao inicial quando entrar em modo manual.",
)
parser.add_argument(
"--iso",
type=int,
default=DEFAULT_ISO,
help="ISO inicial quando entrar em modo manual.",
)
parser.add_argument(
"--focus",
type=int,
default=DEFAULT_FOCUS,
help="Lens position inicial quando entrar em foco manual.",
)
args = parser.parse_args()
out_dir = Path(args.out)
out_dir.mkdir(parents=True, exist_ok=True)
saved_count = len(list(out_dir.glob("*.png")))
print("=" * 72)
print("OAK-D Lite | Captura Dataset Corredor Agricola")
print("=" * 72)
print(f"Saida : {out_dir.resolve()}")
print("Formato : PNG lossless")
print(f"Resolucao : {CAPTURE_W}x{CAPTURE_H}")
print("Fonte : ColorCamera.video")
print(f"FPS camera : {args.fps}")
print(f"Ja existem : {saved_count} PNGs")
print(f"Sessao : {CAPTURE_SESSION_ID}")
print("=" * 72)
print("SPACE/S salvar | A auto-save | E exposure | F foco | Q sair")
pipeline = create_pipeline(args.fps)
saver = ThreadPoolExecutor(
max_workers=1,
thread_name_prefix="png_saver",
)
pending = []
last_saved: Optional[np.ndarray] = None
auto_save = bool(args.auto)
interval_s = max(0.10, float(args.interval))
last_auto_save_t = 0.0
fps_view = 0.0
fps_frames = 0
fps_t0 = time.perf_counter()
fullscreen = False
try:
with dai.Device(pipeline) as device:
control_queue = device.getInputQueue("control")
rgb_queue = device.getOutputQueue(
"rgb",
maxSize=2,
blocking=False,
)
controls = CameraControls(
control_queue,
exposure_us=args.exposure_us,
iso=args.iso,
focus=args.focus,
)
controls.send_initial_auto()
cv2.namedWindow(
WINDOW_NAME,
cv2.WINDOW_NORMAL,
)
cv2.resizeWindow(
WINDOW_NAME,
1600,
900,
)
while True:
packet = rgb_queue.get()
frame = packet.getCvFrame()
if (
frame is None
or frame.ndim != 3
or frame.shape[2] != 3
):
print(
f"[WARN] Frame invalido: "
f"{getattr(frame, 'shape', None)}"
)
continue
h, w = frame.shape[:2]
if (w, h) != (CAPTURE_W, CAPTURE_H):
print(
f"[WARN] Frame veio {w}x{h}, "
f"esperado {CAPTURE_W}x{CAPTURE_H}. "
"Nao sera redimensionado silenciosamente."
)
now_perf = time.perf_counter()
fps_frames += 1
dt_fps = now_perf - fps_t0
if dt_fps >= 1.0:
fps_view = fps_frames / dt_fps
fps_frames = 0
fps_t0 = now_perf
quality = live_quality(frame)
def request_save(current_frame: np.ndarray):
nonlocal saved_count, last_saved
name = timestamp_name() + ".png"
path = out_dir / name
snapshot = current_frame.copy()
future = saver.submit(
save_png,
path,
snapshot,
)
pending.append((future, path))
last_saved = snapshot
saved_count += 1
print(
f"[CAPTURE] #{saved_count} -> {path}"
)
if auto_save:
if (
last_auto_save_t <= 0.0
or now_perf - last_auto_save_t >= interval_s
):
request_save(frame)
last_auto_save_t = now_perf
still_pending = []
for future, path in pending:
if not future.done():
still_pending.append((future, path))
continue
try:
ok, saved_path = future.result()
except Exception as exc:
print(
f"[ERRO] Falha salvando {path}: {exc}"
)
continue
if not ok:
print(
f"[ERRO] cv2.imwrite retornou False: "
f"{saved_path}"
)
pending = still_pending
canvas = build_canvas(
frame,
last_saved,
controls=controls,
auto_save=auto_save,
interval_s=interval_s,
saved_count=saved_count,
out_dir=out_dir,
fps_view=fps_view,
quality=quality,
)
cv2.imshow(
WINDOW_NAME,
canvas,
)
key = cv2.waitKey(1) & 0xFF
if key in (ord("q"), 27):
print("[INFO] Encerrando captura...")
break
if key in (ord("s"), ord(" ")):
request_save(frame)
elif key == ord("a"):
auto_save = not auto_save
last_auto_save_t = 0.0
print(
f"[CAPTURE] Auto-save "
f"{'ON' if auto_save else 'OFF'} "
f"| interval={interval_s:.2f}s"
)
elif key == ord("e"):
controls.toggle_exposure()
elif key in (ord("+"), ord("=")):
controls.adjust_iso(ISO_STEP)
elif key == ord("-"):
controls.adjust_iso(-ISO_STEP)
elif key == ord("m"):
controls.adjust_exposure(EXPOSURE_STEP_US)
elif key == ord("n"):
controls.adjust_exposure(-EXPOSURE_STEP_US)
elif key == ord("f"):
controls.toggle_focus()
elif key == ord("]"):
controls.adjust_focus(FOCUS_STEP)
elif key == ord("["):
controls.adjust_focus(-FOCUS_STEP)
elif key == ord("v"):
fullscreen = not fullscreen
cv2.setWindowProperty(
WINDOW_NAME,
cv2.WND_PROP_FULLSCREEN,
(
cv2.WINDOW_FULLSCREEN
if fullscreen
else cv2.WINDOW_NORMAL
),
)
finally:
cv2.destroyAllWindows()
print(
f"[INFO] Aguardando {len(pending)} PNG(s) "
"que ja foram solicitados..."
)
saver.shutdown(wait=True)
failures = 0
for future, path in pending:
try:
ok, _ = future.result()
if not ok:
failures += 1
except Exception as exc:
failures += 1
print(f"[ERRO] {path}: {exc}")
print("=" * 72)
print("Captura encerrada.")
print(f"Total contabilizado : {saved_count}")
print(f"Falhas finais : {failures}")
print(f"Saida : {out_dir.resolve()}")
print("=" * 72)
if __name__ == "__main__":
main()

2376
Python/OAK/datasets/oak-d/_1_annotate_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

1938
Python/OAK/datasets/oak-d/_2_normalize_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

1682
Python/OAK/datasets/oak-d/_3_split_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

4198
Python/OAK/datasets/oak-d/_4_train_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

4052
Python/OAK/datasets/oak-d/_5_test_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

3285
Python/OAK/datasets/oak-d/_6_export_corridor_onnx.py Normal file
View File

File diff suppressed because it is too large Load Diff

3589
Python/OAK/datasets/oak-d/_7_benchmark_corridor.py Normal file
View File

File diff suppressed because it is too large Load Diff

5125
Python/OAK/datasets/oak-d/audit/review_corridor_dataset_v2.py Normal file
View File

File diff suppressed because it is too large Load Diff

185
Python/OAK/datasets/oak-d/config.json Normal file
View File

@ -0,0 +1,185 @@
{
"camera": "oak-d",
"modelo": "segformer_b0",
"model_name": "corredores_v2",
"main_class_name": "navegavel",
"model_to_use": "geral",
"ckpt_test": "best_operational",
"raw_size": [1920, 1080],
"resolucao": [1024, 576],
"roi_inicio": 0.0,
"roi_tamanho": 1.0,
"channels": 3,
"backbone": "nvidia/mit-b0",
"label_classes": [
"Parado",
"EntrandoRua",
"CaminhandoRua",
"SaindoRua",
"Manobrando",
"Direcionando",
"RetornandoBase",
"Indefinido"
],
"corridor_training": {
"data": {
"strict_labels": true,
"strict_pipeline_contract": true,
"preflight_max_samples": 0
},
"augmentation": {
"enabled": true,
"horizontal_flip_p": 0.50,
"affine_p": 0.60,
"rotate_deg": 4.0,
"scale_min": 0.92,
"scale_max": 1.08,
"translate_frac": 0.03,
"global_gain_p": 0.45,
"global_gain_min": 0.82,
"global_gain_max": 1.18,
"contrast_p": 0.35,
"contrast_min": 0.85,
"contrast_max": 1.15,
"gamma_p": 0.30,
"gamma_min": 0.82,
"gamma_max": 1.18,
"rgb_gain_p": 0.25,
"rgb_gain_min": 0.94,
"rgb_gain_max": 1.06,
"shadow_p": 0.35,
"shadow_strength_min": 0.12,
"shadow_strength_max": 0.42,
"noise_p": 0.18,
"noise_sigma_min": 0.003,
"noise_sigma_max": 0.018,
"blur_p": 0.12,
"blur_kernel": 3,
"occlusion_p": 0.00,
"occlusion_min_frac": 0.04,
"occlusion_max_frac": 0.15,
"ramp_epochs": 6
},
"sampler": {
"mode": "joint",
"joint_power": 0.30,
"status_power": 0.12,
"group_power": 0.10,
"max_weight_ratio": 5.0,
"samples_per_epoch": 0
},
"class_weighting": {
"seg_method": "log_inverse",
"seg_log_offset": 1.02,
"seg_min_weight": 0.35,
"seg_max_weight": 3.0,
"seg_max_samples": 1200,
"status_enabled": true,
"status_power": 0.30,
"status_min_weight": 0.50,
"status_max_weight": 3.0
},
"loss": {
"seg_ce_weight": 0.65,
"seg_dice_weight": 0.35,
"boundary_boost": 0.15,
"unsafe_nav_weight": 0.06,
"status_weight": 0.40,
"status_ramp_epochs": 8,
"status_label_smoothing": 0.03
},
"status_head": {
"pool_h": 3,
"pool_w": 4,
"hidden": 256,
"dropout": 0.20,
"detach_seg_summary": true,
"seg_summary": "probabilities"
},
"optimizer": {
"encoder_lr_mult": 0.50,
"decoder_lr_mult": 1.00,
"status_lr_mult": 2.00,
"freeze_encoder_epochs": 2,
"no_decay_bias": true,
"no_decay_norm": true,
"betas": [0.9, 0.999],
"eps": 1e-8
},
"scheduler": {
"mode": "poly",
"warmup_ratio": 0.05,
"warmup_start_factor": 0.10,
"poly_power": 1.0,
"min_lr_ratio": 0.02
},
"optimization": {
"grad_clip_norm": 1.0,
"matmul_precision": "high",
"cudnn_benchmark": true,
"persistent_workers": true,
"prefetch_factor": 2
},
"score": {
"nav_iou": 0.40,
"nav_f1": 0.20,
"status_macro_f1": 0.20,
"safety": 0.20
},
"operational_gate": {
"enabled": true,
"min_epoch": 5,
"min_nav_iou": 0.70,
"min_nav_f1": 0.80,
"min_non_nav_iou": 0.60,
"min_status_macro_f1": 0.60,
"max_unsafe_nav_rate": 0.03,
"group_unsafe": {
"enabled": true,
"min_non_nav_pixels": 5000,
"max_rate": 0.05,
"max_rate_by_group": {
"naonavegavel": 0.04,
"navegavel_naonavegavel": 0.05
}
},
"status_recall": {
"enabled": false,
"min_support": 5,
"minimums": {}
}
},
"checkpoint": {
"early_stop_patience": 18,
"early_stop_min_delta": 0.0003
}
}
}

148
Python/OAK/datasets/oak-d/roi_seg_dataset.py Normal file
View File

@ -0,0 +1,148 @@
# -*- coding: utf-8 -*-
from PIL import Image
import glob
import os
import cv2
import numpy as np
import torch
from torch.utils.data import Dataset
from utils import carregar_labelmap_completo, compute_roi_indices, resize_keep_width
IMG_EXTS = (".jpg", ".jpeg", ".png")
MSK_EXTS = (".png", ".jpg", ".jpeg") # preferimos .png se existir
def _is_dir(p): return os.path.isdir(p)
def _is_file(p): return os.path.isfile(p)
def _list_groups(group_root):
if not _is_dir(group_root): return []
out = []
for g in sorted(os.listdir(group_root)):
gdir = os.path.join(group_root, g)
if not _is_dir(gdir):
continue
if _is_dir(os.path.join(gdir, "images")) and _is_dir(os.path.join(gdir, "masks")):
out.append(g)
return out
def _mask_for_base(msk_dir, base):
"""Encontra a máscara que casa com o base, priorizando .png."""
best = None
for ext in MSK_EXTS:
cand = os.path.join(msk_dir, base + ext)
if _is_file(cand):
if best is None: best = cand
# mantém .png se aparecer depois
if os.path.splitext(cand)[1].lower() == ".png":
return cand
return best
def _collect_pairs_legacy(root):
"""root/{images,masks}"""
img_dir = os.path.join(root, "images")
msk_dir = os.path.join(root, "masks")
imgs = []
msks = []
for p in sorted(glob.glob(os.path.join(img_dir, "*"))):
base, ext = os.path.splitext(os.path.basename(p))
if ext.lower() not in IMG_EXTS:
continue
m = _mask_for_base(msk_dir, base)
if m:
imgs.append(p)
msks.append(m)
return imgs, msks
def _collect_pairs_grouped(root):
"""Suporta:
- root/group/<g>/{images,masks}
- root/<g>/{images,masks} (quando root já é '.../group')
"""
# case A: root tem subpasta 'group'
group_root = os.path.join(root, "group")
if not _is_dir(group_root):
# case B: root JÁ É a pasta 'group'
group_root = root
groups = _list_groups(group_root)
imgs, msks = [], []
for g in groups:
img_dir = os.path.join(group_root, g, "images")
msk_dir = os.path.join(group_root, g, "masks")
for p in sorted(glob.glob(os.path.join(img_dir, "*"))):
base, ext = os.path.splitext(os.path.basename(p))
if ext.lower() not in IMG_EXTS:
continue
m = _mask_for_base(msk_dir, base)
if m:
imgs.append(p)
msks.append(m)
return imgs, msks
class ROISegDataset(Dataset):
"""
Compatível com o dataset original, mas agora aceita:
- root = '.../split/train' (com 'group' dentro)
- root = '.../split/train/group'
- root = '.../split/train/<grupo>' (ainda funciona via legado se tiver images/masks)
- root legado = '.../split/train' com 'images' e 'masks' diretamente
"""
def __init__(self, root, out_dir, zona_inicio, faixa_atuacao, input_w=384, min_input_h=96, labelmap_path="labelmap.txt",
mean=(0.485, 0.456, 0.406), std=(0.229, 0.224, 0.225)):
self.out_dir = out_dir
self.zona_inicio = zona_inicio
self.faixa_atuacao = faixa_atuacao
self.input_w = input_w
self.min_input_h = min_input_h
self.mean = np.array(mean, dtype=np.float32).reshape(1, 1, 3)
self.std = np.array(std, dtype=np.float32).reshape(1, 1, 3)
# tenta agrupar; se não encontrar, cai pro legado
imgs, msks = _collect_pairs_grouped(root)
if not imgs:
imgs, msks = _collect_pairs_legacy(root)
assert len(imgs) == len(msks) and len(imgs) > 0, f"Nenhuma imagem/máscara encontrada em {root}"
self.img_paths = imgs
self.msk_paths = msks
# labelmap
_, _, self.classes, self.ignore_rgb = carregar_labelmap_completo(labelmap_path)
# tipicamente ignore_rgb é (255,255,255)
self.ignore_id = int(self.ignore_rgb[0]) if isinstance(self.ignore_rgb, (list, tuple)) else int(self.ignore_rgb)
def __len__(self):
return len(self.img_paths)
def __getitem__(self, idx):
img_rgb = np.array(Image.open(self.img_paths[idx]).convert("RGB"))
# Máscara como escala de cinza (IDs já foram normalizados na etapa de normalize)
msk_grayscale = np.array(Image.open(self.msk_paths[idx]).convert("L"))
H, W = img_rgb.shape[:2]
y_fim, y_inicio = compute_roi_indices(H, self.zona_inicio, self.faixa_atuacao)
img_roi = img_rgb[y_fim:y_inicio, 0:W]
msk_roi = msk_grayscale[y_fim:y_inicio, 0:W]
img_in = resize_keep_width(img_roi, self.input_w, self.min_input_h, cv2.INTER_AREA)
msk_ids = resize_keep_width(msk_roi, self.input_w, self.min_input_h, cv2.INTER_NEAREST)
valores_validos = list(range(len(self.classes))) + [255]
msk_ids[np.isin(msk_ids, valores_validos, invert=True)] = self.ignore_id
if np.all(msk_ids == self.ignore_id):
raise ValueError(f"Máscara {self.msk_paths[idx]} está só com valor de ignore ({self.ignore_id})")
img_f = img_in.astype(np.float32) / 255.0
img_f = (img_f - self.mean) / self.std
img_chw = np.transpose(img_f, (2, 0, 1))
if idx == 0:
os.makedirs(self.out_dir, exist_ok=True)
cv2.imwrite(os.path.join(self.out_dir, "debug_roi_input.png"), cv2.cvtColor(img_roi, cv2.COLOR_RGB2BGR))
cv2.imwrite(os.path.join(self.out_dir, "debug_roi_mask.png"), msk_roi)
return torch.from_numpy(img_chw).float(), torch.from_numpy(msk_ids.astype(np.int64))

144
Python/OAK/datasets/oak-d/utils.py Normal file
View File

@ -0,0 +1,144 @@
import cv2
import numpy as np
# ----------------------------
# Helpers LABELMAP
# ----------------------------
def carregar_labelmap_completo(caminho):
cor_para_id = {}
id_para_nome = {}
cores_rgb = []
with open(caminho, 'r') as arquivo:
idx = 0
for linha in arquivo:
if linha.startswith("#") or not linha.strip():
continue
partes = linha.strip().split(':')
if len(partes) >= 2:
nome_classe, cor_rgb_str = partes[0], partes[1]
r, g, b = map(int, cor_rgb_str.split(','))
cor_rgb = (r, g, b)
if nome_classe.lower() == "ignore":
ignore_rgb = cor_rgb
continue # NÃO adiciona ignore no LUT de classes
cor_para_id[cor_rgb] = idx
cores_rgb.append(cor_rgb)
id_para_nome[idx] = nome_classe
idx += 1
#print(f"Mapa: {cor_para_id}")
#print(f"Colormap RGB: {cores_rgb}")
#print(f"Classes: {id_para_nome}")
#print(f"Ignore RGB: {ignore_rgb}")
return cor_para_id, cores_rgb, id_para_nome, ignore_rgb
def converter_mask_rgb_para_ids(img_rgb, mapa_rgb, ignore_id):
# Cria um mapa 256^3 para IDs (usa int32 para indexar)
lut = np.full((256**3,), ignore_id, dtype=np.uint8)
for cor, classe_id in mapa_rgb.items():
r, g, b = cor
lut[(r << 16) + (g << 8) + b] = classe_id
# Converte RGB para índice único
flat_idx = (img_rgb[:,:,0].astype(np.int32) << 16) + \
(img_rgb[:,:,1].astype(np.int32) << 8) + \
img_rgb[:,:,2].astype(np.int32)
# Aplica LUT vetorizada
return lut[flat_idx]
def converter_mask_ids_para_rgb(mask_ids: np.ndarray, colormap_rgb: list, ignore_id: int = 255) -> np.ndarray:
# Criar lookup table (256 cores possíveis)
lut = np.zeros((256, 3), dtype=np.uint8)
for i, color in enumerate(colormap_rgb):
lut[i] = color
lut[ignore_id] = (255, 255, 255)
# Aplicar LUT direto (vetorizado)
return lut[mask_ids]
def converter_mask_ids_para_bgr(mask_ids: np.ndarray, colormap_rgb: list, ignore_id: int = 255) -> np.ndarray:
"""
Converte máscara de IDs para imagem BGR (uint8),
pronta para uso com OpenCV.
"""
lut = np.zeros((256, 3), dtype=np.uint8)
for i, (r, g, b) in enumerate(colormap_rgb):
lut[i] = (b, g, r) # RGB -> BGR
lut[ignore_id] = (255, 255, 255) # branco em BGR = RGB
return lut[mask_ids]
def desenhar_legenda_vertical(colormap_rgb, classes, largura=200):
"""
Retorna uma imagem com a legenda das classes (cor + nome)
"""
nomes_classes = [classes[i] for i in range(len(classes))]
altura_por_classe = 30
altura_total = altura_por_classe * len(colormap_rgb)
legenda = np.ones((altura_total, largura, 3), dtype=np.uint8) * 255
for idx, (rgb, nome) in enumerate(zip(colormap_rgb, nomes_classes)):
y = idx * altura_por_classe
color = tuple(int(c) for c in rgb)
cv2.rectangle(legenda, (10, y + 5), (30, y + 25), color, -1)
cv2.putText(legenda, nome, (40, y + 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 1, cv2.LINE_AA)
return legenda
def desenhar_legenda_horizontal(colormap_rgb, classes, altura=30, largura_por_classe=120):
"""
Retorna uma imagem com a legenda das classes (cor + nome), em uma única linha horizontal
"""
nomes_classes = [classes[i] for i in range(len(classes))]
largura_total = largura_por_classe * len(colormap_rgb)
legenda = np.ones((altura, largura_total, 3), dtype=np.uint8) * 255 # faixa branca
for idx, (rgb, nome) in enumerate(zip(colormap_rgb, nomes_classes)):
x = idx * largura_por_classe
color = tuple(int(c) for c in rgb)
# Retângulo colorido
cv2.rectangle(legenda, (x + 10, 5), (x + 30, 25), color, -1)
# Texto da classe
cv2.putText(legenda, nome, (x + 35, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 1, cv2.LINE_AA)
return legenda
def _infer_ignore_id(ignore_rgb, default_id=255):
import numpy as _np
if isinstance(ignore_rgb, (list, tuple)):
if len(ignore_rgb) == 1 and isinstance(ignore_rgb[0], (int, _np.integer)):
return int(ignore_rgb[0])
if len(ignore_rgb) == 3:
return default_id
if isinstance(ignore_rgb, (int, _np.integer)):
return int(ignore_rgb)
return default_id
# ----------------------------
# Helpers ROI
# ----------------------------
def compute_roi_indices(H: int, zona_inicio: float, faixa_atuacao: float):
y_inicio = int((1.0 - zona_inicio) * H)
y_fim = int((1.0 - (zona_inicio + faixa_atuacao)) * H)
y_fim = max(0, min(H, y_fim))
y_inicio = max(0, min(H, y_inicio))
if y_fim >= y_inicio:
y_fim = max(0, y_inicio - 1)
return y_fim, y_inicio
def resize_keep_width(img: np.ndarray, new_w: int, min_h: int, interpolation: int) -> np.ndarray:
h, w = img.shape[:2]
new_h = int(round(new_w * (h / w)))
if min_h is not None and new_h < min_h:
new_h = min_h
return cv2.resize(img, (new_w, new_h), interpolation=interpolation)