feat: build static EM lateral corridors

2026-08-03 22:49:05 +08:00
parent 06687e294c
commit 2d252ff309
5 changed files with 540 additions and 5 deletions
@@ -0,0 +1,182 @@
+using System;
+using System.Collections.Generic;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class CorridorChecks
+{
+    public static void Run()
+    {
+        VerifiesEmptyAndNarrowedCorridors();
+        VerifiesSeedConnectedIntervalIsPreserved();
+        VerifiesDisappearingSeedIntervalFails();
+    }
+
+    private static void VerifiesEmptyAndNarrowedCorridors()
+    {
+        DirectionSegmentView segment = CreateStraightSegment();
+        CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
+        VehicleParameters vehicle = CreateVehicle();
+        var builder = new StaticCorridorBuilder();
+
+        Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, Array.Empty<FrenetProjection>(),
+            CreateMap(Array.Empty<IMapObstacle>()), vehicle, configuration, out StaticCorridor empty, out string emptyReason),
+            "empty map corridor succeeds: " + emptyReason);
+        VerifyAnchors(empty, 0.2d, 1.8d);
+        for (int index = 1; index + 1 < empty.Stations.Count; index++)
+        {
+            Verification.NearlyEqual(-0.3d, empty.Stations[index].MinimumL, "empty map minimum l");
+            Verification.NearlyEqual(0.3d, empty.Stations[index].MaximumL, "empty map maximum l");
+        }
+
+        IMapObstacle[] leftNarrowing =
+        {
+            new AxisAlignedRectangleObstacle(800f, 1200f, 150f, 400f),
+        };
+        PlanningGridMap narrowedMap = CreateMap(leftNarrowing);
+        IReadOnlyList<FrenetProjection> seed = CreateSeed(segment, -0.2d);
+        Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, seed, narrowedMap, vehicle, configuration,
+            out StaticCorridor narrowed, out string narrowedReason), "narrowed corridor succeeds: " + narrowedReason);
+        LateralInterval narrowedStation = FindStation(narrowed, 1d);
+        Verification.True(narrowedStation.MaximumL < 0.3d, "left obstacle narrows positive-l side");
+        VerifyAcceptedSamplesUseExactFootprints(segment, narrowed, narrowedMap, vehicle, configuration);
+    }
+
+    private static void VerifiesSeedConnectedIntervalIsPreserved()
+    {
+        DirectionSegmentView segment = CreateStraightSegment();
+        CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
+        VehicleParameters vehicle = CreateVehicle();
+        IMapObstacle[] split =
+        {
+            new AxisAlignedRectangleObstacle(800f, 1200f, -50f, 50f),
+        };
+        IReadOnlyList<FrenetProjection> seed = CreateSeed(segment, -0.2d);
+        var builder = new StaticCorridorBuilder();
+
+        Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, seed, CreateMap(split), vehicle, configuration,
+            out StaticCorridor corridor, out string reason), "split corridor succeeds for left seed: " + reason);
+        for (int index = 0; index < corridor.Stations.Count; index++)
+        {
+            LateralInterval station = corridor.Stations[index];
+            Verification.True(station.MinimumL <= station.SeedL && station.SeedL <= station.MaximumL,
+                "chosen interval contains seed at station " + index);
+        }
+        Verification.True(FindStation(corridor, 1d).MaximumL < 0d,
+            "split station retains seed-connected left interval instead of right interval");
+    }
+
+    private static void VerifiesDisappearingSeedIntervalFails()
+    {
+        DirectionSegmentView segment = CreateStraightSegment();
+        CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
+        VehicleParameters vehicle = CreateVehicle();
+        IMapObstacle[] removesLeft =
+        {
+            new AxisAlignedRectangleObstacle(800f, 1200f, -400f, -50f),
+        };
+        var builder = new StaticCorridorBuilder();
+
+        Verification.True(!builder.TryBuild(segment, 0.2d, 1.8d, CreateSeed(segment, -0.2d), CreateMap(removesLeft),
+            vehicle, configuration, out _, out string failureReason),
+            "disappearing seed-connected interval does not switch sides");
+        Verification.True(failureReason.IndexOf("S=", StringComparison.Ordinal) >= 0,
+            "failure identifies the first failed reference-s station: " + failureReason);
+    }
+
+    private static void VerifyAnchors(StaticCorridor corridor, double startS, double endS)
+    {
+        Verification.NearlyEqual(startS, corridor.Stations[0].ReferenceS, "first station is exact requested start");
+        Verification.NearlyEqual(endS, corridor.Stations[corridor.Stations.Count - 1].ReferenceS,
+            "last station is exact requested end");
+    }
+
+    private static void VerifyAcceptedSamplesUseExactFootprints(DirectionSegmentView segment, StaticCorridor corridor,
+        PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration)
+    {
+        var checker = new FootprintCollisionChecker();
+        for (int stationIndex = 0; stationIndex < corridor.Stations.Count; stationIndex++)
+        {
+            LateralInterval station = corridor.Stations[stationIndex];
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, station.ReferenceS);
+            for (double l = station.MinimumL; l <= station.MaximumL + 1e-12d; l += configuration.LateralSampleSpacingMeters)
+            {
+                Verification.True(FrenetTransform.TryReconstruct(reference, l, 0d, 0.2d, out Pose2D pose),
+                    "accepted sample reconstructs");
+                Verification.True(checker.IsPoseCollisionFree(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters,
+                    out _), "accepted sample passes exact rotated footprint");
+            }
+        }
+    }
+
+    private static DirectionSegmentView CreateStraightSegment()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d),
+            Point(1d, 1d),
+            Point(2d, 2d),
+        };
+        return new DirectionSegmentView(0, TravelDirection.Forward, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
+    }
+
+    private static IReadOnlyList<FrenetProjection> CreateSeed(DirectionSegmentView segment, double l)
+    {
+        return new FrenetProjection[]
+        {
+            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0.2d), l, 0d, 0d),
+            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 1.8d), l, 0d, 0d),
+        };
+    }
+
+    private static SmoothedPathPoint Point(double x, double s)
+    {
+        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d,
+            false, SmoothedPathPointSource.Anchor);
+    }
+
+    private static VehicleParameters CreateVehicle()
+    {
+        return new VehicleParameters
+        {
+            LengthMeters = 0.10d,
+            WidthMeters = 0.10d,
+            SafetyMarginMeters = 0d,
+            MaximumCurvaturePerMeter = 1d,
+        };
+    }
+
+    private static PlanningGridMap CreateMap(IReadOnlyList<IMapObstacle> obstacles)
+    {
+        IMapObstacleSource[] sources = obstacles.Count == 0
+            ? Array.Empty<IMapObstacleSource>()
+            : new IMapObstacleSource[] { new ManualObstacleSource("corridor-test", 1L, true, obstacles) };
+        var result = new PlanningMapFactory().Create(new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
+            ResolutionMm = 20f,
+            ObstacleSources = sources,
+            AllowExplicitEmptyMap = obstacles.Count == 0,
+        });
+        Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
+            "corridor test map builds: " + result.FailureReason);
+        return result.Map!;
+    }
+
+    private static LateralInterval FindStation(StaticCorridor corridor, double referenceS)
+    {
+        for (int index = 0; index < corridor.Stations.Count; index++)
+        {
+            if (Math.Abs(corridor.Stations[index].ReferenceS - referenceS) <= 1e-12d)
+                return corridor.Stations[index];
+        }
+        throw new InvalidOperationException("Requested corridor station was not sampled.");
+    }
+}
@@ -6,29 +6,35 @@ internal static class Program
 {
    private static int Main(string[] args)
    {
-        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet"))
+        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
+            args[0] != "corridor" && args[0] != "all-foundation"))
        {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|all-foundation");
            return 2;
        }

        try
        {
-            if (args[0] == "foundation")
+            if (args[0] == "foundation" || args[0] == "all-foundation")
            {
                MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
                Console.WriteLine("PASS foundation");
            }
-            else if (args[0] == "segmentation")
+            if (args[0] == "segmentation" || args[0] == "all-foundation")
            {
                MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run();
                Console.WriteLine("PASS segmentation");
            }
-            else
+            if (args[0] == "frenet" || args[0] == "all-foundation")
            {
                MultiWheelC.TrajectoryPlanning.EMPlanner.FrenetChecks.Run();
                Console.WriteLine("PASS frenet");
            }
+            if (args[0] == "corridor" || args[0] == "all-foundation")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.CorridorChecks.Run();
+                Console.WriteLine("PASS corridor");
+            }
            return 0;
        }
        catch (Exception exception)