adicionado regras taticas imu ao controle direcional

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),
 

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