feat: add local G2 smoothing contracts
This commit is contained in:
梁薄云
@@ -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.'
|
||||
|
||||
Reference in New Issue
Block a user