461 lines
30 KiB
PowerShell
461 lines
30 KiB
PowerShell
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
|
$ErrorActionPreference = 'Stop'
|
|
|
|
$plannerRoot = Join-Path $PSScriptRoot '..\ParkrobTrajplanner'
|
|
$coarsePathReadme = Join-Path $plannerRoot 'CoarsePath\README.md'
|
|
$legacyRootReadme = Join-Path $plannerRoot 'README.md'
|
|
if (-not (Test-Path -LiteralPath $coarsePathReadme -PathType Leaf)) {
|
|
throw 'CoarsePath module README must be located inside the CoarsePath directory.'
|
|
}
|
|
if (Test-Path -LiteralPath $legacyRootReadme -PathType Leaf) {
|
|
throw 'The coarse-path-only README must not remain at the ParkrobTrajplanner root.'
|
|
}
|
|
$coarsePathReadmeContent = Get-Content -LiteralPath $coarsePathReadme -Raw
|
|
if (-not $coarsePathReadmeContent.Contains('## 总预算与取消')) {
|
|
throw 'CoarsePath README must document total timeout and cancellation semantics.'
|
|
}
|
|
if (-not $coarsePathReadmeContent.Contains('PlanningMapBuildStatus')) {
|
|
throw 'CoarsePath README must document map-stage termination status handling.'
|
|
}
|
|
|
|
$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-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.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
|
|
}
|
|
|
|
function Find-Method($Type, [string]$Name, [Type[]]$ParameterTypes) {
|
|
foreach ($candidate in $Type.GetMethods()) {
|
|
if ($candidate.Name -ne $Name) { continue }
|
|
$parameters = $candidate.GetParameters()
|
|
if ($parameters.Length -ne $ParameterTypes.Length) { continue }
|
|
$matches = $true
|
|
for ($index = 0; $index -lt $parameters.Length; $index++) {
|
|
if ($parameters[$index].ParameterType -ne $ParameterTypes[$index]) {
|
|
$matches = $false
|
|
break
|
|
}
|
|
}
|
|
if ($matches) { return $candidate }
|
|
}
|
|
return $null
|
|
}
|
|
|
|
function New-EmptyPlanningMap {
|
|
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
|
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
|
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
|
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
|
$request = [Activator]::CreateInstance($requestType)
|
|
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
|
$request.ResolutionMm = [single]50
|
|
$request.AllowExplicitEmptyMap = $true
|
|
$result = [Activator]::CreateInstance($factoryType).Create($request)
|
|
Assert-True $result.Succeeded 'Empty integration map must be created.'
|
|
Assert-True $result.Map.PlanningReady 'Explicit empty integration map must be ready.'
|
|
return $result.Map
|
|
}
|
|
|
|
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
|
$plannerType = $assembly.GetType($coarsePath + 'HybridAStarPlanner', $true)
|
|
$planner = [Activator]::CreateInstance($plannerType)
|
|
$requestType = $assembly.GetType($coarsePath + 'PlanningRequest', $true)
|
|
$poseType = $assembly.GetType($coarsePath + 'Pose2D', $true)
|
|
$vehicleType = $assembly.GetType($coarsePath + 'VehicleParameters', $true)
|
|
$configurationType = $assembly.GetType($coarsePath + 'HybridAStarConfiguration', $true)
|
|
$goalDirectionType = $assembly.GetType($coarsePath + 'GoalDirectionConstraint', $true)
|
|
$plan = Find-Method $plannerType 'Plan' @($requestType, [Threading.CancellationToken])
|
|
Assert-True ($plan -ne $null) 'HybridAStarPlanner must expose Plan(request, cancellationToken).'
|
|
|
|
$defaultConfiguration = [Activator]::CreateInstance($configurationType)
|
|
Assert-Near 0.15 $defaultConfiguration.GoalPositionToleranceMeters 'Default goal-position tolerance must retain the documented 0.15 m terminal acceptance radius.'
|
|
|
|
$request = [Activator]::CreateInstance($requestType)
|
|
$request.Map = New-EmptyPlanningMap
|
|
$request.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
|
|
$request.Goal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
|
|
$request.Vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$request.Vehicle.LengthMeters = 0.20
|
|
$request.Vehicle.WidthMeters = 0.20
|
|
$request.Vehicle.SafetyMarginMeters = 0.0
|
|
$request.Vehicle.MaximumCurvaturePerMeter = 1.0
|
|
$request.Configuration = [Activator]::CreateInstance($configurationType)
|
|
$request.Configuration.MaximumExpandedNodes = 10000
|
|
$request.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
|
|
$request.Configuration.GoalPositionToleranceMeters = 0.001
|
|
$request.Configuration.GoalHeadingToleranceRadians = 0.001
|
|
$request.Configuration.AllowReverse = $false
|
|
$request.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
|
|
$request.StartVehicleCurvature = [double]0.20
|
|
|
|
$result = $plan.Invoke($planner, @($request, [Threading.CancellationToken]::None))
|
|
Assert-Equal 'Success' $result.Status.ToString() 'Planner must publish a validated direct path on an empty map.'
|
|
Assert-True ($result.Path.Count -ge 2) 'Validated path must retain its start and goal points.'
|
|
Assert-Near 0.0 $result.Path[0].ArcLength 'The first path point must have zero arc length.'
|
|
Assert-Near 0.20 $result.Path[0].VehicleCurvature `
|
|
'The assembled start point must retain the requested physical vehicle curvature.'
|
|
Assert-Equal 'Start' $result.Path[0].Source.ToString() 'The first path point must retain the start source.'
|
|
Assert-Equal 'GoalTruncation' $result.Path[-1].Source.ToString() 'The terminal truncated point must retain its source.'
|
|
Assert-True ($result.Diagnostics.PeakOpenListCount -ge 1) 'Planner diagnostics must retain the actual Open List peak count.'
|
|
Assert-True ($result.Diagnostics.StaleOpenListEntryCount -ge 0) 'Planner diagnostics must retain a non-negative stale Open List count.'
|
|
Assert-True ($result.Diagnostics.GetType().GetProperty('PathSearchElapsed') -ne $null) 'Planner diagnostics must expose path-search elapsed time.'
|
|
Assert-True ($result.Diagnostics.PathSearchElapsed -ge [TimeSpan]::Zero) 'Successful planning must retain non-negative path-search time.'
|
|
Assert-True ($result.Diagnostics.PathSearchElapsed -le $result.Diagnostics.Elapsed) 'Path-search time must not exceed total elapsed time.'
|
|
|
|
$nodeLimitedRequest = [Activator]::CreateInstance($requestType)
|
|
$nodeLimitedRequest.Map = $request.Map
|
|
$nodeLimitedRequest.Start = $request.Start
|
|
$nodeLimitedRequest.Goal = $request.Goal
|
|
$nodeLimitedRequest.Vehicle = $request.Vehicle
|
|
$nodeLimitedRequest.Configuration = [Activator]::CreateInstance($configurationType)
|
|
$nodeLimitedRequest.Configuration.MaximumExpandedNodes = 0
|
|
$nodeLimitedRequest.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2)
|
|
$nodeLimitedRequest.Configuration.GoalPositionToleranceMeters = 0.001
|
|
$nodeLimitedRequest.Configuration.GoalHeadingToleranceRadians = 0.001
|
|
$nodeLimitedRequest.Configuration.AllowReverse = $false
|
|
$nodeLimitedRequest.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
|
|
$nodeLimitedResult = $plan.Invoke($planner, @($nodeLimitedRequest, [Threading.CancellationToken]::None))
|
|
Assert-Equal 'SearchNodeLimitExceeded' $nodeLimitedResult.Status.ToString() 'A zero node limit must fail after planner preflight.'
|
|
Assert-True ($nodeLimitedResult.Diagnostics.PathSearchElapsed -ge [TimeSpan]::Zero) 'Search-stage node-limit failure must retain path-search time.'
|
|
Assert-True ($nodeLimitedResult.Diagnostics.PathSearchElapsed -le $nodeLimitedResult.Diagnostics.Elapsed) 'Failed path-search time must not exceed total elapsed time.'
|
|
Assert-True (-not [string]::IsNullOrWhiteSpace($nodeLimitedResult.Diagnostics.TerminationReason)) 'Planner diagnostics must retain a node-limit reason.'
|
|
|
|
for ($index = 1; $index -lt $result.Path.Count; $index++) {
|
|
$previous = $result.Path[$index - 1]
|
|
$current = $result.Path[$index]
|
|
Assert-True ($current.ArcLength -ge $previous.ArcLength) 'Path arc length must be non-decreasing.'
|
|
Assert-True ([Math]::Abs($current.UnwrappedHeading - $previous.UnwrappedHeading) -le ([Math]::PI + 0.000001)) 'Unwrapped heading must remain continuous between adjacent points.'
|
|
$samePose = ([Math]::Abs($current.X - $previous.X) -le 0.000001) -and
|
|
([Math]::Abs($current.Y - $previous.Y) -le 0.000001) -and
|
|
([Math]::Abs($current.Heading - $previous.Heading) -le 0.000001) -and
|
|
([Math]::Abs($current.ArcLength - $previous.ArcLength) -le 0.000001)
|
|
if ($samePose) {
|
|
Assert-True (($current.Direction -ne $previous.Direction) -and $current.IsGearSwitchPoint) 'Adjacent duplicate points are permitted only as a marked gear-switch pair.'
|
|
}
|
|
}
|
|
|
|
Assert-True ($result.Segments.Count -ge 1) 'Validated path must include inclusive direction segments.'
|
|
Assert-Equal 0 $result.Segments[0].StartIndex 'The first segment must include the first path point.'
|
|
Assert-Equal ($result.Path.Count - 1) $result.Segments[-1].EndIndex 'The final segment must include the final path point.'
|
|
for ($index = 0; $index -lt $result.Segments.Count; $index++) {
|
|
$segment = $result.Segments[$index]
|
|
Assert-Equal $index $segment.SegmentIndex 'Segment indexes must be contiguous.'
|
|
Assert-True ($segment.StartIndex -le $segment.EndIndex) 'Each segment must use inclusive ordered indexes.'
|
|
if ($index -gt 0) {
|
|
$previousSegment = $result.Segments[$index - 1]
|
|
Assert-Equal ($previousSegment.EndIndex + 1) $segment.StartIndex 'Segments must cover each path point exactly once.'
|
|
}
|
|
}
|
|
|
|
Write-Output 'Coarse path integration checks passed.'
|
|
|
|
# Facade checks intentionally use only ASCII text so the script can also run in legacy PowerShell hosts.
|
|
$facade = $coarsePath + 'Facade.'
|
|
$serviceType = $assembly.GetType($facade + 'CoarsePathPlanningService', $true)
|
|
$jobType = $assembly.GetType($facade + 'CoarsePathPlanningJob', $true)
|
|
$jobResultType = $assembly.GetType($facade + 'CoarsePathPlanningJobResult', $true)
|
|
$debugOptionsType = $assembly.GetType($facade + 'PlanningDebugOptions', $true)
|
|
$debugSinkType = $assembly.GetType($facade + 'IPlanningDebugSink', $true)
|
|
$mapRequestType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest', $true)
|
|
$boundsType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true)
|
|
$servicePlan = Find-Method $serviceType 'Plan' @($jobType, [Threading.CancellationToken])
|
|
Assert-True ($servicePlan -ne $null) 'CoarsePathPlanningService must expose Plan(job, cancellationToken).'
|
|
Assert-True ($jobResultType.GetProperty('MapResult') -ne $null) 'Facade result must retain the map result.'
|
|
Assert-True ($jobResultType.GetProperty('PlanningResult') -ne $null) 'Facade result must retain the planning result.'
|
|
Assert-True ($jobResultType.GetProperty('DebugDiagnostics') -ne $null) 'Facade result must retain debug diagnostics.'
|
|
|
|
function New-FacadeJob {
|
|
$job = [Activator]::CreateInstance($jobType)
|
|
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
|
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
|
$mapRequest.ResolutionMm = [single]50
|
|
$mapRequest.AllowExplicitEmptyMap = $true
|
|
$job.MapRequest = $mapRequest
|
|
$job.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
|
|
$job.Goal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
|
|
$job.Vehicle = [Activator]::CreateInstance($vehicleType)
|
|
$job.Vehicle.LengthMeters = 0.20
|
|
$job.Vehicle.WidthMeters = 0.20
|
|
$job.Vehicle.SafetyMarginMeters = 0.0
|
|
$job.Vehicle.MaximumCurvaturePerMeter = 1.0
|
|
$job.Configuration = [Activator]::CreateInstance($configurationType)
|
|
$job.Configuration.MaximumExpandedNodes = 10000
|
|
$job.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
|
|
$job.Configuration.GoalPositionToleranceMeters = 0.001
|
|
$job.Configuration.GoalHeadingToleranceRadians = 0.001
|
|
$job.Configuration.AllowReverse = $false
|
|
$job.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
|
|
return $job
|
|
}
|
|
|
|
$service = [Activator]::CreateInstance($serviceType)
|
|
$firstFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), [Threading.CancellationToken]::None))
|
|
Assert-True $firstFacadeResult.MapResult.Succeeded 'Facade must build the requested map first.'
|
|
Assert-Equal 'Success' $firstFacadeResult.PlanningResult.Status.ToString() 'Facade must pass a successful map to the planner.'
|
|
Assert-Equal 0 $firstFacadeResult.DebugDiagnostics.Count 'Default debug sink must not add diagnostics.'
|
|
|
|
$secondFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), [Threading.CancellationToken]::None))
|
|
Assert-True $secondFacadeResult.MapResult.Succeeded 'Repeated facade request must retain a map result.'
|
|
Assert-Equal 'Input' $secondFacadeResult.MapResult.CacheHit.ToString() 'One facade service must retain its map factory cache.'
|
|
Assert-Equal $firstFacadeResult.MapResult.Map.InputFingerprint $secondFacadeResult.MapResult.Map.InputFingerprint 'Map fingerprint must remain stable for equal input.'
|
|
Assert-Equal $firstFacadeResult.PlanningResult.Status $secondFacadeResult.PlanningResult.Status 'Map cache reuse must not change planning status.'
|
|
Assert-Equal $firstFacadeResult.PlanningResult.Path.Count $secondFacadeResult.PlanningResult.Path.Count 'Map cache reuse must not change path point count.'
|
|
|
|
$invalidJob = [Activator]::CreateInstance($jobType)
|
|
$invalidJob.MapRequest = [Activator]::CreateInstance($mapRequestType)
|
|
$mapFailureResult = $servicePlan.Invoke($service, @($invalidJob, [Threading.CancellationToken]::None))
|
|
Assert-False $mapFailureResult.MapResult.Succeeded 'Invalid map input must be returned as a map failure.'
|
|
Assert-Equal 'InvalidMap' $mapFailureResult.PlanningResult.Status.ToString() 'Map failure must return an empty non-search planning result.'
|
|
Assert-Equal 0 $mapFailureResult.PlanningResult.Diagnostics.ExpandedNodeCount 'Map failure must not start the search.'
|
|
Assert-Equal 0 $mapFailureResult.PlanningResult.Path.Count 'Map failure must not publish a path.'
|
|
Assert-Equal ([TimeSpan]::Zero) $mapFailureResult.PlanningResult.Diagnostics.PathSearchElapsed 'Map failure must report zero path-search time.'
|
|
|
|
$cancelledFacadeSource = New-Object Threading.CancellationTokenSource
|
|
$cancelledFacadeSource.Cancel()
|
|
$cancelledFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), $cancelledFacadeSource.Token))
|
|
Assert-Equal 'Cancelled' $cancelledFacadeResult.MapResult.Status.ToString() 'Facade must retain a cancelled map result.'
|
|
Assert-Equal 'Cancelled' $cancelledFacadeResult.PlanningResult.Status.ToString() 'Facade must map map-stage cancellation to planning cancellation.'
|
|
Assert-Equal 0 $cancelledFacadeResult.PlanningResult.Path.Count 'Cancelled facade planning must publish no path.'
|
|
Assert-Equal 0 $cancelledFacadeResult.PlanningResult.Segments.Count 'Cancelled facade planning must publish no segments.'
|
|
|
|
$timedOutFacadeJob = New-FacadeJob
|
|
$timedOutFacadeJob.Configuration.SearchTimeout = [TimeSpan]::Zero
|
|
$timedOutFacadeResult = $servicePlan.Invoke($service, @($timedOutFacadeJob, [Threading.CancellationToken]::None))
|
|
Assert-Equal 'TimedOut' $timedOutFacadeResult.MapResult.Status.ToString() 'Facade total timeout must stop before publishing a map.'
|
|
Assert-Equal 'SearchTimeout' $timedOutFacadeResult.PlanningResult.Status.ToString() 'Facade must map map-stage timeout to planning timeout.'
|
|
Assert-Equal 0 $timedOutFacadeResult.PlanningResult.Path.Count 'Timed out facade planning must publish no path.'
|
|
|
|
$defaultDebugOptions = [Activator]::CreateInstance($debugOptionsType)
|
|
Assert-False $defaultDebugOptions.Enabled 'Debug output must be opt-in.'
|
|
Assert-True ($defaultDebugOptions.Sink -ne $null) 'Debug options must default to a no-op sink.'
|
|
|
|
$debugSinkSource = @'
|
|
using System;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
|
|
public sealed class FacadeRecordingDebugSink : IPlanningDebugSink
|
|
{
|
|
public int PublishCount { get; private set; }
|
|
public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
|
|
{
|
|
PublishCount++;
|
|
}
|
|
}
|
|
|
|
public sealed class FacadeThrowingDebugSink : IPlanningDebugSink
|
|
{
|
|
public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
|
|
{
|
|
throw new InvalidOperationException("debug sink failure");
|
|
}
|
|
}
|
|
'@
|
|
$runtimeDirectory = [Runtime.InteropServices.RuntimeEnvironment]::GetRuntimeDirectory()
|
|
$debugSinkReferences = @(
|
|
(Join-Path $runtimeDirectory 'mscorlib.dll'),
|
|
(Join-Path $runtimeDirectory 'System.dll'),
|
|
(Join-Path $runtimeDirectory 'System.Core.dll'),
|
|
$AssemblyPath,
|
|
'C:\Program Files (x86)\Reference Assemblies\Microsoft\Framework\.NETFramework\v4.8\Facades\netstandard.dll'
|
|
)
|
|
Add-Type -TypeDefinition $debugSinkSource -ReferencedAssemblies $debugSinkReferences
|
|
$recordingSinkType = 'FacadeRecordingDebugSink' -as [type]
|
|
$throwingSinkType = 'FacadeThrowingDebugSink' -as [type]
|
|
|
|
$disabledDebugOptions = [Activator]::CreateInstance($debugOptionsType)
|
|
$recordingSink = [Activator]::CreateInstance($recordingSinkType)
|
|
$disabledDebugOptions.Sink = $recordingSink
|
|
$disabledJob = New-FacadeJob
|
|
$disabledJob.DebugOptions = $disabledDebugOptions
|
|
$disabledResult = $servicePlan.Invoke($service, @($disabledJob, [Threading.CancellationToken]::None))
|
|
Assert-Equal 0 $recordingSink.PublishCount 'Disabled debug output must not publish to the sink.'
|
|
Assert-Equal $firstFacadeResult.PlanningResult.Status $disabledResult.PlanningResult.Status 'Disabled debug output must not change planning status.'
|
|
|
|
$throwingDebugOptions = [Activator]::CreateInstance($debugOptionsType)
|
|
$throwingDebugOptions.Enabled = $true
|
|
$throwingDebugOptions.Sink = [Activator]::CreateInstance($throwingSinkType)
|
|
$throwingJob = New-FacadeJob
|
|
$throwingJob.DebugOptions = $throwingDebugOptions
|
|
$throwingResult = $servicePlan.Invoke($service, @($throwingJob, [Threading.CancellationToken]::None))
|
|
Assert-Equal $firstFacadeResult.MapResult.Map.InputFingerprint $throwingResult.MapResult.Map.InputFingerprint 'Debug sink errors must not change the map fingerprint.'
|
|
Assert-Equal $firstFacadeResult.PlanningResult.Status $throwingResult.PlanningResult.Status 'Debug sink errors must not change planning status.'
|
|
Assert-Equal $firstFacadeResult.PlanningResult.Path.Count $throwingResult.PlanningResult.Path.Count 'Debug sink errors must not change the path.'
|
|
Assert-Equal 1 $throwingResult.DebugDiagnostics.Count 'Debug sink errors must be retained as diagnostics.'
|
|
|
|
Write-Output 'Coarse path facade checks passed.'
|
|
|
|
# P1 UI scenario factory contract checks. These run against the same built assembly
|
|
# and intentionally fail until the factory is introduced.
|
|
$testNamespace = $coarsePath + 'Test.'
|
|
$scenarioEnumType = $assembly.GetType($testNamespace + 'CoarsePathTestScenario', $false)
|
|
$scenarioFactoryType = $assembly.GetType($testNamespace + 'CoarsePathScenarioFactory', $false)
|
|
Assert-True ($scenarioEnumType -ne $null) 'P1 scenario enum must exist.'
|
|
Assert-True ($scenarioFactoryType -ne $null) 'P1 scenario factory must exist.'
|
|
|
|
$factoryCreate = Find-Method $scenarioFactoryType 'Create' @($scenarioEnumType)
|
|
$factoryCreateAtAmr = Find-Method $scenarioFactoryType 'Create' @(
|
|
$scenarioEnumType, [double], [double], [double])
|
|
$factoryManual = Find-Method $scenarioFactoryType 'CreateManualGoalDemo' @(
|
|
[double], [double], [double], [double], [double], [double])
|
|
Assert-True ($factoryCreate -ne $null) 'P1 scenario factory must expose Create(scenario).'
|
|
Assert-True ($factoryCreateAtAmr -ne $null) 'Scenario factory must expose Create(scenario, amrX, amrY, amrHeading).'
|
|
Assert-True ($factoryManual -ne $null) 'P1 scenario factory must expose CreateManualGoalDemo with six doubles.'
|
|
|
|
$scenarioNames = @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'CacheHit', 'ReverseGearSwitch', 'NoFeasiblePath')
|
|
foreach ($scenarioName in $scenarioNames) {
|
|
$scenario = [Enum]::Parse($scenarioEnumType, $scenarioName)
|
|
$jobA = $factoryCreate.Invoke($null, @($scenario))
|
|
$jobB = $factoryCreate.Invoke($null, @($scenario))
|
|
Assert-True ($jobA -ne $null) "Scenario $scenarioName must return a job."
|
|
Assert-False ([object]::ReferenceEquals($jobA, $jobB)) "Scenario $scenarioName must return a new job per call."
|
|
}
|
|
|
|
foreach ($scenarioName in $scenarioNames) {
|
|
$scenario = [Enum]::Parse($scenarioEnumType, $scenarioName)
|
|
$liveJob = $factoryCreateAtAmr.Invoke($null, @($scenario, 12345.0, -6789.0, 135.0))
|
|
Assert-Near 12.345 $liveJob.Start.X "Live $scenarioName start X must equal the AMR X."
|
|
Assert-Near -6.789 $liveJob.Start.Y "Live $scenarioName start Y must equal the AMR Y."
|
|
Assert-Near (3.0 * [Math]::PI / 4.0) $liveJob.Start.Heading "Live $scenarioName heading must equal the AMR heading."
|
|
}
|
|
|
|
$rectangleScenario = [Enum]::Parse($scenarioEnumType, 'RectangleDetour')
|
|
$liveRectangle = $factoryCreateAtAmr.Invoke($null, @($rectangleScenario, 12000.0, -3000.0, 90.0))
|
|
Assert-Near 12.0 $liveRectangle.Start.X 'Live rectangle start X must equal the AMR X.'
|
|
Assert-Near -3.0 $liveRectangle.Start.Y 'Live rectangle start Y must equal the AMR Y.'
|
|
Assert-Near ([Math]::PI / 2.0) $liveRectangle.Start.Heading 'Live rectangle start heading must equal the AMR heading.'
|
|
Assert-Near 16.0 $liveRectangle.Goal.X 'Live rectangle goal X must preserve the four-metre relative offset.'
|
|
Assert-Near -3.0 $liveRectangle.Goal.Y 'Live rectangle goal Y must preserve the relative offset.'
|
|
Assert-Near ([Math]::PI / 2.0) $liveRectangle.Goal.Heading 'Live rectangle goal heading must preserve the zero baseline heading delta.'
|
|
Assert-Near 11000.0 $liveRectangle.MapRequest.Bounds.XMin 'Live rectangle map X minimum must translate with the AMR.'
|
|
Assert-Near -1000.0 $liveRectangle.MapRequest.Bounds.YMax 'Live rectangle map Y maximum must translate with the AMR.'
|
|
$rectangleProjection = $liveRectangle.MapRequest.ObstacleSources[0].ProjectToWorld()
|
|
$rectangleObstacle = $rectangleProjection.Obstacles[0]
|
|
Assert-Near 13700.0 $rectangleObstacle.XMin 'Live rectangle obstacle X minimum must translate with the AMR.'
|
|
Assert-Near -2200.0 $rectangleObstacle.YMax 'Live rectangle obstacle Y maximum must translate with the AMR.'
|
|
|
|
$multiScenario = [Enum]::Parse($scenarioEnumType, 'ManualAndTwoLeg')
|
|
$baselineMulti = $factoryCreate.Invoke($null, @($multiScenario))
|
|
$liveMulti = $factoryCreateAtAmr.Invoke($null, @($multiScenario, 7000.0, 8000.0, 45.0))
|
|
Assert-Near 7.0 $liveMulti.Start.X 'Live multi-source start X must equal AMR X.'
|
|
Assert-Near 8.0 $liveMulti.Start.Y 'Live multi-source start Y must equal AMR Y.'
|
|
Assert-Near ([Math]::PI / 4.0) $liveMulti.Goal.Heading 'Live multi-source goal heading must follow AMR heading.'
|
|
$baselineTwoLeg = $baselineMulti.MapRequest.ObstacleSources[1].ProjectToWorld().Obstacles
|
|
$liveTwoLeg = $liveMulti.MapRequest.ObstacleSources[1].ProjectToWorld().Obstacles
|
|
Assert-Near ($baselineTwoLeg[0].CenterX + 6000.0) $liveTwoLeg[0].CenterX 'TwoLeg X must translate without rotation.'
|
|
Assert-Near ($baselineTwoLeg[0].CenterY + 7000.0) $liveTwoLeg[0].CenterY 'TwoLeg Y must translate without rotation.'
|
|
|
|
$noPathScenario = [Enum]::Parse($scenarioEnumType, 'NoFeasiblePath')
|
|
$liveNoPath = $factoryCreateAtAmr.Invoke($null, @($noPathScenario, 9000.0, -1000.0, -180.0))
|
|
Assert-Near 9.0 $liveNoPath.Start.X 'Live infeasible scenario must use AMR X.'
|
|
Assert-Near -1.0 $liveNoPath.Start.Y 'Live infeasible scenario must use AMR Y.'
|
|
Assert-Near 8000.0 $liveNoPath.MapRequest.Bounds.XMin 'Live infeasible map must translate with its baseline start.'
|
|
|
|
$invalidLivePoseRejected = $false
|
|
try { $null = $factoryCreateAtAmr.Invoke($null, @($rectangleScenario, [double]::NaN, 0.0, 0.0)) }
|
|
catch [ArgumentOutOfRangeException] {
|
|
$invalidLivePoseRejected = $true
|
|
}
|
|
catch [Reflection.TargetInvocationException] {
|
|
$invalidLivePoseRejected = $_.Exception.InnerException -is [ArgumentOutOfRangeException]
|
|
}
|
|
Assert-True $invalidLivePoseRejected 'Live factory must reject non-finite AMR coordinates.'
|
|
|
|
$manualJob = $factoryManual.Invoke($null, @(1000.0, 2000.0, 90.0, 4000.0, 2000.0, 0.0))
|
|
Assert-Near 1.0 $manualJob.Start.X 'Manual AMR X must convert mm to m.'
|
|
Assert-Near 2.0 $manualJob.Start.Y 'Manual AMR Y must convert mm to m.'
|
|
Assert-Near ([Math]::PI / 2.0) $manualJob.Start.Heading 'Manual AMR heading must convert degrees to radians.'
|
|
Assert-Near 4.0 $manualJob.Goal.X 'Manual goal X must convert mm to m.'
|
|
Assert-Near 0.0 $manualJob.Goal.Heading 'Manual goal heading must convert degrees to radians.'
|
|
Assert-True $manualJob.MapRequest.AllowExplicitEmptyMap 'Manual goal demo must declare its empty map explicitly.'
|
|
|
|
$slowFeasibleJob = $factoryManual.Invoke($null, @(1000.0, 2000.0, 0.0, 1500.0, 2500.0, 90.0))
|
|
$slowFeasibleJob.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(30)
|
|
$slowFeasibleJob.Configuration.MaximumExpandedNodes = 1000000
|
|
$slowFeasibleService = [Activator]::CreateInstance($serviceType)
|
|
$slowFeasibleResult = $servicePlan.Invoke($slowFeasibleService, @($slowFeasibleJob, [Threading.CancellationToken]::None))
|
|
Assert-Equal 'Success' $slowFeasibleResult.PlanningResult.Status.ToString() 'The previously five-second-limited feasible pose must succeed with a caller-selected longer budget.'
|
|
Assert-True ($slowFeasibleResult.PlanningResult.Diagnostics.PathSearchElapsed -le $slowFeasibleResult.PlanningResult.Diagnostics.Elapsed) 'Slow feasible path-search time must remain within total elapsed time.'
|
|
|
|
$manualObstacleKindType = $assembly.GetType($testNamespace + 'ManualCoarsePathObstacleKind', $false)
|
|
$manualObstacleType = $assembly.GetType($testNamespace + 'ManualCoarsePathObstacle', $false)
|
|
Assert-True ($manualObstacleKindType -ne $null) 'Manual obstacle kind enum must exist.'
|
|
Assert-True ($manualObstacleType -ne $null) 'Manual obstacle value type must exist.'
|
|
|
|
$manualCircle = Find-Method $manualObstacleType 'Circle' @([double], [double], [double])
|
|
$manualRectangle = Find-Method $manualObstacleType 'AxisAlignedRectangle' @([double], [double], [double], [double])
|
|
$manualObstacleFactory = $scenarioFactoryType.GetMethods() | Where-Object {
|
|
$_.Name -eq 'CreateManualObstacleDemo' -and $_.GetParameters().Length -eq 8
|
|
} | Select-Object -First 1
|
|
Assert-True ($manualCircle -ne $null) 'Manual obstacle type must create circles from center and radius.'
|
|
Assert-True ($manualRectangle -ne $null) 'Manual obstacle type must create rectangles from center and X/Y dimensions.'
|
|
Assert-True ($manualObstacleFactory -ne $null) 'Scenario factory must expose CreateManualObstacleDemo with six poses, obstacles and version.'
|
|
|
|
$manualObstacles = [Array]::CreateInstance($manualObstacleType, 2)
|
|
$manualObstacles.SetValue($manualCircle.Invoke($null, @([double]6500, [double]2000, [double]200)), 0)
|
|
$manualObstacles.SetValue($manualRectangle.Invoke($null, @([double]-2000, [double]500, [double]600, [double]400)), 1)
|
|
$manualObstacleJob = $manualObstacleFactory.Invoke($null, @(
|
|
1000.0, 2000.0, 0.0, 4000.0, 2000.0, 0.0, $manualObstacles, [long]77))
|
|
Assert-False $manualObstacleJob.MapRequest.AllowExplicitEmptyMap 'Manual obstacles must disable the explicit-empty-map mode.'
|
|
Assert-Equal 1 $manualObstacleJob.MapRequest.ObstacleSources.Count 'Manual obstacles must create one unified source.'
|
|
Assert-Equal 'manual-user-input' $manualObstacleJob.MapRequest.ObstacleSources[0].SourceId 'Manual source ID must be stable.'
|
|
Assert-Equal 77 $manualObstacleJob.MapRequest.ObstacleSources[0].SourceVersion 'Manual source version must be preserved.'
|
|
Assert-True ($manualObstacleJob.MapRequest.Bounds.XMin -le -4300.0) 'Manual map must include the rectangle outline and padding.'
|
|
Assert-True ($manualObstacleJob.MapRequest.Bounds.XMax -ge 8700.0) 'Manual map must include the circle outline and padding.'
|
|
|
|
$emptyManualObstacles = [Array]::CreateInstance($manualObstacleType, 0)
|
|
$emptyManualJob = $manualObstacleFactory.Invoke($null, @(
|
|
1000.0, 2000.0, 0.0, 4000.0, 2000.0, 0.0, $emptyManualObstacles, [long]0))
|
|
Assert-True $emptyManualJob.MapRequest.AllowExplicitEmptyMap 'Zero manual obstacles must retain explicit empty-map mode.'
|
|
Assert-Equal 0 $emptyManualJob.MapRequest.ObstacleSources.Count 'Zero manual obstacles must not create a fake source.'
|
|
|
|
$invalidGeometryRejected = $false
|
|
try {
|
|
$null = $manualCircle.Invoke($null, @([double]1000, [double]1000, [double]0))
|
|
}
|
|
catch [ArgumentOutOfRangeException] { $invalidGeometryRejected = $true }
|
|
catch [Reflection.TargetInvocationException] {
|
|
$invalidGeometryRejected = $_.Exception.InnerException -is [ArgumentOutOfRangeException]
|
|
}
|
|
Assert-True $invalidGeometryRejected 'Invalid manual geometry must report argument range.'
|
|
|
|
$scenarioService = [Activator]::CreateInstance($serviceType)
|
|
foreach ($scenarioName in @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'ReverseGearSwitch')) {
|
|
$job = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, $scenarioName)))
|
|
$result = $servicePlan.Invoke($scenarioService, @($job, [Threading.CancellationToken]::None))
|
|
Assert-Equal 'Success' $result.PlanningResult.Status.ToString() "Scenario $scenarioName must succeed."
|
|
}
|
|
|
|
$reverseJob = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'ReverseGearSwitch')))
|
|
$reverseResult = $servicePlan.Invoke($scenarioService, @($reverseJob, [Threading.CancellationToken]::None))
|
|
Assert-True (($reverseResult.PlanningResult.Path | Where-Object { $_.IsGearSwitchPoint }).Count -ge 1) 'Reverse scenario must expose a gear-switch point.'
|
|
|
|
$noPathJob = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'NoFeasiblePath')))
|
|
$noPathResult = $servicePlan.Invoke($scenarioService, @($noPathJob, [Threading.CancellationToken]::None))
|
|
Assert-Equal 'NoFeasiblePath' $noPathResult.PlanningResult.Status.ToString() 'Barrier scenario must be infeasible.'
|
|
Assert-Equal 0 $noPathResult.PlanningResult.Path.Count 'Infeasible scenario must not publish a path.'
|
|
Assert-True ($noPathResult.PlanningResult.Diagnostics.TerminationReason.Contains('Open List')) 'No-path diagnostics must retain the exact search exhaustion reason.'
|
|
Assert-True (-not [string]::IsNullOrWhiteSpace($noPathResult.PlanningResult.Diagnostics.TerminationReason)) 'No-path diagnostics must retain a reason.'
|
|
|
|
$cacheJobA = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'CacheHit')))
|
|
$cacheJobB = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'CacheHit')))
|
|
$cacheFirst = $servicePlan.Invoke($scenarioService, @($cacheJobA, [Threading.CancellationToken]::None))
|
|
$cacheSecond = $servicePlan.Invoke($scenarioService, @($cacheJobB, [Threading.CancellationToken]::None))
|
|
Assert-Equal 'Input' $cacheSecond.MapResult.CacheHit.ToString() 'Cache-hit scenario must reuse the complete map input.'
|
|
Assert-Equal $cacheFirst.PlanningResult.Status $cacheSecond.PlanningResult.Status 'Map cache reuse must not change planning status.'
|
|
|
|
Write-Output 'Coarse path P1 scenario checks passed.'
|