chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -2,13 +2,16 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.Threading;
-using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

 namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

-/// <summary>以固定预热和五次测量隔离比较所有请求平滑方法的离线入口。</summary>
+/// <summary>
+/// 原始路径基线与一次 Local G2 平滑的离线比较入口。
+/// 此类仅汇总已冻结请求的质量、确定性摘要和耗时，不参与实时控制或修改平滑结果。
+/// </summary>
 public sealed class PathSmoothingComparisonService
 {
    private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
@@ -16,13 +19,16 @@ public sealed class PathSmoothingComparisonService
    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
    private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();

-    /// <summary>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
+    /// <summary>创建原始基线并比较一次 Local G2 运行的质量、耗时和推荐资格。</summary>
+    /// <param name="request">比较请求，包含待平滑的不可变请求快照和比较设置；为 <see langword="null"/> 时返回带失败基线的结果。</param>
+    /// <param name="cancellationToken">取消比较和重复计时的调用方令牌。</param>
+    /// <returns>包含原始基线、候选条目和推荐方法的不可变比较结果；取消时 <c>Cancelled</c> 为 <see langword="true"/>。</returns>
    public PathSmoothingComparisonResult Compare(
        PathSmoothingComparisonRequest request,
        CancellationToken cancellationToken = default)
    {
        PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason);
-        var entries = new List<PathSmoothingComparisonEntry>();
+        var entries = new List<PathSmoothingComparisonEntry>(1);

        if (cancellationToken.IsCancellationRequested)
            return Cancelled(baseline, entries);
@@ -30,33 +36,26 @@ public sealed class PathSmoothingComparisonService
        if (request == null || request.SmoothingRequest == null)
            return new PathSmoothingComparisonResult(baseline, entries, null, false, baselineReason);

-        for (int methodIndex = 0; methodIndex < request.Methods.Count; methodIndex++)
+        if (!TryCompareLocalG2(
+                request.SmoothingRequest,
+                baseline.Metrics,
+                cancellationToken,
+                out PathSmoothingComparisonEntry entry))
        {
-            if (cancellationToken.IsCancellationRequested)
-                return Cancelled(baseline, entries);
-
-            SmoothingMethod method = request.Methods[methodIndex];
-            if (!TryCompareMethod(
-                    request.SmoothingRequest,
-                    method,
-                    baseline.Metrics,
-                    cancellationToken,
-                    out PathSmoothingComparisonEntry entry))
-                return Cancelled(baseline, entries);
-            entries.Add(entry);
+            return Cancelled(baseline, entries);
        }

+        entries.Add(entry);
        return new PathSmoothingComparisonResult(
            baseline,
            entries,
-            SmoothingMethodRanker.Rank(entries),
+            entry.IsEligibleForRecommendation ? SmoothingMethod.LocalG2Quintic : null,
            false,
            baselineReason);
    }

