melhoria do watchdog da trajetoria, usar direcional para buscar centro ao entrar no corredor, PP assume o controle antes da curva começar, recuperação de curva de manobra mais robusto

AgroBase/AgroBase/Models/Variaveis.cs

@@ -2,6 +2,7 @@ using AgroBase.Models.Modules;
 using AgroBase.Services;
 using System;
 using System.Collections.Generic;
+using System.Collections.Concurrent;
 using System.Drawing.Imaging;
 using System.Drawing;
 using System.Linq;
@@ -35,6 +36,168 @@ namespace AgroBase.Models
         public static bool DebugMode { get; set; } = false;
         public static bool Fechando { get; set; }  = false;
 
+        // ============================================================================
+        // LOG GLOBAL DE SESSÃO DO AGROBASE
+        // ============================================================================
+        // Toda chamada a Variaveis.MostrarLog(...) continua indo ao console e é
+        // enfileirada para persistência em um arquivo único por execução do processo.
+        // O thread que chamou MostrarLog NUNCA faz escrita normal de disco.
+        // Assim, logging não entra no caminho crítico do controle.
+        private static readonly object _logSessaoLock = new object();
+        private static readonly ConcurrentQueue<string> _logSessaoFila = new ConcurrentQueue<string>();
+        private static readonly AutoResetEvent _logSessaoSignal = new AutoResetEvent(false);
+        private static StreamWriter _logSessaoWriter;
+        private static Thread _logSessaoThread;
+        private static string _caminhoLogSessao;
+        private static bool _logSessaoFalhou;
+        private static bool _logSessaoHookRegistrado;
+        private static volatile bool _logSessaoEncerrando;
+
+        public static string CaminhoLogSessao
+        {
+            get { return _caminhoLogSessao ?? string.Empty; }
+        }
+
+        private static void GarantirLogSessao()
+        {
+            if (_logSessaoThread != null || _logSessaoFalhou)
+                return;
+
+            lock (_logSessaoLock)
+            {
+                if (_logSessaoThread != null || _logSessaoFalhou)
+                    return;
+
+                try
+                {
+                    string pasta = Path.Combine(CaminhoLogsDispositivos, "Sessoes");
+                    Directory.CreateDirectory(pasta);
+
+                    DateTime agora = DateTime.Now;
+                    int pid = Process.GetCurrentProcess().Id;
+                    string nome = $"AgroBase_{agora:yyyyMMdd_HHmmss_fff}_PID{pid}.log";
+
+                    _caminhoLogSessao = Path.Combine(pasta, nome);
+                    _logSessaoWriter = new StreamWriter(_caminhoLogSessao, true);
+                    _logSessaoWriter.AutoFlush = false;
+                    _logSessaoEncerrando = false;
+
+                    _logSessaoWriter.WriteLine("============================================================");
+                    _logSessaoWriter.WriteLine("AGROBASE SESSION LOG");
+                    _logSessaoWriter.WriteLine($"Inicio: {agora:yyyy-MM-dd HH:mm:ss.fff}");
+                    _logSessaoWriter.WriteLine($"PID: {pid}");
+                    _logSessaoWriter.WriteLine($"Aplicacao: {NomeAplicacao}");
+                    _logSessaoWriter.WriteLine($"Versao: {Versao}");
+                    _logSessaoWriter.WriteLine($"Machine: {Environment.MachineName}");
+                    _logSessaoWriter.WriteLine("============================================================");
+                    _logSessaoWriter.Flush();
+
+                    _logSessaoThread = new Thread(ProcessarFilaLogSessao);
+                    _logSessaoThread.IsBackground = true;
+                    _logSessaoThread.Name = "AgroBase.SessionLogger";
+                    _logSessaoThread.Start();
+
+                    if (!_logSessaoHookRegistrado)
+                    {
+                        _logSessaoHookRegistrado = true;
+                        AppDomain.CurrentDomain.ProcessExit += (sender, args) => EncerrarLogSessao();
+                    }
+                }
+                catch (Exception ex)
+                {
+                    _logSessaoFalhou = true;
+                    try
+                    {
+                        _logSessaoWriter?.Dispose();
+                        _logSessaoWriter = null;
+                        Debug.WriteLine("[AGROBASE/LOGGER] Falha ao iniciar log de sessão: " + ex.Message);
+                    }
+                    catch { }
+                }
+            }
+        }
+
+        private static void ProcessarFilaLogSessao()
+        {
+            try
+            {
+                while (!_logSessaoEncerrando || !_logSessaoFila.IsEmpty)
+                {
+                    bool escreveu = false;
+                    string linha;
+
+                    while (_logSessaoFila.TryDequeue(out linha))
+                    {
+                        _logSessaoWriter?.WriteLine(linha);
+                        escreveu = true;
+                    }
+
+                    if (escreveu)
+                        _logSessaoWriter?.Flush();
+
+                    if (!_logSessaoEncerrando)
+                        _logSessaoSignal.WaitOne(500);
+                }
+
+                _logSessaoWriter?.Flush();
+            }
+            catch (Exception ex)
+            {
+                _logSessaoFalhou = true;
+                try
+                {
+                    Debug.WriteLine("[AGROBASE/LOGGER] Falha no writer de sessão: " + ex.Message);
+                }
+                catch { }
+            }
+        }
+
+        public static void EncerrarLogSessao()
+        {
+            Thread thread;
+
+            lock (_logSessaoLock)
+            {
+                if (_logSessaoThread == null && _logSessaoWriter == null)
+                    return;
+
+                try
+                {
+                    _logSessaoFila.Enqueue(
+                        $"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff} " +
+                        $"[T{Thread.CurrentThread.ManagedThreadId}] [AGROBASE] [SESSION] Encerrando log."
+                    );
+                }
+                catch { }
+
+                _logSessaoEncerrando = true;
+                _logSessaoSignal.Set();
+                thread = _logSessaoThread;
+            }
+
+            try
+            {
+                if (thread != null && thread != Thread.CurrentThread)
+                    thread.Join(1500);
+            }
+            catch { }
+
+            lock (_logSessaoLock)
+            {
+                try
+                {
+                    _logSessaoWriter?.Flush();
+                    _logSessaoWriter?.Dispose();
+                }
+                catch { }
+                finally
+                {
+                    _logSessaoWriter = null;
+                    _logSessaoThread = null;
+                }
+            }
+        }
+
         public static string CaminhoSistema { get; set; } = AppDomain.CurrentDomain.BaseDirectory;
         public static string NomeAplicacao
         {
@@ -784,7 +947,37 @@ namespace AgroBase.Models
 
         public static void MostrarLog(string Mensagem)
         {
-            Console.WriteLine($"[AGROBASE] {Mensagem}");
+            string mensagem = Mensagem ?? string.Empty;
+            string linhaConsole = $"[AGROBASE] {mensagem}";
+
+            // Mantém o comportamento histórico do console.
+            Console.WriteLine(linhaConsole);
+
+            // A thread chamadora apenas enfileira. Escrita/flush ficam no logger.
+            try
+            {
+                GarantirLogSessao();
+
+                if (_logSessaoFalhou || _logSessaoThread == null)
+                    return;
+
+                string linhaArquivo =
+                    $"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff} " +
+                    $"[T{Thread.CurrentThread.ManagedThreadId}] " +
+                    linhaConsole;
+
+                _logSessaoFila.Enqueue(linhaArquivo);
+                _logSessaoSignal.Set();
+            }
+            catch (Exception ex)
+            {
+                // Logging nunca pode impactar controle/movimento.
+                try
+                {
+                    Debug.WriteLine("[AGROBASE/LOGGER] Falha ao enfileirar log: " + ex.Message);
+                }
+                catch { }
+            }
         }
 
 
@@ -825,8 +1018,9 @@ namespace AgroBase.Models
         public static double LarguraDireitaCm { get; } = 53.0; // 22
         public static double ComprimentoFrenteCm { get; } = 90.0; // 7
         public static double ComprimentoTrasCm { get; } = 40.0; // 107
-        public static double DistanciaEntreEixosCm { get; set; } = 92.0;
+        public static double DistanciaEntreEixosCm { get; } = 92.0;
-        public static double BitolaRodasCm { get; set; } = 75.0;
+        public static double BitolaRodasCm { get; } = 75.0;
+        public static double KuDirecional { get; } = 2.65;
         public static double LeverArmFrontalCm { get; set; } = 37.0; // cm
         public static double LeverArmLateralCm { get; set; } = 0.0; // cm
         public static double LarguraEquipamentoMm
@@ -1179,7 +1373,7 @@ namespace AgroBase.Models
             // ------------------------------------------------------------
             // Deve ser o MESMO valor usado em:
             // mpc_v16 -> cinematica_4ws_ku
-            double Ku = 2.5;
+            double Ku = KuDirecional;
 
             double kappaEf =
                 kappaGeom /

AgroBase/AgroBase/Services/Operadores/HealthWorkerService.cs

@@ -252,6 +252,7 @@ namespace AgroBase.Services.Operadores
                     ("serial_number", VariaveisEquipamento.Parametros.serial_number),
                     ("pct_vel_min", VariaveisEquipamento.PercentualVelMin),
                     ("largura", VariaveisEquipamento.LarguraEquipamentoMm / 1000.0),
+                    ("ku_direcional", VariaveisEquipamento.KuDirecional),
                     ("distancia_entre_eixos", VariaveisEquipamento.DistanciaEntreEixosCm / 100.0),
                     ("qtd_bicos", VariaveisEquipamento.QuantidadeBicosPulverizadores),
                     ("percentual_reservatorio_min", VariaveisEquipamento.PercentualReservatorioMin),
@@ -389,6 +390,7 @@ namespace AgroBase.Services.Operadores
                                 tempo_entre_comandos = (op.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Enums.T_Code.Dir)?.DelayEnvioComando ?? 500) / 1000.0,
                                 horizonte = pControle.MpcHorizonte,
                                 passos_atraso = 1,
