131 lines
7.3 KiB
PowerShell
131 lines
7.3 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, [string]$Message) {
|
|
if ([Math]::Abs($Expected - $Actual) -gt 0.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
|
|
}
|
|
|
|
function New-TestMap {
|
|
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
|
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
|
$obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
|
|
$rectangleType = $assembly.GetType($mapping + 'AxisAlignedRectangleObstacle', $true)
|
|
$sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
|
|
$manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
|
|
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
|
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
|
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
|
|
$obstacle = [Activator]::CreateInstance($rectangleType, @([single]1500, [single]1550, [single]1200, [single]1800))
|
|
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
|
$obstacles.SetValue($obstacle, 0)
|
|
$source = [Activator]::CreateInstance($manualType, @('manual', [long]1, $true, $obstacles))
|
|
$sources = [Array]::CreateInstance($sourceType, 1)
|
|
$sources.SetValue($source, 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 'Test map must be created.'
|
|
Assert-True $result.Map.PlanningReady 'Test map must be ready.'
|
|
return $result.Map
|
|
}
|
|
|
|
function New-DiagonalCellMap {
|
|
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
|
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
|
$obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
|
|
$circleType = $assembly.GetType($mapping + 'CircleObstacle', $true)
|
|
$sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
|
|
$manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
|
|
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
|
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
|
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
|
|
$obstacle = [Activator]::CreateInstance($circleType, @([single]1525, [single]1525, [single]0))
|
|
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
|
$obstacles.SetValue($obstacle, 0)
|
|
$source = [Activator]::CreateInstance($manualType, @('diagonal-cell', [long]1, $true, $obstacles))
|
|
$sources = [Array]::CreateInstance($sourceType, 1)
|
|
$sources.SetValue($source, 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 'Diagonal cell map must be created.'
|
|
Assert-True $result.Map.PlanningReady 'Diagonal cell map must be ready.'
|
|
Assert-True $result.Map.IsOccupied(30, 30) 'Diagonal cell obstacle must occupy its 50mm cell.'
|
|
return $result.Map
|
|
}
|
|
|
|
$map = New-TestMap
|
|
$vehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
|
|
$vehicle.LengthMeters = 0.20
|
|
$vehicle.WidthMeters = 0.20
|
|
$vehicle.SafetyMarginMeters = 0.0
|
|
$checker = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle.FootprintCollisionChecker
|
|
|
|
$diagonalCellMap = New-DiagonalCellMap
|
|
$diagonalCellVehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
|
|
$diagonalCellVehicle.LengthMeters = 0.10
|
|
$diagonalCellVehicle.WidthMeters = 0.10
|
|
$diagonalCellVehicle.SafetyMarginMeters = 0.0
|
|
$diagonalCellPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.299, 1.299, 0.0)
|
|
$diagonalCellClearance = 0.0
|
|
$diagonalCellSafe = $checker.IsPoseCollisionFree($diagonalCellPose, $diagonalCellMap, $diagonalCellVehicle, 0.20, [ref]$diagonalCellClearance)
|
|
Assert-False $diagonalCellSafe 'Expanded diagonal footprint must not bypass an occupied 50mm cell.'
|
|
|
|
$edgePose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.35, 1.50, 0.0)
|
|
$edgeClearance = 0.0
|
|
$edgeSafe = $checker.IsPoseCollisionFree($edgePose, $map, $vehicle, 0.0, [ref]$edgeClearance)
|
|
Assert-False $edgeSafe 'Touching an occupied cell must be a collision.'
|
|
|
|
$farPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(0.50, 0.50, 0.0)
|
|
$farClearance = 0.0
|
|
$farSafe = $checker.IsPoseCollisionFree($farPose, $map, $vehicle, 0.0, [ref]$farClearance)
|
|
Assert-True $farSafe 'Strict distance field clearance must allow the distant pose.'
|
|
Assert-True ($farClearance -gt 0.0) 'Distant pose must report positive body clearance.'
|
|
|
|
$from = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.20, 1.50, 0.0)
|
|
$to = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.80, 1.50, 0.0)
|
|
$sweepClearance = 0.0
|
|
$sweepSafe = $checker.IsSweptMotionCollisionFree($from, $to, $map, $vehicle, 0.50, [ref]$sweepClearance)
|
|
Assert-False $sweepSafe 'A swept vehicle must not pass through an occupied cell.'
|
|
|
|
$gridCenterPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.475, 1.475, 0.0)
|
|
$gridCenterClearance = 0.0
|
|
Assert-False $checker.IsPoseCollisionFree($gridCenterPose, $map, $vehicle, 0.0, [ref]$gridCenterClearance) 'A pose at an occupied grid center must collide.'
|
|
|
|
$subGridPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.481, 1.463, 0.37)
|
|
$subGridClearance = 0.0
|
|
Assert-False $checker.IsPoseCollisionFree($subGridPose, $map, $vehicle, 0.0, [ref]$subGridClearance) 'An arbitrary sub-grid heading must collide with the thin obstacle.'
|
|
|
|
$diagonalPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.32, 1.50, ([Math]::PI / 4.0))
|
|
$diagonalClearance = 0.0
|
|
Assert-False $checker.IsPoseCollisionFree($diagonalPose, $map, $vehicle, 0.0, [ref]$diagonalClearance) 'A 45 degree footprint overlap must collide.'
|
|
|
|
$outsidePose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(0.05, 0.05, 0.0)
|
|
$outsideClearance = 0.0
|
|
Assert-False $checker.IsPoseCollisionFree($outsidePose, $map, $vehicle, 0.0, [ref]$outsideClearance) 'Any footprint corner outside the map must collide.'
|
|
|
|
$curveVehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
|
|
$curveVehicle.MaximumCurvaturePerMeter = 0.80
|
|
$curveVehicle.MinimumTurningRadiusMeters = 2.0
|
|
$maximumCurvature = 0.0
|
|
$hasMaximumCurvature = [MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle.VehicleKinematics]::TryGetMaximumCurvaturePerMeter($curveVehicle, [ref]$maximumCurvature)
|
|
Assert-True $hasMaximumCurvature 'Vehicle curvature constraints must be accepted.'
|
|
Assert-Near 0.50 $maximumCurvature 'Both curvature constraints must use the conservative minimum.'
|
|
|
|
Write-Output 'Coarse path collision checks passed.'
|