-    private bool TryCompareMethod(
-        PathSmoothingRequest sourceRequest,
-        SmoothingMethod method,
+    private bool TryCompareLocalG2(
+        PathSmoothingRequest smoothingRequest,
        PathQualityMetrics rawMetrics,
        CancellationToken cancellationToken,
        out PathSmoothingComparisonEntry entry)
@@ -64,9 +63,8 @@ public sealed class PathSmoothingComparisonService
        entry = null;
        try
        {
-            PathSmoothingRequest methodRequest = CreateMethodRequest(sourceRequest, method);
            cancellationToken.ThrowIfCancellationRequested();
-            PathSmoothingResult warmup = _smoothingService.Smooth(methodRequest, cancellationToken);
+            PathSmoothingResult warmup = _smoothingService.Smooth(smoothingRequest, cancellationToken);
            if (warmup.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;

            var timings = new List<double>(5);
@@ -75,7 +73,7 @@ public sealed class PathSmoothingComparisonService
            {
                cancellationToken.ThrowIfCancellationRequested();
                Stopwatch stopwatch = Stopwatch.StartNew();
-                PathSmoothingResult result = _smoothingService.Smooth(methodRequest, cancellationToken);
+                PathSmoothingResult result = _smoothingService.Smooth(smoothingRequest, cancellationToken);
                stopwatch.Stop();
                if (result.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
                timings.Add(stopwatch.Elapsed.TotalMilliseconds);
@@ -88,12 +86,11 @@ public sealed class PathSmoothingComparisonService
            string diagnostic = string.IsNullOrWhiteSpace(timing.Diagnostic)
                ? canonical.Diagnostics.TerminationReason
                : timing.Diagnostic;
-
-            PathQualityMetrics metrics = canonical.Status == PathSmoothingStatus.Success
+            PathQualityMetrics metrics = PathSmoothingComparisonEntry.IsPublishedLocalG2Status(canonical.Status)
                ? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics)
                : new PathQualityMetrics();
            entry = PathSmoothingComparisonEntry.CreateCandidate(
-                method,
+                SmoothingMethod.LocalG2Quintic,
                canonical.Status,
                metrics,
                timing,
@@ -110,7 +107,7 @@ public sealed class PathSmoothingComparisonService
        catch (Exception exception)
        {
            entry = PathSmoothingComparisonEntry.CreateCandidate(
-                method,
+                SmoothingMethod.LocalG2Quintic,
                PathSmoothingStatus.Failed,
                new PathQualityMetrics(),
                new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, exception.GetType().Name),
@@ -128,12 +125,17 @@ public sealed class PathSmoothingComparisonService
    {
        reason = string.Empty;
        if (request == null || request.SmoothingRequest == null)
-            return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求为空。", out reason);
+            return FailedBaseline(PathSmoothingStatus.InvalidInput, "Comparison request is required.", out reason);

        PathSmoothingRequest smoothingRequest = request.SmoothingRequest;
        PathSmoothingConfiguration configuration = smoothingRequest.Configuration;
        if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null)
-            return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求缺少可用的地图、车辆或配置。", out reason);
+        {
+            return FailedBaseline(
+                PathSmoothingStatus.InvalidInput,
+                "A planning-ready map, vehicle, and configuration are required.",
+                out reason);
+        }

        if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason))
            return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
@@ -146,11 +148,13 @@ public sealed class PathSmoothingComparisonService
                configuration.MaximumCollisionCheckStepMeters,
                out RawPathBaseline rawPath,
                out reason))
+        {
            return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
+        }

-        string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, rawPath.Path, rawPath.Segments);
+        string digest = StableGeometryDigest.Compute(PathSmoothingStatus.NotNeeded, null, rawPath.Path, rawPath.Segments);
        return PathSmoothingComparisonEntry.CreateRawPathBaseline(
-            PathSmoothingStatus.Success,
+            PathSmoothingStatus.NotNeeded,
            rawPath.Metrics,
            digest,
            string.Empty,
@@ -177,20 +181,7 @@ public sealed class PathSmoothingComparisonService
        PathSmoothingComparisonEntry baseline,
        IReadOnlyList<PathSmoothingComparisonEntry> entries)
    {
-        return new PathSmoothingComparisonResult(baseline, entries, null, true, "路径平滑比较已取消。");
-    }
-
-    private static PathSmoothingRequest CreateMethodRequest(PathSmoothingRequest source, SmoothingMethod method)
-    {
-        PathSmoothingConfiguration configuration = source.Configuration;
-        configuration.Method = method;
-        configuration.AllowFallbackToCoarsePath = false;
-        return new PathSmoothingRequest(
-            source.CoarsePath,
-            source.Segments,
-            source.Map,
-            source.Vehicle,
-            configuration);
+        return new PathSmoothingComparisonResult(baseline, entries, null, true, "Path smoothing comparison was cancelled.");
    }

    private static PathQualityMetrics NormalizeMetrics(
@@ -1,28 +1,30 @@
 using System;
-using System.Collections.Generic;
 using System.Diagnostics;
 using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
-using MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
 using MultiWheelC.TrajectoryPlanning.Utils;

 namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

-/// <summary>正式单算法路径平滑入口，负责输入校验、有限重试与经过复核的粗路径回退。</summary>
+/// <summary>
+/// Local G2 路径平滑的公开、同步业务入口。
+/// 它只消费调用方冻结的请求，依次执行输入校验、预处理、原始基线、局部 G2 和独立复核；从不发布未经验证的部分路径。
+/// </summary>
 public sealed class PathSmoothingService
 {
    private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
-    private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
    private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
-    private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
-    private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother();
-    private readonly IPathSmoother _quintic = new PiecewiseQuinticSmoother();
+    private readonly LocalG2PreSmoothingPipeline _localG2Pipeline = new LocalG2PreSmoothingPipeline();

-    /// <summary>执行一次经过完整安全复核的单算法平滑。</summary>
+    /// <summary>在请求携带的不可变粗路径、地图和车辆快照上执行一次 Local G2 平滑。</summary>
+    /// <param name="request">平滑输入快照，包含粗路径/方向段、规划地图、车辆和配置；路径单位为 m/rad，曲率单位为 1/m。</param>
+    /// <param name="cancellationToken">调用方取消令牌；取消时返回 <see cref="PathSmoothingStatus.Cancelled"/>，不会发布部分路径。</param>
+    /// <returns>成功时携带完整独立复核路径的结果；输入、预处理、碰撞、曲率或运行失败时路径和分段为空，原因位于诊断中。</returns>
    public PathSmoothingResult Smooth(
        PathSmoothingRequest request,
        CancellationToken cancellationToken = default)
@@ -32,10 +34,10 @@ public sealed class PathSmoothingService
        {
            cancellationToken.ThrowIfCancellationRequested();
            if (!TryValidateRequest(request, out PathSmoothingConfiguration configuration, out string reason))
-                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
+                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, reason);

            if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
-                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
+                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, reason);

            if (!RawPathBaselineBuilder.TryCreate(
                    request,
@@ -44,126 +46,24 @@ public sealed class PathSmoothingService
                    configuration.OutputSpacingMeters,
                    _validator,
                    configuration.MaximumCollisionCheckStepMeters,
-                    out _,
+                    out RawPathBaseline rawBaseline,
                    out reason))
            {
-                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
+                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, reason);
            }

-            cancellationToken.ThrowIfCancellationRequested();
-            var input = new SmoothingAlgorithmInput(
-                preparedPath,
-                request.Map,
-                request.Vehicle,
-                configuration.MaximumCollisionCheckStepMeters,
-                configuration.MinimumClearanceReserveMeters,
-                new SmoothingOptionsSnapshot(configuration));
-            SmoothingAlgorithmRunner.AlgorithmRunResult runResult = _runner.Run(
-                Resolve(configuration.Method), input, configuration, cancellationToken);
-            int retryCount = GetRetryCount(runResult.AttemptedStrengths);
-            PathSmoothingDiagnostics diagnostics = new PathSmoothingDiagnostics(
-                runResult.Metrics,
-                stopwatch.Elapsed,
-                retryCount,
-                runResult.AcceptedStrength,
-                runResult.Reason);
-
-            if (runResult.Status == PathSmoothingStatus.Success)
-            {
-                return PathSmoothingResult.Success(
-                    configuration.Method,
-                    runResult.Path,
-                    runResult.Segments,
-                    diagnostics);
-            }
-
-            if (!configuration.AllowFallbackToCoarsePath)
-                return PathSmoothingResult.Failure(runResult.Status, diagnostics);
-
-            cancellationToken.ThrowIfCancellationRequested();
-            if (!TryCreateVerifiedFallback(
-                    request,
-                    configuration,
-                    cancellationToken,
-                    out IReadOnlyList<SmoothedPathPoint> fallbackPath,
-                    out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
-                    out PathQualityMetrics fallbackMetrics,
-                    out reason))
-            {
-                return PathSmoothingResult.Failure(runResult.Status, diagnostics);
-            }
-
-            var fallbackDiagnostics = new PathSmoothingDiagnostics(
-                fallbackMetrics,
-                stopwatch.Elapsed,
-                retryCount,
-                runResult.AcceptedStrength,
-                runResult.Reason);
-            return PathSmoothingResult.Fallback(
-                configuration.Method,
-                fallbackPath,
-                fallbackSegments,
-                fallbackDiagnostics);
+            return _localG2Pipeline.Smooth(request, preparedPath, rawBaseline, cancellationToken);
        }
        catch (OperationCanceledException)
        {
-            return Failure(PathSmoothingStatus.Cancelled, stopwatch, 0, 0d, "路径平滑已取消。");
+            return Failure(PathSmoothingStatus.Cancelled, stopwatch, "Path smoothing was cancelled.");
        }
        catch (Exception exception)
        {
-            return Failure(PathSmoothingStatus.Failed, stopwatch, 0, 0d, exception.Message);
+            return Failure(PathSmoothingStatus.Failed, stopwatch, exception.Message);
        }
    }

