chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,411 @@
|
||||
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-Null($Actual, [string]$Message) {
|
||||
if ($null -ne $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||
try {
|
||||
& $Action
|
||||
}
|
||||
catch {
|
||||
return
|
||||
}
|
||||
|
||||
throw $Message
|
||||
}
|
||||
|
||||
function Find-Method($Type, [string]$Name, [Type[]]$ParameterTypes) {
|
||||
foreach ($candidate in $Type.GetMethods()) {
|
||||
if ($candidate.Name -ne $Name) { continue }
|
||||
$parameters = $candidate.GetParameters()
|
||||
if ($parameters.Length -ne $ParameterTypes.Length) { continue }
|
||||
$matches = $true
|
||||
for ($index = 0; $index -lt $parameters.Length; $index++) {
|
||||
if ($parameters[$index].ParameterType -ne $ParameterTypes[$index]) {
|
||||
$matches = $false
|
||||
break
|
||||
}
|
||||
}
|
||||
if ($matches) { return $candidate }
|
||||
}
|
||||
return $null
|
||||
}
|
||||
|
||||
function Find-NonPublicStaticMethod($Type, [string]$Name, [Type[]]$ParameterTypes) {
|
||||
foreach ($candidate in $Type.GetMethods([Reflection.BindingFlags]'Static,NonPublic')) {
|
||||
if ($candidate.Name -ne $Name) { continue }
|
||||
$parameters = $candidate.GetParameters()
|
||||
if ($parameters.Length -ne $ParameterTypes.Length) { continue }
|
||||
$matches = $true
|
||||
for ($index = 0; $index -lt $parameters.Length; $index++) {
|
||||
if ($parameters[$index].ParameterType -ne $ParameterTypes[$index]) { $matches = $false; break }
|
||||
}
|
||||
if ($matches) { return $candidate }
|
||||
}
|
||||
return $null
|
||||
}
|
||||
|
||||
function New-TestMap {
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
||||
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Bounds = $bounds
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
$result = [Activator]::CreateInstance($factoryType).Create($request)
|
||||
Assert-True $result.Succeeded 'Test map must be created.'
|
||||
Assert-True $result.Map.PlanningReady 'Test map must be ready.'
|
||||
return $result.Map
|
||||
}
|
||||
|
||||
function New-CornerBlockedMap {
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
||||
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
||||
$obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
|
||||
$circleType = $assembly.GetType($mapping + 'CircleObstacle', $true)
|
||||
$manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
|
||||
$sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]150, [single]0, [single]150))
|
||||
$obstacles = [Array]::CreateInstance($obstacleType, 2)
|
||||
$obstacles.SetValue([Activator]::CreateInstance($circleType, @([single]75, [single]25, [single]0)), 0)
|
||||
$obstacles.SetValue([Activator]::CreateInstance($circleType, @([single]25, [single]75, [single]0)), 1)
|
||||
$sources = [Array]::CreateInstance($sourceType, 1)
|
||||
$sources.SetValue([Activator]::CreateInstance($manualType, @('corner-blocks', [long]1, $true, $obstacles)), 0)
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Bounds = $bounds
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.ObstacleSources = $sources
|
||||
$request.AllowExplicitEmptyMap = $false
|
||||
$result = [Activator]::CreateInstance($factoryType).Create($request)
|
||||
Assert-True $result.Succeeded 'Corner-blocked map must be created.'
|
||||
Assert-True $result.Map.PlanningReady 'Corner-blocked map must be ready.'
|
||||
return $result.Map
|
||||
}
|
||||
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$search = $coarsePath + 'Search.'
|
||||
$primitiveType = $assembly.GetType($search + 'MotionPrimitive', $true)
|
||||
$generatorType = $assembly.GetType($search + 'MotionPrimitiveGenerator', $true)
|
||||
$goalCheckerType = $assembly.GetType($search + 'GoalToleranceChecker', $true)
|
||||
|
||||
Assert-True ($primitiveType.GetProperty('ActualLengthMeters') -ne $null) 'MotionPrimitive must expose actual length.'
|
||||
Assert-True ($primitiveType.GetProperty('Points') -ne $null) 'MotionPrimitive must expose integration points.'
|
||||
Assert-True ($primitiveType.GetProperty('IsGoalTruncation') -ne $null) 'MotionPrimitive must expose goal truncation state.'
|
||||
|
||||
$poseType = $assembly.GetType($coarsePath + 'Pose2D', $true)
|
||||
$requestType = $assembly.GetType($coarsePath + 'PlanningRequest', $true)
|
||||
$configurationType = $assembly.GetType($coarsePath + 'HybridAStarConfiguration', $true)
|
||||
$vehicleType = $assembly.GetType($coarsePath + 'VehicleParameters', $true)
|
||||
$directionType = $assembly.GetType($coarsePath + 'TravelDirection', $true)
|
||||
$goalDirectionType = $assembly.GetType($coarsePath + 'GoalDirectionConstraint', $true)
|
||||
$generate = Find-Method $generatorType 'Generate' @($poseType, [double], $directionType, $requestType)
|
||||
Assert-True ($generate -ne $null) 'MotionPrimitiveGenerator must expose Generate(start, curvature, direction, request).'
|
||||
$isSatisfied = Find-Method $goalCheckerType 'IsSatisfied' @($poseType, $poseType, $configurationType, $directionType, $goalDirectionType)
|
||||
Assert-True ($isSatisfied -ne $null) 'GoalToleranceChecker must expose the planned goal check.'
|
||||
|
||||
$map = New-TestMap
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = 0.20
|
||||
$vehicle.WidthMeters = 0.20
|
||||
$vehicle.SafetyMarginMeters = 0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = 1.0
|
||||
$config = [Activator]::CreateInstance($configurationType)
|
||||
$config.PrimitiveLengthMeters = 0.50
|
||||
$config.IntegrationStepMeters = 0.05
|
||||
$config.MaximumCollisionCheckStepMeters = 0.025
|
||||
$config.GoalPositionToleranceMeters = 0.001
|
||||
$config.GoalHeadingToleranceRadians = 0.001
|
||||
$config.CurvatureLevelCount = 5
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anyDirection = [Enum]::Parse($goalDirectionType, 'Any')
|
||||
$forwardOnly = [Enum]::Parse($goalDirectionType, 'Forward')
|
||||
$start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
|
||||
$generator = [Activator]::CreateInstance($generatorType)
|
||||
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Map = $map
|
||||
$request.Vehicle = $vehicle
|
||||
$request.Configuration = $config
|
||||
$request.Goal = [Activator]::CreateInstance($poseType, @(3.0, 1.0, 0.0))
|
||||
$request.GoalDirection = $anyDirection
|
||||
$straight = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
|
||||
Assert-True ($straight -ne $null) 'Straight primitive must be generated.'
|
||||
Assert-Near 0.50 $straight.ActualLengthMeters 'Straight primitive must use the configured cap.'
|
||||
Assert-Near 1.50 $straight.End.X 'Straight primitive must use analytic integration.'
|
||||
Assert-Near 1.00 $straight.End.Y 'Straight primitive must not drift laterally.'
|
||||
Assert-Near 0.00 $straight.End.Heading 'Straight primitive must keep heading.'
|
||||
Assert-True ($straight.Points.Count -ge 20) 'Point spacing must honor the collision check step.'
|
||||
$previous = $start
|
||||
foreach ($point in $straight.Points) {
|
||||
$distance = [Math]::Sqrt(($point.X - $previous.X) * ($point.X - $previous.X) + ($point.Y - $previous.Y) * ($point.Y - $previous.Y))
|
||||
Assert-True ($distance -le 0.025001) 'Point spacing must not exceed the map-safe collision step.'
|
||||
$previous = $point
|
||||
}
|
||||
|
||||
$curve = $generate.Invoke($generator, @($start, 1.0, $forward, $request))
|
||||
Assert-True ($curve -ne $null) 'Arc primitive must be generated.'
|
||||
Assert-Near ([Math]::Sin(0.50) + 1.0) $curve.End.X 'Arc X must use analytic integration.'
|
||||
Assert-Near (1.0 - [Math]::Cos(0.50) + 1.0) $curve.End.Y 'Arc Y must use analytic integration.'
|
||||
Assert-Near 0.50 $curve.End.Heading 'Arc heading must use signed distance.'
|
||||
|
||||
$reversePrimitive = $generate.Invoke($generator, @($start, 0.0, $reverse, $request))
|
||||
Assert-True ($reversePrimitive -ne $null) 'Reverse primitive must be generated.'
|
||||
Assert-Near 0.50 $reversePrimitive.End.X 'Reverse straight primitive must move behind the vehicle.'
|
||||
|
||||
$request.Goal = [Activator]::CreateInstance($poseType, @(1.30, 1.0, 0.0))
|
||||
$truncated = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
|
||||
Assert-True ($truncated -ne $null) 'Goal-truncated primitive must be generated.'
|
||||
Assert-Near 0.30 $truncated.ActualLengthMeters 'A 0.30m goal must truncate at its first internal point.'
|
||||
Assert-True $truncated.IsGoalTruncation 'Truncated primitive must record its goal source.'
|
||||
Assert-Near 1.30 $truncated.End.X 'Truncated primitive must end at the goal point.'
|
||||
|
||||
$request.Goal = $start
|
||||
$zeroLength = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
|
||||
Assert-True ($zeroLength -ne $null) 'Start-at-goal must produce a zero-length candidate.'
|
||||
Assert-Near 0.0 $zeroLength.ActualLengthMeters 'Start-at-goal candidate must have zero length.'
|
||||
Assert-True $zeroLength.IsGoalTruncation 'Start-at-goal candidate must have goal truncation source.'
|
||||
|
||||
$wrapPose = [Activator]::CreateInstance($poseType, @(1.0, 1.0, (-[Math]::PI + 0.0005)))
|
||||
$wrapGoal = [Activator]::CreateInstance($poseType, @(1.0, 1.0, ([Math]::PI - 0.0005)))
|
||||
$config.GoalHeadingToleranceRadians = 0.002
|
||||
Assert-True $isSatisfied.Invoke($null, @($wrapPose, $wrapGoal, $config, $forward, $forwardOnly)) 'Goal heading tolerance must wrap across pi.'
|
||||
Assert-False $isSatisfied.Invoke($null, @($wrapPose, $wrapGoal, $config, $reverse, $forwardOnly)) 'Goal direction constraint must reject reverse entry.'
|
||||
|
||||
$levelsMethod = Find-Method $generatorType 'GetCurvatureLevels' @($vehicleType, $configurationType)
|
||||
Assert-True ($levelsMethod -ne $null) 'MotionPrimitiveGenerator must expose curvature levels.'
|
||||
$levels = $levelsMethod.Invoke($generator, @($vehicle, $config))
|
||||
Assert-True ($levels.Count -eq 5) 'Five configured curvature levels must be generated.'
|
||||
Assert-Near -1.0 $levels[0] 'First curvature level must be negative maximum.'
|
||||
Assert-Near 0.0 $levels[2] 'Middle curvature level must be straight.'
|
||||
Assert-Near 1.0 $levels[4] 'Last curvature level must be positive maximum.'
|
||||
$adjacent = Find-Method $generatorType 'AreCurvatureLevelsAdjacent' @([int], [int])
|
||||
Assert-True ($adjacent -ne $null) 'MotionPrimitiveGenerator must expose curvature adjacency.'
|
||||
Assert-True $adjacent.Invoke($null, @(2, 3)) 'Neighboring curvature levels must be allowed.'
|
||||
Assert-False $adjacent.Invoke($null, @(2, 4)) 'Non-neighboring curvature levels must be rejected.'
|
||||
|
||||
$heapGenericType = $assembly.GetType($search + 'BinaryMinHeap`1', $true)
|
||||
$heapType = $heapGenericType.MakeGenericType([string])
|
||||
$heap = [Activator]::CreateInstance($heapType)
|
||||
$push = Find-Method $heapType 'Push' @([string], [double], [double], [double])
|
||||
$pop = Find-Method $heapType 'Pop' @()
|
||||
Assert-True ($push -ne $null) 'BinaryMinHeap must expose Push(item, f, h, g).'
|
||||
Assert-True ($pop -ne $null) 'BinaryMinHeap must expose Pop().'
|
||||
$push.Invoke($heap, @('node-a', 8.0, 2.0, 1.0))
|
||||
$push.Invoke($heap, @('node-b', 8.0, 1.0, 1.0))
|
||||
$push.Invoke($heap, @('node-c', 8.0, 1.0, 3.0))
|
||||
$push.Invoke($heap, @('node-d', 7.0, 9.0, 0.0))
|
||||
$push.Invoke($heap, @('node-e', 8.0, 1.0, 3.0))
|
||||
Assert-Equal 'node-d' $pop.Invoke($heap, @()) 'Lower F must win.'
|
||||
Assert-Equal 'node-c' $pop.Invoke($heap, @()) 'Equal F and H must prefer larger G.'
|
||||
Assert-Equal 'node-e' $pop.Invoke($heap, @()) 'Equal F, H and G must preserve insertion order.'
|
||||
Assert-Equal 'node-b' $pop.Invoke($heap, @()) 'Equal F must prefer lower H.'
|
||||
Assert-Equal 'node-a' $pop.Invoke($heap, @()) 'Remaining item must be returned last.'
|
||||
$firstDeterministicHeap = [Activator]::CreateInstance($heapType)
|
||||
$secondDeterministicHeap = [Activator]::CreateInstance($heapType)
|
||||
foreach ($deterministicHeap in @($firstDeterministicHeap, $secondDeterministicHeap)) {
|
||||
$push.Invoke($deterministicHeap, @('repeat-a', 5.0, 2.0, 1.0))
|
||||
$push.Invoke($deterministicHeap, @('repeat-b', 5.0, 1.0, 1.0))
|
||||
$push.Invoke($deterministicHeap, @('repeat-c', 5.0, 1.0, 3.0))
|
||||
}
|
||||
$firstDeterministicOrder = @($pop.Invoke($firstDeterministicHeap, @()), $pop.Invoke($firstDeterministicHeap, @()), $pop.Invoke($firstDeterministicHeap, @())) -join ','
|
||||
$secondDeterministicOrder = @($pop.Invoke($secondDeterministicHeap, @()), $pop.Invoke($secondDeterministicHeap, @()), $pop.Invoke($secondDeterministicHeap, @())) -join ','
|
||||
Assert-Equal $firstDeterministicOrder $secondDeterministicOrder 'Equivalent heap inputs must have deterministic pop order.'
|
||||
|
||||
$calculatorType = $assembly.GetType($search + 'SearchCostCalculator', $true)
|
||||
$calculator = [Activator]::CreateInstance($calculatorType)
|
||||
$calculate = Find-Method $calculatorType 'Calculate' @([double], $directionType, [bool], [double], [double], [int], [double], $configurationType)
|
||||
Assert-True ($calculate -ne $null) 'SearchCostCalculator must expose the planned equivalent-meter calculation.'
|
||||
$costConfig = [Activator]::CreateInstance($configurationType)
|
||||
$cost = $calculate.Invoke($calculator, @(2.0, $reverse, $true, 0.5, 1.0, 2, 0.25, $costConfig))
|
||||
Assert-Near 4.55 $cost 'Reverse, gear, curvature and clearance costs must be accumulated.'
|
||||
$negativeWeightConfig = [Activator]::CreateInstance($configurationType)
|
||||
$negativeWeightConfig.CurvatureMagnitudeWeight = -0.01
|
||||
Assert-Throws { $calculate.Invoke($calculator, @(1.0, $forward, $false, 0.0, 1.0, 0, 1.0, $negativeWeightConfig)) } 'Negative cost weights must be rejected.'
|
||||
$nonFiniteWeightConfig = [Activator]::CreateInstance($configurationType)
|
||||
$nonFiniteWeightConfig.ClearanceCostWeight = [double]::NaN
|
||||
Assert-Throws { $calculate.Invoke($calculator, @(1.0, $forward, $false, 0.0, 1.0, 0, 1.0, $nonFiniteWeightConfig)) } 'Non-finite cost weights must be rejected.'
|
||||
|
||||
$dijkstraType = $assembly.GetType($search + 'GridDijkstraHeuristic', $true)
|
||||
$dijkstraConstructor = $dijkstraType.GetConstructor(@($map.GetType(), [int], [int]))
|
||||
Assert-True ($dijkstraConstructor -ne $null) 'GridDijkstraHeuristic must expose the planned grid constructor.'
|
||||
$getCost = Find-Method $dijkstraType 'GetCost' @([int], [int])
|
||||
Assert-True ($getCost -ne $null) 'GridDijkstraHeuristic must expose GetCost(row, col).'
|
||||
$openHeuristic = $dijkstraConstructor.Invoke(@($map, 2, 2))
|
||||
$expectedDiagonalCost = 2.0 * [Math]::Sqrt(2.0) * $map.ResolutionMeters
|
||||
Assert-Near $expectedDiagonalCost $getCost.Invoke($openHeuristic, @(0, 0)) 'Open-grid diagonal movement must cost sqrt(2) per cell.'
|
||||
$cornerBlockedMap = New-CornerBlockedMap
|
||||
$blockedHeuristic = $dijkstraConstructor.Invoke(@($cornerBlockedMap, 1, 1))
|
||||
Assert-True ([double]::IsPositiveInfinity($getCost.Invoke($blockedHeuristic, @(0, 0)))) 'Diagonal corner cutting through two occupied orthogonal cells must be forbidden.'
|
||||
|
||||
$operationBudgetType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationBudget', $true)
|
||||
$operationStopReasonType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationStopReason', $true)
|
||||
$budgetConstructor = $operationBudgetType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@([Threading.CancellationToken], [TimeSpan]), $null)
|
||||
Assert-True ($budgetConstructor -ne $null) 'PlanningOperationBudget must expose its internal cancellation and timeout constructor.'
|
||||
$tryCreateDijkstra = Find-NonPublicStaticMethod $dijkstraType 'TryCreate' @($map.GetType(), [int], [int], $operationBudgetType,
|
||||
$dijkstraType.MakeByRefType(), $operationStopReasonType.MakeByRefType())
|
||||
Assert-True ($tryCreateDijkstra -ne $null) 'GridDijkstraHeuristic must expose an internal budget-aware TryCreate method.'
|
||||
$expiredBudget = $budgetConstructor.Invoke(@([Threading.CancellationToken]::None, [TimeSpan]::Zero))
|
||||
$dijkstraArguments = [object[]]@($map, 2, 2, $expiredBudget, $null, $null)
|
||||
$dijkstraCreated = $tryCreateDijkstra.Invoke($null, $dijkstraArguments)
|
||||
Assert-False $dijkstraCreated 'An expired total budget must stop Dijkstra construction.'
|
||||
Assert-Null $dijkstraArguments[4] 'Stopped Dijkstra construction must not publish a partial heuristic.'
|
||||
Assert-Equal 'TimedOut' $dijkstraArguments[5].ToString() 'Expired Dijkstra construction must report timeout.'
|
||||
|
||||
$largeBounds = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true),
|
||||
@([single]0, [single]40000, [single]0, [single]20000))
|
||||
$largeMapRequest = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest', $true))
|
||||
$largeMapRequest.Bounds = $largeBounds
|
||||
$largeMapRequest.ResolutionMm = [single]20
|
||||
$largeMapRequest.AllowExplicitEmptyMap = $true
|
||||
$largeMap = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory', $true)).Create($largeMapRequest)
|
||||
Assert-True $largeMap.Succeeded 'Large empty map for Dijkstra cancellation must be created before cancellation starts.'
|
||||
$midDijkstraCancellation = New-Object Threading.CancellationTokenSource
|
||||
$midDijkstraBudget = $budgetConstructor.Invoke(@($midDijkstraCancellation.Token, [TimeSpan]::FromSeconds(2)))
|
||||
$midDijkstraCancellation.CancelAfter(1)
|
||||
$midDijkstraTimer = [Diagnostics.Stopwatch]::StartNew()
|
||||
$midDijkstraArguments = [object[]]@($largeMap.Map, 500, 1000, $midDijkstraBudget, $null, $null)
|
||||
$midDijkstraCreated = $tryCreateDijkstra.Invoke($null, $midDijkstraArguments)
|
||||
$midDijkstraTimer.Stop()
|
||||
Assert-False $midDijkstraCreated 'Cancellation during Dijkstra initialization must stop construction.'
|
||||
Assert-Null $midDijkstraArguments[4] 'Mid-Dijkstra cancellation must not publish a partial heuristic.'
|
||||
Assert-Equal 'Cancelled' $midDijkstraArguments[5].ToString() 'Mid-Dijkstra cancellation must retain cancellation status.'
|
||||
Assert-True ($midDijkstraTimer.Elapsed -lt [TimeSpan]::FromSeconds(2)) 'Dijkstra cancellation must return within the configured response bound.'
|
||||
|
||||
Write-Output 'Coarse path search primitive checks passed.'
|
||||
|
||||
$nodeType = $assembly.GetType($search + 'HybridAStarNode', $true)
|
||||
$nodeKeyType = $assembly.GetType($search + 'HybridAStarNodeKey', $true)
|
||||
$searchType = $assembly.GetType($search + 'HybridAStarSearch', $true)
|
||||
$searchResultType = $assembly.GetType($search + 'HybridAStarSearchResult', $true)
|
||||
$searchMethod = Find-Method $searchType 'Search' @($requestType, [Threading.CancellationToken])
|
||||
Assert-True ($searchMethod -ne $null) 'HybridAStarSearch must expose Search(request, cancellationToken).'
|
||||
Assert-True ($searchResultType.GetProperty('Status') -ne $null) 'Search result must expose planning status.'
|
||||
Assert-True ($searchResultType.GetProperty('TerminationReason') -ne $null) 'Search result must expose its original termination reason.'
|
||||
Assert-True ($searchResultType.GetProperty('SuccessNodeIndex') -ne $null) 'Search result must expose the successful node index.'
|
||||
Assert-True ($searchResultType.GetProperty('ReopenedNodeCount') -ne $null) 'Search result must expose reopened node count.'
|
||||
Assert-True ($searchResultType.GetProperty('StaleOpenListEntryCount') -ne $null) 'Search result must expose stale Open List entry count.'
|
||||
Assert-True ($searchResultType.GetProperty('PeakOpenListCount') -ne $null) 'Search result must expose Open List peak count.'
|
||||
Assert-True ($nodeType.GetProperty('ParentNodeIndex') -ne $null) 'Search node must expose its parent node index.'
|
||||
Assert-True ($nodeKeyType.GetProperty('HeadingIndex') -ne $null) 'Search node key must contain a heading index.'
|
||||
Assert-True ($nodeKeyType.GetProperty('Direction') -ne $null) 'Search node key must contain a travel direction.'
|
||||
Assert-True ($nodeKeyType.GetProperty('CurvatureLevelIndex') -ne $null) 'Search node key must contain a curvature level index.'
|
||||
|
||||
$goalCandidateProperty = $nodeType.GetProperty('IsGoalCandidate')
|
||||
Assert-True ($goalCandidateProperty -ne $null) 'Search node must mark a goal-truncated candidate explicitly.'
|
||||
$nodeConstructor = $nodeType.GetConstructor(@([int], $nodeKeyType, $poseType, [int], $primitiveType, [double], [double], [double], [bool]))
|
||||
Assert-True ($nodeConstructor -ne $null) 'Search node must accept the explicit goal-candidate marker.'
|
||||
$admissionMethod = $searchType.GetMethod('ShouldEnqueueSuccessor', [Reflection.BindingFlags]'NonPublic,Static')
|
||||
Assert-True ($admissionMethod -ne $null) 'Search must expose its private successor admission policy for reflection regression coverage.'
|
||||
|
||||
$candidateStart = [Activator]::CreateInstance($poseType, @(1.000, 1.000, 0.0))
|
||||
$candidateGoal = [Activator]::CreateInstance($poseType, @(1.024, 1.000, 0.0))
|
||||
$candidatePoints = [Array]::CreateInstance($poseType, 1)
|
||||
$candidatePoints.SetValue($candidateGoal, 0)
|
||||
$candidatePrimitive = [Activator]::CreateInstance($primitiveType, @($candidateStart, $forward, 0.0, 0.024, $candidatePoints, [double[]]@(1.0), $true))
|
||||
$sharedKey = [Activator]::CreateInstance($nodeKeyType, @(20, 20, 0, $forward, 2))
|
||||
$normalNode = $nodeConstructor.Invoke(@(10, $sharedKey, $candidateStart, 0, $null, 0.0, 0.250, 0.0, $false))
|
||||
$goalCandidateNode = $nodeConstructor.Invoke(@(11, $sharedKey, $candidateGoal, 10, $candidatePrimitive, 0.0, 0.300, 0.0, $true))
|
||||
$dominanceConfiguration = [Activator]::CreateInstance($configurationType)
|
||||
$dominanceConfiguration.GoalPositionToleranceMeters = 0.001
|
||||
$dominanceConfiguration.GoalHeadingToleranceRadians = 0.001
|
||||
Assert-True $candidatePrimitive.IsGoalTruncation 'Regression setup must use a goal-truncated motion primitive.'
|
||||
Assert-True $normalNode.Key.Equals($goalCandidateNode.Key) 'Regression setup must share one discrete state key.'
|
||||
Assert-False $isSatisfied.Invoke($null, @($normalNode.Pose, $candidateGoal, $dominanceConfiguration, $forward, $forwardOnly)) 'Lower-G normal node must remain outside the continuous goal tolerance.'
|
||||
Assert-True $isSatisfied.Invoke($null, @($goalCandidateNode.Pose, $candidateGoal, $dominanceConfiguration, $forward, $forwardOnly)) 'Goal candidate must satisfy the continuous goal tolerance.'
|
||||
Assert-False $admissionMethod.Invoke($null, @($normalNode, 0.250)) 'Equal-cost normal state must be dominated by the best same-key label.'
|
||||
Assert-True $admissionMethod.Invoke($null, @($goalCandidateNode, 0.250)) 'Goal candidate must enter Open List even when a lower-G normal state has the same key.'
|
||||
|
||||
function New-SearchRequest([object]$Goal, [object]$GoalDirection, [bool]$AllowReverse) {
|
||||
$searchRequest = [Activator]::CreateInstance($requestType)
|
||||
$searchRequest.Map = $map
|
||||
$searchRequest.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
|
||||
$searchRequest.Goal = $Goal
|
||||
$searchRequest.Vehicle = $vehicle
|
||||
$searchConfiguration = [Activator]::CreateInstance($configurationType)
|
||||
$searchConfiguration.PrimitiveLengthMeters = 0.50
|
||||
$searchConfiguration.IntegrationStepMeters = 0.05
|
||||
$searchConfiguration.MaximumCollisionCheckStepMeters = 0.025
|
||||
$searchConfiguration.GoalPositionToleranceMeters = 0.001
|
||||
$searchConfiguration.GoalHeadingToleranceRadians = 0.001
|
||||
$searchConfiguration.CurvatureLevelCount = 5
|
||||
$searchConfiguration.MaximumExpandedNodes = 10000
|
||||
$searchConfiguration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
|
||||
$searchConfiguration.AllowReverse = $AllowReverse
|
||||
$searchRequest.Configuration = $searchConfiguration
|
||||
$searchRequest.GoalDirection = $GoalDirection
|
||||
return $searchRequest
|
||||
}
|
||||
|
||||
$searcher = [Activator]::CreateInstance($searchType)
|
||||
$searchGoal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
|
||||
$searchRequest = New-SearchRequest $searchGoal $forwardOnly $false
|
||||
$searchResult = $searchMethod.Invoke($searcher, @($searchRequest, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'Success' $searchResult.Status.ToString() 'Empty map forward search must succeed.'
|
||||
Assert-True ($null -ne $searchResult.SuccessNodeIndex) 'Success must be reported only with a popped goal candidate node.'
|
||||
Assert-True ($searchResult.ExpandedNodeCount -gt 0) 'A successful search must expand the selected goal candidate.'
|
||||
|
||||
$atGoal = New-SearchRequest $searchRequest.Start $forwardOnly $false
|
||||
$atGoalResult = $searchMethod.Invoke($searcher, @($atGoal, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'Success' $atGoalResult.Status.ToString() 'A zero-length goal candidate must enter and leave the open list successfully.'
|
||||
Assert-True ($null -ne $atGoalResult.SuccessNodeIndex) 'A zero-length goal candidate must have a node index.'
|
||||
|
||||
$cancelledRequest = New-SearchRequest $searchGoal $forwardOnly $false
|
||||
$cancellationSource = New-Object Threading.CancellationTokenSource
|
||||
$cancellationSource.Cancel()
|
||||
$cancelledResult = $searchMethod.Invoke($searcher, @($cancelledRequest, $cancellationSource.Token))
|
||||
Assert-Equal 'Cancelled' $cancelledResult.Status.ToString() 'Cancellation must be observed before node expansion.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($cancelledResult.TerminationReason)) 'Cancelled search must retain a non-empty reason.'
|
||||
|
||||
$limitedRequest = New-SearchRequest $searchGoal $forwardOnly $false
|
||||
$limitedRequest.Configuration.MaximumExpandedNodes = 0
|
||||
$limitedResult = $searchMethod.Invoke($searcher, @($limitedRequest, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'SearchNodeLimitExceeded' $limitedResult.Status.ToString() 'Node limit must be checked before expansion.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($limitedResult.TerminationReason)) 'Node-limited search must retain a non-empty reason.'
|
||||
|
||||
$timedOutRequest = New-SearchRequest $searchGoal $forwardOnly $false
|
||||
$timedOutRequest.Configuration.SearchTimeout = [TimeSpan]::Zero
|
||||
$timedOutResult = $searchMethod.Invoke($searcher, @($timedOutRequest, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'SearchTimeout' $timedOutResult.Status.ToString() 'Timeout must be checked before expansion.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($timedOutResult.TerminationReason)) 'Timed-out search must retain a non-empty reason.'
|
||||
Assert-False ($cancelledResult.TerminationReason -eq $limitedResult.TerminationReason) 'Cancelled and node-limited searches must retain different reasons.'
|
||||
Assert-False ($limitedResult.TerminationReason -eq $timedOutResult.TerminationReason) 'Node-limited and timed-out searches must retain different reasons.'
|
||||
|
||||
$internalSearchMethod = $searchType.GetMethods([Reflection.BindingFlags]'Instance,NonPublic') |
|
||||
Where-Object {
|
||||
$_.Name -eq 'Search' -and
|
||||
$_.GetParameters().Length -eq 2 -and
|
||||
$_.GetParameters()[1].ParameterType -eq $operationBudgetType
|
||||
} |
|
||||
Select-Object -First 1
|
||||
Assert-True ($internalSearchMethod -ne $null) 'Search must retain its internal shared-budget overload.'
|
||||
$internalErrorResult = $internalSearchMethod.Invoke($searcher, @($searchRequest, $null))
|
||||
Assert-Equal 'InternalError' $internalErrorResult.Status.ToString() 'A missing internal budget must be mapped to InternalError.'
|
||||
Assert-True ($internalErrorResult.TerminationReason.Contains('ArgumentNullException')) 'Internal search errors must retain the exception type.'
|
||||
|
||||
Write-Output 'Coarse path Hybrid A star search checks passed.'
|
||||
Reference in New Issue
Block a user