diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs index 31fb88d..f2ca415 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs +++ b/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 EmptyPath() { return new ReadOnlyCollection(new List()); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 3230c50..27123e4 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/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"); }