231 lines
13 KiB
PowerShell
231 lines
13 KiB
PowerShell
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
|
|
|
$ErrorActionPreference = 'Stop'
|
|
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
|
|
|
function Assert-True($Actual, [string]$Message) {
|
|
if (-not $Actual) { throw $Message }
|
|
}
|
|
|
|
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
|
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
|
}
|
|
|
|
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
|
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
|
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
|
}
|
|
}
|
|
|
|
function Get-RequiredType([string]$Name) {
|
|
return $assembly.GetType($Name, $true)
|
|
}
|
|
|
|
function New-Map {
|
|
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
|
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
|
$mapRequest.ResolutionMm = [single]50
|
|
$mapRequest.AllowExplicitEmptyMap = $true
|
|
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
|
Assert-True ($null -ne $map) 'Comparison test must create a planning map.'
|
|
return $map
|
|
}
|
|
|
|
function New-Vehicle {
|
|
$vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$vehicle.LengthMeters = [double]0.20
|
|
$vehicle.WidthMeters = [double]0.20
|
|
$vehicle.SafetyMarginMeters = [double]0.0
|
|
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
|
return $vehicle
|
|
}
|
|
|
|
function New-CoarsePoint([double]$X, [double]$Y, [double]$ArcLength) {
|
|
return [Activator]::CreateInstance($coarsePointType, @(
|
|
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $forward,
|
|
[double]0.0, [double]1.0, $false, $coarseAnchor))
|
|
}
|
|
|
|
function New-SmoothingRequest {
|
|
$points = [Array]::CreateInstance($coarsePointType, 2)
|
|
$points.SetValue((New-CoarsePoint 0.5 0.5 0.0), 0)
|
|
$points.SetValue((New-CoarsePoint 1.5 0.5 1.0), 1)
|
|
$segments = [Array]::CreateInstance($coarseSegmentType, 1)
|
|
$segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
|
|
$configuration = [Activator]::CreateInstance($configurationType)
|
|
return [Activator]::CreateInstance($smoothingRequestType, @($points, $segments, (New-Map), (New-Vehicle), $configuration))
|
|
}
|
|
|
|
function New-CornerSmoothingRequest {
|
|
$points = [Array]::CreateInstance($coarsePointType, 4)
|
|
$points.SetValue((New-CoarsePoint 0.5 0.5 0.0), 0)
|
|
$points.SetValue((New-CoarsePoint 1.0 0.5 0.5), 1)
|
|
$points.SetValue((New-CoarsePoint 1.0 1.0 1.0), 2)
|
|
$points.SetValue((New-CoarsePoint 1.5 1.0 1.5), 3)
|
|
$segments = [Array]::CreateInstance($coarseSegmentType, 1)
|
|
$segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 3, $false, $false)), 0)
|
|
$configuration = [Activator]::CreateInstance($configurationType)
|
|
return [Activator]::CreateInstance($smoothingRequestType, @($points, $segments, (New-Map), (New-Vehicle), $configuration))
|
|
}
|
|
|
|
function New-Metrics(
|
|
[double]$VariationEnergy,
|
|
[double]$PeakCurvature,
|
|
[double]$MinimumClearance,
|
|
[double]$LengthChangePercent) {
|
|
return [Activator]::CreateInstance($metricsType, @(
|
|
$true, [double]1.0, $PeakCurvature, [double]0.0, [double]0.0, $VariationEnergy,
|
|
$MinimumClearance, $LengthChangePercent, [double]0.0, [double]0.0, [double]0.0))
|
|
}
|
|
|
|
function New-Timing([double]$MedianMilliseconds) {
|
|
[double[]]$samples = @($MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds)
|
|
return [Activator]::CreateInstance($timingType, @($samples, $true, ''))
|
|
}
|
|
|
|
function New-Entry(
|
|
$Method,
|
|
[double]$VariationEnergy,
|
|
[double]$PeakCurvature,
|
|
[double]$MinimumClearance,
|
|
[double]$LengthChangePercent,
|
|
[double]$MedianMilliseconds) {
|
|
[object[]]$arguments = @(
|
|
$Method, $successStatus, (New-Metrics $VariationEnergy $PeakCurvature $MinimumClearance $LengthChangePercent),
|
|
(New-Timing $MedianMilliseconds), ('synthetic-' + $Method.ToString()), '')
|
|
return [Activator]::CreateInstance($entryType, $arguments)
|
|
}
|
|
|
|
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
|
$comparison = $root + 'Comparison.'
|
|
$facade = $root + 'Facade.'
|
|
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
|
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
|
|
|
$comparisonServiceType = Get-RequiredType ($facade + 'PathSmoothingComparisonService')
|
|
$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest')
|
|
$comparisonResultType = Get-RequiredType ($comparison + 'PathSmoothingComparisonResult')
|
|
$entryType = Get-RequiredType ($comparison + 'PathSmoothingComparisonEntry')
|
|
$timingType = Get-RequiredType ($comparison + 'SmoothingTimingSummary')
|
|
$rankerType = Get-RequiredType ($comparison + 'SmoothingMethodRanker')
|
|
$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
|
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
|
$metricsType = Get-RequiredType ($root + 'PathQualityMetrics')
|
|
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
|
|
$statusType = Get-RequiredType ($root + 'PathSmoothingStatus')
|
|
$smoothingResultType = Get-RequiredType ($root + 'PathSmoothingResult')
|
|
$diagnosticsType = Get-RequiredType ($root + 'PathSmoothingDiagnostics')
|
|
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
|
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
|
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
|
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
|
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
|
$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
|
|
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
|
|
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')
|
|
|
|
$forward = [Enum]::Parse($directionType, 'Forward')
|
|
$coarseAnchor = [Enum]::Parse($coarsePointSourceType, 'Start')
|
|
$cubicBSpline = [Enum]::Parse($methodType, 'CubicBSpline')
|
|
$localCubicBezier = [Enum]::Parse($methodType, 'LocalCubicBezier')
|
|
$piecewiseQuintic = [Enum]::Parse($methodType, 'PiecewiseQuintic')
|
|
$successStatus = [Enum]::Parse($statusType, 'Success')
|
|
|
|
Assert-True $comparisonServiceType.IsPublic 'Comparison service must be public.'
|
|
$compareMethod = $comparisonServiceType.GetMethod('Compare', [Type[]]@($comparisonRequestType, [Threading.CancellationToken]))
|
|
Assert-True ($null -ne $compareMethod) 'Comparison service must expose Compare(PathSmoothingComparisonRequest, CancellationToken).'
|
|
Assert-Equal $comparisonResultType $compareMethod.ReturnType 'Compare must return PathSmoothingComparisonResult.'
|
|
|
|
$methods = [Array]::CreateInstance($methodType, 3)
|
|
$methods.SetValue($cubicBSpline, 0)
|
|
$methods.SetValue($localCubicBezier, 1)
|
|
$methods.SetValue($piecewiseQuintic, 2)
|
|
$comparisonRequest = [Activator]::CreateInstance($comparisonRequestType, @((New-SmoothingRequest), $methods))
|
|
$comparisonService = [Activator]::CreateInstance($comparisonServiceType)
|
|
$result = $compareMethod.Invoke($comparisonService, @($comparisonRequest, [Threading.CancellationToken]::None))
|
|
|
|
Assert-True ($null -ne $result.RawPathBaseline) 'Comparison must publish a separately analyzed raw-path baseline.'
|
|
Assert-True $result.RawPathBaseline.IsRawPathBaseline 'Raw baseline must be explicitly marked and excluded from candidates.'
|
|
Assert-Equal 3 $result.Entries.Count 'Comparison must contain exactly one entry for every requested method.'
|
|
Assert-True ($null -ne $result.RecommendedMethod) 'A comparison with feasible methods must select a recommendation.'
|
|
foreach ($entry in $result.Entries) {
|
|
Assert-True (-not $entry.IsRawPathBaseline) 'Candidate entries must not be marked as the raw baseline.'
|
|
Assert-Equal 5 $entry.Timing.MeasuredElapsedMilliseconds.Count 'Warm-up must be excluded and exactly five measurements retained.'
|
|
Assert-True $entry.Timing.IsDeterministic 'Repeated deterministic smoothing geometry must remain eligible for recommendation.'
|
|
Assert-True (-not [string]::IsNullOrWhiteSpace($entry.StableGeometryDigest)) 'Every measured candidate must expose a stable geometry digest.'
|
|
}
|
|
|
|
$fromMeasurementsMethod = $timingType.GetMethod('FromMeasurements')
|
|
Assert-True ($null -ne $fromMeasurementsMethod) 'Timing summary must analyze the five measured outputs for deterministic geometry.'
|
|
$failureFactory = $smoothingResultType.GetMethod('Failure')
|
|
$invalidInputStatus = [Enum]::Parse($statusType, 'InvalidInput')
|
|
$infeasibleStatus = [Enum]::Parse($statusType, 'Infeasible')
|
|
$inconsistentResults = [Array]::CreateInstance($smoothingResultType, 5)
|
|
for ($index = 0; $index -lt 5; $index++) {
|
|
$status = if ($index -eq 4) { $infeasibleStatus } else { $invalidInputStatus }
|
|
[object[]]$failureArguments = New-Object object[] 2
|
|
$failureArguments[0] = $status
|
|
$failureArguments[1] = [Activator]::CreateInstance($diagnosticsType)
|
|
$inconsistentResults.SetValue($failureFactory.Invoke($null, $failureArguments), $index)
|
|
}
|
|
[object[]]$timingArguments = New-Object object[] 2
|
|
$timingArguments[0] = [double[]]@(1.0, 2.0, 3.0, 4.0, 5.0)
|
|
$timingArguments[1] = $inconsistentResults
|
|
$nonDeterministicTiming = $fromMeasurementsMethod.Invoke($null, $timingArguments)
|
|
Assert-True (-not $nonDeterministicTiming.IsDeterministic) 'A status, point-count, segment-count, or digest mismatch must be non-deterministic.'
|
|
Assert-True (-not [string]::IsNullOrWhiteSpace($nonDeterministicTiming.Diagnostic)) 'Non-deterministic measurements must publish a stable diagnostic.'
|
|
|
|
# All published comparison metrics must be normalized against the separately analyzed raw baseline.
|
|
$cornerMethods = [Array]::CreateInstance($methodType, 1)
|
|
$cornerMethods.SetValue($cubicBSpline, 0)
|
|
$cornerRequest = [Activator]::CreateInstance($comparisonRequestType, @((New-CornerSmoothingRequest), $cornerMethods))
|
|
$cornerResult = $compareMethod.Invoke($comparisonService, @($cornerRequest, [Threading.CancellationToken]::None))
|
|
$cornerEntry = $cornerResult.Entries[0]
|
|
Assert-Equal 'Success' $cornerEntry.Status.ToString() 'The unconstrained empty-map corner fixture must produce a B-spline comparison candidate.'
|
|
$expectedLengthChange = (($cornerEntry.Path[$cornerEntry.Path.Count - 1].ArcLength - $cornerResult.RawPathBaseline.Metrics.PathLengthMeters) /
|
|
$cornerResult.RawPathBaseline.Metrics.PathLengthMeters) * 100.0
|
|
Assert-Near $expectedLengthChange $cornerEntry.Metrics.LengthChangePercent 0.000001 'Candidate length change must be normalized relative to the raw baseline.'
|
|
|
|
# The ranker must apply every public tie-break in order. Each pair ties all prior criteria.
|
|
$entryListType = [Collections.Generic.IReadOnlyList``1].MakeGenericType(@($entryType))
|
|
$rankMethod = $rankerType.GetMethod('Rank', [Type[]]@($entryListType))
|
|
Assert-True ($null -ne $rankMethod) 'SmoothingMethodRanker must expose Rank(IReadOnlyList<PathSmoothingComparisonEntry>).'
|
|
|
|
function Assert-Rank($ExpectedMethod, [object[]]$Entries, [string]$Message) {
|
|
$typedEntries = [Array]::CreateInstance($entryType, $Entries.Count)
|
|
for ($index = 0; $index -lt $Entries.Count; $index++) { $typedEntries.SetValue($Entries[$index], $index) }
|
|
[object[]]$invokeArguments = New-Object object[] 1
|
|
$invokeArguments[0] = $typedEntries
|
|
$actual = $rankMethod.Invoke($null, $invokeArguments)
|
|
Assert-Equal $ExpectedMethod.ToString() $actual.ToString() $Message
|
|
}
|
|
|
|
Assert-Rank $cubicBSpline @(
|
|
(New-Entry $cubicBSpline 1.0 0.5 0.8 5.0 10.0),
|
|
(New-Entry $localCubicBezier 2.0 0.1 1.0 1.0 1.0)) 'Variation-energy tie-break must take priority over later criteria.'
|
|
Assert-Rank $cubicBSpline @(
|
|
(New-Entry $cubicBSpline 1.0 0.2 0.8 5.0 10.0),
|
|
(New-Entry $localCubicBezier 1.0 0.3 1.0 1.0 1.0)) 'Peak-curvature tie-break must follow variation energy.'
|
|
Assert-Rank $cubicBSpline @(
|
|
(New-Entry $cubicBSpline 1.0 0.2 0.9 5.0 10.0),
|
|
(New-Entry $localCubicBezier 1.0 0.2 0.8 1.0 1.0)) 'Clearance-loss tie-break must follow peak curvature.'
|
|
Assert-Rank $cubicBSpline @(
|
|
(New-Entry $cubicBSpline 1.0 0.2 0.9 2.0 10.0),
|
|
(New-Entry $localCubicBezier 1.0 0.2 0.9 3.0 1.0)) 'Length-change tie-break must follow clearance loss.'
|
|
Assert-Rank $cubicBSpline @(
|
|
(New-Entry $cubicBSpline 1.0 0.2 0.9 2.0 5.0),
|
|
(New-Entry $localCubicBezier 1.0 0.2 0.9 2.0 6.0)) 'Median elapsed tie-break must be last.'
|
|
|
|
$cancelSource = [Threading.CancellationTokenSource]::new()
|
|
try {
|
|
$cancelSource.Cancel()
|
|
$cancelled = $compareMethod.Invoke($comparisonService, @($comparisonRequest, $cancelSource.Token))
|
|
Assert-True $cancelled.IsCancelled 'Cancellation must stop comparison before subsequent methods start.'
|
|
Assert-Equal $null $cancelled.RecommendedMethod 'Cancelled comparison must not make a recommendation.'
|
|
}
|
|
finally {
|
|
$cancelSource.Dispose()
|
|
}
|
|
|
|
Write-Output 'Path smoothing comparison checks passed.'
|