+                                ku_direcional = VariaveisEquipamento.KuDirecional,
                             }
                         }
                     ),

AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/direcional.py

@@ -518,6 +518,7 @@ def _montar_contexto_direcional(*, operacao, controle, contexto_global, equipame
         "Equipamento": {
             "largura": _float(equipamento.get("largura", 0.85), 0.85),
             "entre_eixos": _float(equipamento.get("distancia_entre_eixos", 0.92), 0.92),
+            "ku_direcional": _float(equipamento.get("ku_direcional", 2.65), 2.65),
             "imu_roll_direita_sinal": _float(equipamento.get("imu_roll_direita_sinal", 1.0), 1.0),
             "dir_angulo_direita_sinal": _float(equipamento.get("dir_angulo_direita_sinal", 1.0), 1.0),
             "imu_dir_roll_min_aux": _float(equipamento.get("imu_dir_roll_min_aux", 6.0), 6.0),

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

@@ -450,6 +450,21 @@ class ControladorMPC:
             1.0, min_value=0.25, max_value=2.0,
         )
 
+        # V18 - handoff antecipado para o Pure Pursuit.
+        #
+        # SaindoRua ja nasce alguns metros antes da BordaSaida para reduzir a
+        # velocidade. Nessa fase o MPC pesado nao deve mais antecipar a curva:
+        # o mesmo Pure Pursuit que executa a Dubins assume cedo, segue reto
+        # enquanto a centerline ainda e reta e comeca a esterçar naturalmente
+        # quando o look-ahead passa a enxergar a ferradura.
+        #
+        # A busca por mudanca de corredor evita ativar esse handoff no fim do
+        # ultimo corredor, onde nao existe uma manobra Dubins seguinte.
+        self._dubins_handoff_busca_frente_m = _safe_float(
+            parametros_mpc.get("dubins_handoff_busca_frente_m", 8.0),
+            8.0, min_value=2.0, max_value=20.0,
+        )
+
         # ------------------------------------------------------------------
         # PATH TRACKING V2 - referência geométrica da passada
         # ------------------------------------------------------------------
@@ -527,6 +542,39 @@ class ControladorMPC:
             3.0, min_value=0.5, max_value=8.0,
         )
 
