ajustes no IMU e deteccao de obstaculos
This commit is contained in:
Diego Freitas
parent
c86ab0b4c6
commit
b0c9aa281a
|
|
@ -53,6 +53,16 @@ class IMUCamera(ModuloDiagnosticoBase):
|
|||
self.imu_em_falha = False
|
||||
|
||||
if imuData is not None:
|
||||
# dt estimado por pacote
|
||||
now = time.time()
|
||||
if self.last_imu_ts is None:
|
||||
dt = 1.0 / max(1e-3, f_tick) # fallback
|
||||
else:
|
||||
dt = max(0.0, now - self.last_imu_ts)
|
||||
self.last_imu_ts = now
|
||||
dt = min(dt, 0.05) # clamp anti-bursts (<=50 ms)
|
||||
self.filtro_imu.Dt = float(dt)
|
||||
|
||||
for packet in imuData.packets:
|
||||
accel = packet.acceleroMeter
|
||||
gyro = packet.gyroscope
|
||||
|
|
@ -78,15 +88,6 @@ class IMUCamera(ModuloDiagnosticoBase):
|
|||
yaw -= self.yaw_inicial
|
||||
|
||||
Rwb = r.as_matrix()
|
||||
# dt estimado por pacote
|
||||
now = time.time()
|
||||
if self.last_imu_ts is None:
|
||||
dt = 1.0 / max(1e-3, f_tick) # fallback
|
||||
else:
|
||||
dt = max(0.0, now - self.last_imu_ts)
|
||||
self.last_imu_ts = now
|
||||
dt = min(dt, 0.05) # clamp anti-bursts (<=50 ms)
|
||||
self.filtro_imu.Dt = float(dt)
|
||||
|
||||
# 1) aceleração no mundo
|
||||
a_body = np.array([ax, ay, az], dtype=np.float64)
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -217,25 +217,27 @@ def _regras_taticas(contexto):
|
|||
blocked = bool(block.get("blocked")) if isinstance(block, dict) else False
|
||||
reason = (block.get("reason") if isinstance(block, dict) else "none") or "none"
|
||||
|
||||
if blocked:
|
||||
mostrar_log(f"🟥 Bloqueio detectado ({reason}). d_obs_min={d_obs_min if d_obs_min is not None else '–'} m. Parando.")
|
||||
if blocked and reason == "blackout":
|
||||
mostrar_log(f"🟥 Blackout de percepção. Parando.")
|
||||
return _comando_direcional_parado(True)
|
||||
|
||||
if (d_obs_min is not None) and (d_obs_min <= dist_necessaria):
|
||||
mostrar_log(f"🟥 Obstáculo à {d_obs_min:.2f} m < distancia necessária {dist_necessaria:.2f} m. Parando.")
|
||||
return _comando_direcional_parado(True)
|
||||
if blocked and reason == "obstacle":
|
||||
if (d_obs_min is not None) and (d_obs_min <= dist_necessaria):
|
||||
mostrar_log(f"🟥 Obstáculo a {d_obs_min:.2f} m ≤ {dist_necessaria:.2f} m (necessária). Parando.")
|
||||
return _comando_direcional_parado(True)
|
||||
else:
|
||||
# opcional: limitar velocidade para manter margem de frenagem
|
||||
# v_max_safe = sqrt(2*a*(dmin - margem)), se dmin existir
|
||||
if d_obs_min is not None and d_obs_min > margem_parada:
|
||||
v_max_safe = (2.0 * a_max_freio * max(0.0, d_obs_min - margem_parada)) ** 0.5
|
||||
contexto.setdefault("DirecionalHints", {})["v_max_sugerida_mps"] = float(v_max_safe)
|
||||
|
||||
# opcional: dica de veto lateral pro MPC (se você consumir)
|
||||
side_bias = None
|
||||
if isinstance(block, dict):
|
||||
sb = block.get("side_bias") or {}
|
||||
side_bias = sb.get("value", None)
|
||||
if side_bias is not None:
|
||||
# motivo narrow: não para, mas dá dica lateral
|
||||
if isinstance(block, dict) and reason == "narrow":
|
||||
sb = (block.get("side_bias") or {}).get("value", 0.0)
|
||||
hints = contexto.setdefault("DirecionalHints", {})
|
||||
if side_bias > 0.25: # mais fechado à direita -> evite virar p/ direita
|
||||
hints["vetar_direita"] = True
|
||||
elif side_bias < -0.25: # mais fechado à esquerda -> evite virar p/ esquerda
|
||||
hints["vetar_esquerda"] = True
|
||||
if sb > 0.25: hints["vetar_direita"] = True
|
||||
if sb < -0.25: hints["vetar_esquerda"] = True
|
||||
|
||||
# se chegou até aqui, pode seguir
|
||||
return _comando_direcional_parado(False)
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -2,9 +2,10 @@ import base64
|
|||
import math
|
||||
import cv2
|
||||
import numpy as np
|
||||
import cupy as cp
|
||||
|
||||
from manager_worker.config import mostrar_log
|
||||
from shared.contexto_global_redis import ContextoGlobalRedis
|
||||
from shared.enums import StatusModulo, T_Code
|
||||
|
||||
|
||||
|
||||
|
|
@ -206,3 +207,18 @@ def resize_keep_width(img: np.ndarray, new_w: int, min_h: int) -> np.ndarray:
|
|||
new_h = min_h
|
||||
return cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA)
|
||||
|
||||
# ----------------------------
|
||||
# Helpers equipamento
|
||||
# ----------------------------
|
||||
def get_velocidade_atual_ms():
|
||||
velocidade = 0.0
|
||||
try:
|
||||
velocidade = ContextoGlobalRedis.get_contexto().get("Gerais", {}).get("velocidade_ms", 0.0)
|
||||
if velocidade == 0:
|
||||
imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
|
||||
if imu is not None and imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) == StatusModulo.OPERANTE.value:
|
||||
velocidade = imu.get("vel_mps", 0.0)
|
||||
except Exception as e:
|
||||
mostrar_log(f"Erro ao consultar velocidade atual: {e}")
|
||||
finally:
|
||||
return velocidade
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -14,7 +14,7 @@ from visual_worker.processamento.radar_top_down import Radar2DManager
|
|||
from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao, SegmentacaoManager
|
||||
from visual_worker.processamento.costmap_fuser import CostmapFuser, unpack_snapshot
|
||||
from shared.enums import StatusModulo, T_Code, CameraFrameType
|
||||
from shared.utils import analisar_linhas_por_profundidade, decode_image_base64, encode_image_base64, fazer_overlay
|
||||
from shared.utils import analisar_linhas_por_profundidade, decode_image_base64, encode_image_base64, fazer_overlay, get_velocidade_atual_ms
|
||||
from shared.gps_handler import GPSHandler
|
||||
from camera_worker.camera_oak import CameraOak
|
||||
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
|
||||
|
|
@ -401,6 +401,8 @@ class CameraManager:
|
|||
if depth_frame_np is None or depth_frame_np.size == 0: return
|
||||
segmentacao = self._ultima_analise_segmentacao.get("classes")
|
||||
if segmentacao is None: return
|
||||
|
||||
vel = get_velocidade_atual_ms()
|
||||
|
||||
t0 = time.time()
|
||||
#grid_conf = self._gerar_grid_confianca(depth_frame_np, segmentacao, dist_max)
|
||||
|
|
@ -409,7 +411,7 @@ class CameraManager:
|
|||
t1 = time.time()
|
||||
grid_conf["ultima_chamada"] = self._ultima_analise_matriz_confianca.get("ultima_chamada", t0)
|
||||
self._calcular_performance(t0, t1, grid_conf)
|
||||
snapshot = self.data_fuser.update(grid_conf, ts=t1)
|
||||
snapshot = self.data_fuser.update(grid_conf, ts=t1, velocidade_ms=vel)
|
||||
self._ultima_analise_matriz_confianca = grid_conf
|
||||
#self._ultima_analise_segmentacao["corredor_perfil"] = self.segmentacao_manager.calcular_perfil_corredor(matriz, fov_h)
|
||||
ContextoGlobalRedis.atualizar_ctx_dict(
|
||||
|
|
@ -422,11 +424,6 @@ class CameraManager:
|
|||
#self._mostrar_debug_grid_confianca(self._ultimo_rgb_frame, grid_conf["matriz"], True, self._ultima_analise_segmentacao["mask_color"])
|
||||
#key, vis = self.debug_show_visualworker(frame_bgr=self._ultimo_rgb_frame, grid=grid_conf, wait=1, text_mode="full", draw_grid=True, draw_cells=True, draw_legend=True)
|
||||
|
||||
vel = 0.0
|
||||
imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
|
||||
if imu is not None and imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) == StatusModulo.OPERANTE.value:
|
||||
vel = imu.get("vel_mps", 0.0)
|
||||
|
||||
vis, metrics = self.debug_blockage_imshow(self._ultimo_rgb_frame, snapshot, velocidade_media=vel)
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"❌ Erro na geracao da matriz de confianca: {e}")
|
||||
|
|
|
|||
|
|
@ -32,7 +32,6 @@ def iniciar_camera_manager(mx_id):
|
|||
if manager.camera is not None and manager.operante:
|
||||
mostrar_log(f"✅ Camera manager iniciado, com MX_ID: {mx_id}")
|
||||
|
||||
|
||||
_CONFIG_PATH = os.path.join(os.path.dirname(__file__), "config.json")
|
||||
_CONFIG_CACHE = None
|
||||
_CONFIG_MTIME = None
|
||||
|
|
|
|||
|
|
@ -42,6 +42,12 @@ class CostmapFuser:
|
|||
self.buf_zref = [] # opcional
|
||||
self.buf_ts = []
|
||||
|
||||
self._blk_state = {
|
||||
"on": 0, "off": 0, "latched": False,
|
||||
"reason": "none", "dmin": None,
|
||||
"last_decision": "LIVRE"
|
||||
}
|
||||
|
||||
self.seq = 0
|
||||
|
||||
def _stack(self, lst, fallback_val=0.0):
|
||||
|
|
@ -112,7 +118,7 @@ class CostmapFuser:
|
|||
# metros por coluna
|
||||
return (row_width / float(self.grid_w)).astype(np.float32) # (H,)
|
||||
|
||||
def update(self, grid_dict, ts=None):
|
||||
def update(self, grid_dict, ts=None, velocidade_ms=0.0):
|
||||
"""
|
||||
grid_dict deve conter (grid_h,grid_w): "custo","conf","anom","navegavel"
|
||||
opcional: "z_ref" (m) 2D
|
||||
|
|
@ -189,6 +195,15 @@ class CostmapFuser:
|
|||
# escala X por linha (m/col)
|
||||
row_scale_x = self._row_scale_x(row_dist) # (H,) ou None
|
||||
|
||||
block = self._compute_blockage_metrics(
|
||||
custo_f, anom_f, conf_f, nav_f,
|
||||
row_dist_m, row_scale_x, self.central_cols,
|
||||
use_persistence=True,
|
||||
velocidade_mps=velocidade_ms,
|
||||
a_max_freio=0.8, margem_parada=0.25,
|
||||
N_on=2, N_off=5, blackout_imediato=True
|
||||
)
|
||||
|
||||
# incrementa seq
|
||||
self.seq += 1
|
||||
|
||||
|
|
@ -210,13 +225,14 @@ class CostmapFuser:
|
|||
"near_is_bottom": bool(self.near_is_bottom),
|
||||
},
|
||||
"y_range_m": [float(self.y_range_m[0]), float(self.y_range_m[1])],
|
||||
#"d_obs_min": (None if d_obs_min is None else float(d_obs_min)),
|
||||
"row_dist_m": row_dist_m.tolist(),
|
||||
"row_scale_x_m": row_scale_x.tolist(),
|
||||
"custo_u8": to_u8_list(custo_f),
|
||||
"conf_u8": to_u8_list(conf_f),
|
||||
"anom_u8": to_u8_list(anom_f),
|
||||
"nav_mask": nav_f.astype(np.uint8).ravel().tolist()
|
||||
"nav_mask": nav_f.astype(np.uint8).ravel().tolist(),
|
||||
"d_obs_min": block["d_obs_min"],
|
||||
"block": block
|
||||
}
|
||||
|
||||
# (opcional) incluir z_ref_u8 pra debug/visualização
|
||||
|
|
@ -224,16 +240,6 @@ class CostmapFuser:
|
|||
# zref_u8 = np.clip(zref_f / self.y_range_m[1] * 255.0, 0, 255).astype(np.uint8)
|
||||
# snap["zref_u8"] = zref_u8.ravel().tolist()
|
||||
|
||||
block = self._compute_blockage_metrics(
|
||||
custo_f, anom_f, conf_f, nav_f,
|
||||
row_dist_m, row_scale_x,
|
||||
central_cols=self.central_cols,
|
||||
robot_width_m=self.robot_width_m, # defina no __init__ ou config
|
||||
margin_m=0.12,
|
||||
)
|
||||
snap["block"] = block
|
||||
snap["d_obs_min"] = block["d_obs_min"] # mantém campo raiz por compatibilidade
|
||||
|
||||
return snap
|
||||
|
||||
def _compute_blockage_metrics(
|
||||
|
|
@ -243,57 +249,69 @@ class CostmapFuser:
|
|||
robot_width_m=0.84, margin_m=0.12,
|
||||
thr_anom_block=0.50, thr_cost_block=0.65, thr_conf_low=0.35,
|
||||
rho_block_central=0.70, rho_block_global=0.60,
|
||||
near_is_bottom=True
|
||||
near_is_bottom=True,
|
||||
# ------ NOVOS (opcionais) ------
|
||||
use_persistence=False,
|
||||
velocidade_mps=None, # m/s; se None, decisão não usa distância de frenagem
|
||||
a_max_freio=0.8, # m/s²
|
||||
margem_parada=0.25, # m
|
||||
N_on=2, # frames p/ entrar
|
||||
N_off=5, # frames p/ sair
|
||||
blackout_imediato=True
|
||||
):
|
||||
"""
|
||||
Retorna dict com:
|
||||
- d_obs_min (m) ou None
|
||||
- blocked (bool)
|
||||
- blocked (bool) -> se use_persistence=False: igual ao "raw"; se True: já com persistência
|
||||
- blocked_raw (bool)
|
||||
- reason ('obstacle','blackout','narrow','none')
|
||||
- coverage: {'central_max':..., 'global':...}
|
||||
- side_bias: {'value': -1..+1, 'left_frac':..., 'right_frac':...}
|
||||
- j_block (índice da linha que bloqueia) ou None
|
||||
- j_block (índice) ou None
|
||||
- decision: { 'parar': bool, 'dist_necessaria': float, 'v_max_sugerida_mps': float|None,
|
||||
'frames_on':int, 'frames_off':int, 'N_on':int, 'N_off':int }
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
H, W = custo_f.shape
|
||||
c0, c1 = central_cols # intervalo central sugerido pelo seu snap
|
||||
c0, c1 = central_cols
|
||||
c0 = max(0, min(W-1, int(c0)))
|
||||
c1 = max(0, min(W, int(c1)))
|
||||
if c1 <= c0:
|
||||
c0, c1 = W//3, 2*W//3 # fallback
|
||||
|
||||
# 1) Máscaras “inseguras”
|
||||
# 1) Máscaras inseguras
|
||||
mask_anom = (anom_f >= thr_anom_block)
|
||||
mask_cost = (custo_f >= thr_cost_block)
|
||||
mask_conf = (conf_f < thr_conf_low)
|
||||
unsafe = mask_anom | mask_cost | mask_conf
|
||||
|
||||
# 2) Largura em colunas por linha (corredor = robô + margem)
|
||||
# 2) Largura em colunas por linha
|
||||
width_need_m = robot_width_m + margin_m
|
||||
cols_need = []
|
||||
for j in range(H):
|
||||
sx = row_scale_x_m[j] if row_scale_x_m is not None else (width_need_m / max(1, (c1 - c0)))
|
||||
if sx is None or sx <= 1e-6:
|
||||
cols_need.append(c1 - c0) # fallback
|
||||
cols_need.append(c1 - c0)
|
||||
else:
|
||||
ncols = int(np.ceil(width_need_m / sx))
|
||||
cols_need.append(max(1, min(W, ncols)))
|
||||
cols_need = np.asarray(cols_need, dtype=int)
|
||||
|
||||
# 3) Varredura por linha: janela central com largura cols_need[j]
|
||||
# 3) Varredura central
|
||||
def central_window(j, ncols):
|
||||
# centra no meio de [c0,c1)
|
||||
mid = (c0 + c1) // 2
|
||||
half = ncols // 2
|
||||
a = max(0, mid - half)
|
||||
b = min(W, a + ncols)
|
||||
# ajusta se estourou esquerda/direita
|
||||
a = max(0, b - ncols)
|
||||
return a, b
|
||||
|
||||
coverage_central = np.zeros(H, np.float32)
|
||||
j_block = None
|
||||
for j in (range(H-1, -1, -1) if near_is_bottom else range(H)): # começa pelo "mais perto"
|
||||
it = (range(H-1, -1, -1) if near_is_bottom else range(H))
|
||||
for j in it:
|
||||
a, b = central_window(j, cols_need[j])
|
||||
unsafe_row = unsafe[j, a:b]
|
||||
coverage = unsafe_row.mean() if (b > a) else 1.0
|
||||
|
|
@ -302,45 +320,109 @@ class CostmapFuser:
|
|||
j_block = j
|
||||
break
|
||||
|
||||
# 4) Cobertura global (fallback diagnóstico)
|
||||
# 4) Cobertura global
|
||||
global_cov = unsafe.mean()
|
||||
|
||||
# 5) d_obs_min em metros
|
||||
# 5) Distância do 1º bloqueio
|
||||
d_obs_min = None if j_block is None else float(row_dist_m[j_block])
|
||||
|
||||
# 6) Viés lateral (onde está mais “fechado”)
|
||||
# mede cobertura do lado esquerdo vs direito dentro do intervalo [c0,c1)
|
||||
# 6) Viés lateral
|
||||
left = unsafe[:, c0:(c0+c1)//2].mean() if (c1-c0) >= 2 else 0.0
|
||||
right = unsafe[:, (c0+c1)//2:c1].mean() if (c1-c0) >= 2 else 0.0
|
||||
side_bias_val = float(np.clip((right - left) / max(1e-6, (right + left)), -1.0, 1.0))
|
||||
|
||||
# 7) Decisão de bloqueio e razão
|
||||
blocked = False
|
||||
# 7) Decisão "raw" (sem persistência)
|
||||
blocked_raw = False
|
||||
reason = "none"
|
||||
if j_block is not None:
|
||||
blocked = True
|
||||
blocked_raw = True
|
||||
reason = "obstacle"
|
||||
elif global_cov >= rho_block_global and (conf_f.mean() < 0.45):
|
||||
blocked = True
|
||||
reason = "blackout" # visão ruim / depth ruim geral
|
||||
# opcional: “narrow” se central ok mas laterais muito ruins
|
||||
blocked_raw = True
|
||||
reason = "blackout"
|
||||
elif coverage_central.max() > 0.45 and (left > 0.7 or right > 0.7):
|
||||
reason = "narrow"
|
||||
|
||||
# 8) Persistência / histerese + decisão de parada (opcional)
|
||||
decision = {
|
||||
"parar": False,
|
||||
"dist_necessaria": None,
|
||||
"v_max_sugerida_mps": None,
|
||||
"frames_on": 0,
|
||||
"frames_off": 0,
|
||||
"N_on": N_on,
|
||||
"N_off": N_off
|
||||
}
|
||||
|
||||
blocked_out = blocked_raw # default: compatível com antes
|
||||
|
||||
if use_persistence:
|
||||
# calcula distância necessária se tivermos velocidade
|
||||
v = float(max(0.0, velocidade_mps or 0.0))
|
||||
dist_freio = max(0.30, (v*v) / max(1e-9, 2.0 * a_max_freio))
|
||||
dist_necessaria = dist_freio + margem_parada
|
||||
decision["dist_necessaria"] = float(dist_necessaria)
|
||||
|
||||
# regra "stop_now" crua (antes do debounce)
|
||||
stop_now = False
|
||||
v_max_sug = None
|
||||
|
||||
if blocked_raw and reason == "blackout":
|
||||
stop_now = True if blackout_imediato else False
|
||||
|
||||
if blocked_raw and reason == "obstacle":
|
||||
if (d_obs_min is not None) and np.isfinite(d_obs_min) and (d_obs_min <= dist_necessaria):
|
||||
stop_now = True
|
||||
else:
|
||||
if (d_obs_min is not None) and np.isfinite(d_obs_min) and (d_obs_min > margem_parada):
|
||||
v_max_sug = float(np.sqrt(max(0.0, 2.0 * a_max_freio * (d_obs_min - margem_parada))))
|
||||
|
||||
# histerese
|
||||
st = self._blk_state
|
||||
if stop_now:
|
||||
st["on"] += 1
|
||||
st["off"] = 0
|
||||
else:
|
||||
st["off"] += 1
|
||||
st["on"] = 0
|
||||
|
||||
if not st["latched"] and st["on"] >= N_on:
|
||||
st["latched"] = True
|
||||
elif st["latched"] and st["off"] >= N_off:
|
||||
st["latched"] = False
|
||||
|
||||
# saída persistente
|
||||
decision.update({
|
||||
"parar": bool(st["latched"]),
|
||||
"v_max_sugerida_mps": v_max_sug,
|
||||
"frames_on": int(st["on"]),
|
||||
"frames_off": int(st["off"])
|
||||
})
|
||||
|
||||
# quando usamos persistência, o 'blocked' exposto passa a refletir a decisão debounced:
|
||||
blocked_out = bool(st["latched"])
|
||||
|
||||
# guarda debug
|
||||
st["reason"] = reason
|
||||
st["dmin"] = (None if d_obs_min is None else float(d_obs_min))
|
||||
st["last_decision"] = "PARAR" if st["latched"] else "LIVRE"
|
||||
|
||||
return {
|
||||
"d_obs_min": d_obs_min,
|
||||
"blocked": bool(blocked),
|
||||
"blocked": bool(blocked_out), # <- já com persistência se habilitada
|
||||
"blocked_raw": bool(blocked_raw), # <- útil pra debug/HUD
|
||||
"reason": reason,
|
||||
"coverage": {
|
||||
"central_max": float(coverage_central.max()),
|
||||
"global": float(global_cov),
|
||||
},
|
||||
"side_bias": {
|
||||
"value": side_bias_val, # <0 = mais fechado à esquerda; >0 = direita
|
||||
"value": side_bias_val,
|
||||
"left_frac": float(left),
|
||||
"right_frac": float(right),
|
||||
},
|
||||
"j_block": (None if j_block is None else int(j_block)),
|
||||
"decision": decision
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue