diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs index 3c42601..68c4138 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs @@ -13,7 +13,6 @@ public sealed class PathSmoothingComparisonService { private readonly PathSmoothingService _smoothingService = new PathSmoothingService(); private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor(); - private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer(); private readonly SmoothedPathValidator _validator = new SmoothedPathValidator(); /// 比较所有请求方法;一个方法的失败不会阻止其他方法,取消会停止后续启动。 @@ -137,31 +136,23 @@ public sealed class PathSmoothingComparisonService if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason)) return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason); - if (!_analyzer.TryAnalyze(preparedPath.Segments, configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason)) - return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason); - if (!_validator.TryValidate( - analysis.Path, - analysis.Segments, + if (!RawPathBaselineBuilder.TryCreate( + smoothingRequest, preparedPath, - smoothingRequest.Map, - smoothingRequest.Vehicle, + _validator, configuration.MaximumCollisionCheckStepMeters, - out IReadOnlyList safePath, - out double minimumClearanceMeters, + out RawPathBaseline rawPath, out reason)) - { return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason); - } - PathQualityMetrics metrics = CreateMetrics(analysis, minimumClearanceMeters); - string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, safePath, analysis.Segments); + string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, rawPath.Path, rawPath.Segments); return PathSmoothingComparisonEntry.CreateRawPathBaseline( PathSmoothingStatus.Success, - metrics, + rawPath.Metrics, digest, string.Empty, - safePath, - analysis.Segments); + rawPath.Path, + rawPath.Segments); } private static PathSmoothingComparisonEntry FailedBaseline( @@ -199,22 +190,6 @@ public sealed class PathSmoothingComparisonService configuration); } - private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters) - { - return new PathQualityMetrics( - true, - analysis.PathLengthMeters, - analysis.MaximumAbsoluteVehicleCurvaturePerMeter, - analysis.RootMeanSquareVehicleCurvaturePerMeter, - analysis.TotalAbsoluteCurvatureVariationPerMeter, - analysis.CurvatureVariationEnergy, - minimumClearanceMeters, - 0d, - 0d, - 0d, - 0d); - } - private static PathQualityMetrics NormalizeMetrics( PathQualityMetrics candidate, PathQualityMetrics raw) diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs index d8a28c3..fe5eb12 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs @@ -16,7 +16,6 @@ public sealed class PathSmoothingService { private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor(); private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner(); - private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer(); private readonly SmoothedPathValidator _validator = new SmoothedPathValidator(); private readonly IPathSmoother _bSpline = new CubicBSplineSmoother(); private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother(); @@ -37,15 +36,11 @@ public sealed class PathSmoothingService if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason)) return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason); - if (!TryAnalyzeAndValidate( + if (!RawPathBaselineBuilder.TryCreate( request, preparedPath, - preparedPath.Segments, - configuration, - false, - cancellationToken, - out _, - out _, + _validator, + configuration.MaximumCollisionCheckStepMeters, out _, out reason)) { @@ -136,64 +131,20 @@ public sealed class PathSmoothingService if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false; cancellationToken.ThrowIfCancellationRequested(); - return TryAnalyzeAndValidate( - request, - revalidatedPath, - ToFallbackSegments(revalidatedPath), - configuration, - true, - cancellationToken, - out fallbackPath, - out fallbackSegments, - out fallbackMetrics, - out reason); - } - - private bool TryAnalyzeAndValidate( - PathSmoothingRequest request, - PreparedPath originalPath, - IReadOnlyList candidateSegments, - PathSmoothingConfiguration configuration, - bool useFallbackSource, - CancellationToken cancellationToken, - out IReadOnlyList safePath, - out IReadOnlyList safeSegments, - out PathQualityMetrics metrics, - out string reason) - { - safePath = null; - safeSegments = null; - metrics = null; - reason = string.Empty; - if (!_analyzer.TryAnalyze( - candidateSegments, - configuration.OutputSpacingMeters, - out PathGeometryAnalysis analysis, - out reason)) - { - return false; - } - - IReadOnlyList pathForValidation = useFallbackSource - ? ToFallbackPoints(analysis.Path) - : analysis.Path; - cancellationToken.ThrowIfCancellationRequested(); - if (!_validator.TryValidate( - pathForValidation, - analysis.Segments, - originalPath, - request.Map, - request.Vehicle, + if (!RawPathBaselineBuilder.TryCreate( + request, + revalidatedPath, + _validator, configuration.MaximumCollisionCheckStepMeters, - out safePath, - out double minimumClearanceMeters, + out RawPathBaseline rawPath, out reason)) { return false; } - safeSegments = analysis.Segments; - metrics = CreateMetrics(analysis, minimumClearanceMeters); + fallbackPath = ToFallbackPoints(rawPath.Path); + fallbackSegments = rawPath.Segments; + fallbackMetrics = rawPath.Metrics; return true; } @@ -268,36 +219,6 @@ public sealed class PathSmoothingService return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI; } - private static IReadOnlyList ToFallbackSegments(PreparedPath path) - { - var segments = new List(path.Segments.Count); - for (int segmentIndex = 0; segmentIndex < path.Segments.Count; segmentIndex++) - { - PreparedDirectionSegment segment = path.Segments[segmentIndex]; - var points = new List(segment.Points.Count); - for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++) - { - SmoothingPoint2D point = segment.Points[pointIndex]; - points.Add(new SmoothingPoint2D( - point.X, - point.Y, - point.ArcLength, - point.Heading, - point.UnwrappedHeading, - point.BodyClearance, - point.IsGearSwitchPoint, - SmoothedPathPointSource.CoarsePathFallback)); - } - segments.Add(new PreparedDirectionSegment( - segment.SegmentIndex, - segment.Direction, - points, - segment.StartsAtGearSwitch, - segment.EndsAtGearSwitch)); - } - return segments; - } - private static IReadOnlyList ToFallbackPoints(IReadOnlyList path) { var points = new List(path.Count); @@ -320,22 +241,6 @@ public sealed class PathSmoothingService return points; } - private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters) - { - return new PathQualityMetrics( - true, - analysis.PathLengthMeters, - analysis.MaximumAbsoluteVehicleCurvaturePerMeter, - analysis.RootMeanSquareVehicleCurvaturePerMeter, - analysis.TotalAbsoluteCurvatureVariationPerMeter, - analysis.CurvatureVariationEnergy, - minimumClearanceMeters, - 0d, - 0d, - 0d, - 0d); - } - private static int GetRetryCount(IReadOnlyList attemptedStrengths) { return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1; diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs index bb06f31..cf8723f 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs @@ -144,21 +144,14 @@ public sealed class PathGeometryAnalyzer for (int index = 0; index < count; index++) { double travelHeading; - if (count == 1) + if (index == 0 || index == count - 1) { + // Coarse-path endpoints encode the vehicle pose at the exact integration anchor. + // A chord across a finite integration step is not that pose's heading. travelHeading = segment.Direction == TravelDirection.Forward ? samples[index].Heading : samples[index].Heading - Math.PI; } - else if (index == 0) - { - travelHeading = Math.Atan2(samples[1].Y - samples[0].Y, samples[1].X - samples[0].X); - } - else if (index == count - 1) - { - travelHeading = Math.Atan2(samples[index].Y - samples[index - 1].Y, - samples[index].X - samples[index - 1].X); - } else { travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y, diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs new file mode 100644 index 0000000..42d4fa6 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs @@ -0,0 +1,166 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing; + +/// 将已验证的粗路径转换为不改变任何锚点位姿或车辆曲率的安全比较基线。 +internal static class RawPathBaselineBuilder +{ + private const double MinimumArcDeltaMeters = 1e-12d; + + internal static bool TryCreate( + PathSmoothingRequest request, + PreparedPath preparedPath, + SmoothedPathValidator validator, + double maximumCollisionCheckStepMeters, + out RawPathBaseline baseline, + out string reason) + { + baseline = null; + reason = string.Empty; + if (request == null || preparedPath == null || validator == null) + { + reason = "原始粗路径基线缺少请求、预处理路径或安全验证器。"; + return false; + } + + IReadOnlyList candidatePath = CreatePoints(request.CoarsePath); + IReadOnlyList candidateSegments = CreateSegments(request.Segments); + if (!validator.TryValidate( + candidatePath, + candidateSegments, + preparedPath, + request.Map, + request.Vehicle, + maximumCollisionCheckStepMeters, + out IReadOnlyList safePath, + out double minimumClearanceMeters, + out reason)) + { + return false; + } + + baseline = new RawPathBaseline(safePath, candidateSegments, CreateMetrics(safePath, candidateSegments, minimumClearanceMeters)); + return true; + } + + private static IReadOnlyList CreatePoints(IReadOnlyList coarsePath) + { + var points = new List(coarsePath == null ? 0 : coarsePath.Count); + if (coarsePath != null) + { + for (int index = 0; index < coarsePath.Count; index++) + { + CoarsePathPoint point = coarsePath[index]; + points.Add(new SmoothedPathPoint( + point.X, + point.Y, + point.Heading, + point.UnwrappedHeading, + point.ArcLength, + point.Direction, + point.VehicleCurvature, + point.VehicleCurvature, + point.BodyClearance, + point.IsGearSwitchPoint, + point.IsGearSwitchPoint ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor)); + } + } + return new ReadOnlyCollection(points); + } + + private static IReadOnlyList CreateSegments(IReadOnlyList coarseSegments) + { + var segments = new List(coarseSegments == null ? 0 : coarseSegments.Count); + if (coarseSegments != null) + { + for (int index = 0; index < coarseSegments.Count; index++) + { + PathSegment segment = coarseSegments[index]; + segments.Add(new SmoothedPathSegment( + segment.SegmentIndex, + segment.Direction, + segment.StartIndex, + segment.EndIndex, + segment.StartsAtGearSwitch, + segment.EndsAtGearSwitch)); + } + } + return new ReadOnlyCollection(segments); + } + + private static PathQualityMetrics CreateMetrics( + IReadOnlyList path, + IReadOnlyList segments, + double minimumClearanceMeters) + { + double maximumAbsoluteVehicleCurvature = 0d; + double curvatureSquareSum = 0d; + int curvatureSampleCount = 0; + double totalAbsoluteCurvatureVariation = 0d; + double curvatureVariationEnergy = 0d; + + for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++) + { + SmoothedPathSegment segment = segments[segmentIndex]; + SmoothedPathPoint previous = null; + for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++) + { + SmoothedPathPoint current = path[pointIndex]; + double curvature = current.VehicleCurvature; + maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(curvature)); + curvatureSquareSum += curvature * curvature; + curvatureSampleCount++; + if (previous != null) + { + double arcDelta = current.ArcLength - previous.ArcLength; + if (arcDelta > MinimumArcDeltaMeters) + { + double curvatureDelta = curvature - previous.VehicleCurvature; + totalAbsoluteCurvatureVariation += Math.Abs(curvatureDelta); + curvatureVariationEnergy += curvatureDelta * curvatureDelta / arcDelta; + } + } + previous = current; + } + } + + double rootMeanSquareVehicleCurvature = curvatureSampleCount == 0 + ? 0d + : Math.Sqrt(curvatureSquareSum / curvatureSampleCount); + double pathLengthMeters = path.Count == 0 ? 0d : path[path.Count - 1].ArcLength; + return new PathQualityMetrics( + true, + pathLengthMeters, + maximumAbsoluteVehicleCurvature, + rootMeanSquareVehicleCurvature, + totalAbsoluteCurvatureVariation, + curvatureVariationEnergy, + minimumClearanceMeters, + 0d, + 0d, + 0d, + 0d); + } +} + +/// 已通过完整车体复核的原始粗路径及其比较指标。 +internal sealed class RawPathBaseline +{ + internal RawPathBaseline( + IReadOnlyList path, + IReadOnlyList segments, + PathQualityMetrics metrics) + { + Path = path; + Segments = segments; + Metrics = metrics; + } + + internal IReadOnlyList Path { get; } + internal IReadOnlyList Segments { get; } + internal PathQualityMetrics Metrics { get; } +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 b/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 index 4d9f8f2..2a2b88d 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 @@ -196,6 +196,15 @@ $straightEnd = $straightAnalysis.Path[$straightAnalysis.Path.Count - 1] Assert-Near 1.0 $straightEnd.X 0.0 'Resampling must retain the exact final X coordinate.' Assert-Near 0.0 $straightEnd.Y 0.0 'Resampling must retain the exact final Y coordinate.' +# Raw coarse anchors carry the vehicle pose, which may differ slightly from the chord tangent of a finite integration step. +$poseAnchoredCurve = New-DirectionSegment 0 $forward @( + (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0), + (New-GeometryPoint 1.0 0.2 1.0 0.4 0.4)) +$poseAnchoredAnalysis = Invoke-Analysis @($poseAnchoredCurve) +Assert-Near 0.0 $poseAnchoredAnalysis.Path[0].Heading 0.000000000001 'Geometry analysis must retain the first coarse-anchor heading rather than replace it with a chord tangent.' +$poseAnchoredEnd = $poseAnchoredAnalysis.Path[$poseAnchoredAnalysis.Path.Count - 1] +Assert-Near 0.4 $poseAnchoredEnd.Heading 0.000000000001 'Geometry analysis must retain the final coarse-anchor heading rather than replace it with a chord tangent.' + # A forward R=2 quarter circle has positive +0.5 1/m vehicle curvature. $forwardArcPoints = New-Object System.Collections.Generic.List[object] for ($index = 0; $index -le 32; $index++) {