fix: validate coarse path before smoothing

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