48 lines
4.3 KiB
PowerShell
48 lines
4.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-Equal($Expected, $Actual, [string]$Message) { if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" } }
|
|
$boundsType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true)
|
|
$gridType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.EnvironmentGridMap', $true)
|
|
$circleType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.CircleObstacle', $true)
|
|
$rectangleType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.AxisAlignedRectangleObstacle', $true)
|
|
$rasterizer = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapObstacleRasterizer', $true)
|
|
$adapter = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapAdapter', $true)
|
|
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]105, [single]0, [single]105))
|
|
$grid = [Activator]::CreateInstance($gridType, @($bounds, [single]20))
|
|
Assert-Equal $true ($grid.IsWorldInBounds([single]104.9, [single]104.9)) 'Last partial cell must be inside.'
|
|
Assert-Equal $false ($grid.IsWorldInBounds([single]105, [single]50)) 'XMax must be exclusive.'
|
|
Assert-Equal $true ($grid.IsOccupiedWorld([single]-0.1, [single]20)) 'Outside world must be occupied.'
|
|
$circle = [Activator]::CreateInstance($circleType, @([single]40, [single]40, [single]0))
|
|
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $circle)) | Out-Null
|
|
Assert-True ($grid.OccupiedCount -ge 4) 'Circle touching a grid intersection must conservatively occupy adjacent cells.'
|
|
$rectangle = [Activator]::CreateInstance($rectangleType, @([single]80, [single]100, [single]80, [single]100))
|
|
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $rectangle)) | Out-Null
|
|
Assert-True ($grid.IsOccupiedWorld([single]90, [single]90)) 'Rectangle must occupy its intersecting cells.'
|
|
$outside = [Activator]::CreateInstance($circleType, @([single]500, [single]500, [single]10))
|
|
$before = $grid.OccupiedCount
|
|
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $outside)) | Out-Null
|
|
Assert-Equal $before $grid.OccupiedCount 'Outside obstacle must not mark cells.'
|
|
$planning = $adapter.GetMethod('Create').Invoke($null, @($grid))
|
|
Assert-True ($planning.IsOccupiedWorld(0.04, 0.04)) 'Planning map must use metre world coordinates.'
|
|
Assert-Equal 0.0 ($planning.GetConservativeObstacleDistanceMeters(0.04, 0.04)) 'Occupied cell clearance must be zero.'
|
|
Assert-Equal 0.0 ($planning.GetConservativeObstacleDistanceMeters(-1.0, 0.0)) 'Outside map clearance must be zero.'
|
|
$singleBounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
|
|
$singleGrid = [Activator]::CreateInstance($gridType, @($singleBounds, [single]50))
|
|
$singleCircle = [Activator]::CreateInstance($circleType, @([single]1525, [single]1525, [single]0))
|
|
$rasterizer.GetMethod('Rasterize').Invoke($null, @($singleGrid, $singleCircle)) | Out-Null
|
|
Assert-Equal 1 $singleGrid.OccupiedCount 'Zero radius circle at a cell center must occupy exactly one cell.'
|
|
$singlePlanning = $adapter.GetMethod('Create').Invoke($null, @($singleGrid))
|
|
$singleDistance = $singlePlanning.GetConservativeObstacleDistanceMeters(1.275, 1.275)
|
|
$singleNearestCellCenterDistance = [Math]::Sqrt(0.25 * 0.25 + 0.25 * 0.25)
|
|
Assert-True ((-not [double]::IsNaN($singleDistance)) -and (-not [double]::IsInfinity($singleDistance))) 'Single occupied cell clearance must be finite.'
|
|
Assert-True ($singleDistance -le $singleNearestCellCenterDistance) 'Single occupied cell clearance must not exceed nearest occupied cell geometry distance.'
|
|
$emptyGrid = [Activator]::CreateInstance($gridType, @($bounds, [single]20))
|
|
$emptyPlanning = $adapter.GetMethod('Create').Invoke($null, @($emptyGrid))
|
|
Assert-True ([double]::IsPositiveInfinity($emptyPlanning.GetConservativeObstacleDistanceMeters(0.01, 0.01))) 'Empty map clearance must be positive infinity.'
|
|
$invalidResolutionRejected = $false
|
|
try { [Activator]::CreateInstance($gridType, @($bounds, [single]10)) | Out-Null } catch { $invalidResolutionRejected = $true }
|
|
Assert-True $invalidResolutionRejected 'Resolution below 20 mm must fail.'
|
|
Write-Output 'Planning map adapter checks passed.'
|