-    private bool TryCreateVerifiedFallback(
-        PathSmoothingRequest request,
-        PathSmoothingConfiguration configuration,
-        CancellationToken cancellationToken,
-        out IReadOnlyList<SmoothedPathPoint> fallbackPath,
-        out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
-        out PathQualityMetrics fallbackMetrics,
-        out string reason)
-    {
-        fallbackPath = null;
-        fallbackSegments = null;
-        fallbackMetrics = null;
-        reason = string.Empty;
-
-        // Reprepare from the immutable request instead of reusing the algorithm input: fallback is a
-        // separately published output and must repeat the coarse-path contract validation.
-        if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false;
-        cancellationToken.ThrowIfCancellationRequested();
-
-        if (!RawPathBaselineBuilder.TryCreate(
-                request,
-                revalidatedPath,
-                _analyzer,
-                configuration.OutputSpacingMeters,
-                _validator,
-                configuration.MaximumCollisionCheckStepMeters,
-                out RawPathBaseline rawPath,
-                out reason))
-        {
-            return false;
-        }
-
-        fallbackPath = ToFallbackPoints(rawPath.Path);
-        fallbackSegments = rawPath.Segments;
-        fallbackMetrics = rawPath.Metrics;
-        return true;
-    }
-
-    private IPathSmoother Resolve(SmoothingMethod method)
-    {
-        return method switch
-        {
-            SmoothingMethod.CubicBSpline => _bSpline,
-            SmoothingMethod.LocalCubicBezier => _bezier,
-            SmoothingMethod.PiecewiseQuintic => _quintic,
-            _ => throw new ArgumentOutOfRangeException(nameof(method)),
-        };
-    }
-
    private static bool TryValidateRequest(
        PathSmoothingRequest request,
        out PathSmoothingConfiguration configuration,
@@ -173,7 +73,7 @@ public sealed class PathSmoothingService
        reason = string.Empty;
        if (request == null)
        {
-            reason = "平滑请求为空。";
+            reason = "Path smoothing request is required.";
            return false;
        }

@@ -181,7 +81,7 @@ public sealed class PathSmoothingService
        VehicleParameters vehicle = request.Vehicle;
        if (configuration == null || request.Map == null || !request.Map.PlanningReady || vehicle == null)
        {
-            reason = "平滑请求缺少可用的地图、车辆或配置。";
+            reason = "A planning-ready map, vehicle, and configuration are required.";
            return false;
        }

@@ -190,13 +90,7 @@ public sealed class PathSmoothingService
            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _))
        {
-            reason = "平滑请求中的车辆几何或曲率约束无效。";
-            return false;
-        }
-
-        if (!Enum.IsDefined(typeof(SmoothingMethod), configuration.Method))
-        {
-            reason = "平滑方法无效。";
+            reason = "Vehicle geometry or curvature constraints are invalid.";
            return false;
        }

