257 lines
18 KiB
PowerShell
257 lines
18 KiB
PowerShell
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
|
$ErrorActionPreference = 'Stop'
|
|
|
|
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
|
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
|
$coarsePathRoot = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
|
$mappingRoot = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
|
|
|
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, [string]$Message) {
|
|
if ([Math]::Abs($Expected - $Actual) -gt 0.000001d) {
|
|
throw "$Message Expected=$Expected Actual=$Actual"
|
|
}
|
|
}
|
|
|
|
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
|
$threw = $false
|
|
try { & $Action }
|
|
catch { $threw = $true }
|
|
if (-not $threw) { throw $Message }
|
|
}
|
|
|
|
function Assert-ReadOnlyCollection($Collection, [string]$Message) {
|
|
$list = [System.Collections.IList]$Collection
|
|
Assert-True ($null -ne $list) "$Message The collection must implement IList."
|
|
Assert-True $list.IsReadOnly "$Message The collection must report IsReadOnly."
|
|
Assert-Throws { $list.Add($null) } "$Message The collection must reject Add."
|
|
}
|
|
|
|
function Get-RequiredType([string]$Name) {
|
|
return $assembly.GetType($Name, $true)
|
|
}
|
|
|
|
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
|
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
|
|
$statusType = Get-RequiredType ($root + 'PathSmoothingStatus')
|
|
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
|
$pointType = Get-RequiredType ($root + 'SmoothedPathPoint')
|
|
$segmentType = Get-RequiredType ($root + 'SmoothedPathSegment')
|
|
$bsplineOptionsType = Get-RequiredType ($root + 'CubicBSplineOptions')
|
|
$bezierOptionsType = Get-RequiredType ($root + 'LocalCubicBezierOptions')
|
|
$quinticOptionsType = Get-RequiredType ($root + 'PiecewiseQuinticOptions')
|
|
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
|
$metricsType = Get-RequiredType ($root + 'PathQualityMetrics')
|
|
$diagnosticsType = Get-RequiredType ($root + 'PathSmoothingDiagnostics')
|
|
$resultType = Get-RequiredType ($root + 'PathSmoothingResult')
|
|
$directionType = Get-RequiredType ($coarsePathRoot + 'TravelDirection')
|
|
$coarsePointType = Get-RequiredType ($coarsePathRoot + 'CoarsePathPoint')
|
|
$coarseSegmentType = Get-RequiredType ($coarsePathRoot + 'PathSegment')
|
|
$mapType = Get-RequiredType ($mappingRoot + 'PlanningGridMap')
|
|
$vehicleType = Get-RequiredType ($coarsePathRoot + 'VehicleParameters')
|
|
|
|
Assert-Equal $true $methodType.IsEnum 'SmoothingMethod must be a public enum.'
|
|
Assert-Equal $true $statusType.IsEnum 'PathSmoothingStatus must be a public enum.'
|
|
Assert-Equal $true $sourceType.IsEnum 'SmoothedPathPointSource must be a public enum.'
|
|
Assert-Equal 'CubicBSpline,LocalCubicBezier,PiecewiseQuintic' ([string]::Join(',', [Enum]::GetNames($methodType))) 'Smoothing method members and order must remain stable.'
|
|
Assert-Equal 'Success,FallbackToCoarsePath,InvalidInput,Infeasible,Failed,Cancelled' ([string]::Join(',', [Enum]::GetNames($statusType))) 'Smoothing status members and order must remain stable.'
|
|
Assert-Equal 'Anchor,Interpolated,GearSwitch,CoarsePathFallback' ([string]::Join(',', [Enum]::GetNames($sourceType))) 'Smoothed point source members and order must remain stable.'
|
|
|
|
$configuration = [Activator]::CreateInstance($configurationType)
|
|
Assert-Near 0.05 $configuration.OutputSpacingMeters 'Default output spacing must be 0.05 m.'
|
|
Assert-Near 0.025 $configuration.MaximumCollisionCheckStepMeters 'Default collision step must be 0.025 m.'
|
|
Assert-Near 0.02 $configuration.MinimumClearanceReserveMeters 'Default clearance reserve must be 0.02 m.'
|
|
Assert-Near 1.0 $configuration.SmoothingStrength 'Default smoothing strength must be 1.0.'
|
|
Assert-Equal $true $configuration.AllowFallbackToCoarsePath 'Fallback must be enabled by default.'
|
|
Assert-Equal 4 $configuration.RetryStrengthScales.Count 'Retry schedule must contain four entries.'
|
|
Assert-Near 1.0 $configuration.RetryStrengthScales[0] 'First retry scale must be 1.0.'
|
|
Assert-Near 0.75 $configuration.RetryStrengthScales[1] 'Second retry scale must be 0.75.'
|
|
Assert-Near 0.50 $configuration.RetryStrengthScales[2] 'Third retry scale must be 0.50.'
|
|
Assert-Near 0.25 $configuration.RetryStrengthScales[3] 'Last retry scale must be 0.25.'
|
|
for ($index = 1; $index -lt $configuration.RetryStrengthScales.Count; $index++) {
|
|
Assert-True ($configuration.RetryStrengthScales[$index] -lt $configuration.RetryStrengthScales[$index - 1]) 'Retry schedule must be strictly decreasing.'
|
|
}
|
|
Assert-ReadOnlyCollection $configuration.RetryStrengthScales 'Retry schedule must be immutable.'
|
|
Assert-Near (1.0 / 3.0) ([Activator]::CreateInstance($bsplineOptionsType)).EndpointTangentScale 'B-spline endpoint tangent default must be one third.'
|
|
$bezier = [Activator]::CreateInstance($bezierOptionsType)
|
|
Assert-Near ([Math]::PI / 18.0) $bezier.CornerHeadingThresholdRadians 'Bezier corner threshold must be 10 degrees.'
|
|
Assert-Near 0.60 $bezier.MaximumWindowLengthMeters 'Bezier window default must be 0.60 m.'
|
|
Assert-Near (1.0 / 3.0) $bezier.HandleLengthRatio 'Bezier handle default must be one third.'
|
|
$quintic = [Activator]::CreateInstance($quinticOptionsType)
|
|
Assert-Near 0.50 $quintic.KnotSpacingMeters 'Quintic knot spacing must be 0.50 m.'
|
|
Assert-Near 0.10 $quintic.MinimumKnotSpacingMeters 'Quintic minimum knot spacing must be 0.10 m.'
|
|
|
|
$forward = [Enum]::Parse($directionType, 'Forward')
|
|
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
|
$point = [Activator]::CreateInstance($pointType, @(
|
|
[double]1.25, [double]-2.50, [double]0.30, [double]6.58, [double]4.75,
|
|
$forward, [double]0.12, [double]0.12, [double]0.44, $false, $anchor))
|
|
Assert-Near 1.25 $point.X 'Smoothed point X must be stored in m.'
|
|
Assert-Near -2.50 $point.Y 'Smoothed point Y must be stored in m.'
|
|
Assert-Near 0.30 $point.Heading 'Smoothed point heading must be stored in rad.'
|
|
Assert-Near 6.58 $point.UnwrappedHeading 'Smoothed point unwrapped heading must be stored in rad.'
|
|
Assert-Near 4.75 $point.ArcLength 'Smoothed point arc length must be stored in m.'
|
|
Assert-Equal 'Forward' $point.Direction.ToString() 'Smoothed point direction must be preserved.'
|
|
Assert-Near 0.12 $point.GeometricCurvature 'Smoothed point geometric curvature must be stored in 1/m.'
|
|
Assert-Near 0.12 $point.VehicleCurvature 'Smoothed point vehicle curvature must be stored in 1/m.'
|
|
Assert-Near 0.44 $point.BodyClearance 'Smoothed point clearance must be stored in m.'
|
|
Assert-Equal $false $point.IsGearSwitchPoint 'Smoothed point gear-switch marker must be preserved.'
|
|
Assert-Equal 'Anchor' $point.Source.ToString() 'Smoothed point source must be preserved.'
|
|
|
|
$segmentA = [Activator]::CreateInstance($segmentType, @(0, $forward, 0, 2, $false, $true))
|
|
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
|
$segmentB = [Activator]::CreateInstance($segmentType, @(1, $reverse, 3, 5, $true, $false))
|
|
Assert-Equal 0 $segmentA.SegmentIndex 'First smoothing segment index must be retained.'
|
|
Assert-Equal 'Forward' $segmentA.Direction.ToString() 'First smoothing segment direction must be retained.'
|
|
Assert-Equal 2 $segmentA.EndIndex 'First smoothing segment end index must be retained.'
|
|
Assert-Equal $true $segmentA.EndsAtGearSwitch 'First smoothing segment switch flag must be retained.'
|
|
Assert-Equal 1 $segmentB.SegmentIndex 'Second smoothing segment index must be retained.'
|
|
Assert-Equal 'Reverse' $segmentB.Direction.ToString() 'Second smoothing segment direction must be retained.'
|
|
Assert-Equal $true $segmentB.StartsAtGearSwitch 'Second smoothing segment switch flag must be retained.'
|
|
|
|
$metrics = [Activator]::CreateInstance($metricsType)
|
|
Assert-Equal $false $metrics.IsFeasible 'Default metrics must be infeasible until analysis accepts a candidate.'
|
|
Assert-Near 0.0 $metrics.PathLengthMeters 'Default metrics must be zero-valued.'
|
|
Assert-Near 0.0 $metrics.MinimumBodyClearanceMeters 'Default metrics must be zero-valued.'
|
|
$diagnostics = [Activator]::CreateInstance($diagnosticsType)
|
|
Assert-True ($diagnostics.Metrics -ne $null) 'Default diagnostics must provide quality metrics.'
|
|
Assert-Equal 0 $diagnostics.RetryCount 'Default diagnostics must have no retries.'
|
|
Assert-Near 0.0 $diagnostics.AcceptedStrength 'Default diagnostics must have zero accepted strength.'
|
|
$feasibleMetrics = [Activator]::CreateInstance($metricsType, @(
|
|
$true,
|
|
[double]1.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0,
|
|
[double]0.5, [double]0.0, [double]0.0, [double]0.0, [double]0.0))
|
|
$feasibleDiagnostics = [Activator]::CreateInstance($diagnosticsType, @(
|
|
$feasibleMetrics, [TimeSpan]::Zero, 0, [double]1.0, 'test feasible diagnostics'))
|
|
|
|
$pointArray = [Array]::CreateInstance($pointType, 1)
|
|
$pointArray.SetValue($point, 0)
|
|
$segmentArray = [Array]::CreateInstance($segmentType, 2)
|
|
$segmentArray.SetValue($segmentA, 0)
|
|
$segmentArray.SetValue($segmentB, 1)
|
|
$method = [Enum]::Parse($methodType, 'CubicBSpline')
|
|
$successMethod = $resultType.GetMethod('Success')
|
|
Assert-True ($null -ne $successMethod) 'PathSmoothingResult must expose Success.'
|
|
Assert-Throws { $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $diagnostics)) } 'Success factory must reject diagnostics that are not feasible.'
|
|
Assert-Throws { $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $null)) } 'Success factory must reject null diagnostics.'
|
|
Assert-Throws { $successMethod.Invoke($null, @([Enum]::ToObject($methodType, 99), $pointArray, $segmentArray, $feasibleDiagnostics)) } 'Success factory must reject undefined smoothing methods.'
|
|
$success = $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $feasibleDiagnostics))
|
|
Assert-Equal 'Success' $success.Status.ToString() 'Success factory must publish Success status.'
|
|
Assert-Equal 'CubicBSpline' $success.Method.ToString() 'Success factory must retain the selected method.'
|
|
Assert-Equal 1 $success.Path.Count 'Success factory must publish the provided path.'
|
|
Assert-Equal 2 $success.Segments.Count 'Success factory must publish the provided segments.'
|
|
Assert-ReadOnlyCollection $success.Path 'Success path must be immutable.'
|
|
Assert-ReadOnlyCollection $success.Segments 'Success segments must be immutable.'
|
|
$pointArray.SetValue($null, 0)
|
|
$segmentArray.SetValue($null, 0)
|
|
Assert-True ($null -ne $success.Path[0]) 'Success factory must copy path collections.'
|
|
Assert-True ($null -ne $success.Segments[0]) 'Success factory must copy segment collections.'
|
|
|
|
$fallbackMethod = $resultType.GetMethod('Fallback')
|
|
Assert-True ($null -ne $fallbackMethod) 'PathSmoothingResult must expose Fallback.'
|
|
$fallbackPath = [Array]::CreateInstance($pointType, 1)
|
|
$fallbackPath.SetValue($point, 0)
|
|
$fallbackSegments = [Array]::CreateInstance($segmentType, 1)
|
|
$fallbackSegments.SetValue($segmentA, 0)
|
|
Assert-Throws { $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $diagnostics)) } 'Fallback factory must reject diagnostics that are not feasible.'
|
|
Assert-Throws { $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $null)) } 'Fallback factory must reject null diagnostics.'
|
|
Assert-Throws { $fallbackMethod.Invoke($null, @([Enum]::ToObject($methodType, 99), $fallbackPath, $fallbackSegments, $feasibleDiagnostics)) } 'Fallback factory must reject undefined smoothing methods.'
|
|
$fallback = $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $feasibleDiagnostics))
|
|
Assert-Equal 'FallbackToCoarsePath' $fallback.Status.ToString() 'Fallback factory must publish an explicit fallback status.'
|
|
Assert-Equal 1 $fallback.Path.Count 'Fallback factory must publish a validated fallback path.'
|
|
|
|
$failureMethod = $resultType.GetMethod('Failure')
|
|
Assert-True ($null -ne $failureMethod) 'PathSmoothingResult must expose Failure.'
|
|
$failed = $failureMethod.Invoke(
|
|
$null,
|
|
@([Enum]::Parse($statusType, 'InvalidInput'),
|
|
[Activator]::CreateInstance($diagnosticsType)))
|
|
Assert-Equal 'InvalidInput' $failed.Status.ToString() 'Failure factory must retain failure status.'
|
|
Assert-Equal 0 $failed.Path.Count 'Failure must publish no path.'
|
|
Assert-Equal 0 $failed.Segments.Count 'Failure must publish no segments.'
|
|
Assert-ReadOnlyCollection $failed.Path 'Failure path must be immutable.'
|
|
Assert-ReadOnlyCollection $failed.Segments 'Failure segments must be immutable.'
|
|
Assert-Throws { $failureMethod.Invoke($null, @([Enum]::Parse($statusType, 'Success'), $diagnostics)) } 'Failure factory must reject Success.'
|
|
Assert-Throws { $failureMethod.Invoke($null, @([Enum]::Parse($statusType, 'FallbackToCoarsePath'), $diagnostics)) } 'Failure factory must reject fallback status.'
|
|
Assert-Throws { $failureMethod.Invoke($null, @([Enum]::ToObject($statusType, 99), $diagnostics)) } 'Failure factory must reject undefined statuses.'
|
|
Assert-Throws { $successMethod.Invoke($null, @($method, [Array]::CreateInstance($pointType, 0), $fallbackSegments, $diagnostics)) } 'Success factory must reject an empty path.'
|
|
Assert-Throws { $successMethod.Invoke($null, @($method, $fallbackPath, [Array]::CreateInstance($segmentType, 0), $diagnostics)) } 'Success factory must reject empty segments.'
|
|
|
|
$requestConstructor = $requestType.GetConstructor(@(
|
|
[System.Collections.Generic.IReadOnlyList``1].MakeGenericType($coarsePointType),
|
|
[System.Collections.Generic.IReadOnlyList``1].MakeGenericType($coarseSegmentType),
|
|
$mapType,
|
|
$vehicleType,
|
|
$configurationType))
|
|
Assert-True ($null -ne $requestConstructor) 'PathSmoothingRequest must expose the public five-argument constructor.'
|
|
$boundsType = Get-RequiredType ($mappingRoot + 'MapBoundsMm')
|
|
$mapRequestType = Get-RequiredType ($mappingRoot + 'PlanningMapRequest')
|
|
$mapFactoryType = Get-RequiredType ($mappingRoot + 'PlanningMapFactory')
|
|
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
|
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]1000, [single]0, [single]1000))
|
|
$mapRequest.ResolutionMm = [single]50
|
|
$mapRequest.AllowExplicitEmptyMap = $true
|
|
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
|
Assert-True ($null -ne $map) 'Contract test must create an explicit empty planning map.'
|
|
$requestCoarsePath = [Array]::CreateInstance($coarsePointType, 1)
|
|
$requestCoarsePath.SetValue([Activator]::CreateInstance($coarsePointType, @(
|
|
[double]0.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0,
|
|
$forward, [double]0.0, [double]1.0, $false,
|
|
[Enum]::Parse((Get-RequiredType ($coarsePathRoot + 'CoarsePathPointSource')), 'Start'))), 0)
|
|
$requestSegments = [Array]::CreateInstance($coarseSegmentType, 1)
|
|
$requestSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 0, $false, $false)), 0)
|
|
$vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$vehicle.LengthMeters = [double]0.80
|
|
$vehicle.WidthMeters = [double]0.60
|
|
$vehicle.SafetyMarginMeters = [double]0.05
|
|
$vehicle.MaximumCurvaturePerMeter = [double]0.8333333333333334
|
|
$requestConfiguration = [Activator]::CreateInstance($configurationType)
|
|
$request = $requestConstructor.Invoke(@($requestCoarsePath, $requestSegments, $map, $vehicle, $requestConfiguration))
|
|
Assert-ReadOnlyCollection $request.CoarsePath 'Request coarse path must be immutable.'
|
|
Assert-ReadOnlyCollection $request.Segments 'Request segments must be immutable.'
|
|
$requestCoarsePath.SetValue($null, 0)
|
|
$requestSegments.SetValue($null, 0)
|
|
$vehicle.LengthMeters = [double]9.99
|
|
$vehicle.MaximumCurvaturePerMeter = [double]0.1
|
|
$requestConfiguration.OutputSpacingMeters = [double]0.99
|
|
$requestConfiguration.CubicBSpline.EndpointTangentScale = [double]0.99
|
|
$requestConfiguration.LocalCubicBezier.HandleLengthRatio = [double]0.99
|
|
$requestConfiguration.PiecewiseQuintic.KnotSpacingMeters = [double]0.99
|
|
Assert-True ($null -ne $request.CoarsePath[0]) 'Request must copy the coarse-path collection.'
|
|
Assert-True ($null -ne $request.Segments[0]) 'Request must copy the segment collection.'
|
|
Assert-Near 0.80 $request.Vehicle.LengthMeters 'Request must snapshot vehicle parameters.'
|
|
Assert-Near (1.0 / 1.20) $request.Vehicle.MaximumCurvaturePerMeter 'Request must snapshot nullable vehicle curvature.'
|
|
Assert-Near 0.05 $request.Configuration.OutputSpacingMeters 'Request must snapshot common configuration.'
|
|
Assert-Near (1.0 / 3.0) $request.Configuration.CubicBSpline.EndpointTangentScale 'Request must snapshot B-spline options.'
|
|
Assert-Near (1.0 / 3.0) $request.Configuration.LocalCubicBezier.HandleLengthRatio 'Request must snapshot Bezier options.'
|
|
Assert-Near 0.50 $request.Configuration.PiecewiseQuintic.KnotSpacingMeters 'Request must snapshot quintic options.'
|
|
$request.Vehicle.WidthMeters = [double]9.99
|
|
$request.Vehicle.SafetyMarginMeters = [double]9.99
|
|
$request.Vehicle.MinimumTurningRadiusMeters = [double]9.99
|
|
$request.Configuration.MaximumCollisionCheckStepMeters = [double]0.99
|
|
$request.Configuration.MinimumClearanceReserveMeters = [double]0.99
|
|
$request.Configuration.SmoothingStrength = [double]0.99
|
|
$request.Configuration.AllowFallbackToCoarsePath = $false
|
|
$request.Configuration.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.99
|
|
$request.Configuration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.99
|
|
$request.Configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.99
|
|
Assert-Near 0.60 $request.Vehicle.WidthMeters 'Request vehicle getter must not expose mutable state.'
|
|
Assert-Near 0.05 $request.Vehicle.SafetyMarginMeters 'Request vehicle getter must not expose mutable state.'
|
|
Assert-True ($null -eq $request.Vehicle.MinimumTurningRadiusMeters) 'Request vehicle getter must not expose mutable nullable state.'
|
|
Assert-Near 0.025 $request.Configuration.MaximumCollisionCheckStepMeters 'Request configuration getter must not expose mutable state.'
|
|
Assert-Near 0.02 $request.Configuration.MinimumClearanceReserveMeters 'Request configuration getter must not expose mutable state.'
|
|
Assert-Near 1.0 $request.Configuration.SmoothingStrength 'Request configuration getter must not expose mutable state.'
|
|
Assert-Equal $true $request.Configuration.AllowFallbackToCoarsePath 'Request configuration getter must not expose mutable state.'
|
|
Assert-Near ([Math]::PI / 18.0) $request.Configuration.LocalCubicBezier.CornerHeadingThresholdRadians 'Request configuration getter must not expose mutable Bezier options.'
|
|
Assert-Near 0.60 $request.Configuration.LocalCubicBezier.MaximumWindowLengthMeters 'Request configuration getter must not expose mutable Bezier options.'
|
|
Assert-Near 0.10 $request.Configuration.PiecewiseQuintic.MinimumKnotSpacingMeters 'Request configuration getter must not expose mutable quintic options.'
|
|
|
|
Write-Output 'Path smoothing contract checks passed.'
|