+        # ------------------------------------------------------------------
+        # V19 - ENTRY CAPTURE pós-Dubins
+        # ------------------------------------------------------------------
+        # O campo mostrou que, ao terminar a ferradura, o rover pode entrar na
+        # nova passada com 20-70 cm de cross-track e ainda 5-12 graus de yaw.
+        # A política antiga priorizava yaw e só depois liberava o crab, fazendo
+        # o rover consumir alguns metros dentro da rua antes de centralizar.
+        #
+        # Agora o fim do Pure Pursuit arma uma janela curta "centro primeiro":
+        # - se o erro lateral ainda é relevante e |heading| <= 15 graus,
+        #   MovimentoDiagonal recebe prioridade mesmo sem heading perfeito;
+        # - heading muito grande continua com Arco/Dianteira normalmente;
+        # - assim que |e_lat| <= 10 cm, a política normal volta a mandar;
+        # - existe limite de distância para o latch nunca permanecer indefinido.
+        self._entry_capture_enabled = bool(
+            parametros_mpc.get("entry_capture_enabled", True)
+        )
+        self._entry_capture_heading_max_graus = _safe_float(
+            parametros_mpc.get("entry_capture_heading_max_graus", 15.0),
+            15.0, min_value=3.0, max_value=25.0,
+        )
+        self._entry_capture_lateral_release_m = _safe_float(
+            parametros_mpc.get("entry_capture_lateral_release_m", 0.10),
+            0.10, min_value=0.03, max_value=0.30,
+        )
+        self._entry_capture_max_dist_m = _safe_float(
+            parametros_mpc.get("entry_capture_max_dist_m", 4.0),
+            4.0, min_value=1.0, max_value=10.0,
+        )
+        self._entry_capture_active = False
+        self._entry_capture_dist_m = 0.0
+        self._dubins_pp_ativo_ciclo_anterior = False
+
         self._lateral_deadband_m = _safe_float(
             parametros_mpc.get("lateral_deadband_m", 0.05),
             0.05, min_value=0.0, max_value=0.30,
@@ -699,9 +747,14 @@ class ControladorMPC:
         # é o oposto do comando de alto nível (efeito empilhadeira).
         #
         # O fator Ku preserva a mesma correção dinâmica usada pelo simulador C#.
+        #equipamento = _as_dict(_as_dict(contexto).get("Equipamento", {}))
+        #self._cinematica_4ws_ku = _safe_float(
+        #    equipamento.get("ku_direcional", 2.65),
+        #    2.65, min_value=0.0, max_value=20.0,
+        #)
         self._cinematica_4ws_ku = _safe_float(
-            parametros_mpc.get("cinematica_4ws_ku", 2.5),
+            parametros_mpc.get("ku_direcional", 2.65),
-            2.5, min_value=0.0, max_value=20.0,
+            2.65, min_value=0.0, max_value=20.0,
         )
         self._cinematica_entre_eixos_m = _safe_float(
             parametros_mpc.get("cinematica_entre_eixos_m", 0.94),
@@ -1366,17 +1419,53 @@ class ControladorMPC:
 
         return traj
 
-    def _usar_seguidor_dubins(self, status_carro, idx_base):
+    def _ha_transicao_corredor_adiante(self, idx_base):
-        """V12: cabeceira estrutural usa Pure Pursuit dedicado em vez da arvore MPC.
+        """Confirma que existe uma troca de corredor logo adiante na centerline.
 
-        O C# ja coloca Status=Manobrando quando o ponto atual/proximo pertence
+        Usado somente para o handoff antecipado SaindoRua -> Pure Pursuit.
-        a geometria de manobra. Para o primeiro A/B de campo/simulador usamos
+        A verificacao e geometrica/estrutural, nao altera progresso visitado.
-        esse contrato diretamente, evitando depender do valor numerico privado
+        """
-        de TipoPontoRua no Python.
+        if not self.pontos_info or self._s_nodes is None or len(self._s_nodes) == 0:
+            return False
+
+        n = len(self.pontos_info)
+        idx = max(0, min(_safe_int(idx_base, 0), n - 1))
+        corredor_base = self._idx_corredor(idx)
+        if corredor_base < 0:
+            return False
+
+        s_base = float(self._s_nodes[idx])
+        limite_m = max(2.0, float(self._dubins_handoff_busca_frente_m))
+
+        for j in range(idx + 1, n):
+            if float(self._s_nodes[j]) - s_base > limite_m:
+                break
+
+            corredor_j = self._idx_corredor(j)
+            if corredor_j >= 0 and corredor_j != corredor_base:
+                return True
+
+        return False
+
+    def _usar_seguidor_dubins(self, status_carro, idx_base):
+        """V18: Pure Pursuit assume a cabeceira antes da BordaSaida.
+
+        - Manobrando: contrato ja validado em campo.
+        - SaindoRua: assume antecipadamente SOMENTE se ha uma troca real de
+          corredor logo adiante na centerline. Assim o MPC pesado deixa de
+          "prometer" uma curva antes do handoff e o PP chega contextualizado
+          desde o trecho reto de aproximacao.
         """
         if not bool(self._seguidor_dubins_enabled):
             return False
-        return _status_in(status_carro, [StatusCarroMapa.Manobrando])
+
+        if _status_in(status_carro, [StatusCarroMapa.Manobrando]):
+            return True
+
+        if _status_in(status_carro, [StatusCarroMapa.SaindoRua]):
+            return self._ha_transicao_corredor_adiante(idx_base)
+
+        return False
 
     def _comando_seguidor_dubins(
         self, contexto, comando_anterior, x, y, theta, velocidade, status_carro,
@@ -1425,7 +1514,11 @@ class ControladorMPC:
             "erro_heading_caminho_graus": abs(float(math.degrees(dbg["erro_heading"]))),
             "debug_custo": {},
             "debug_seguidor_dubins": {
-                "modo": "pure_pursuit_4ws",
+                "modo": (
+                    "pure_pursuit_4ws_antecipado"
+                    if _status_in(status_carro, [StatusCarroMapa.SaindoRua])
+                    else "pure_pursuit_4ws"
+                ),
                 "lookahead_m": float(dbg["lookahead_m"]),
                 "distancia_alvo_m": float(dbg["distancia_alvo_m"]),
                 "alpha_graus": float(math.degrees(dbg["alpha"])),
@@ -2489,6 +2582,31 @@ class ControladorMPC:
         e_head_deg = abs(math.degrees(erro_heading_rad))
         tipo_prev = _movimento_from_value(tipo_preferido)
 
+        # V19 - captura lateral imediatamente após a ferradura.
+        #
+        # Este override é propositalmente ANTES da política normal de
+        # CaminhandoRua/EntrandoRua. O estado é armado apenas pela saída real do
+        # seguidor Dubins, portanto não altera a centralização nominal de uma rua
+        # comum. Enquanto ativo, o crab pode começar com heading moderadamente
+        # errado, preservando a prioridade de manter o rover entre as linhas.
+        if (
+            bool(getattr(self, "_entry_capture_active", False))
+            and _status_in(
+                status_carro,
+                [
+                    StatusCarroMapa.EntrandoRua,
+                    StatusCarroMapa.CaminhandoRua,
+                    StatusCarroMapa.Direcionando,
+                ],
+            )
+            and abs(e_lat) > float(self._entry_capture_lateral_release_m)
+            and e_head_deg <= float(self._entry_capture_heading_max_graus)
+        ):
+            return (
+                TipoMovimentoDirecional.MovimentoDiagonal,
+                self._angulo_diagonal_tracking(e_lat, v),
+            )
+
         if _status_in(status_carro, [StatusCarroMapa.CaminhandoRua]):
             return self._referencia_supervisor_corredor(
                 e_lat,
@@ -3774,11 +3892,21 @@ class ControladorMPC:
                 velocidade,
             )
 
-            # V12 - cabeceira Dubins: a rota ja esta pronta. Em Manobrando nao
+            # V18 - handoff antecipado da cabeceira. SaindoRua ja opera lento e,
-            # abrimos a arvore MPC; usamos Pure Pursuit 4WS mirando um ponto
+            # quando existe troca de corredor logo adiante, entrega o volante ao
-            # fisico adiante na propria curva. O heading/tangente fica apenas
+            # mesmo Pure Pursuit 4WS usado em Manobrando. Enquanto a centerline
-            # como telemetria e nao alivia o comando antes da hora.
+            # ainda e reta o comando permanece praticamente reto; quando o alvo
-            if self._usar_seguidor_dubins(status_carro, idx_alvo_correcao):
+            # entra na Dubins a curva nasce sem a previsao concorrente do MPC.
+            usando_pp_dubins = self._usar_seguidor_dubins(
+                status_carro, idx_alvo_correcao
+            )
+            if usando_pp_dubins:
+                # V19: enquanto PP possui o volante apenas memorizamos que a
+                # próxima saída dele deve armar o Entry Capture.
+                self._dubins_pp_ativo_ciclo_anterior = True
+                self._entry_capture_active = False
+                self._entry_capture_dist_m = 0.0
+
                 return self._comando_seguidor_dubins(
                     contexto=contexto,
                     comando_anterior=comando_anterior,
@@ -3791,6 +3919,77 @@ class ControladorMPC:
                     ponto_alvo_real=ponto_alvo_real,
                 )
 
+            # ------------------------------------------------------------
+            # V19 - arma/atualiza ENTRY CAPTURE somente no ciclo REAL.
+            # ------------------------------------------------------------
+            status_entry_ok = _status_in(
+                status_carro,
+                [
+                    StatusCarroMapa.EntrandoRua,
+                    StatusCarroMapa.CaminhandoRua,
+                    StatusCarroMapa.Direcionando,
+                ],
+            )
+
+            if bool(getattr(self, "_dubins_pp_ativo_ciclo_anterior", False)):
+                self._dubins_pp_ativo_ciclo_anterior = False
+                if bool(self._entry_capture_enabled) and status_entry_ok:
+                    self._entry_capture_active = True
+                    self._entry_capture_dist_m = 0.0
+                    _log(
+                        "[MPC/V19 ENTRY] Capture armado após Dubins | "
+                        f"status={_enum_name(status_carro, StatusCarroMapa)}"
+                    )
+
+            if bool(getattr(self, "_entry_capture_active", False)):
+                if not status_entry_ok:
+                    self._entry_capture_active = False
+                    self._entry_capture_dist_m = 0.0
+                    _log(
+                        "[MPC/V19 ENTRY] Capture encerrado por mudança de estado | "
+                        f"status={_enum_name(status_carro, StatusCarroMapa)}"
+                    )
+                else:
+                    dt_entry = _safe_float(
+                        self.tempo_execucao_local,
+                        0.5,
+                        min_value=0.05,
+                        max_value=1.5,
+                    )
+                    self._entry_capture_dist_m += (
+                        abs(float(velocidade)) * dt_entry
+                    )
+
+                    try:
+                        ref_entry = self._referencia_caminho_lookahead(
+                            x, y, idx_alvo_correcao, velocidade, status_carro
+                        )
+                        e_lat_entry = abs(float(ref_entry["e_lat"]))
+                        theta_entry = float(ref_entry["theta_ref"])
+                        e_head_entry = abs(math.degrees(
+                            self._erro_heading_caminho(theta, theta_entry)
+                        ))
+                    except Exception:
+                        e_lat_entry = float("inf")
+                        e_head_entry = float("inf")
+
+                    if e_lat_entry <= float(self._entry_capture_lateral_release_m):
+                        self._entry_capture_active = False
+                        _log(
+                            "[MPC/V19 ENTRY] Capture concluído | "
+                            f"e_lat={e_lat_entry:.2f}m | "
+                            f"e_head={e_head_entry:.1f}deg | "
+                            f"dist={self._entry_capture_dist_m:.2f}m"
+                        )
+                    elif self._entry_capture_dist_m >= float(self._entry_capture_max_dist_m):
+                        self._entry_capture_active = False
+                        _log(
+                            "[MPC/V19 ENTRY] Capture expirado por distância | "
+                            f"e_lat={e_lat_entry:.2f}m | "
+                            f"e_head={e_head_entry:.1f}deg | "
+                            f"dist={self._entry_capture_dist_m:.2f}m"
+                        )
+
             # -------------------- Planejamento: horizonte ESPACIAL fixo --------------------
             S_ALVO   = float(self.horizonte)
             V_FLOOR  = float(getattr(self, "_vmin_planejamento", 0.30))