完善Detour状态估计与轨迹跟踪验证

MultiWheelC/Movements/RotateInPlace.cs

@@ -200,6 +200,8 @@ namespace MultiWheelC
                         adapter
                             .StopXYThDrivePreserveSteeringState();
 
+                        // 到位稳定只依赖已经单独校验的航向。
+                        // 位置候选留到停车后处理，避免位置抖动中断航向闭环。
                         if (thPid.IsArrived())
                             break;
 
@@ -253,6 +255,40 @@ namespace MultiWheelC
                 CommandAngularSpeedObserver?.Invoke(0f);
                 adapter.StopXYThDrivePreserveSteeringState();
 
+                if (IsLocalizationRecoveryPending(
+                        stateProvider))
+                {
+                    BeginPostRotationPositionRecovery(
+                        stateProvider);
+                    var recoveryStarted = DateTime.Now;
+
+                    while (true)
+                    {
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+
+                        if (stateProvider.TryGetState(out _) &&
+                            !IsLocalizationRecoveryPending(
+                                stateProvider))
+                        {
+                            break;
+                        }
+
+                        if ((DateTime.Now - recoveryStarted)
+                                .TotalSeconds >
+                            config
+                                .ParkingDetourJumpConfirmationTimeoutSeconds)
+                        {
+                            throw new InvalidOperationException(
+                                "原地自转完成后Detour位置在限定时间内未恢复。" +
+                                GetStateProviderFailureReason(
+                                    stateProvider));
+                        }
+
+                        yield return true;
+                    }
+                }
+
                 // 航向正常到位后复用统一回正动作；异常或取消会直接进入finally停车。
                 var wheelPreparation =
                     new PrepareWheelsForward();
@@ -356,6 +392,36 @@ namespace MultiWheelC
                     angleDegrees);
             }
 
+            if (stateProvider is
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                if (!wheelProvider.TryGetHeadingRadians(
+                        out var wheelHeadingRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法从Detour状态源读取有效车辆航向。" +
+                        wheelProvider.LastHeadingFailureReason);
+                }
+
+                return (float)AngleMath.RadiansToDegrees(
+                    wheelHeadingRadians);
+            }
+
+            if (stateProvider is
+                DetourVehicleStateProvider detourProvider)
+            {
+                if (!detourProvider.TryGetHeadingRadians(
+                        out var detourHeadingRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法从Detour状态源读取有效车辆航向。" +
+                        detourProvider.LastHeadingFailureReason);
+                }
+
+                return (float)AngleMath.RadiansToDegrees(
+                    detourHeadingRadians);
+            }
+
             if (stateProvider == null ||
                 !stateProvider.TryGetState(out var state))
             {
@@ -369,6 +435,26 @@ namespace MultiWheelC
                 state.PoseInWorld.YawRadians);
         }
 
+        /// <summary>
+        /// 通知配置化状态源：车辆已经停车，可以重新确认旋转期间的位置候选。
+        /// </summary>
+        private static void BeginPostRotationPositionRecovery(
+            IVehicleStateProvider stateProvider)
+        {
+            if (stateProvider is
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                wheelProvider.BeginPostRotationPositionRecovery();
+                return;
+            }
+
+            if (stateProvider is
+                DetourVehicleStateProvider detourProvider)
+            {
+                detourProvider.BeginPostRotationPositionRecovery();
+            }
+        }
+
         /// <summary>
         /// 获取已知停车状态源最近一次失败原因，未知实现返回空字符串。
         /// </summary>
@@ -390,6 +476,31 @@ namespace MultiWheelC
             return string.Empty;
         }
 
+        /// <summary>
+        /// 判断Detour是否仍在使用轮组预测确认疑似位姿不连续。
+        /// </summary>
+        private static bool IsLocalizationRecoveryPending(
+            IVehicleStateProvider stateProvider)
+        {
+            if (stateProvider is
+                    WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                return wheelProvider.TryGetLatestDetourDiagnostics(
+                           out var jumpCandidateActive,
+                           out _,
+                           out _,
+                           out _,
+                           out _,
+                           out _,
+                           out _) &&
+                       jumpCandidateActive;
+            }
+
+            return stateProvider is
+                       DetourVehicleStateProvider detourProvider &&
+                   detourProvider.IsJumpCandidateActive;
+        }
+
         /// <summary>
         /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
         /// </summary>