fix: validate coarse path before smoothing

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<PreparedDirectionSegment> candidateSegments,
+        PathSmoothingConfiguration configuration,
+        bool useFallbackSource,
+        CancellationToken cancellationToken,
+        out IReadOnlyList<SmoothedPathPoint> safePath,
+        out IReadOnlyList<SmoothedPathSegment> 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<SmoothedPathPoint> fallbackSourcePath = ToFallbackPoints(analysis.Path);
+        IReadOnlyList<SmoothedPathPoint> 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<SmoothedPathPoint> safePath,
+                out safePath,
                 out double minimumClearanceMeters,
                 out reason))
         {
             return false;
         }
 
-        fallbackPath = safePath;
+        safeSegments = analysis.Segments;
-        fallbackSegments = analysis.Segments;
+        metrics = CreateMetrics(analysis, minimumClearanceMeters);
-        fallbackMetrics = CreateMetrics(analysis, minimumClearanceMeters);
         return true;
     }
 

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()