218 lines
11 KiB
PowerShell
218 lines
11 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" }
|
|
}
|
|
|
|
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 Get-PropertyValue($Instance, [string]$Name) {
|
|
$property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
|
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
|
return $property.GetValue($Instance)
|
|
}
|
|
|
|
function New-Point(
|
|
[double]$X,
|
|
[double]$Y,
|
|
[double]$ArcLength,
|
|
[double]$Heading,
|
|
[double]$BodyClearance,
|
|
[bool]$IsGearSwitch = $false) {
|
|
return [Activator]::CreateInstance($pointType, @(
|
|
$X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $IsGearSwitch, $anchor))
|
|
}
|
|
|
|
function New-DirectionSegment(
|
|
[int]$Index,
|
|
$Direction,
|
|
[object[]]$Points,
|
|
[bool]$StartsAtGearSwitch = $false,
|
|
[bool]$EndsAtGearSwitch = $false) {
|
|
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
|
|
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
|
|
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
|
|
}
|
|
return [Activator]::CreateInstance($segmentType, @(
|
|
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
|
|
}
|
|
|
|
function New-EmptyMap {
|
|
$request = [Activator]::CreateInstance($mapRequestType)
|
|
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
|
$request.ResolutionMm = [single]50
|
|
$request.AllowExplicitEmptyMap = $true
|
|
$map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
|
|
Assert-True ($null -ne $map) 'B-spline test must create an explicit empty planning map.'
|
|
return $map
|
|
}
|
|
|
|
function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
|
|
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
|
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
|
$typedSegments.SetValue($Segments[$index], $index)
|
|
}
|
|
$preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$typedSegments))
|
|
$vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$vehicle.LengthMeters = [double]0.20
|
|
$vehicle.WidthMeters = [double]0.20
|
|
$vehicle.SafetyMarginMeters = [double]0.0
|
|
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
|
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
|
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters))
|
|
}
|
|
|
|
function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
|
|
$candidate = $smoothMethod.Invoke($smoother, @(
|
|
(New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None))
|
|
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
|
|
return @(Get-PropertyValue $candidate 'Segments')
|
|
}
|
|
|
|
function Get-PointDistance($Left, $Right) {
|
|
$deltaX = $Left.X - $Right.X
|
|
$deltaY = $Left.Y - $Right.Y
|
|
return [Math]::Sqrt($deltaX * $deltaX + $deltaY * $deltaY)
|
|
}
|
|
|
|
function Get-DistanceToSegment($Point, $Left, $Right) {
|
|
$deltaX = $Right.X - $Left.X
|
|
$deltaY = $Right.Y - $Left.Y
|
|
$lengthSquared = $deltaX * $deltaX + $deltaY * $deltaY
|
|
if ($lengthSquared -le 0.0) { return Get-PointDistance $Point $Left }
|
|
$projection = (($Point.X - $Left.X) * $deltaX + ($Point.Y - $Left.Y) * $deltaY) / $lengthSquared
|
|
$projection = [Math]::Max(0.0, [Math]::Min(1.0, $projection))
|
|
$closest = New-Object PSObject -Property @{
|
|
X = $Left.X + $projection * $deltaX
|
|
Y = $Left.Y + $projection * $deltaY
|
|
}
|
|
return Get-PointDistance $Point $closest
|
|
}
|
|
|
|
function Get-DistanceToPolyline($Point, [object[]]$SourcePoints) {
|
|
$minimum = [double]::PositiveInfinity
|
|
for ($index = 1; $index -lt $SourcePoints.Count; $index++) {
|
|
$minimum = [Math]::Min($minimum, (Get-DistanceToSegment $Point $SourcePoints[$index - 1] $SourcePoints[$index]))
|
|
}
|
|
return $minimum
|
|
}
|
|
|
|
function Get-TravelAngle($Left, $Right) {
|
|
return [Math]::Atan2($Right.Y - $Left.Y, $Right.X - $Left.X)
|
|
}
|
|
|
|
function Get-AngleDifference([double]$Left, [double]$Right) {
|
|
$difference = $Left - $Right
|
|
while ($difference -gt [Math]::PI) { $difference -= 2.0 * [Math]::PI }
|
|
while ($difference -lt -[Math]::PI) { $difference += 2.0 * [Math]::PI }
|
|
return [Math]::Abs($difference)
|
|
}
|
|
|
|
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
|
$processing = $root + 'Processing.'
|
|
$algorithms = $root + 'Algorithms.'
|
|
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
|
|
|
$smootherType = Get-RequiredType ($algorithms + 'CubicBSplineSmoother')
|
|
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
|
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
|
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
|
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
|
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
|
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
|
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
|
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
|
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
|
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
|
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
|
|
|
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
|
@($preparedPathType, $mapType, $vehicleType, [double], [double]), $null)
|
|
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry the minimum clearance reserve for per-anchor movement limits.'
|
|
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
|
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
|
Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.'
|
|
Assert-Equal 'CubicBSpline' $smoother.Method.ToString() 'B-spline smoother must identify its public smoothing method.'
|
|
|
|
$forward = [Enum]::Parse($directionType, 'Forward')
|
|
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
|
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
|
|
|
# Straight samples are returned exactly, so a straight is never distorted or densified.
|
|
$straightSource = @(
|
|
(New-Point 0.0 0.0 0.0 0.0 0.03),
|
|
(New-Point 1.0 0.0 1.0 0.0 0.03),
|
|
(New-Point 2.0 0.0 2.0 0.0 0.03),
|
|
(New-Point 3.0 0.0 3.0 0.0 0.03))
|
|
$straightResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)) 0.02
|
|
Assert-Equal 1 $straightResult.Count 'A single direction segment must produce exactly one candidate segment.'
|
|
Assert-Equal $straightSource.Count $straightResult[0].Points.Count 'A straight must retain its original samples.'
|
|
for ($index = 0; $index -lt $straightSource.Count; $index++) {
|
|
Assert-Near $straightSource[$index].X $straightResult[0].Points[$index].X 0.0 'Straight X coordinates must remain exact.'
|
|
Assert-Near $straightSource[$index].Y $straightResult[0].Points[$index].Y 0.0 'Straight Y coordinates must remain exact.'
|
|
}
|
|
|
|
# A five-anchor corner uses the preprocessor's 0.05 m sampling scale. It must retain exact endpoint poses,
|
|
# follow endpoint travel tangents, turn continuously, and stay within the per-anchor clearance reserve radius.
|
|
$cornerSource = @(
|
|
(New-Point 0.0 0.0 0.0 0.0 0.08),
|
|
(New-Point 0.05 0.0 0.05 0.0 0.08),
|
|
(New-Point 0.10 0.0 0.10 0.0 0.08),
|
|
(New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08),
|
|
(New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08))
|
|
$cornerResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02
|
|
$cornerPoints = @($cornerResult[0].Points)
|
|
Assert-True ($cornerPoints.Count -gt $cornerSource.Count) 'A non-straight B-spline candidate must provide sampled curve geometry.'
|
|
$cornerStart = $cornerPoints[0]
|
|
$cornerEnd = $cornerPoints[$cornerPoints.Count - 1]
|
|
Assert-Near $cornerSource[0].X $cornerStart.X 0.0 'B-spline start X must be exact.'
|
|
Assert-Near $cornerSource[0].Y $cornerStart.Y 0.0 'B-spline start Y must be exact.'
|
|
Assert-Near $cornerSource[$cornerSource.Count - 1].X $cornerEnd.X 0.0 'B-spline end X must be exact.'
|
|
Assert-Near $cornerSource[$cornerSource.Count - 1].Y $cornerEnd.Y 0.0 'B-spline end Y must be exact.'
|
|
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[0] $cornerPoints[1]) 0.0) 0.02 'B-spline start travel tangent must follow the supplied forward heading.'
|
|
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[$cornerPoints.Count - 2] $cornerPoints[$cornerPoints.Count - 1]) ([Math]::PI / 2.0)) 0.02 'B-spline end travel tangent must follow the supplied forward heading.'
|
|
for ($index = 2; $index -lt $cornerPoints.Count; $index++) {
|
|
$previousAngle = Get-TravelAngle $cornerPoints[$index - 2] $cornerPoints[$index - 1]
|
|
$currentAngle = Get-TravelAngle $cornerPoints[$index - 1] $cornerPoints[$index]
|
|
Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
|
|
}
|
|
|
|
# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
|
|
$allowedRadius = 0.06
|
|
foreach ($point in $cornerPoints) {
|
|
Assert-True ((Get-DistanceToPolyline $point $cornerSource) -le ($allowedRadius + 0.000000001)) 'Every B-spline displacement must stay inside the per-anchor clearance reserve radius.'
|
|
}
|
|
|
|
# Adjacent direction segments retain their duplicated switch pose and independent topology; no fit may cross the switch.
|
|
$reverseSource = @(
|
|
(New-Point 0.10 0.10 0.0 ([Math]::PI / 2.0) 0.08 $true),
|
|
(New-Point 0.10 0.05 0.05 ([Math]::PI / 2.0) 0.08),
|
|
(New-Point 0.10 0.0 0.10 ([Math]::PI / 2.0) 0.08))
|
|
$switchResult = Invoke-Smoothing @(
|
|
(New-DirectionSegment 0 $forward $cornerSource $false $true),
|
|
(New-DirectionSegment 1 $reverse $reverseSource $true $false)) 0.02
|
|
Assert-Equal 2 $switchResult.Count 'B-spline smoothing must preserve each direction segment boundary.'
|
|
Assert-True $switchResult[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
|
|
Assert-True $switchResult[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
|
|
$switchLeft = $switchResult[0].Points[$switchResult[0].Points.Count - 1]
|
|
$switchRight = $switchResult[1].Points[0]
|
|
Assert-Near $switchLeft.X $switchRight.X 0.0 'B-spline smoothing must retain the duplicated gear-switch X pose.'
|
|
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'B-spline smoothing must retain the duplicated gear-switch Y pose.'
|
|
|
|
Write-Output 'Path smoothing cubic B-spline checks passed.'
|