@@ -204,63 +98,48 @@ public sealed class PathSmoothingService
            !NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
            !NumericGuard.IsFinite(configuration.MinimumClearanceReserveMeters) ||
            configuration.MinimumClearanceReserveMeters < 0d ||
-            !NumericGuard.IsPositiveFinite(configuration.SmoothingStrength) ||
-            !NumericGuard.IsPositiveFinite(configuration.CubicBSpline.EndpointTangentScale) ||
-            !IsValidBezierThreshold(configuration.LocalCubicBezier.CornerHeadingThresholdRadians) ||
-            !NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.MaximumWindowLengthMeters) ||
-            !NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.HandleLengthRatio) ||
-            !NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.KnotSpacingMeters) ||
-            !NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.MinimumKnotSpacingMeters) ||
-            configuration.PiecewiseQuintic.KnotSpacingMeters < configuration.PiecewiseQuintic.MinimumKnotSpacingMeters)
+            !CurvatureLimitPolicy.TryGetAllowedMaximumVehicleCurvaturePerMeter(
+                vehicle, configuration.CurvatureLimitRadiusToleranceMeters, out _))
        {
-            reason = "平滑配置包含非法数值或不满足方法契约。";
+            reason = "Shared Local G2 configuration values are invalid.";
            return false;
        }
