diff --git a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs index 9ce260382..2b61575d9 100644 --- a/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs @@ -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), diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py index 483be8c19..9c3b2fbc2 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py @@ -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