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