-        return true;
+
+        return IsValidLocalG2Options(configuration.LocalG2Quintic, out reason);
    }

-    private static bool IsValidBezierThreshold(double thresholdRadians)
+    private static bool IsValidLocalG2Options(LocalG2QuinticOptions options, out string reason)
    {
-        return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI;
-    }
-
-    private static IReadOnlyList<SmoothedPathPoint> ToFallbackPoints(IReadOnlyList<SmoothedPathPoint> path)
-    {
-        var points = new List<SmoothedPathPoint>(path.Count);
-        for (int index = 0; index < path.Count; index++)
+        reason = string.Empty;
+        if (options != null &&
+            NumericGuard.IsPositiveFinite(options.MinimumWindowLengthMeters) &&
+            NumericGuard.IsFinite(options.PreferredWindowLengthMeters) &&
+            options.PreferredWindowLengthMeters >= options.MinimumWindowLengthMeters &&
+            NumericGuard.IsFinite(options.MaximumWindowLengthMeters) &&
+            options.MaximumWindowLengthMeters >= options.PreferredWindowLengthMeters &&
+            NumericGuard.IsPositiveFinite(options.MaximumDeviationMeters) &&
+            NumericGuard.IsPositiveFinite(options.AbsoluteCurvatureJumpFloorPerMeter) &&
+            NumericGuard.IsFinite(options.CurvatureJumpRatioOfMaximum) &&
+            options.CurvatureJumpRatioOfMaximum > 0d && options.CurvatureJumpRatioOfMaximum <= 1d &&
+            NumericGuard.IsFinite(options.MinimumPeakGradientImprovementRatio) &&
+            options.MinimumPeakGradientImprovementRatio > 0d && options.MinimumPeakGradientImprovementRatio < 1d &&
+            NumericGuard.IsFinite(options.MaximumVariationCostRegressionRatio) &&
+            options.MaximumVariationCostRegressionRatio >= 0d && options.MaximumCandidatesPerRegion >= 1)
        {
-            SmoothedPathPoint point = path[index];
-            points.Add(new SmoothedPathPoint(
-                point.X,
-                point.Y,
-                point.Heading,
-                point.UnwrappedHeading,
-                point.ArcLength,
-                point.Direction,
-                point.GeometricCurvature,
-                point.VehicleCurvature,
-                point.VehicleCurvatureDerivative,
-                point.BodyClearance,
-                point.IsGearSwitchPoint,
-                SmoothedPathPointSource.CoarsePathFallback));
+            return true;
        }
-        return points;
-    }

-    private static int GetRetryCount(IReadOnlyList<double> attemptedStrengths)
-    {
-        return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1;
+        reason = "Local G2 configuration values are invalid.";
+        return false;
    }

    private static PathSmoothingResult Failure(
        PathSmoothingStatus status,
        Stopwatch stopwatch,
-        int retryCount,
-        double acceptedStrength,
        string reason)
    {
        return PathSmoothingResult.Failure(
            status,
-            new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, retryCount, acceptedStrength, reason));
+            new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, reason));
    }
 }