287 lines
16 KiB
PowerShell
287 lines
16 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,
|
|
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
|
$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
|
|
$configuration = [Activator]::CreateInstance($configurationType)
|
|
$configuration.CubicBSpline.EndpointTangentScale = $EndpointTangentScale
|
|
$options = $optionsConstructor.Invoke(@($configuration))
|
|
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
|
}
|
|
|
|
function Invoke-Candidate(
|
|
[object[]]$Segments,
|
|
[double]$ReserveMeters,
|
|
[double]$Strength = 1.0,
|
|
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
|
return $smoothMethod.Invoke($smoother, @(
|
|
(New-AlgorithmInput $Segments $ReserveMeters $EndpointTangentScale), $Strength, [Threading.CancellationToken]::None))
|
|
}
|
|
|
|
function Invoke-Smoothing(
|
|
[object[]]$Segments,
|
|
[double]$ReserveMeters,
|
|
[double]$Strength = 1.0,
|
|
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
|
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength $EndpointTangentScale
|
|
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')
|
|
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
|
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
|
|
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
|
$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], $optionsType), $null)
|
|
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable smoothing options and the minimum clearance reserve for per-anchor movement limits.'
|
|
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
|
Assert-True ($null -ne $optionsConstructor) 'B-spline tests must create an immutable options snapshot.'
|
|
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
|
|
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose arc-length interpolation.'
|
|
$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')
|
|
|
|
# Arc-length interpolation must bracket by local arc rather than by sample index.
|
|
$nonUniformPoints = [Array]::CreateInstance($pointType, 4)
|
|
$nonUniformPoints.SetValue((New-Point 0.0 0.0 0.000 0.0 1.0), 0)
|
|
$nonUniformPoints.SetValue((New-Point 5.0 0.0 0.050 0.1 0.9), 1)
|
|
$nonUniformPoints.SetValue((New-Point 10.0 0.0 0.100 0.2 0.8), 2)
|
|
$nonUniformPoints.SetValue((New-Point 20.0 0.0 0.125 0.3 0.7), 3)
|
|
$interpolateArguments = [object[]]@($nonUniformPoints, [double]0.1125, $null, $null)
|
|
Assert-True $interpolateMethod.Invoke($null, $interpolateArguments) 'Arc-length interpolation must accept a target within the final non-uniform interval.'
|
|
$arcReference = $interpolateArguments[2]
|
|
Assert-Near 15.0 $arcReference.X 0.000000001 'Target arc length 0.1125 must lie halfway through the final 0.100-0.125 interval, independent of point count.'
|
|
Assert-Near 0.1125 $arcReference.ArcLength 0.000000001 'Arc-length interpolation must preserve the requested target arc length.'
|
|
Assert-Near 0.25 $arcReference.Heading 0.000000001 'Arc-length interpolation must linearly interpolate heading.'
|
|
Assert-Near 0.75 $arcReference.BodyClearance 0.000000001 'Arc-length interpolation must linearly interpolate clearance.'
|
|
|
|
# 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.'
|
|
}
|
|
|
|
# Endpoint tangent scale is an immutable option and must influence the clamped B-spline endpoint handle.
|
|
$shortHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.20)[0].Points
|
|
$longHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.60)[0].Points
|
|
Assert-True (($longHandlePoints[2].X - $shortHandlePoints[2].X) -gt 0.0001) 'A custom endpoint tangent scale must change the B-spline start handle and early curve samples.'
|
|
|
|
# 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.'
|
|
}
|
|
|
|
# A 0.035 m reserve radius must reject this corner by its parameter-matched source reference,
|
|
# even though the candidate's nearest-polyline distance is smaller than 0.035 m.
|
|
$parameterMatchedReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.045
|
|
Assert-True (-not (Get-PropertyValue $parameterMatchedReserveCandidate 'Succeeded')) 'A medium reserve must reject B-spline geometry that exceeds its parameter-matched movement radius.'
|
|
|
|
# A tight reserve may not publish an evaluated B-spline that leaves its 0.005 m movement radius.
|
|
$tightReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.075
|
|
|
|
# A tight endpoint circle that cannot meet the heading=0.2 rad tangent ray must fail, not silently rotate the handle.
|
|
$misalignedHeadingSource = @(
|
|
(New-Point 0.0 0.0 0.0 0.2 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))
|
|
$misalignedHeadingCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $misalignedHeadingSource)) 0.075
|
|
$requiredFailures = New-Object System.Collections.Generic.List[string]
|
|
if (Get-PropertyValue $tightReserveCandidate 'Succeeded') {
|
|
[void]$requiredFailures.Add('A tight reserve published an evaluated candidate outside its permitted movement radius.')
|
|
}
|
|
if (Get-PropertyValue $misalignedHeadingCandidate 'Succeeded') {
|
|
[void]$requiredFailures.Add('A tight reserve silently accepted an endpoint handle that cannot follow the supplied travel tangent.')
|
|
}
|
|
Assert-Equal 0 $requiredFailures.Count ([string]::Join(' ', $requiredFailures))
|
|
|
|
# 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.'
|