test: add path smoothing scenarios and fixtures
This commit is contained in:
梁薄云
@@ -0,0 +1,140 @@
|
||||
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, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
function Get-RequiredType([string]$Name) { return $assembly.GetType($Name, $true) }
|
||||
|
||||
function Assert-CoarsePathGeometry($ExpectedPath, $ActualPath, $Map, [string]$ScenarioName) {
|
||||
Assert-Equal $ExpectedPath.Count $ActualPath.Count "Raw comparison request $ScenarioName must preserve every coarse-path point."
|
||||
for ($index = 0; $index -lt $ExpectedPath.Count; $index++) {
|
||||
$expected = $ExpectedPath[$index]
|
||||
$actual = $ActualPath[$index]
|
||||
Assert-Near $expected.X $actual.X "Raw comparison request $ScenarioName point $index must preserve X."
|
||||
Assert-Near $expected.Y $actual.Y "Raw comparison request $ScenarioName point $index must preserve Y."
|
||||
Assert-Near $expected.Heading $actual.Heading "Raw comparison request $ScenarioName point $index must preserve heading."
|
||||
Assert-Near $expected.UnwrappedHeading $actual.UnwrappedHeading "Raw comparison request $ScenarioName point $index must preserve unwrapped heading."
|
||||
Assert-Near $expected.ArcLength $actual.ArcLength "Raw comparison request $ScenarioName point $index must preserve arc length."
|
||||
Assert-Equal $expected.Direction $actual.Direction "Raw comparison request $ScenarioName point $index must preserve travel direction."
|
||||
Assert-Near $expected.VehicleCurvature $actual.VehicleCurvature "Raw comparison request $ScenarioName point $index must preserve vehicle curvature."
|
||||
$expectedClearance = $expected.BodyClearance
|
||||
if ([double]::IsPositiveInfinity($expectedClearance)) {
|
||||
$widthMeters = ($Map.Bounds.XMax - $Map.Bounds.XMin) / 1000.0
|
||||
$heightMeters = ($Map.Bounds.YMax - $Map.Bounds.YMin) / 1000.0
|
||||
$expectedClearance = [Math]::Sqrt($widthMeters * $widthMeters + $heightMeters * $heightMeters)
|
||||
}
|
||||
Assert-Near $expectedClearance $actual.BodyClearance "Raw comparison request $ScenarioName point $index must preserve or normalize clearance for smoothing."
|
||||
Assert-Equal $expected.IsGearSwitchPoint $actual.IsGearSwitchPoint "Raw comparison request $ScenarioName point $index must preserve gear-switch flag."
|
||||
Assert-Equal $expected.Source $actual.Source "Raw comparison request $ScenarioName point $index must preserve point source."
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-SegmentTopology($ExpectedSegments, $ActualPath, $ActualSegments, [string]$Description) {
|
||||
Assert-Equal $ExpectedSegments.Count $ActualSegments.Count "$Description must preserve segment count."
|
||||
$expectedStartIndex = 0
|
||||
for ($index = 0; $index -lt $ActualSegments.Count; $index++) {
|
||||
$expected = $ExpectedSegments[$index]
|
||||
$actual = $ActualSegments[$index]
|
||||
Assert-Equal $index $actual.SegmentIndex "$Description segment $index must retain its stable index."
|
||||
Assert-Equal $expected.Direction $actual.Direction "$Description segment $index must preserve travel direction."
|
||||
Assert-Equal $expected.StartsAtGearSwitch $actual.StartsAtGearSwitch "$Description segment $index must preserve start gear-switch topology."
|
||||
Assert-Equal $expected.EndsAtGearSwitch $actual.EndsAtGearSwitch "$Description segment $index must preserve end gear-switch topology."
|
||||
Assert-Equal $expectedStartIndex $actual.StartIndex "$Description segment $index must start directly after the prior segment."
|
||||
Assert-True ($actual.EndIndex -ge $actual.StartIndex -and $actual.EndIndex -lt $ActualPath.Count) "$Description segment $index must cover valid path indices."
|
||||
Assert-Equal $actual.StartsAtGearSwitch $ActualPath[$actual.StartIndex].IsGearSwitchPoint "$Description segment $index start flag must match its path point."
|
||||
for ($pointIndex = $actual.StartIndex; $pointIndex -le $actual.EndIndex; $pointIndex++) {
|
||||
Assert-Equal $actual.Direction $ActualPath[$pointIndex].Direction "$Description segment $index may not contain mixed directions."
|
||||
}
|
||||
$hasNext = $index + 1 -lt $ActualSegments.Count
|
||||
$expectedEndSwitch = $hasNext -and $ActualPath[$actual.EndIndex + 1].IsGearSwitchPoint
|
||||
Assert-Equal $expectedEndSwitch $actual.EndsAtGearSwitch "$Description segment $index end flag must match the next gear switch."
|
||||
$expectedStartIndex = $actual.EndIndex + 1
|
||||
}
|
||||
Assert-Equal $ActualPath.Count $expectedStartIndex "$Description segments must cover every path point."
|
||||
}
|
||||
|
||||
function Assert-GearSwitchGeometry($ExpectedPath, $ActualPath, [string]$Description) {
|
||||
$expectedSwitches = @($ExpectedPath | Where-Object { $_.IsGearSwitchPoint })
|
||||
$actualSwitches = @($ActualPath | Where-Object { $_.IsGearSwitchPoint })
|
||||
Assert-Equal $expectedSwitches.Count $actualSwitches.Count "$Description must preserve gear-switch count."
|
||||
for ($index = 0; $index -lt $expectedSwitches.Count; $index++) {
|
||||
Assert-Near $expectedSwitches[$index].X $actualSwitches[$index].X "$Description gear switch $index must preserve X."
|
||||
Assert-Near $expectedSwitches[$index].Y $actualSwitches[$index].Y "$Description gear switch $index must preserve Y."
|
||||
Assert-Near $expectedSwitches[$index].Heading $actualSwitches[$index].Heading "$Description gear switch $index must preserve heading."
|
||||
Assert-Equal $expectedSwitches[$index].Direction $actualSwitches[$index].Direction "$Description gear switch $index must preserve direction."
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-RawBaselinePath($ExpectedPath, $ActualPath, [string]$ScenarioName) {
|
||||
Assert-Equal $ExpectedPath.Count $ActualPath.Count "Raw baseline $ScenarioName must preserve every coarse-path point."
|
||||
for ($index = 0; $index -lt $ExpectedPath.Count; $index++) {
|
||||
$expected = $ExpectedPath[$index]
|
||||
$actual = $ActualPath[$index]
|
||||
Assert-Near $expected.X $actual.X "Raw baseline $ScenarioName point $index must preserve X."
|
||||
Assert-Near $expected.Y $actual.Y "Raw baseline $ScenarioName point $index must preserve Y."
|
||||
Assert-Near $expected.Heading $actual.Heading "Raw baseline $ScenarioName point $index must preserve heading."
|
||||
Assert-Near $expected.UnwrappedHeading $actual.UnwrappedHeading "Raw baseline $ScenarioName point $index must preserve unwrapped heading."
|
||||
Assert-Near $expected.ArcLength $actual.ArcLength "Raw baseline $ScenarioName point $index must preserve arc length."
|
||||
Assert-Equal $expected.Direction $actual.Direction "Raw baseline $ScenarioName point $index must preserve travel direction."
|
||||
Assert-Near $expected.VehicleCurvature $actual.VehicleCurvature "Raw baseline $ScenarioName point $index must preserve vehicle curvature."
|
||||
Assert-Equal $expected.IsGearSwitchPoint $actual.IsGearSwitchPoint "Raw baseline $ScenarioName point $index must preserve gear-switch topology."
|
||||
}
|
||||
}
|
||||
|
||||
$coarse = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$coarseFacade = $coarse + 'Facade.'
|
||||
$coarseTest = $coarse + 'Test.'
|
||||
$smoothingTest = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Test.'
|
||||
$smoothingFacade = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade.'
|
||||
$comparison = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison.'
|
||||
|
||||
$coarseServiceType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningService')
|
||||
$coarseJobType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningJob')
|
||||
$coarseResultType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningJobResult')
|
||||
$scenarioType = Get-RequiredType ($coarseTest + 'CoarsePathTestScenario')
|
||||
$coarseFactoryType = Get-RequiredType ($coarseTest + 'CoarsePathScenarioFactory')
|
||||
$scenarioFactoryType = Get-RequiredType ($smoothingTest + 'SmoothingScenarioFactory')
|
||||
$comparisonServiceType = Get-RequiredType ($smoothingFacade + 'PathSmoothingComparisonService')
|
||||
$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest')
|
||||
|
||||
$coarseCreate = $coarseFactoryType.GetMethod('Create', [Type[]]@($scenarioType))
|
||||
$coarsePlan = $coarseServiceType.GetMethod('Plan', [Type[]]@($coarseJobType, [Threading.CancellationToken]))
|
||||
$createComparisonRequest = $scenarioFactoryType.GetMethod('CreateEndToEndRequest', [Type[]]@($coarseJobType, $coarseResultType))
|
||||
$compare = $comparisonServiceType.GetMethod('Compare', [Type[]]@($comparisonRequestType, [Threading.CancellationToken]))
|
||||
Assert-True ($null -ne $createComparisonRequest) 'Smoothing scenario factory must convert a successful coarse planning job into a comparison request.'
|
||||
|
||||
$planner = [Activator]::CreateInstance($coarseServiceType)
|
||||
$comparisonService = [Activator]::CreateInstance($comparisonServiceType)
|
||||
foreach ($scenarioName in @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'ReverseGearSwitch')) {
|
||||
$scenario = [Enum]::Parse($scenarioType, $scenarioName)
|
||||
$job = $coarseCreate.Invoke($null, @($scenario))
|
||||
$planningTimer = [Diagnostics.Stopwatch]::StartNew()
|
||||
$planned = $coarsePlan.Invoke($planner, @($job, [Threading.CancellationToken]::None))
|
||||
$planningTimer.Stop()
|
||||
Assert-Equal 'Success' $planned.PlanningResult.Status.ToString() "Coarse scenario $scenarioName must succeed before smoothing comparison."
|
||||
$request = $createComparisonRequest.Invoke($null, @($job, $planned))
|
||||
Assert-CoarsePathGeometry $planned.PlanningResult.Path $request.SmoothingRequest.CoarsePath $planned.MapResult.Map $scenarioName
|
||||
$comparisonTimer = [Diagnostics.Stopwatch]::StartNew()
|
||||
$comparisonResult = $compare.Invoke($comparisonService, @($request, [Threading.CancellationToken]::None))
|
||||
$comparisonTimer.Stop()
|
||||
Assert-True (-not $comparisonResult.IsCancelled) "Comparison $scenarioName must not be cancelled."
|
||||
Assert-Equal 'Success' $comparisonResult.RawPathBaseline.Status.ToString() "Raw baseline $scenarioName must remain a feasible, verified copy of the coarse path."
|
||||
Assert-RawBaselinePath $planned.PlanningResult.Path $comparisonResult.RawPathBaseline.Path $scenarioName
|
||||
Assert-SegmentTopology $planned.PlanningResult.Segments $comparisonResult.RawPathBaseline.Path $comparisonResult.RawPathBaseline.Segments "Raw baseline $scenarioName"
|
||||
Assert-GearSwitchGeometry $planned.PlanningResult.Path $comparisonResult.RawPathBaseline.Path "Raw baseline $scenarioName"
|
||||
foreach ($entry in $comparisonResult.Entries | Where-Object { $_.Status.ToString() -eq 'Success' }) {
|
||||
Assert-SegmentTopology $comparisonResult.RawPathBaseline.Segments $entry.Path $entry.Segments "Successful $($entry.Method) $scenarioName"
|
||||
Assert-GearSwitchGeometry $comparisonResult.RawPathBaseline.Path $entry.Path "Successful $($entry.Method) $scenarioName"
|
||||
}
|
||||
$successfulEntryCount = @($comparisonResult.Entries | Where-Object { $_.Status.ToString() -eq 'Success' }).Count
|
||||
Write-Output ("$scenarioName diagnostics: planning=$([Math]::Round($planningTimer.Elapsed.TotalSeconds, 3))s; comparison=$([Math]::Round($comparisonTimer.Elapsed.TotalSeconds, 3))s; successfulMethods=$successfulEntryCount")
|
||||
}
|
||||
|
||||
Write-Output 'Path smoothing end-to-end integration checks passed.'
|
||||
Reference in New Issue
Block a user