diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs new file mode 100644 index 0000000..7bc1e64 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs @@ -0,0 +1,15 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 局部 G2 五次过渡的可配置阈值。 +public sealed class LocalG2QuinticOptions +{ + public double MinimumWindowLengthMeters { get; set; } = 0.20d; + public double PreferredWindowLengthMeters { get; set; } = 0.50d; + public double MaximumWindowLengthMeters { get; set; } = 0.80d; + public double MaximumDeviationMeters { get; set; } = 0.10d; + public double AbsoluteCurvatureJumpFloorPerMeter { get; set; } = 0.001d; + public double CurvatureJumpRatioOfMaximum { get; set; } = 0.05d; + public double MinimumPeakGradientImprovementRatio { get; set; } = 0.20d; + public double MaximumVariationCostRegressionRatio { get; set; } = 0.02d; + public int MaximumCandidatesPerRegion { get; set; } = 12; +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs index fdd088e..251b387 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs @@ -5,7 +5,7 @@ public sealed class PathQualityMetrics { /// 创建全零、不可行的质量指标。 public PathQualityMetrics() - : this(false, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d) + : this(false, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d) { } @@ -22,10 +22,41 @@ public sealed class PathQualityMetrics double peakCurvatureChangePercent, double curvatureVariationChangePercent, double minimumClearanceChangeMeters) + : this( + isFeasible, + pathLengthMeters, + maximumAbsoluteVehicleCurvaturePerMeter, + 0d, + rootMeanSquareVehicleCurvaturePerMeter, + totalAbsoluteCurvatureVariationPerMeter, + curvatureVariationEnergy, + minimumBodyClearanceMeters, + lengthChangePercent, + peakCurvatureChangePercent, + curvatureVariationChangePercent, + minimumClearanceChangeMeters) + { + } + + /// 创建带有曲率导数峰值的完整质量指标快照。 + public PathQualityMetrics( + bool isFeasible, + double pathLengthMeters, + double maximumAbsoluteVehicleCurvaturePerMeter, + double maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, + double rootMeanSquareVehicleCurvaturePerMeter, + double totalAbsoluteCurvatureVariationPerMeter, + double curvatureVariationEnergy, + double minimumBodyClearanceMeters, + double lengthChangePercent, + double peakCurvatureChangePercent, + double curvatureVariationChangePercent, + double minimumClearanceChangeMeters) { IsFeasible = isFeasible; PathLengthMeters = pathLengthMeters; MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter; + MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter = maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter; RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter; TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter; CurvatureVariationEnergy = curvatureVariationEnergy; @@ -45,6 +76,9 @@ public sealed class PathQualityMetrics /// 绝对车辆曲率峰值，单位 1/m。 public double MaximumAbsoluteVehicleCurvaturePerMeter { get; } + /// 绝对车辆曲率导数峰值，单位 1/m²。 + public double MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter { get; } + /// 车辆曲率均方根，单位 1/m。 public double RootMeanSquareVehicleCurvaturePerMeter { get; } @@ -54,6 +88,9 @@ public sealed class PathQualityMetrics /// 逐方向段计算的曲率变化能量。 public double CurvatureVariationEnergy { get; } + /// 曲率变化代价的兼容名称。 + public double CurvatureVariationCost => CurvatureVariationEnergy; + /// 完整扩大车体的最小保守净空，单位 m。 public double MinimumBodyClearanceMeters { get; } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs index ab1f2a1..1aca9c6 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs @@ -47,4 +47,7 @@ public sealed class PathSmoothingConfiguration /// 分段五次多项式专用参数。 public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions(); + + /// 局部 G2 五次过渡专用参数。 + public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions(); } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs new file mode 100644 index 0000000..91b5bf2 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs @@ -0,0 +1,17 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 局部 G2 区域未替换原始路径的稳定原因。 +public enum PathSmoothingRegionFailureReason +{ + None, + WindowUnavailable, + CandidateGenerationFailed, + Collision, + InsufficientClearance, + CurvatureExceeded, + CurvatureOvershoot, + DeviationExceeded, + InsufficientImprovement, + VariationCostRegression, + GlobalValidationRollback, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs new file mode 100644 index 0000000..4004314 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs @@ -0,0 +1,83 @@ +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 单个局部 G2 平滑区域的不可变发布报告。 +public sealed class PathSmoothingRegionReport +{ + public PathSmoothingRegionReport( + int segmentIndex, + double startArcLengthMeters, + double endArcLengthMeters, + IReadOnlyList curvatureJumpsPerMeter, + double plannedWindowLengthMeters, + double actualWindowLengthMeters, + double leftWindowLengthMeters, + double rightWindowLengthMeters, + int candidateCount, + int selectedCandidateIndex, + PathSmoothingRegionStatus status, + PathSmoothingRegionFailureReason failureReason, + double rawPeakCurvatureDerivativePerSquareMeter, + double resultPeakCurvatureDerivativePerSquareMeter, + double rawCurvatureVariationCost, + double resultCurvatureVariationCost, + double maximumDeviationMeters, + double minimumBodyClearanceMeters, + double maximumAbsoluteVehicleCurvaturePerMeter) + { + SegmentIndex = segmentIndex; + StartArcLengthMeters = startArcLengthMeters; + EndArcLengthMeters = endArcLengthMeters; + CurvatureJumpsPerMeter = CopyReadOnly(curvatureJumpsPerMeter); + PlannedWindowLengthMeters = plannedWindowLengthMeters; + ActualWindowLengthMeters = actualWindowLengthMeters; + LeftWindowLengthMeters = leftWindowLengthMeters; + RightWindowLengthMeters = rightWindowLengthMeters; + CandidateCount = candidateCount; + SelectedCandidateIndex = status == PathSmoothingRegionStatus.Improved + ? selectedCandidateIndex + : -1; + Status = status; + FailureReason = failureReason; + RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter; + ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter; + RawCurvatureVariationCost = rawCurvatureVariationCost; + ResultCurvatureVariationCost = resultCurvatureVariationCost; + MaximumDeviationMeters = maximumDeviationMeters; + MinimumBodyClearanceMeters = minimumBodyClearanceMeters; + MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter; + } + + public int SegmentIndex { get; } + public double StartArcLengthMeters { get; } + public double EndArcLengthMeters { get; } + public IReadOnlyList CurvatureJumpsPerMeter { get; } + public double PlannedWindowLengthMeters { get; } + public double ActualWindowLengthMeters { get; } + public double LeftWindowLengthMeters { get; } + public double RightWindowLengthMeters { get; } + public int CandidateCount { get; } + public int SelectedCandidateIndex { get; } + public PathSmoothingRegionStatus Status { get; } + public PathSmoothingRegionFailureReason FailureReason { get; } + public double RawPeakCurvatureDerivativePerSquareMeter { get; } + public double ResultPeakCurvatureDerivativePerSquareMeter { get; } + public double RawCurvatureVariationCost { get; } + public double ResultCurvatureVariationCost { get; } + public double MaximumDeviationMeters { get; } + public double MinimumBodyClearanceMeters { get; } + public double MaximumAbsoluteVehicleCurvaturePerMeter { get; } + + private static IReadOnlyList CopyReadOnly(IReadOnlyList source) + { + var copy = new List(source == null ? 0 : source.Count); + if (source != null) + { + for (int index = 0; index < source.Count; index++) + copy.Add(source[index]); + } + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs new file mode 100644 index 0000000..752d3ef --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs @@ -0,0 +1,8 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 单个局部 G2 平滑区域的处理结果。 +public enum PathSmoothingRegionStatus +{ + Improved, + RetainedOriginal, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs index 2d12340..ebe8106 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs @@ -83,6 +83,15 @@ public sealed class PathSmoothingRequest copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio; copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters; copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters; + copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters; + copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters; + copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters; + copy.LocalG2Quintic.MaximumDeviationMeters = source.LocalG2Quintic.MaximumDeviationMeters; + copy.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = source.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter; + copy.LocalG2Quintic.CurvatureJumpRatioOfMaximum = source.LocalG2Quintic.CurvatureJumpRatioOfMaximum; + copy.LocalG2Quintic.MinimumPeakGradientImprovementRatio = source.LocalG2Quintic.MinimumPeakGradientImprovementRatio; + copy.LocalG2Quintic.MaximumVariationCostRegressionRatio = source.LocalG2Quintic.MaximumVariationCostRegressionRatio; + copy.LocalG2Quintic.MaximumCandidatesPerRegion = source.LocalG2Quintic.MaximumCandidatesPerRegion; return copy; } } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs index 1555b4f..d964caa 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs @@ -11,18 +11,22 @@ public sealed class PathSmoothingResult new ReadOnlyCollection(new List()); private static readonly IReadOnlyList EmptySegments = new ReadOnlyCollection(new List()); + private static readonly IReadOnlyList EmptyRegionReports = + new ReadOnlyCollection(new List()); private PathSmoothingResult( PathSmoothingStatus status, SmoothingMethod? method, IReadOnlyList path, IReadOnlyList segments, - PathSmoothingDiagnostics diagnostics) + PathSmoothingDiagnostics diagnostics, + IReadOnlyList regionReports) { Status = status; Method = method; Path = path; Segments = segments; + RegionReports = regionReports; Diagnostics = diagnostics ?? new PathSmoothingDiagnostics( new PathQualityMetrics(), TimeSpan.Zero, @@ -43,6 +47,9 @@ public sealed class PathSmoothingResult /// 覆盖 的方向分段；其他状态始终为空且不可变。 public IReadOnlyList Segments { get; } + /// 局部 G2 各检测区域的不可变报告；传统算法结果为空。 + public IReadOnlyList RegionReports { get; } + /// 本次平滑的质量和终止诊断；始终非空。 public PathSmoothingDiagnostics Diagnostics { get; } @@ -59,7 +66,8 @@ public sealed class PathSmoothingResult method, CopyReadOnly(path), CopyReadOnly(segments), - diagnostics); + diagnostics, + EmptyRegionReports); } /// 创建经过完整复核的原始粗路径回退结果。 @@ -75,17 +83,49 @@ public sealed class PathSmoothingResult attemptedMethod, CopyReadOnly(path), CopyReadOnly(segments), - diagnostics); + diagnostics, + EmptyRegionReports); + } + + /// 发布经过完整复核的局部 G2 预平滑结果。 + public static PathSmoothingResult PublishLocalG2( + PathSmoothingStatus status, + IReadOnlyList path, + IReadOnlyList segments, + PathSmoothingDiagnostics diagnostics, + IReadOnlyList regionReports) + { + if (status != PathSmoothingStatus.Complete && + status != PathSmoothingStatus.PartialImprovement && + status != PathSmoothingStatus.NotNeeded && + status != PathSmoothingStatus.Unchanged) + throw new ArgumentException("Use a Local G2 publication status.", nameof(status)); + if (regionReports == null) + throw new ArgumentNullException(nameof(regionReports)); + + ValidatePublishedResult(SmoothingMethod.LocalG2Quintic, path, segments, diagnostics); + return new PathSmoothingResult( + status, + SmoothingMethod.LocalG2Quintic, + CopyReadOnly(path), + CopyReadOnly(segments), + diagnostics, + CopyReadOnly(regionReports)); } /// 创建不发布路径的失败、不可行、取消或输入无效结果。 public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics) { - if (status == PathSmoothingStatus.Success || status == PathSmoothingStatus.FallbackToCoarsePath) + if (status == PathSmoothingStatus.Success || + status == PathSmoothingStatus.FallbackToCoarsePath || + status == PathSmoothingStatus.Complete || + status == PathSmoothingStatus.PartialImprovement || + status == PathSmoothingStatus.NotNeeded || + status == PathSmoothingStatus.Unchanged) throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status)); if (!Enum.IsDefined(typeof(PathSmoothingStatus), status)) throw new ArgumentOutOfRangeException(nameof(status)); - return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics); + return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics, EmptyRegionReports); } private static void ValidatePublishedResult( diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs index 1212230..df1cdab 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs @@ -9,4 +9,8 @@ public enum PathSmoothingStatus Infeasible, Failed, Cancelled, + Complete, + PartialImprovement, + NotNeeded, + Unchanged, } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs index 5103a4a..b0a3c8a 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs @@ -17,6 +17,36 @@ public sealed class SmoothedPathPoint double bodyClearanceMeters, bool isGearSwitchPoint, SmoothedPathPointSource source) + : this( + xMeters, + yMeters, + headingRadians, + unwrappedHeadingRadians, + arcLengthMeters, + direction, + geometricCurvaturePerMeter, + vehicleCurvaturePerMeter, + 0d, + bodyClearanceMeters, + isGearSwitchPoint, + source) + { + } + + /// 创建带有车辆曲率对弧长导数的不可变采样点。 + public SmoothedPathPoint( + double xMeters, + double yMeters, + double headingRadians, + double unwrappedHeadingRadians, + double arcLengthMeters, + TravelDirection direction, + double geometricCurvaturePerMeter, + double vehicleCurvaturePerMeter, + double vehicleCurvatureDerivativePerSquareMeter, + double bodyClearanceMeters, + bool isGearSwitchPoint, + SmoothedPathPointSource source) { X = xMeters; Y = yMeters; @@ -26,6 +56,7 @@ public sealed class SmoothedPathPoint Direction = direction; GeometricCurvature = geometricCurvaturePerMeter; VehicleCurvature = vehicleCurvaturePerMeter; + VehicleCurvatureDerivative = vehicleCurvatureDerivativePerSquareMeter; BodyClearance = bodyClearanceMeters; IsGearSwitchPoint = isGearSwitchPoint; Source = source; @@ -55,6 +86,9 @@ public sealed class SmoothedPathPoint /// 车辆模型使用的有符号曲率，单位 1/m。 public double VehicleCurvature { get; } + /// 车辆曲率对弧长的导数 dκ/ds，单位 1/m²。 + public double VehicleCurvatureDerivative { get; } + /// 扩大车体后的保守净空下界，单位 m。 public double BodyClearance { get; } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs index f3140e1..9469fc6 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs @@ -7,4 +7,5 @@ public enum SmoothedPathPointSource Interpolated, GearSwitch, CoarsePathFallback, + LocalG2Transition, } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs index 41d57e6..890e02f 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs @@ -6,4 +6,5 @@ public enum SmoothingMethod CubicBSpline, LocalCubicBezier, PiecewiseQuintic, + LocalG2Quintic, } diff --git a/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 b/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 index b6b36a6..6838d1f 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 @@ -47,6 +47,10 @@ $segmentType = Get-RequiredType ($root + 'SmoothedPathSegment') $bsplineOptionsType = Get-RequiredType ($root + 'CubicBSplineOptions') $bezierOptionsType = Get-RequiredType ($root + 'LocalCubicBezierOptions') $quinticOptionsType = Get-RequiredType ($root + 'PiecewiseQuinticOptions') +$localOptionsType = Get-RequiredType ($root + 'LocalG2QuinticOptions') +$regionStatusType = Get-RequiredType ($root + 'PathSmoothingRegionStatus') +$regionFailureType = Get-RequiredType ($root + 'PathSmoothingRegionFailureReason') +$regionReportType = Get-RequiredType ($root + 'PathSmoothingRegionReport') $requestType = Get-RequiredType ($root + 'PathSmoothingRequest') $metricsType = Get-RequiredType ($root + 'PathQualityMetrics') $diagnosticsType = Get-RequiredType ($root + 'PathSmoothingDiagnostics') @@ -60,9 +64,11 @@ $vehicleType = Get-RequiredType ($coarsePathRoot + 'VehicleParameters') Assert-Equal $true $methodType.IsEnum 'SmoothingMethod must be a public enum.' Assert-Equal $true $statusType.IsEnum 'PathSmoothingStatus must be a public enum.' Assert-Equal $true $sourceType.IsEnum 'SmoothedPathPointSource must be a public enum.' -Assert-Equal 'CubicBSpline,LocalCubicBezier,PiecewiseQuintic' ([string]::Join(',', [Enum]::GetNames($methodType))) 'Smoothing method members and order must remain stable.' -Assert-Equal 'Success,FallbackToCoarsePath,InvalidInput,Infeasible,Failed,Cancelled' ([string]::Join(',', [Enum]::GetNames($statusType))) 'Smoothing status members and order must remain stable.' -Assert-Equal 'Anchor,Interpolated,GearSwitch,CoarsePathFallback' ([string]::Join(',', [Enum]::GetNames($sourceType))) 'Smoothed point source members and order must remain stable.' +Assert-Equal 'CubicBSpline,LocalCubicBezier,PiecewiseQuintic,LocalG2Quintic' ([string]::Join(',', [Enum]::GetNames($methodType))) 'The Local G2 method must be appended without reordering legacy methods.' +Assert-Equal 'Success,FallbackToCoarsePath,InvalidInput,Infeasible,Failed,Cancelled,Complete,PartialImprovement,NotNeeded,Unchanged' ([string]::Join(',', [Enum]::GetNames($statusType))) 'Local G2 statuses must be appended without reordering legacy statuses.' +Assert-Equal 'Anchor,Interpolated,GearSwitch,CoarsePathFallback,LocalG2Transition' ([string]::Join(',', [Enum]::GetNames($sourceType))) 'Local G2 point source must be appended without reordering legacy sources.' +Assert-Equal 'Improved,RetainedOriginal' ([string]::Join(',', [Enum]::GetNames($regionStatusType))) 'Local G2 region statuses must be stable.' +Assert-Equal 'None,WindowUnavailable,CandidateGenerationFailed,Collision,InsufficientClearance,CurvatureExceeded,CurvatureOvershoot,DeviationExceeded,InsufficientImprovement,VariationCostRegression,GlobalValidationRollback' ([string]::Join(',', [Enum]::GetNames($regionFailureType))) 'Local G2 region failure reasons must be stable.' $configuration = [Activator]::CreateInstance($configurationType) Assert-Near 0.05 $configuration.OutputSpacingMeters 'Default output spacing must be 0.05 m.' @@ -87,6 +93,16 @@ Assert-Near (1.0 / 3.0) $bezier.HandleLengthRatio 'Bezier handle default must be $quintic = [Activator]::CreateInstance($quinticOptionsType) Assert-Near 0.50 $quintic.KnotSpacingMeters 'Quintic knot spacing must be 0.50 m.' Assert-Near 0.10 $quintic.MinimumKnotSpacingMeters 'Quintic minimum knot spacing must be 0.10 m.' +$local = [Activator]::CreateInstance($localOptionsType) +Assert-Near 0.20 $local.MinimumWindowLengthMeters 'Minimum Local G2 window must be 0.20 m.' +Assert-Near 0.50 $local.PreferredWindowLengthMeters 'Preferred Local G2 window must be 0.50 m.' +Assert-Near 0.80 $local.MaximumWindowLengthMeters 'Maximum Local G2 window must be 0.80 m.' +Assert-Near 0.10 $local.MaximumDeviationMeters 'Maximum Local G2 deviation must be 0.10 m.' +Assert-Near 0.001 $local.AbsoluteCurvatureJumpFloorPerMeter 'Absolute jump floor must be 0.001 1/m.' +Assert-Near 0.05 $local.CurvatureJumpRatioOfMaximum 'Relative jump threshold must be 5 percent.' +Assert-Near 0.20 $local.MinimumPeakGradientImprovementRatio 'Peak improvement must be 20 percent.' +Assert-Near 0.02 $local.MaximumVariationCostRegressionRatio 'Variation cost tolerance must be 2 percent.' +Assert-Equal 12 $local.MaximumCandidatesPerRegion 'At most twelve candidates are allowed.' $forward = [Enum]::Parse($directionType, 'Forward') $anchor = [Enum]::Parse($sourceType, 'Anchor') @@ -104,6 +120,11 @@ Assert-Near 0.12 $point.VehicleCurvature 'Smoothed point vehicle curvature must Assert-Near 0.44 $point.BodyClearance 'Smoothed point clearance must be stored in m.' Assert-Equal $false $point.IsGearSwitchPoint 'Smoothed point gear-switch marker must be preserved.' Assert-Equal 'Anchor' $point.Source.ToString() 'Smoothed point source must be preserved.' +Assert-True ($pointType.GetProperty('VehicleCurvatureDerivative') -ne $null) 'Smoothed points must expose d-kappa/d-s.' +$pointWithDerivative = [Activator]::CreateInstance($pointType, @( + [double]1.25, [double]-2.50, [double]0.30, [double]6.58, [double]4.75, + $forward, [double]0.12, [double]0.12, [double]0.37, [double]0.44, $false, $anchor)) +Assert-Near 0.37 $pointWithDerivative.VehicleCurvatureDerivative 'Smoothed point curvature derivative must be stored in 1/m^2.' $segmentA = [Activator]::CreateInstance($segmentType, @(0, $forward, 0, 2, $false, $true)) $reverse = [Enum]::Parse($directionType, 'Reverse') @@ -120,6 +141,13 @@ $metrics = [Activator]::CreateInstance($metricsType) Assert-Equal $false $metrics.IsFeasible 'Default metrics must be infeasible until analysis accepts a candidate.' Assert-Near 0.0 $metrics.PathLengthMeters 'Default metrics must be zero-valued.' Assert-Near 0.0 $metrics.MinimumBodyClearanceMeters 'Default metrics must be zero-valued.' +Assert-Near 0.0 $metrics.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 'Default derivative metric must be zero-valued.' +Assert-Near 0.0 $metrics.CurvatureVariationCost 'Curvature variation cost compatibility alias must be available.' +$metricsWithDerivative = [Activator]::CreateInstance($metricsType, @( + $true, + [double]1.0, [double]0.50, [double]0.75, [double]0.0, [double]0.0, + [double]0.0, [double]0.25, [double]0.0, [double]0.0, [double]0.0, [double]0.0)) +Assert-Near 0.75 $metricsWithDerivative.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 'Derivative-aware metrics constructor must retain the peak derivative.' $diagnostics = [Activator]::CreateInstance($diagnosticsType) Assert-True ($diagnostics.Metrics -ne $null) 'Default diagnostics must provide quality metrics.' Assert-Equal 0 $diagnostics.RetryCount 'Default diagnostics must have no retries.' @@ -166,6 +194,7 @@ Assert-Throws { $fallbackMethod.Invoke($null, @([Enum]::ToObject($methodType, 99 $fallback = $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $feasibleDiagnostics)) Assert-Equal 'FallbackToCoarsePath' $fallback.Status.ToString() 'Fallback factory must publish an explicit fallback status.' Assert-Equal 1 $fallback.Path.Count 'Fallback factory must publish a validated fallback path.' +Assert-Equal 0 $fallback.RegionReports.Count 'Legacy fallback results must publish empty immutable region reports.' $failureMethod = $resultType.GetMethod('Failure') Assert-True ($null -ne $failureMethod) 'PathSmoothingResult must expose Failure.' @@ -183,6 +212,44 @@ Assert-Throws { $failureMethod.Invoke($null, @([Enum]::Parse($statusType, 'Fallb Assert-Throws { $failureMethod.Invoke($null, @([Enum]::ToObject($statusType, 99), $diagnostics)) } 'Failure factory must reject undefined statuses.' Assert-Throws { $successMethod.Invoke($null, @($method, [Array]::CreateInstance($pointType, 0), $fallbackSegments, $diagnostics)) } 'Success factory must reject an empty path.' Assert-Throws { $successMethod.Invoke($null, @($method, $fallbackPath, [Array]::CreateInstance($segmentType, 0), $diagnostics)) } 'Success factory must reject empty segments.' +Assert-Equal 0 $success.RegionReports.Count 'Legacy success results must publish empty immutable region reports.' +Assert-ReadOnlyCollection $success.RegionReports 'Legacy success region reports must be immutable.' + +$curvatureJumps = [System.Collections.Generic.List[double]]::new() +$curvatureJumps.Add([double]0.20) +$report = [Activator]::CreateInstance($regionReportType, @( + 0, [double]0.0, [double]0.5, $curvatureJumps, + [double]0.5, [double]0.5, [double]0.25, [double]0.25, + 1, 0, + [Enum]::Parse($regionStatusType, 'Improved'), [Enum]::Parse($regionFailureType, 'None'), + [double]1.0, [double]0.5, [double]2.0, [double]1.0, + [double]0.05, [double]0.10, [double]0.80)) +Assert-ReadOnlyCollection $report.CurvatureJumpsPerMeter 'Region report curvature jumps must be immutable.' +$curvatureJumps[0] = [double]9.99 +Assert-Near 0.20 $report.CurvatureJumpsPerMeter[0] 'Region report must copy curvature jumps.' +$retainedReport = [Activator]::CreateInstance($regionReportType, @( + 0, [double]0.0, [double]0.5, $curvatureJumps, + [double]0.5, [double]0.5, [double]0.25, [double]0.25, + 1, 7, + [Enum]::Parse($regionStatusType, 'RetainedOriginal'), [Enum]::Parse($regionFailureType, 'InsufficientImprovement'), + [double]1.0, [double]1.0, [double]2.0, [double]2.0, + [double]0.0, [double]0.10, [double]0.80)) +Assert-Equal -1 $retainedReport.SelectedCandidateIndex 'A region without a selected candidate must publish -1.' +$publishLocalG2Method = $resultType.GetMethod('PublishLocalG2') +Assert-True ($null -ne $publishLocalG2Method) 'PathSmoothingResult must expose PublishLocalG2.' +$reports = [Array]::CreateInstance($regionReportType, 1) +$reports.SetValue($report, 0) +$localG2Method = [Enum]::Parse($methodType, 'LocalG2Quintic') +$complete = [Enum]::Parse($statusType, 'Complete') +$localG2Result = $publishLocalG2Method.Invoke($null, @($complete, $fallbackPath, $fallbackSegments, $feasibleDiagnostics, $reports)) +Assert-Equal 'Complete' $localG2Result.Status.ToString() 'PublishLocalG2 must retain Local G2 publication status.' +Assert-Equal 'LocalG2Quintic' $localG2Result.Method.ToString() 'PublishLocalG2 must publish the Local G2 method.' +Assert-Equal 1 $localG2Result.RegionReports.Count 'PublishLocalG2 must publish region reports.' +Assert-ReadOnlyCollection $localG2Result.RegionReports 'Local G2 result region reports must be immutable.' +$reports.SetValue($null, 0) +Assert-True ($null -ne $localG2Result.RegionReports[0]) 'PublishLocalG2 must copy region reports.' +Assert-Throws { $publishLocalG2Method.Invoke($null, @([Enum]::Parse($statusType, 'Success'), $fallbackPath, $fallbackSegments, $feasibleDiagnostics, $reports)) } 'PublishLocalG2 must reject legacy statuses.' +Assert-Throws { $publishLocalG2Method.Invoke($null, @($complete, [Array]::CreateInstance($pointType, 0), $fallbackSegments, $feasibleDiagnostics, $reports)) } 'PublishLocalG2 must reject an empty path.' $requestConstructor = $requestType.GetConstructor(@( [System.Collections.Generic.IReadOnlyList``1].MakeGenericType($coarsePointType), @@ -235,6 +302,15 @@ $requestConfiguration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.99 $requestConfiguration.LocalCubicBezier.HandleLengthRatio = [double]0.99 $requestConfiguration.PiecewiseQuintic.KnotSpacingMeters = [double]0.99 $requestConfiguration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.99 +$requestConfiguration.LocalG2Quintic.MinimumWindowLengthMeters = [double]0.99 +$requestConfiguration.LocalG2Quintic.PreferredWindowLengthMeters = [double]0.99 +$requestConfiguration.LocalG2Quintic.MaximumWindowLengthMeters = [double]0.99 +$requestConfiguration.LocalG2Quintic.MaximumDeviationMeters = [double]0.99 +$requestConfiguration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = [double]0.99 +$requestConfiguration.LocalG2Quintic.CurvatureJumpRatioOfMaximum = [double]0.99 +$requestConfiguration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = [double]0.99 +$requestConfiguration.LocalG2Quintic.MaximumVariationCostRegressionRatio = [double]0.99 +$requestConfiguration.LocalG2Quintic.MaximumCandidatesPerRegion = 99 Assert-True ($null -ne $request.CoarsePath[0]) 'Request must copy the coarse-path collection.' Assert-True ($null -ne $request.Segments[0]) 'Request must copy the segment collection.' Assert-Near 0.80 $request.Vehicle.LengthMeters 'Request must snapshot vehicle parameters.' @@ -254,6 +330,15 @@ Assert-Near 0.60 $request.Configuration.LocalCubicBezier.MaximumWindowLengthMete Assert-Near (1.0 / 3.0) $request.Configuration.LocalCubicBezier.HandleLengthRatio 'Request must snapshot Bezier options.' Assert-Near 0.50 $request.Configuration.PiecewiseQuintic.KnotSpacingMeters 'Request must snapshot quintic options.' Assert-Near 0.10 $request.Configuration.PiecewiseQuintic.MinimumKnotSpacingMeters 'Request must snapshot quintic minimum spacing.' +Assert-Near 0.20 $request.Configuration.LocalG2Quintic.MinimumWindowLengthMeters 'Request must snapshot Local G2 minimum window.' +Assert-Near 0.50 $request.Configuration.LocalG2Quintic.PreferredWindowLengthMeters 'Request must snapshot Local G2 preferred window.' +Assert-Near 0.80 $request.Configuration.LocalG2Quintic.MaximumWindowLengthMeters 'Request must snapshot Local G2 maximum window.' +Assert-Near 0.10 $request.Configuration.LocalG2Quintic.MaximumDeviationMeters 'Request must snapshot Local G2 maximum deviation.' +Assert-Near 0.001 $request.Configuration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter 'Request must snapshot Local G2 absolute jump floor.' +Assert-Near 0.05 $request.Configuration.LocalG2Quintic.CurvatureJumpRatioOfMaximum 'Request must snapshot Local G2 relative jump threshold.' +Assert-Near 0.20 $request.Configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio 'Request must snapshot Local G2 peak improvement threshold.' +Assert-Near 0.02 $request.Configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio 'Request must snapshot Local G2 variation tolerance.' +Assert-Equal 12 $request.Configuration.LocalG2Quintic.MaximumCandidatesPerRegion 'Request must snapshot Local G2 candidate count.' $request.Vehicle.WidthMeters = [double]9.99 $request.Vehicle.SafetyMarginMeters = [double]9.99 $request.Vehicle.MinimumTurningRadiusMeters = [double]9.99 @@ -264,6 +349,15 @@ $request.Configuration.AllowFallbackToCoarsePath = $false $request.Configuration.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.99 $request.Configuration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.99 $request.Configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.99 +$request.Configuration.LocalG2Quintic.MinimumWindowLengthMeters = [double]0.99 +$request.Configuration.LocalG2Quintic.PreferredWindowLengthMeters = [double]0.99 +$request.Configuration.LocalG2Quintic.MaximumWindowLengthMeters = [double]0.99 +$request.Configuration.LocalG2Quintic.MaximumDeviationMeters = [double]0.99 +$request.Configuration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = [double]0.99 +$request.Configuration.LocalG2Quintic.CurvatureJumpRatioOfMaximum = [double]0.99 +$request.Configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = [double]0.99 +$request.Configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio = [double]0.99 +$request.Configuration.LocalG2Quintic.MaximumCandidatesPerRegion = 99 Assert-Near 0.60 $request.Vehicle.WidthMeters 'Request vehicle getter must not expose mutable state.' Assert-Near 0.05 $request.Vehicle.SafetyMarginMeters 'Request vehicle getter must not expose mutable state.' Assert-True ($null -eq $request.Vehicle.MinimumTurningRadiusMeters) 'Request vehicle getter must not expose mutable nullable state.' @@ -274,5 +368,14 @@ Assert-Equal $true $request.Configuration.AllowFallbackToCoarsePath 'Request con Assert-Near ([Math]::PI / 18.0) $request.Configuration.LocalCubicBezier.CornerHeadingThresholdRadians 'Request configuration getter must not expose mutable Bezier options.' Assert-Near 0.60 $request.Configuration.LocalCubicBezier.MaximumWindowLengthMeters 'Request configuration getter must not expose mutable Bezier options.' Assert-Near 0.10 $request.Configuration.PiecewiseQuintic.MinimumKnotSpacingMeters 'Request configuration getter must not expose mutable quintic options.' +Assert-Near 0.20 $request.Configuration.LocalG2Quintic.MinimumWindowLengthMeters 'Request configuration getter must not expose mutable Local G2 minimum window.' +Assert-Near 0.50 $request.Configuration.LocalG2Quintic.PreferredWindowLengthMeters 'Request configuration getter must not expose mutable Local G2 preferred window.' +Assert-Near 0.80 $request.Configuration.LocalG2Quintic.MaximumWindowLengthMeters 'Request configuration getter must not expose mutable Local G2 maximum window.' +Assert-Near 0.10 $request.Configuration.LocalG2Quintic.MaximumDeviationMeters 'Request configuration getter must not expose mutable Local G2 maximum deviation.' +Assert-Near 0.001 $request.Configuration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter 'Request configuration getter must not expose mutable Local G2 absolute jump floor.' +Assert-Near 0.05 $request.Configuration.LocalG2Quintic.CurvatureJumpRatioOfMaximum 'Request configuration getter must not expose mutable Local G2 relative jump threshold.' +Assert-Near 0.20 $request.Configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio 'Request configuration getter must not expose mutable Local G2 peak improvement threshold.' +Assert-Near 0.02 $request.Configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio 'Request configuration getter must not expose mutable Local G2 variation tolerance.' +Assert-Equal 12 $request.Configuration.LocalG2Quintic.MaximumCandidatesPerRegion 'Request configuration getter must not expose mutable Local G2 candidate count.' Write-Output 'Path smoothing contract checks passed.'