fix: preserve coarse path start curvature
This commit is contained in:
梁薄云
@@ -0,0 +1,216 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 将回溯原语装配为调用方可消费的稠密路径和包含式方向分段。
|
||||
/// 装配器保留原语边界的换向双点，其余相邻重复位姿会被删除。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathAssembler
|
||||
{
|
||||
private const double DuplicateTolerance = 1e-8d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认连续车体检查器的路径装配器。</summary>
|
||||
public CoarsePathAssembler()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车体检查器的路径装配器。</summary>
|
||||
public CoarsePathAssembler(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从已回溯的恒曲率原语构造稠密路径。
|
||||
/// 参数：backtrackedPath 提供原语顺序；request 提供地图和车辆；path、segments 为成功时的只读输出。
|
||||
/// 返回：首点可通过连续车体检查、每个原语积分点数据一致且方向分段完整覆盖时为 true；否则返回 false 且输出为空。
|
||||
/// </summary>
|
||||
public bool TryAssemble(BacktrackedPath backtrackedPath, PlanningRequest request,
|
||||
out IReadOnlyList<CoarsePathPoint> path, out IReadOnlyList<PathSegment> segments, out string failureReason)
|
||||
{
|
||||
path = EmptyPath();
|
||||
segments = EmptySegments();
|
||||
failureReason = string.Empty;
|
||||
if (backtrackedPath == null || request == null || request.Map == null || request.Vehicle == null ||
|
||||
!IsFinitePose(backtrackedPath.Start) || !IsTravelDirection(backtrackedPath.StartDirection) ||
|
||||
!NumericGuard.IsFinite(backtrackedPath.StartCurvaturePerMeter))
|
||||
{
|
||||
failureReason = "路径装配输入无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_collisionChecker.IsPoseCollisionFree(backtrackedPath.Start, request.Map, request.Vehicle, 0d, out double startClearanceMeters))
|
||||
{
|
||||
failureReason = "回溯路径起点未通过连续车体检查。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var points = new List<CoarsePathPoint>();
|
||||
TravelDirection currentDirection = backtrackedPath.Primitives.Count > 0
|
||||
? backtrackedPath.Primitives[0].Direction
|
||||
: backtrackedPath.StartDirection;
|
||||
double currentCurvature = request.StartVehicleCurvature;
|
||||
double normalizedStartHeading = AngleMath.NormalizeRadians(backtrackedPath.Start.Heading);
|
||||
if (!NumericGuard.IsFinite(normalizedStartHeading))
|
||||
{
|
||||
failureReason = "回溯路径起点航向无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
points.Add(new CoarsePathPoint(backtrackedPath.Start.X, backtrackedPath.Start.Y, normalizedStartHeading,
|
||||
backtrackedPath.Start.Heading, 0d, currentDirection, currentCurvature, startClearanceMeters, false,
|
||||
CoarsePathPointSource.Start));
|
||||
|
||||
for (int primitiveIndex = 0; primitiveIndex < backtrackedPath.Primitives.Count; primitiveIndex++)
|
||||
{
|
||||
MotionPrimitive primitive = backtrackedPath.Primitives[primitiveIndex];
|
||||
if (!IsValidPrimitive(primitive))
|
||||
{
|
||||
failureReason = "回溯路径包含无效原语。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint lastPoint = points[points.Count - 1];
|
||||
if (primitive.Direction != lastPoint.Direction)
|
||||
{
|
||||
// 换向处的旧方向终点和新方向起点必须共存，二者位置、航向、弧长完全相同。
|
||||
points.Add(new CoarsePathPoint(lastPoint.X, lastPoint.Y, lastPoint.Heading, lastPoint.UnwrappedHeading,
|
||||
lastPoint.ArcLength, primitive.Direction, primitive.CurvaturePerMeter, lastPoint.BodyClearance,
|
||||
true, CoarsePathPointSource.MotionPrimitive));
|
||||
}
|
||||
|
||||
Pose2D previousPose = primitive.Start;
|
||||
for (int pointIndex = 0; pointIndex < primitive.Points.Count; pointIndex++)
|
||||
{
|
||||
Pose2D pose = primitive.Points[pointIndex];
|
||||
double bodyClearanceMeters = primitive.BodyClearancesMeters[pointIndex];
|
||||
if (!IsFinitePose(pose) || !IsValidClearance(bodyClearanceMeters))
|
||||
{
|
||||
failureReason = "原语积分点或净空无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint previousPoint = points[points.Count - 1];
|
||||
double arcIncrementMeters = CalculateArcIncrement(previousPose, pose, primitive.CurvaturePerMeter);
|
||||
if (!NumericGuard.IsFinite(arcIncrementMeters) || arcIncrementMeters <= 0d)
|
||||
{
|
||||
failureReason = "原语积分点未产生正弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (IsSamePose(previousPoint, pose))
|
||||
{
|
||||
// 非换向情况下不允许重复采样点泄露到对外路径。
|
||||
previousPose = pose;
|
||||
continue;
|
||||
}
|
||||
|
||||
double normalizedHeading = AngleMath.NormalizeRadians(pose.Heading);
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(previousPoint.Heading, normalizedHeading);
|
||||
if (!NumericGuard.IsFinite(normalizedHeading) || !NumericGuard.IsFinite(headingDelta))
|
||||
{
|
||||
failureReason = "原语积分点航向无法展开。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPointSource source = primitive.IsGoalTruncation && pointIndex == primitive.Points.Count - 1
|
||||
? CoarsePathPointSource.GoalTruncation
|
||||
: CoarsePathPointSource.MotionPrimitive;
|
||||
points.Add(new CoarsePathPoint(pose.X, pose.Y, normalizedHeading,
|
||||
previousPoint.UnwrappedHeading + headingDelta, previousPoint.ArcLength + arcIncrementMeters,
|
||||
primitive.Direction, primitive.CurvaturePerMeter, bodyClearanceMeters, false, source));
|
||||
previousPose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
if (points.Count == 0)
|
||||
{
|
||||
failureReason = "路径装配未产生起点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
path = new ReadOnlyCollection<CoarsePathPoint>(points);
|
||||
segments = BuildSegments(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> BuildSegments(IReadOnlyList<CoarsePathPoint> points)
|
||||
{
|
||||
var segments = new List<PathSegment>();
|
||||
int startIndex = 0;
|
||||
TravelDirection direction = points[0].Direction;
|
||||
for (int index = 1; index < points.Count; index++)
|
||||
{
|
||||
if (points[index].Direction == direction) continue;
|
||||
segments.Add(new PathSegment(segments.Count, direction, startIndex, index - 1,
|
||||
points[startIndex].IsGearSwitchPoint, true));
|
||||
startIndex = index;
|
||||
direction = points[index].Direction;
|
||||
}
|
||||
|
||||
segments.Add(new PathSegment(segments.Count, direction, startIndex, points.Count - 1,
|
||||
points[startIndex].IsGearSwitchPoint, false));
|
||||
return new ReadOnlyCollection<PathSegment>(segments);
|
||||
}
|
||||
|
||||
private static double CalculateArcIncrement(Pose2D from, Pose2D to, double curvaturePerMeter)
|
||||
{
|
||||
if (!IsFinitePose(from) || !IsFinitePose(to) || !NumericGuard.IsFinite(curvaturePerMeter)) return double.NaN;
|
||||
if (Math.Abs(curvaturePerMeter) < 1e-12d)
|
||||
{
|
||||
double deltaX = to.X - from.X;
|
||||
double deltaY = to.Y - from.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
|
||||
return Math.Abs(headingDelta / curvaturePerMeter);
|
||||
}
|
||||
|
||||
private static bool IsValidPrimitive(MotionPrimitive primitive)
|
||||
{
|
||||
return primitive != null && primitive.Start != null && primitive.Points != null && primitive.BodyClearancesMeters != null &&
|
||||
primitive.Points.Count == primitive.BodyClearancesMeters.Count && primitive.Points.Count > 0 &&
|
||||
IsTravelDirection(primitive.Direction) && NumericGuard.IsFinite(primitive.CurvaturePerMeter) &&
|
||||
NumericGuard.IsPositiveFinite(primitive.ActualLengthMeters);
|
||||
}
|
||||
|
||||
private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
|
||||
{
|
||||
return Math.Abs(point.X - pose.X) <= DuplicateTolerance && Math.Abs(point.Y - pose.Y) <= DuplicateTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= DuplicateTolerance;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsValidClearance(double clearanceMeters)
|
||||
{
|
||||
return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> EmptyPath()
|
||||
{
|
||||
return new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> EmptySegments()
|
||||
{
|
||||
return new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,460 @@
|
||||
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.'
|
||||
Reference in New Issue
Block a user