adicionado regras taticas imu ao controle direcional
This commit is contained in:
Diego Freitas
parent
11ccdca22e
commit
c1f9b88e3c
|
|
@ -230,7 +230,12 @@ namespace AgroBase.Services.Operadores
|
|||
("path_ia_module_params_ervas", VersionamentoService.ArquivoModeloModuleParamsWeedDetector.CaminhoCompleto),
|
||||
("ia_backbone_ervas", "C:\\AgroBaseModels\\Backbones\\nvidia_mit_b1"),
|
||||
("angulo_roll_max", VariaveisEquipamento.AnguloInclinacaoRollMax),
|
||||
("angulo_pitch_max", VariaveisEquipamento.AnguloInclinacaoPitchMax)
|
||||
("angulo_pitch_max", VariaveisEquipamento.AnguloInclinacaoPitchMax),
|
||||
("imu_roll_direita_sinal", 1),
|
||||
("dir_angulo_direita_sinal", 1),
|
||||
("imu_dir_roll_min_aux", 6.0),
|
||||
("imu_dir_bias_attention_deg", 4.0),
|
||||
("imu_dir_bias_risk_deg", 9.0)
|
||||
);
|
||||
}
|
||||
catch (Exception ex)
|
||||
|
|
@ -356,6 +361,9 @@ namespace AgroBase.Services.Operadores
|
|||
("frenagem_automatica", pControle.FrenagemAutomaticaAoParar),
|
||||
("oak_parada_por_bloqueio", pControle.OakParadaPorObstaculo),
|
||||
("imu_parada_por_inclinacao", pControle.ImuParadaPorInclinacao),
|
||||
("imu_auxilio_movimento", false),
|
||||
("imu_auxilio_direcional", false),
|
||||
("imu_direcional_preferir_arco", false),
|
||||
("dir_auxilio_sonar", pControle.DirAuxilioSonar),
|
||||
("mov_auxilio_sonar", pControle.MovAuxilioSonar),
|
||||
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
|
|||
_contexto = ContextoGlobalRedis.get_contexto()
|
||||
_equipamento = ContextoGlobalRedis.get_equipamento()
|
||||
_trajetoria = _contexto.get("Trajetoria", {})
|
||||
_imu = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {}
|
||||
imu_contexto = _montar_contexto_imu_direcional(_imu)
|
||||
|
||||
usar_auxilio_visual = _controle.get("dir_auxilio_sonar", False)
|
||||
dados_visual_worker = None
|
||||
|
|
@ -140,6 +142,7 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
|
|||
"PassosAtraso": _operacao.get("Dir", {}).get("mpc", {}).get("passos_atraso", 1),
|
||||
},
|
||||
"VisualWorker": dados_visual_worker,
|
||||
"IMU": imu_contexto,
|
||||
"Equipamento": {
|
||||
"largura": _equipamento.get("largura", 0.85),
|
||||
"entre_eixos": _equipamento.get("distancia_entre_eixos", 0.92)
|
||||
|
|
@ -192,6 +195,7 @@ def definir_comando(pid: PIDAdaptativo, envio_necessario: bool):
|
|||
"tipo": tipo_movimento.value,
|
||||
"simulacao": []
|
||||
}
|
||||
_cmd = _aplicar_auxilio_imu_direcional(_cmd, contexto)
|
||||
return _montar_comando_retorno(_cmd)
|
||||
|
||||
except Exception as e:
|
||||
|
|
@ -239,6 +243,8 @@ def _comando_mapa_gps_mpc(contexto):
|
|||
if (comando is None):
|
||||
mostrar_log(f"🧭 Direcional MPC | Erro ao definir comando")
|
||||
|
||||
comando = _aplicar_auxilio_imu_direcional(comando, contexto)
|
||||
|
||||
#input("⏸️ Pressione Enter para continuar...")
|
||||
|
||||
return _montar_comando_retorno(comando)
|
||||
|
|
@ -544,3 +550,179 @@ def _montar_costmap_direcional(
|
|||
"debug": {}
|
||||
}
|
||||
|
||||
def _montar_contexto_imu_direcional(_imu: dict):
|
||||
try:
|
||||
ts = float(_imu.get("timestamp", 0.0) or 0.0)
|
||||
ts_s = ts / 1000.0 if ts > 100000000000.0 else ts
|
||||
atualizado = (time.time() - ts_s) <= 1.0 if ts_s > 0 else False
|
||||
|
||||
saude_status = StatusModulo(
|
||||
_imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)
|
||||
)
|
||||
|
||||
operante = saude_status in [StatusModulo.OPERANTE, StatusModulo.ALERTA]
|
||||
|
||||
return {
|
||||
"Valido": bool(_imu.get("valido", False)) and atualizado and operante,
|
||||
"Operante": operante,
|
||||
"Atualizado": atualizado,
|
||||
"StatusSaude": saude_status.value,
|
||||
|
||||
"Roll": float(_imu.get("roll_filtrado", _imu.get("roll_seg", _imu.get("roll", 0.0))) or 0.0),
|
||||
"Pitch": float(_imu.get("pitch_filtrado", _imu.get("pitch_seg", _imu.get("pitch", 0.0))) or 0.0),
|
||||
|
||||
"RollRateDps": float(_imu.get("roll_rate_dps", 0.0) or 0.0),
|
||||
"PitchRateDps": float(_imu.get("pitch_rate_dps", 0.0) or 0.0),
|
||||
|
||||
"RiskLevel": str(_imu.get("risk_level", "unknown") or "unknown"),
|
||||
"RiskLevelNum": int(_imu.get("risk_level_num", 0) or 0),
|
||||
"RiskScore": float(_imu.get("risk_score", 0.0) or 0.0),
|
||||
|
||||
"VelocidadeFactor": float(_imu.get("velocidade_factor", 1.0) or 1.0),
|
||||
"Stop": bool(_imu.get("stop", False)),
|
||||
"EmergencyStop": bool(_imu.get("emergency_stop", False)),
|
||||
"BloquearAvanco": bool(_imu.get("bloquear_avanco", False)),
|
||||
"Motivos": _imu.get("motivos_risco", []) or [],
|
||||
}
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"⚠️ Erro ao montar contexto IMU direcional: {e}")
|
||||
return {
|
||||
"Valido": False,
|
||||
"Operante": False,
|
||||
"Atualizado": False,
|
||||
"RiskLevelNum": 4,
|
||||
"Stop": True,
|
||||
"EmergencyStop": True,
|
||||
"BloquearAvanco": True,
|
||||
"Motivos": [str(e)],
|
||||
}
|
||||
|
||||
def _aplicar_auxilio_imu_direcional(comando: dict, contexto: dict):
|
||||
try:
|
||||
if not isinstance(comando, dict):
|
||||
return comando
|
||||
|
||||
_controle = ContextoGlobalRedis.get_controle()
|
||||
_operacao = ContextoGlobalRedis.get_operacao()
|
||||
_equipamento = ContextoGlobalRedis.get_equipamento()
|
||||
|
||||
usar_auxilio = bool(_controle.get("imu_auxilio_direcional", True))
|
||||
if not usar_auxilio:
|
||||
return comando
|
||||
|
||||
imu = contexto.get("IMU", {}) or {}
|
||||
|
||||
if not bool(imu.get("Valido", False)):
|
||||
return comando
|
||||
|
||||
risk_num = int(imu.get("RiskLevelNum", 0) or 0)
|
||||
|
||||
# Critical/emergency não é trabalho do direcional.
|
||||
# RegrasTaticas deve parar.
|
||||
if risk_num >= 3:
|
||||
return comando
|
||||
|
||||
# Sem risco, não mexe.
|
||||
if risk_num <= 0:
|
||||
return comando
|
||||
|
||||
roll = float(imu.get("Roll", 0.0) or 0.0)
|
||||
pitch = float(imu.get("Pitch", 0.0) or 0.0)
|
||||
roll_rate = float(imu.get("RollRateDps", 0.0) or 0.0)
|
||||
|
||||
# O auxílio direcional é principalmente para roll lateral.
|
||||
# Pitch é melhor tratar com velocidade/parada.
|
||||
roll_abs = abs(roll)
|
||||
|
||||
roll_min_aux = float(_equipamento.get("imu_dir_roll_min_aux", 6.0))
|
||||
if roll_abs < roll_min_aux:
|
||||
return comando
|
||||
|
||||
ang_max = float(_operacao.get("Dir", {}).get("angulo_max", _controle.get("angulo_max", 30.0)) or 30.0)
|
||||
|
||||
angulo_atual = float(comando.get("angulo", 0.0) or 0.0)
|
||||
tipo_atual = comando.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value)
|
||||
|
||||
# Convenções configuráveis:
|
||||
# imu_roll_direita_sinal:
|
||||
# +1 se roll positivo significa tombando/inclinando para direita
|
||||
# -1 se roll negativo significa direita
|
||||
#
|
||||
# dir_angulo_direita_sinal:
|
||||
# +1 se ângulo positivo esterça para direita
|
||||
# -1 se ângulo negativo esterça para direita
|
||||
roll_direita_sinal = float(_equipamento.get("imu_roll_direita_sinal", 1.0) or 1.0)
|
||||
angulo_direita_sinal = float(_equipamento.get("dir_angulo_direita_sinal", 1.0) or 1.0)
|
||||
|
||||
lado_queda = np.sign(roll * roll_direita_sinal)
|
||||
|
||||
if lado_queda == 0:
|
||||
return comando
|
||||
|
||||
# Direção de esterço que tende a aliviar a rolagem:
|
||||
# se está caindo para direita, viés para direita.
|
||||
sinal_angulo_estabilizante = lado_queda * angulo_direita_sinal
|
||||
|
||||
# Intensidade conservadora.
|
||||
bias_max_attention = float(_equipamento.get("imu_dir_bias_attention_deg", 4.0))
|
||||
bias_max_risk = float(_equipamento.get("imu_dir_bias_risk_deg", 9.0))
|
||||
|
||||
# Usa risk_score e roll para modular.
|
||||
risk_score = float(imu.get("RiskScore", 0.0) or 0.0)
|
||||
intensidade_roll = min(1.0, max(0.0, (roll_abs - roll_min_aux) / max(1e-6, 12.0 - roll_min_aux)))
|
||||
intensidade_risk = min(1.0, max(0.0, risk_score / 100.0))
|
||||
intensidade_rate = min(1.0, abs(roll_rate) / 60.0)
|
||||
|
||||
intensidade = max(intensidade_roll, intensidade_risk, intensidade_rate)
|
||||
|
||||
bias_max = bias_max_risk if risk_num >= 2 else bias_max_attention
|
||||
bias = sinal_angulo_estabilizante * bias_max * intensidade
|
||||
|
||||
# Se o comando atual está contra a estabilidade, reduz mais.
|
||||
comando_contra_estabilidade = (angulo_atual * sinal_angulo_estabilizante) < 0
|
||||
|
||||
if comando_contra_estabilidade:
|
||||
# Remove parte do comando contrário e aplica viés estabilizante.
|
||||
angulo_novo = (angulo_atual * 0.35) + bias
|
||||
else:
|
||||
# Se já está para o lado certo, só ajuda de leve.
|
||||
angulo_novo = angulo_atual + (bias * 0.50)
|
||||
|
||||
angulo_novo = max(-ang_max, min(ang_max, angulo_novo))
|
||||
|
||||
comando["angulo_original_sem_imu"] = round(angulo_atual, 2)
|
||||
comando["angulo"] = round(angulo_novo, 2)
|
||||
|
||||
debug = comando.get("debug_custo", {})
|
||||
if not isinstance(debug, dict):
|
||||
debug = {}
|
||||
|
||||
debug["imu_direcional"] = {
|
||||
"ativo": True,
|
||||
"risk_num": risk_num,
|
||||
"roll": round(roll, 3),
|
||||
"pitch": round(pitch, 3),
|
||||
"roll_rate_dps": round(roll_rate, 3),
|
||||
"lado_queda": float(lado_queda),
|
||||
"bias_deg": round(float(bias), 3),
|
||||
"angulo_original": round(float(angulo_atual), 3),
|
||||
"angulo_final": round(float(angulo_novo), 3),
|
||||
"comando_contra_estabilidade": bool(comando_contra_estabilidade),
|
||||
}
|
||||
|
||||
comando["debug_custo"] = debug
|
||||
|
||||
# Em risco 2, preferir MovimentoArco pode ser melhor que rodas dianteiras,
|
||||
# desde que o robô suporte isso com suavidade.
|
||||
usar_arco_imu = bool(_controle.get("imu_direcional_preferir_arco", True))
|
||||
|
||||
if usar_arco_imu and risk_num >= 2:
|
||||
comando["tipo_original_sem_imu"] = tipo_atual
|
||||
comando["tipo"] = TipoMovimentoDirecional.MovimentoArco.value
|
||||
|
||||
return comando
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"⚠️ Erro ao aplicar auxílio direcional IMU: {e}")
|
||||
return comando
|
||||
|
|
|
|||
Loading…
Reference in New Issue