diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs index 7e8fb5c..d8a28c3 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs @@ -37,6 +37,21 @@ public sealed class PathSmoothingService if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason)) return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason); + if (!TryAnalyzeAndValidate( + request, + preparedPath, + preparedPath.Segments, + configuration, + false, + cancellationToken, + out _, + out _, + out _, + out reason)) + { + return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason); + } + cancellationToken.ThrowIfCancellationRequested(); var input = new SmoothingAlgorithmInput( preparedPath, @@ -121,8 +136,37 @@ public sealed class PathSmoothingService if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false; cancellationToken.ThrowIfCancellationRequested(); + return TryAnalyzeAndValidate( + request, + revalidatedPath, + ToFallbackSegments(revalidatedPath), + configuration, + true, + cancellationToken, + out fallbackPath, + out fallbackSegments, + out fallbackMetrics, + out reason); + } + + private bool TryAnalyzeAndValidate( + PathSmoothingRequest request, + PreparedPath originalPath, + IReadOnlyList candidateSegments, + PathSmoothingConfiguration configuration, + bool useFallbackSource, + CancellationToken cancellationToken, + out IReadOnlyList safePath, + out IReadOnlyList safeSegments, + out PathQualityMetrics metrics, + out string reason) + { + safePath = null; + safeSegments = null; + metrics = null; + reason = string.Empty; if (!_analyzer.TryAnalyze( - ToFallbackSegments(revalidatedPath), + candidateSegments, configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason)) @@ -130,25 +174,26 @@ public sealed class PathSmoothingService return false; } - IReadOnlyList fallbackSourcePath = ToFallbackPoints(analysis.Path); + IReadOnlyList pathForValidation = useFallbackSource + ? ToFallbackPoints(analysis.Path) + : analysis.Path; cancellationToken.ThrowIfCancellationRequested(); if (!_validator.TryValidate( - fallbackSourcePath, + pathForValidation, analysis.Segments, - revalidatedPath, + originalPath, request.Map, request.Vehicle, configuration.MaximumCollisionCheckStepMeters, - out IReadOnlyList safePath, + out safePath, out double minimumClearanceMeters, out reason)) { return false; } - fallbackPath = safePath; - fallbackSegments = analysis.Segments; - fallbackMetrics = CreateMetrics(analysis, minimumClearanceMeters); + safeSegments = analysis.Segments; + metrics = CreateMetrics(analysis, minimumClearanceMeters); return true; } diff --git a/ClumsyPilot/tests/verify_path_smoothing_service.ps1 b/ClumsyPilot/tests/verify_path_smoothing_service.ps1 index 39c2237..7bd0b96 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_service.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_service.ps1 @@ -21,16 +21,31 @@ function Get-RequiredType([string]$Name) { return $assembly.GetType($Name, $true) } -function New-EmptyMap { +function New-Map([bool]$WithObstacle) { $mapRequest = [Activator]::CreateInstance($mapRequestType) $mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000)) $mapRequest.ResolutionMm = [single]50 - $mapRequest.AllowExplicitEmptyMap = $true + if ($WithObstacle) { + $obstacle = [Activator]::CreateInstance($rectangleType, @([single]900, [single]1100, [single]400, [single]600)) + $obstacles = [Array]::CreateInstance($obstacleType, 1) + $obstacles.SetValue($obstacle, 0) + $source = [Activator]::CreateInstance($manualSourceType, @('service-safety-obstacle', [long]1, $true, $obstacles)) + $sources = [Array]::CreateInstance($obstacleSourceType, 1) + $sources.SetValue($source, 0) + $mapRequest.ObstacleSources = $sources + } + else { + $mapRequest.AllowExplicitEmptyMap = $true + } $map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map - Assert-True ($null -ne $map) 'Service test must create an explicit empty planning map.' + Assert-True ($null -ne $map) 'Service test must create a planning map.' return $map } +function New-EmptyMap { return New-Map $false } + +function New-CollidingMap { return New-Map $true } + function New-Vehicle { $vehicle = [Activator]::CreateInstance($vehicleType) $vehicle.LengthMeters = [double]0.20 @@ -58,7 +73,8 @@ function New-Configuration($Method = $cubicBSpline) { return $configuration } -function New-Request([object[]]$Points, $Configuration) { +function New-Request([object[]]$Points, $Configuration, $Map = $null) { + if ($null -eq $Map) { $Map = New-EmptyMap } $typedPoints = [Array]::CreateInstance($coarsePointType, $Points.Count) for ($index = 0; $index -lt $Points.Count; $index++) { $typedPoints.SetValue($Points[$index], $index) @@ -67,7 +83,7 @@ function New-Request([object[]]$Points, $Configuration) { $segments = [Array]::CreateInstance($coarseSegmentType, 1) $segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @( 0, $forward, 0, ($Points.Count - 1), $false, $false)), 0) - return [Activator]::CreateInstance($requestType, @($typedPoints, $segments, (New-EmptyMap), (New-Vehicle), $Configuration)) + return [Activator]::CreateInstance($requestType, @($typedPoints, $segments, $Map, (New-Vehicle), $Configuration)) } function New-StraightRequest($Configuration) { @@ -119,6 +135,10 @@ $directionType = Get-RequiredType ($coarsePath + 'TravelDirection') $coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource') $vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters') $boundsType = Get-RequiredType ($mapping + 'MapBoundsMm') +$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle') +$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle') +$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource') +$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource') $mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest') $mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory') @@ -185,6 +205,15 @@ Assert-Equal 'InvalidInput' $invalidCoarseResult.Status.ToString() 'A non-finite Assert-NoGeometry $invalidCoarseResult 'Invalid coarse path' Assert-Equal 0 $invalidCoarseResult.Diagnostics.RetryCount 'Invalid coarse input must be rejected before retries.' +# A finite, structurally valid coarse path may still be unsafe for the requested map and vehicle. +# It must be rejected before method selection/retries and may never use fallback to publish the unsafe geometry. +$unsafeCoarseResult = Invoke-Smooth (New-Request @( + (New-CoarsePoint 0.5 0.5 0.0 $forward), + (New-CoarsePoint 1.5 0.5 1.0 $forward)) (New-Configuration) (New-CollidingMap)) +Assert-Equal 'InvalidInput' $unsafeCoarseResult.Status.ToString() 'A colliding coarse path must be invalid before smoothing starts.' +Assert-NoGeometry $unsafeCoarseResult 'Unsafe coarse path' +Assert-Equal 0 $unsafeCoarseResult.Diagnostics.RetryCount 'Unsafe coarse geometry must be rejected before retry execution.' + $cancelledConfiguration = New-Configuration $cancellationSource = [Threading.CancellationTokenSource]::new() $cancellationSource.Cancel()