fix: normalize empty-map path clearance

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs

@@ -65,6 +65,12 @@ public sealed class SmoothedPathValidator
             return false;
         }
 
+        if (!TryGetFiniteClearanceCap(map, out double clearanceCapMeters))
+        {
+            reason = "Planning map bounds cannot produce a finite clearance cap.";
+            return false;
+        }
+
         var checkedClearances = new double[candidatePath.Count];
         double minimumClearance = double.PositiveInfinity;
         for (int index = 0; index < candidatePath.Count; index++)
@@ -83,6 +89,12 @@ public sealed class SmoothedPathValidator
                 return false;
             }
 
+            if (!TryNormalizeClearance(poseClearance, clearanceCapMeters, out poseClearance))
+            {
+                reason = "Pose collision verification returned an invalid clearance.";
+                return false;
+            }
+
             checkedClearances[index] = poseClearance;
             minimumClearance = Math.Min(minimumClearance, poseClearance);
             if (index == 0) continue;
@@ -118,6 +130,12 @@ public sealed class SmoothedPathValidator
                 return false;
             }
 
+            if (!TryNormalizeClearance(sweptClearance, clearanceCapMeters, out sweptClearance))
+            {
+                reason = "Swept collision verification returned an invalid clearance.";
+                return false;
+            }
+
             checkedClearances[index - 1] = Math.Min(checkedClearances[index - 1], sweptClearance);
             checkedClearances[index] = Math.Min(checkedClearances[index], sweptClearance);
             minimumClearance = Math.Min(minimumClearance, sweptClearance);
@@ -238,6 +256,30 @@ public sealed class SmoothedPathValidator
         return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
     }
 
+    private static bool TryGetFiniteClearanceCap(PlanningGridMap map, out double capMeters)
+    {
+        capMeters = 0d;
+        if (map == null || map.Bounds == null) return false;
+        double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
+        double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
+        capMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
+        return NumericGuard.IsPositiveFinite(capMeters);
+    }
+
+    private static bool TryNormalizeClearance(double clearanceMeters, double capMeters, out double normalizedMeters)
+    {
+        normalizedMeters = 0d;
+        if (double.IsPositiveInfinity(clearanceMeters))
+        {
+            normalizedMeters = capMeters;
+            return true;
+        }
+        if (!NumericGuard.IsFinite(clearanceMeters) || clearanceMeters < 0d)
+            return false;
+        normalizedMeters = clearanceMeters;
+        return true;
+    }
+
     private static IReadOnlyList<SmoothedPathPoint> EmptyPath()
     {
         return new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -586,8 +586,16 @@ internal static class TrajectoryObservationChecks
         Verification.True(bootstrap.Map != null, "observer empty-map bootstrap returns a map");
         Verification.Equal(PlanningStatus.Success, bootstrap.CoarseResult.PlanningResult.Status,
             "observer empty-map coarse planning succeeds");
-        Verification.True(bootstrap.SmoothedPath != null && IsPublishableSmoothingStatus(bootstrap.SmoothedPath.Status),
+        PathSmoothingResult smoothedPath = bootstrap.SmoothedPath ?? throw new InvalidOperationException(
+            "observer empty-map bootstrap has no smoothing result");
+        Verification.True(IsPublishableSmoothingStatus(smoothedPath.Status),
             "observer empty-map bootstrap returns a publishable smoothing result");
+        for (int index = 0; index < smoothedPath.Path.Count; index++)
+        {
+            Verification.True(!double.IsNaN(smoothedPath.Path[index].BodyClearance) &&
+                    !double.IsInfinity(smoothedPath.Path[index].BodyClearance),
+                "observer empty-map smoothing path has finite body clearance at " + index);
+        }
         Verification.True(bootstrap.Segments.Count > 0,
             "observer empty-map bootstrap returns at least one direction segment");
     }