From d7ceb761b7eddd63a29e49bc616a10b91e0ac673 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 30 Jul 2026 16:24:39 +0800 Subject: [PATCH] fix: preserve coarse path start curvature --- .../CoarsePath/Output/CoarsePathAssembler.cs | 216 ++++++++ .../tests/verify_coarse_path_integration.ps1 | 460 ++++++++++++++++++ 2 files changed, 676 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs create mode 100644 ClumsyPilot/tests/verify_coarse_path_integration.ps1 diff --git a/ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs b/ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs new file mode 100644 index 0000000..29df53d --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs @@ -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; + +/// +/// 将回溯原语装配为调用方可消费的稠密路径和包含式方向分段。 +/// 装配器保留原语边界的换向双点，其余相邻重复位姿会被删除。 +/// +public sealed class CoarsePathAssembler +{ + private const double DuplicateTolerance = 1e-8d; + private readonly FootprintCollisionChecker _collisionChecker; + + /// 创建使用默认连续车体检查器的路径装配器。 + public CoarsePathAssembler() + : this(new FootprintCollisionChecker()) + { + } + + /// 创建使用指定连续车体检查器的路径装配器。 + public CoarsePathAssembler(FootprintCollisionChecker collisionChecker) + { + _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker)); + } + + /// + /// 从已回溯的恒曲率原语构造稠密路径。 + /// 参数：backtrackedPath 提供原语顺序；request 提供地图和车辆；path、segments 为成功时的只读输出。 + /// 返回：首点可通过连续车体检查、每个原语积分点数据一致且方向分段完整覆盖时为 true；否则返回 false 且输出为空。 + /// + public bool TryAssemble(BacktrackedPath backtrackedPath, PlanningRequest request, + out IReadOnlyList path, out IReadOnlyList 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(); + 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(points); + segments = BuildSegments(points); + return true; + } + + private static IReadOnlyList BuildSegments(IReadOnlyList points) + { + var segments = new List(); + 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(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 EmptyPath() + { + return new ReadOnlyCollection(new List()); + } + + private static IReadOnlyList EmptySegments() + { + return new ReadOnlyCollection(new List()); + } +} diff --git a/ClumsyPilot/tests/verify_coarse_path_integration.ps1 b/ClumsyPilot/tests/verify_coarse_path_integration.ps1 new file mode 100644 index 0000000..6051426 --- /dev/null +++ b/ClumsyPilot/tests/verify_coarse_path_integration.ps1 @@ -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.'