169 lines
9.6 KiB
PowerShell
169 lines
9.6 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-False($Actual, [string]$Message) {
|
|
if ($Actual) { throw $Message }
|
|
}
|
|
|
|
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([bool]$WithObstacle) {
|
|
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]4000, [single]0, [single]4000))
|
|
$request = [Activator]::CreateInstance($mapRequestType)
|
|
$request.Bounds = $bounds
|
|
$request.ResolutionMm = [single]50
|
|
if ($WithObstacle) {
|
|
$obstacle = [Activator]::CreateInstance($rectangleType, @([single]1900, [single]2100, [single]1800, [single]2200))
|
|
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
|
$obstacles.SetValue($obstacle, 0)
|
|
$source = [Activator]::CreateInstance($manualSourceType, @('validator-obstacle', [long]1, $true, $obstacles))
|
|
$sources = [Array]::CreateInstance($obstacleSourceType, 1)
|
|
$sources.SetValue($source, 0)
|
|
$request.ObstacleSources = $sources
|
|
} else {
|
|
$request.ObstacleSources = [Array]::CreateInstance($obstacleSourceType, 0)
|
|
$request.AllowExplicitEmptyMap = $true
|
|
}
|
|
$result = [Activator]::CreateInstance($mapFactoryType).Create($request)
|
|
Assert-True $result.Succeeded 'Validation test map must be built.'
|
|
Assert-True $result.Map.PlanningReady 'Validation test map must be ready.'
|
|
return $result.Map
|
|
}
|
|
|
|
function New-SmoothedPoint([double]$X, [double]$Y, [double]$ArcLength, $Direction,
|
|
[double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0) {
|
|
return [Activator]::CreateInstance($smoothedPointType, @(
|
|
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
|
|
$VehicleCurvature, $VehicleCurvature, $Clearance, $IsGearSwitch, $anchor))
|
|
}
|
|
|
|
function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$IsGearSwitch = $false) {
|
|
return [Activator]::CreateInstance($smoothingPointType, @(
|
|
$X, $Y, $ArcLength, [double]0.0, [double]0.0, [double]999.0, $IsGearSwitch, $anchor))
|
|
}
|
|
|
|
function New-OneSegmentCase([double]$X0, [double]$Y0, [double]$X1, [double]$Y1, [double]$VehicleCurvature = 0.0,
|
|
[double]$StartArcLength = 0.0) {
|
|
$candidatePath = [Array]::CreateInstance($smoothedPointType, 2)
|
|
$candidatePath.SetValue((New-SmoothedPoint $X0 $Y0 $StartArcLength $forward), 0)
|
|
$candidatePath.SetValue((New-SmoothedPoint $X1 $Y1 ($StartArcLength + 1.0) $forward $VehicleCurvature), 1)
|
|
$candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 1)
|
|
$candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
|
|
$preparedPoints = [Array]::CreateInstance($smoothingPointType, 2)
|
|
$preparedPoints.SetValue((New-PreparedPoint $X0 $Y0 0.0), 0)
|
|
$preparedPoints.SetValue((New-PreparedPoint $X1 $Y1 1.0), 1)
|
|
$preparedSegments = [Array]::CreateInstance($preparedSegmentType, 1)
|
|
$preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedPoints, $false, $false)), 0)
|
|
return [PSCustomObject]@{
|
|
CandidatePath = $candidatePath
|
|
CandidateSegments = $candidateSegments
|
|
Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
|
|
}
|
|
}
|
|
|
|
function Invoke-Validation($Case, $Map) {
|
|
$arguments = [object[]]@($Case.CandidatePath, $Case.CandidateSegments, $Case.Original, $Map, $vehicle,
|
|
[double]0.05, $null, [double]0.0, $null)
|
|
$accepted = $validateMethod.Invoke($validator, $arguments)
|
|
return [PSCustomObject]@{ Accepted = $accepted; Path = $arguments[6]; MinimumClearance = $arguments[7]; Reason = $arguments[8] }
|
|
}
|
|
|
|
function New-MovedGearSwitchCase {
|
|
$candidatePath = [Array]::CreateInstance($smoothedPointType, 4)
|
|
$candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 0.0 $forward), 0)
|
|
$candidatePath.SetValue((New-SmoothedPoint 1.5 0.5 1.0 $forward), 1)
|
|
$candidatePath.SetValue((New-SmoothedPoint 1.6 0.5 1.0 $reverse 0.0 $true), 2)
|
|
$candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 2.0 $reverse), 3)
|
|
$candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 2)
|
|
$candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
|
|
$candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
|
|
$preparedFirst = [Array]::CreateInstance($smoothingPointType, 2)
|
|
$preparedFirst.SetValue((New-PreparedPoint 0.5 0.5 0.0), 0)
|
|
$preparedFirst.SetValue((New-PreparedPoint 1.5 0.5 1.0), 1)
|
|
$preparedSecond = [Array]::CreateInstance($smoothingPointType, 2)
|
|
$preparedSecond.SetValue((New-PreparedPoint 1.5 0.5 0.0 $true), 0)
|
|
$preparedSecond.SetValue((New-PreparedPoint 0.5 0.5 1.0), 1)
|
|
$preparedSegments = [Array]::CreateInstance($preparedSegmentType, 2)
|
|
$preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedFirst, $false, $true)), 0)
|
|
$preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(1, $reverse, $preparedSecond, $true, $false)), 1)
|
|
return [PSCustomObject]@{
|
|
CandidatePath = $candidatePath
|
|
CandidateSegments = $candidateSegments
|
|
Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
|
|
}
|
|
}
|
|
|
|
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
|
$processing = $root + 'Processing.'
|
|
$validation = $root + 'Validation.'
|
|
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
|
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
|
|
|
$validatorType = Get-RequiredType ($validation + 'SmoothedPathValidator')
|
|
$smoothedPointType = Get-RequiredType ($root + 'SmoothedPathPoint')
|
|
$smoothedSegmentType = Get-RequiredType ($root + 'SmoothedPathSegment')
|
|
$smoothingPointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
|
$preparedSegmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
|
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
|
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
|
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
|
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
|
$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
|
|
$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle')
|
|
$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle')
|
|
$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource')
|
|
$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource')
|
|
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
|
|
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')
|
|
|
|
$validator = [Activator]::CreateInstance($validatorType)
|
|
$validateMethod = $validatorType.GetMethod('TryValidate')
|
|
Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
|
|
Assert-True ($validateMethod.GetParameters().Length -eq 9) 'TryValidate must accept path, segments, originals, map, vehicle, step, path, clearance, and reason.'
|
|
$forward = [Enum]::Parse($directionType, 'Forward')
|
|
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
|
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
|
$vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$vehicle.LengthMeters = 0.20
|
|
$vehicle.WidthMeters = 0.20
|
|
$vehicle.SafetyMarginMeters = 0.0
|
|
$vehicle.MaximumCurvaturePerMeter = 1.0
|
|
$vehicle.MinimumTurningRadiusMeters = 1.0
|
|
|
|
$emptyMap = New-Map $false
|
|
$obstacleMap = New-Map $true
|
|
$valid = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5) $obstacleMap
|
|
Assert-True $valid.Accepted ('A valid straight candidate must pass. Reason=' + $valid.Reason)
|
|
Assert-True ($null -ne $valid.Path) 'A valid candidate must return clearance-recomputed points.'
|
|
Assert-True ($valid.Path[0].BodyClearance -lt 999.0) 'Validated output must replace an overclaimed candidate clearance.'
|
|
Assert-True ($valid.MinimumClearance -ge 0.0) 'A valid candidate must report non-negative conservative clearance.'
|
|
|
|
$pointCollision = Invoke-Validation (New-OneSegmentCase 2.0 2.0 2.5 2.0) $obstacleMap
|
|
Assert-False $pointCollision.Accepted 'A smoothing candidate that touches an obstacle must be rejected.'
|
|
$sweptCollision = Invoke-Validation (New-OneSegmentCase 1.5 2.0 2.5 2.0) $obstacleMap
|
|
Assert-False $sweptCollision.Accepted 'A smoothing candidate whose sweep cuts through an obstacle must be rejected.'
|
|
$outsideBounds = Invoke-Validation (New-OneSegmentCase 0.05 0.5 0.5 0.5) $emptyMap
|
|
Assert-False $outsideBounds.Accepted 'A smoothing candidate outside map bounds must be rejected.'
|
|
$overCurvature = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 2.0) $emptyMap
|
|
Assert-False $overCurvature.Accepted 'A smoothing candidate above vehicle maximum curvature must be rejected.'
|
|
$nonzeroStartArc = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 0.0 1.0) $emptyMap
|
|
Assert-False $nonzeroStartArc.Accepted 'A smoothing candidate must start at global arc length zero.'
|
|
$movedSwitch = Invoke-Validation (New-MovedGearSwitchCase) $emptyMap
|
|
Assert-False $movedSwitch.Accepted 'A smoothing candidate with a moved gear-switch pose must be rejected.'
|
|
|
|
Write-Output 'Path smoothing validation checks passed.'
|