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
@@ -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));
    }
 }