fix: normalize empty-map path clearance

2026-08-05 10:31:14 +08:00
parent 0be35d73d4
commit 71cc454e89
2 changed files with 51 additions and 1 deletions
@@ -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");
    }