feat: build static EM lateral corridors

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/LateralInterval.cs

@@ -0,0 +1,29 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Connected free lateral interval at one exact reference-S station.</summary>
+public sealed class LateralInterval
+{
+    public LateralInterval(double referenceS, double minimumL, double maximumL, double seedL)
+    {
+        if (!IsFinite(referenceS) || !IsFinite(minimumL) || !IsFinite(maximumL) || !IsFinite(seedL) ||
+            minimumL > maximumL || seedL < minimumL - 1e-12d || seedL > maximumL + 1e-12d)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+
+        ReferenceS = referenceS;
+        MinimumL = minimumL;
+        MaximumL = maximumL;
+        SeedL = seedL;
+    }
+
+    public double ReferenceS { get; }
+    public double MinimumL { get; }
+    public double MaximumL { get; }
+    public double SeedL { get; }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridor.cs

@@ -0,0 +1,29 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable lateral hard bounds for one already-selected topological corridor.</summary>
+public sealed class StaticCorridor
+{
+    public StaticCorridor(IReadOnlyList<LateralInterval> stations)
+    {
+        if (stations == null || stations.Count == 0)
+            throw new ArgumentException("A static corridor requires stations.", nameof(stations));
+
+        var copy = new List<LateralInterval>(stations.Count);
+        double previousS = double.NegativeInfinity;
+        for (int index = 0; index < stations.Count; index++)
+        {
+            LateralInterval station = stations[index];
+            if (station == null || station.ReferenceS < previousS)
+                throw new ArgumentException("Static corridor stations must be non-null and sorted.", nameof(stations));
+            copy.Add(station);
+            previousS = station.ReferenceS;
+        }
+        Stations = new ReadOnlyCollection<LateralInterval>(copy);
+    }
+
+    public IReadOnlyList<LateralInterval> Stations { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridorBuilder.cs

@@ -0,0 +1,289 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Builds only the seed-connected static free-space corridor of a direction segment.</summary>
+public sealed class StaticCorridorBuilder
+{
+    private const double Epsilon = 1e-12d;
+    private const double ReconstructionDenominator = 1e-12d;
+
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    public StaticCorridorBuilder()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    public StaticCorridorBuilder(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    public bool TryBuild(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
+        IReadOnlyList<FrenetProjection> seed, PlanningGridMap map, VehicleParameters vehicle,
+        CorridorConfiguration configuration, out StaticCorridor corridor, out string failureReason)
+    {
+        corridor = null;
+        failureReason = string.Empty;
+        if (!TryValidateInput(segment, startReferenceS, endReferenceS, map, vehicle, configuration, out failureReason))
+            return false;
+        if (!TryReadSeeds(seed, segment, out List<SeedSample> seeds, out failureReason))
+            return false;
+
+        var stations = new List<LateralInterval>();
+        LateralInterval previous = null;
+        foreach (double referenceS in CreateStations(startReferenceS, endReferenceS,
+                     configuration.LongitudinalSampleSpacingMeters))
+        {
+            FrenetReferencePoint reference;
+            try
+            {
+                reference = ReferencePathInterpolator.Interpolate(segment, referenceS);
+            }
+            catch (ArgumentException)
+            {
+                failureReason = "Reference interpolation failed at S=" + referenceS + ".";
+                return false;
+            }
+
+            double seedL = GetSeedL(seeds, referenceS);
+            if (seedL < -configuration.MaximumLateralOffsetMeters - Epsilon ||
+                seedL > configuration.MaximumLateralOffsetMeters + Epsilon)
+            {
+                failureReason = "Seed lateral offset is outside configured bounds at S=" + referenceS + ".";
+                return false;
+            }
+
+            if (!TrySelectSeedConnectedInterval(reference, seedL, map, vehicle, configuration,
+                    out double minimumL, out double maximumL))
+            {
+                failureReason = "Seed-connected free interval disappeared at S=" + referenceS + ".";
+                return false;
+            }
+
+            var selected = new LateralInterval(referenceS, minimumL, maximumL, seedL);
+            if (previous != null && Math.Max(previous.MinimumL, selected.MinimumL) >
+                Math.Min(previous.MaximumL, selected.MaximumL) + Epsilon)
+            {
+                failureReason = "Seed-connected corridor loses overlap at S=" + referenceS + ".";
+                return false;
+            }
+
+            stations.Add(selected);
+            previous = selected;
+        }
+
+        corridor = new StaticCorridor(stations);
+        return true;
+    }
+
+    private static bool TryValidateInput(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
+        PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (segment == null || map == null || vehicle == null || configuration == null || !map.PlanningReady)
+        {
+            failureReason = "A planning-ready map, vehicle, segment, and corridor configuration are required.";
+            return false;
+        }
+        if (!IsFinite(startReferenceS) || !IsFinite(endReferenceS) || startReferenceS < 0d ||
+            endReferenceS < startReferenceS || endReferenceS > segment.LengthMeters + Epsilon)
+        {
+            failureReason = "Requested corridor S anchors are invalid.";
+            return false;
+        }
+        if (!IsPositiveFinite(configuration.LongitudinalSampleSpacingMeters) ||
+            !IsPositiveFinite(configuration.LateralSampleSpacingMeters) ||
+            !IsFinite(configuration.MaximumLateralOffsetMeters) || configuration.MaximumLateralOffsetMeters < 0d ||
+            !IsFinite(configuration.AdditionalClearanceReserveMeters) || configuration.AdditionalClearanceReserveMeters < 0d ||
+            !IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters))
+        {
+            failureReason = "Corridor configuration is invalid.";
+            return false;
+        }
+        if (!IsPositiveFinite(vehicle.LengthMeters) || !IsPositiveFinite(vehicle.WidthMeters) ||
+            !IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d)
+        {
+            failureReason = "Vehicle footprint dimensions are invalid.";
+            return false;
+        }
+        return true;
+    }
+
+    private static bool TryReadSeeds(IReadOnlyList<FrenetProjection> input, DirectionSegmentView segment,
+        out List<SeedSample> seeds, out string failureReason)
+    {
+        seeds = new List<SeedSample>();
+        failureReason = string.Empty;
+        if (input == null)
+            return true;
+
+        for (int index = 0; index < input.Count; index++)
+        {
+            FrenetProjection projection = input[index];
+            if (projection == null || projection.ReferencePoint == null ||
+                projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon ||
+                !IsFinite(projection.LateralOffset))
+            {
+                failureReason = "Corridor seeds must belong to the selected direction segment.";
+                return false;
+            }
+            seeds.Add(new SeedSample(projection.ReferenceS, projection.LateralOffset));
+        }
+        seeds.Sort((left, right) => left.ReferenceS.CompareTo(right.ReferenceS));
+        return true;
+    }
+
+    private static IEnumerable<double> CreateStations(double startReferenceS, double endReferenceS, double spacing)
+    {
+        yield return startReferenceS;
+        for (double candidate = startReferenceS + spacing; candidate < endReferenceS - Epsilon; candidate += spacing)
+            yield return candidate;
+        if (endReferenceS > startReferenceS + Epsilon)
+            yield return endReferenceS;
+    }
+
+    private bool TrySelectSeedConnectedInterval(FrenetReferencePoint reference, double seedL, PlanningGridMap map,
+        VehicleParameters vehicle, CorridorConfiguration configuration, out double minimumL, out double maximumL)
+    {
+        minimumL = 0d;
+        maximumL = 0d;
+        List<double> samples = CreateLateralSamples(seedL, configuration.MaximumLateralOffsetMeters,
+            configuration.LateralSampleSpacingMeters);
+        int index = 0;
+        while (index < samples.Count)
+        {
+            if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
+            {
+                index++;
+                continue;
+            }
+
+            double groupMinimum = samples[index];
+            double groupMaximum = samples[index];
+            bool containsSeed = Math.Abs(samples[index] - seedL) <= Epsilon;
+            index++;
+            while (index < samples.Count && samples[index] - groupMaximum <= configuration.LateralSampleSpacingMeters + Epsilon)
+            {
+                if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
+                {
+                    index++;
+                    break;
+                }
+                groupMaximum = samples[index];
+                containsSeed |= Math.Abs(samples[index] - seedL) <= Epsilon;
+                index++;
+            }
+
+            if (containsSeed)
+            {
+                minimumL = groupMinimum;
+                maximumL = groupMaximum;
+                return true;
+            }
+        }
+        return false;
+    }
+
+    private static List<double> CreateLateralSamples(double seedL, double maximumOffset, double spacing)
+    {
+        var samples = new List<double>();
+        for (double candidate = -maximumOffset; candidate < maximumOffset - Epsilon; candidate += spacing)
+            AddSortedUnique(samples, candidate);
+        AddSortedUnique(samples, maximumOffset);
+        AddSortedUnique(samples, -maximumOffset);
+        AddSortedUnique(samples, seedL);
+        samples.Sort();
+        return samples;
+    }
+
+    private bool IsCollisionFree(FrenetReferencePoint reference, double lateralOffset, PlanningGridMap map,
+        VehicleParameters vehicle, CorridorConfiguration configuration)
+    {
+        if (!FrenetTransform.TryReconstruct(reference, lateralOffset, 0d, ReconstructionDenominator, out Pose2D pose))
+            return false;
+
+        if (IsObviouslyClear(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters))
+            return true;
+        return _collisionChecker.IsPoseCollisionFree(pose, map, vehicle,
+            configuration.AdditionalClearanceReserveMeters, out _);
+    }
+
+    private static bool IsObviouslyClear(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
+        double additionalClearanceReserveMeters)
+    {
+        double totalMargin = vehicle.SafetyMarginMeters + additionalClearanceReserveMeters;
+        double halfLength = vehicle.LengthMeters / 2d + totalMargin;
+        double halfWidth = vehicle.WidthMeters / 2d + totalMargin;
+        double radius = Math.Sqrt(halfLength * halfLength + halfWidth * halfWidth);
+        if (!IsFinite(radius) || map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y) <= radius)
+            return false;
+
+        double cosine = Math.Cos(pose.Heading);
+        double sine = Math.Sin(pose.Heading);
+        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
+        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
+        {
+            double x = pose.X + longitudinalSign * halfLength * cosine - lateralSign * halfWidth * sine;
+            double y = pose.Y + longitudinalSign * halfLength * sine + lateralSign * halfWidth * cosine;
+            if (!map.TryWorldToGrid(x, y, out _, out _))
+                return false;
+        }
+        return true;
+    }
+
+    private static double GetSeedL(IReadOnlyList<SeedSample> seeds, double referenceS)
+    {
+        if (seeds.Count == 0)
+            return 0d;
+        if (referenceS <= seeds[0].ReferenceS)
+            return seeds[0].LateralOffset;
+        for (int index = 1; index < seeds.Count; index++)
+        {
+            SeedSample upper = seeds[index];
+            if (referenceS <= upper.ReferenceS)
+            {
+                SeedSample lower = seeds[index - 1];
+                double span = upper.ReferenceS - lower.ReferenceS;
+                return span <= Epsilon ? upper.LateralOffset : lower.LateralOffset +
+                    (upper.LateralOffset - lower.LateralOffset) * (referenceS - lower.ReferenceS) / span;
+            }
+        }
+        return seeds[seeds.Count - 1].LateralOffset;
+    }
+
+    private static void AddSortedUnique(List<double> samples, double value)
+    {
+        for (int index = 0; index < samples.Count; index++)
+            if (Math.Abs(samples[index] - value) <= Epsilon)
+                return;
+        samples.Add(value);
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    private sealed class SeedSample
+    {
+        public SeedSample(double referenceS, double lateralOffset)
+        {
+            ReferenceS = referenceS;
+            LateralOffset = lateralOffset;
+        }
+
+        public double ReferenceS { get; }
+        public double LateralOffset { get; }
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/CorridorChecks.cs

@@ -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.");
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs

@@ -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)