diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCandidate.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCandidate.cs
new file mode 100644
index 0000000..9fb4863
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCandidate.cs
@@ -0,0 +1,114 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// One discrete lateral iterate expressed against exact reference-S stations.
+public sealed class LateralCandidate
+{
+ public LateralCandidate(IReadOnlyList referenceStations, IReadOnlyList l, IReadOnlyList dl,
+ IReadOnlyList ddl, IReadOnlyList dddl)
+ {
+ ReferenceStations = CopyStations(referenceStations);
+ int stationCount = ReferenceStations.Count;
+ L = CopyValues(l, stationCount, nameof(l));
+ DL = CopyValues(dl, stationCount, nameof(dl));
+ DDL = CopyValues(ddl, stationCount, nameof(ddl));
+ DDDL = CopyValues(dddl, stationCount - 1, nameof(dddl));
+ }
+
+ public IReadOnlyList ReferenceStations { get; }
+
+ public IReadOnlyList L { get; }
+
+ public IReadOnlyList DL { get; }
+
+ public IReadOnlyList DDL { get; }
+
+ public IReadOnlyList DDDL { get; }
+
+ public static LateralCandidate Integrate(IReadOnlyList referenceStations, double initialL, double initialDL,
+ double initialDDL, IReadOnlyList dddl)
+ {
+ IReadOnlyList stations = CopyStations(referenceStations);
+ if (!IsFinite(initialL) || !IsFinite(initialDL) || !IsFinite(initialDDL))
+ throw new ArgumentOutOfRangeException(nameof(initialL));
+ IReadOnlyList copiedJerk = CopyValues(dddl, stations.Count - 1, nameof(dddl));
+
+ var l = new double[stations.Count];
+ var dl = new double[stations.Count];
+ var ddl = new double[stations.Count];
+ l[0] = initialL;
+ dl[0] = initialDL;
+ ddl[0] = initialDDL;
+ for (int index = 0; index < copiedJerk.Count; index++)
+ {
+ double ds = stations[index + 1] - stations[index];
+ double jerk = copiedJerk[index];
+ ddl[index + 1] = ddl[index] + ds * jerk;
+ dl[index + 1] = dl[index] + ds * ddl[index] + 0.5d * ds * ds * jerk;
+ l[index + 1] = l[index] + ds * dl[index] + 0.5d * ds * ds * ddl[index] +
+ ds * ds * ds * jerk / 6d;
+ }
+ return new LateralCandidate(stations, l, dl, ddl, copiedJerk);
+ }
+
+ public bool SatisfiesExactDiscreteDynamics(double tolerance)
+ {
+ if (!IsFinite(tolerance) || tolerance < 0d)
+ throw new ArgumentOutOfRangeException(nameof(tolerance));
+
+ for (int index = 0; index < DDDL.Count; index++)
+ {
+ double ds = ReferenceStations[index + 1] - ReferenceStations[index];
+ double jerk = DDDL[index];
+ if (Math.Abs(DDL[index + 1] - (DDL[index] + ds * jerk)) > tolerance ||
+ Math.Abs(DL[index + 1] - (DL[index] + ds * DDL[index] + 0.5d * ds * ds * jerk)) > tolerance ||
+ Math.Abs(L[index + 1] - (L[index] + ds * DL[index] + 0.5d * ds * ds * DDL[index] +
+ ds * ds * ds * jerk / 6d)) > tolerance)
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ private static IReadOnlyList CopyStations(IReadOnlyList values)
+ {
+ if (values == null || values.Count < 2)
+ throw new ArgumentException("At least two reference-S stations are required.", nameof(values));
+
+ var copy = new List(values.Count);
+ double previous = double.NegativeInfinity;
+ for (int index = 0; index < values.Count; index++)
+ {
+ double value = values[index];
+ if (!IsFinite(value) || value <= previous)
+ throw new ArgumentException("Reference-S stations must be finite and strictly increasing.", nameof(values));
+ copy.Add(value);
+ previous = value;
+ }
+ return new ReadOnlyCollection(copy);
+ }
+
+ private static IReadOnlyList CopyValues(IReadOnlyList values, int expectedCount, string parameterName)
+ {
+ if (values == null || values.Count != expectedCount)
+ throw new ArgumentException("Lateral value count does not match the station layout.", parameterName);
+
+ var copy = new List(values.Count);
+ for (int index = 0; index < values.Count; index++)
+ {
+ if (!IsFinite(values[index]))
+ throw new ArgumentOutOfRangeException(parameterName);
+ copy.Add(values[index]);
+ }
+ return new ReadOnlyCollection(copy);
+ }
+
+ private static bool IsFinite(double value)
+ {
+ return !double.IsNaN(value) && !double.IsInfinity(value);
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPath.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPath.cs
new file mode 100644
index 0000000..e093134
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPath.cs
@@ -0,0 +1,29 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Immutable reconstructed lateral path, marked only after independent validation.
+public sealed class LateralPath
+{
+ public LateralPath(IReadOnlyList points, bool independentlyValidated)
+ {
+ if (points == null)
+ throw new ArgumentNullException(nameof(points));
+
+ var copy = new List(points.Count);
+ for (int index = 0; index < points.Count; index++)
+ {
+ if (points[index] == null)
+ throw new ArgumentException("Lateral path points cannot contain null values.", nameof(points));
+ copy.Add(points[index]);
+ }
+ Points = new ReadOnlyCollection(copy);
+ IsIndependentlyValidated = independentlyValidated;
+ }
+
+ public IReadOnlyList Points { get; }
+
+ public bool IsIndependentlyValidated { get; }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPathPoint.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPathPoint.cs
new file mode 100644
index 0000000..49a127c
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPathPoint.cs
@@ -0,0 +1,61 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Immutable world-space sample reconstructed from one lateral candidate.
+public sealed class LateralPathPoint
+{
+ public LateralPathPoint(double referenceS, double pathS, double l, double dl, double ddl, double dddl,
+ double x, double y, double vehicleYaw, double geometricCurvature, double vehicleCurvature,
+ double vehicleCurvatureDerivative)
+ {
+ RequireFinite(referenceS, nameof(referenceS));
+ RequireFinite(pathS, nameof(pathS));
+ RequireFinite(l, nameof(l));
+ RequireFinite(dl, nameof(dl));
+ RequireFinite(ddl, nameof(ddl));
+ RequireFinite(dddl, nameof(dddl));
+ RequireFinite(x, nameof(x));
+ RequireFinite(y, nameof(y));
+ RequireFinite(vehicleYaw, nameof(vehicleYaw));
+ RequireFinite(geometricCurvature, nameof(geometricCurvature));
+ RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+ RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+ if (referenceS < 0d)
+ throw new ArgumentOutOfRangeException(nameof(referenceS));
+ if (pathS < 0d)
+ throw new ArgumentOutOfRangeException(nameof(pathS));
+
+ ReferenceS = referenceS;
+ PathS = pathS;
+ L = l;
+ DL = dl;
+ DDL = ddl;
+ DDDL = dddl;
+ X = x;
+ Y = y;
+ VehicleYaw = vehicleYaw;
+ GeometricCurvature = geometricCurvature;
+ VehicleCurvature = vehicleCurvature;
+ VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+ }
+
+ public double ReferenceS { get; }
+ public double PathS { get; }
+ public double L { get; }
+ public double DL { get; }
+ public double DDL { get; }
+ public double DDDL { get; }
+ public double X { get; }
+ public double Y { get; }
+ public double VehicleYaw { get; }
+ public double GeometricCurvature { get; }
+ public double VehicleCurvature { get; }
+ public double VehicleCurvatureDerivative { get; }
+
+ private static void RequireFinite(double value, string parameterName)
+ {
+ if (double.IsNaN(value) || double.IsInfinity(value))
+ throw new ArgumentOutOfRangeException(parameterName);
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningInput.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningInput.cs
new file mode 100644
index 0000000..ceb0de7
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningInput.cs
@@ -0,0 +1,140 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Immutable inputs for one lateral solve over a single direction segment.
+public sealed class LateralPlanningInput
+{
+ private const double Epsilon = 1e-12d;
+
+ public LateralPlanningInput(DirectionSegmentView referenceSegment, StaticCorridor corridor,
+ FrenetProjection startProjection, EmTerminalType terminalType, VehicleParameters vehicle,
+ EmPlannerConfiguration configuration, IReadOnlyList previousTrajectorySeed)
+ {
+ if (referenceSegment == null)
+ throw new ArgumentNullException(nameof(referenceSegment));
+ if (corridor == null)
+ throw new ArgumentNullException(nameof(corridor));
+ if (startProjection == null)
+ throw new ArgumentNullException(nameof(startProjection));
+ if (vehicle == null)
+ throw new ArgumentNullException(nameof(vehicle));
+ if (configuration == null)
+ throw new ArgumentNullException(nameof(configuration));
+ if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+ throw new ArgumentOutOfRangeException(nameof(terminalType));
+
+ ReferenceSegment = referenceSegment;
+ Corridor = CopyCorridor(corridor);
+ ReferenceStations = CopyStationValues(Corridor.Stations);
+ ValidateCorridorMatchesInput(referenceSegment, Corridor.Stations, startProjection);
+ StartProjection = startProjection;
+ TerminalType = terminalType;
+ Vehicle = CopyVehicle(vehicle);
+ Configuration = configuration.Copy();
+ PreviousTrajectorySeed = CopySeed(previousTrajectorySeed, referenceSegment);
+ }
+
+ public DirectionSegmentView ReferenceSegment { get; }
+
+ public StaticCorridor Corridor { get; }
+
+ public IReadOnlyList ReferenceStations { get; }
+
+ public FrenetProjection StartProjection { get; }
+
+ public EmTerminalType TerminalType { get; }
+
+ public VehicleParameters Vehicle { get; }
+
+ public EmPlannerConfiguration Configuration { get; }
+
+ public IReadOnlyList PreviousTrajectorySeed { get; }
+
+ private static StaticCorridor CopyCorridor(StaticCorridor source)
+ {
+ var stations = new List(source.Stations.Count);
+ for (int index = 0; index < source.Stations.Count; index++)
+ {
+ LateralInterval station = source.Stations[index];
+ if (station == null)
+ throw new ArgumentException("Corridor stations cannot be null.", nameof(source));
+ stations.Add(new LateralInterval(station.ReferenceS, station.MinimumL, station.MaximumL, station.SeedL));
+ }
+ return new StaticCorridor(stations);
+ }
+
+ private static IReadOnlyList CopyStationValues(IReadOnlyList stations)
+ {
+ if (stations.Count < 2)
+ throw new ArgumentException("A lateral planning input requires at least two corridor stations.", nameof(stations));
+
+ var copy = new List(stations.Count);
+ double previous = double.NegativeInfinity;
+ for (int index = 0; index < stations.Count; index++)
+ {
+ double referenceS = stations[index].ReferenceS;
+ if (referenceS <= previous)
+ throw new ArgumentException("Corridor reference-S stations must be strictly increasing.", nameof(stations));
+ copy.Add(referenceS);
+ previous = referenceS;
+ }
+ return new ReadOnlyCollection(copy);
+ }
+
+ private static void ValidateCorridorMatchesInput(DirectionSegmentView segment, IReadOnlyList stations,
+ FrenetProjection start)
+ {
+ if (start.ReferencePoint == null || start.ReferencePoint.Direction != segment.Direction ||
+ start.ReferenceS < -Epsilon || start.ReferenceS > segment.LengthMeters + Epsilon)
+ {
+ throw new ArgumentException("Start projection must belong to the selected direction segment.", nameof(start));
+ }
+ if (Math.Abs(stations[0].ReferenceS - start.ReferenceS) > Epsilon)
+ throw new ArgumentException("The first corridor station must match the start projection reference S.", nameof(stations));
+ if (start.LateralOffset < stations[0].MinimumL - Epsilon || start.LateralOffset > stations[0].MaximumL + Epsilon)
+ throw new ArgumentException("Start projection is outside the first hard corridor interval.", nameof(start));
+
+ for (int index = 0; index < stations.Count; index++)
+ {
+ if (stations[index].ReferenceS < -Epsilon || stations[index].ReferenceS > segment.LengthMeters + Epsilon)
+ throw new ArgumentException("Corridor stations must lie inside the selected direction segment.", nameof(stations));
+ }
+ }
+
+ private static IReadOnlyList CopySeed(IReadOnlyList seed,
+ DirectionSegmentView segment)
+ {
+ var copy = new List(seed == null ? 0 : seed.Count);
+ if (seed != null)
+ {
+ for (int index = 0; index < seed.Count; index++)
+ {
+ FrenetProjection projection = seed[index];
+ if (projection == null || projection.ReferencePoint.Direction != segment.Direction ||
+ projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon)
+ {
+ throw new ArgumentException("Previous lateral seed must belong to the selected direction segment.", nameof(seed));
+ }
+ copy.Add(projection);
+ }
+ }
+ return new ReadOnlyCollection(copy);
+ }
+
+ private static VehicleParameters CopyVehicle(VehicleParameters vehicle)
+ {
+ return new VehicleParameters
+ {
+ LengthMeters = vehicle.LengthMeters,
+ WidthMeters = vehicle.WidthMeters,
+ SafetyMarginMeters = vehicle.SafetyMarginMeters,
+ MaximumCurvaturePerMeter = vehicle.MaximumCurvaturePerMeter,
+ MinimumTurningRadiusMeters = vehicle.MinimumTurningRadiusMeters,
+ };
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningResult.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningResult.cs
new file mode 100644
index 0000000..c1259e3
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningResult.cs
@@ -0,0 +1,29 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Result of the lateral stage; only independently validated paths may be successful.
+public sealed class LateralPlanningResult
+{
+ public LateralPlanningResult(EmPlanningStatus status, LateralPath path, string failureReason)
+ {
+ if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
+ throw new ArgumentOutOfRangeException(nameof(status));
+
+ bool successful = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+ if (successful && (path == null || path.Points.Count == 0 || !path.IsIndependentlyValidated))
+ throw new ArgumentException("Successful lateral results require a non-empty independently validated path.", nameof(path));
+ if (!successful && path != null)
+ throw new ArgumentException("Failed lateral results cannot contain a path.", nameof(path));
+
+ Status = status;
+ Path = path;
+ FailureReason = failureReason ?? string.Empty;
+ }
+
+ public EmPlanningStatus Status { get; }
+
+ public LateralPath Path { get; }
+
+ public string FailureReason { get; }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralVariableLayout.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralVariableLayout.cs
new file mode 100644
index 0000000..7ef1f44
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralVariableLayout.cs
@@ -0,0 +1,63 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Deterministic variable ranges for one lateral QP discretized on N stations.
+public sealed class LateralVariableLayout
+{
+ public LateralVariableLayout(int stationCount)
+ {
+ if (stationCount < 2)
+ throw new ArgumentOutOfRangeException(nameof(stationCount), "At least two reference-S stations are required.");
+
+ StationCount = stationCount;
+ LStart = 0;
+ DLStart = stationCount;
+ DDLStart = 2 * stationCount;
+ DDDLStart = 3 * stationCount;
+ VariableCount = 4 * stationCount - 1;
+ }
+
+ public int StationCount { get; }
+
+ public int LStart { get; }
+
+ public int DLStart { get; }
+
+ public int DDLStart { get; }
+
+ public int DDDLStart { get; }
+
+ public int VariableCount { get; }
+
+ public int L(int stationIndex)
+ {
+ RequireStationIndex(stationIndex);
+ return LStart + stationIndex;
+ }
+
+ public int DL(int stationIndex)
+ {
+ RequireStationIndex(stationIndex);
+ return DLStart + stationIndex;
+ }
+
+ public int DDL(int stationIndex)
+ {
+ RequireStationIndex(stationIndex);
+ return DDLStart + stationIndex;
+ }
+
+ public int DDDL(int intervalIndex)
+ {
+ if (intervalIndex < 0 || intervalIndex >= StationCount - 1)
+ throw new ArgumentOutOfRangeException(nameof(intervalIndex));
+ return DDDLStart + intervalIndex;
+ }
+
+ private void RequireStationIndex(int stationIndex)
+ {
+ if (stationIndex < 0 || stationIndex >= StationCount)
+ throw new ArgumentOutOfRangeException(nameof(stationIndex));
+ }
+}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs
new file mode 100644
index 0000000..40d8130
--- /dev/null
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.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.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class LateralModelChecks
+{
+ public static void Run()
+ {
+ VerifiesDeterministicVariableLayout();
+ VerifiesExactDiscreteDynamicsForUnequalStations();
+ VerifiesPlanningInputBoundariesAndDefensiveCopies();
+ VerifiesLateralResultPublicationContract();
+ }
+
+ private static void VerifiesDeterministicVariableLayout()
+ {
+ LateralVariableLayout layout = CreateLayout(4);
+
+ Verification.Equal(15, layout.VariableCount, "layout variable count");
+ for (int index = 0; index < 4; index++)
+ {
+ Verification.Equal(index, layout.L(index), "l index " + index);
+ Verification.Equal(4 + index, layout.DL(index), "dl index " + index);
+ Verification.Equal(8 + index, layout.DDL(index), "ddl index " + index);
+ }
+ for (int index = 0; index < 3; index++)
+ Verification.Equal(12 + index, layout.DDDL(index), "dddl index " + index);
+
+ ExpectArgumentException(() => CreateLayout(1), "layout rejects fewer than two stations");
+ ExpectArgumentException(() => layout.L(4), "l index bounds check");
+ ExpectArgumentException(() => layout.DL(-1), "dl index bounds check");
+ ExpectArgumentException(() => layout.DDL(4), "ddl index bounds check");
+ ExpectArgumentException(() => layout.DDDL(3), "dddl index bounds check");
+ }
+
+ private static void VerifiesExactDiscreteDynamicsForUnequalStations()
+ {
+ double[] stations = { 0d, 0.4d, 1.25d, 2.5d };
+ double[] jerks = { 0.5d, -0.3d, 0.2d };
+ LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, -0.2d, 0.3d, jerks);
+
+ for (int index = 0; index < jerks.Length; index++)
+ {
+ double ds = stations[index + 1] - stations[index];
+ Verification.NearlyEqual(candidate.DDL[index] + ds * candidate.DDDL[index], candidate.DDL[index + 1],
+ "exact ddl integration " + index);
+ Verification.NearlyEqual(candidate.DL[index] + ds * candidate.DDL[index] + 0.5d * ds * ds * candidate.DDDL[index],
+ candidate.DL[index + 1], "exact dl integration " + index);
+ Verification.NearlyEqual(candidate.L[index] + ds * candidate.DL[index] + 0.5d * ds * ds * candidate.DDL[index] +
+ ds * ds * ds * candidate.DDDL[index] / 6d, candidate.L[index + 1], "exact l integration " + index);
+ }
+ Verification.True(candidate.SatisfiesExactDiscreteDynamics(1e-12d), "integrated candidate validates exact dynamics");
+
+ LateralCandidate inconsistent = new LateralCandidate(new[] { 0d, 1d }, new[] { 0d, 1d },
+ new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d });
+ Verification.True(!inconsistent.SatisfiesExactDiscreteDynamics(1e-12d), "candidate detects inconsistent dynamics");
+ ExpectArgumentException(() => new LateralCandidate(new[] { 0d, 0d }, new[] { 0d, 0d },
+ new[] { 0d, 0d }, new[] { 0d, 0d }, new[] { 0d }), "candidate rejects non-increasing stations");
+ }
+
+ private static void VerifiesPlanningInputBoundariesAndDefensiveCopies()
+ {
+ DirectionSegmentView segment = CreateStraightSegment();
+ var corridorStations = new[]
+ {
+ new LateralInterval(0d, -0.3d, 0.3d, 0d),
+ new LateralInterval(1d, -0.3d, 0.3d, 0d),
+ new LateralInterval(2d, -0.3d, 0.3d, 0d),
+ };
+ var seeds = new[]
+ {
+ new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
+ };
+ LateralPlanningInput input = new LateralPlanningInput(segment, new StaticCorridor(corridorStations), seeds[0],
+ EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), seeds);
+ corridorStations[1] = new LateralInterval(1d, -0.1d, 0.1d, 0d);
+ seeds[0] = new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d);
+
+ Verification.NearlyEqual(-0.3d, input.Corridor.Stations[1].MinimumL, "input copies corridor stations");
+ Verification.NearlyEqual(0d, input.PreviousTrajectorySeed[0].LateralOffset, "input copies seed list");
+ ExpectArgumentException(() => new LateralPlanningInput(segment,
+ new StaticCorridor(new[] { new LateralInterval(0d, -0.3d, 0.3d, 0d) }),
+ input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty()),
+ "input rejects fewer than two stations");
+ ExpectArgumentException(() => new LateralPlanningInput(segment,
+ new StaticCorridor(new[]
+ {
+ new LateralInterval(0d, -0.3d, 0.3d, 0d),
+ new LateralInterval(0d, -0.3d, 0.3d, 0d),
+ }), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty()),
+ "input rejects non-increasing corridor stations");
+ ExpectArgumentException(() => new LateralPlanningInput(segment,
+ new StaticCorridor(new[]
+ {
+ new LateralInterval(0.1d, -0.3d, 0.3d, 0d),
+ new LateralInterval(2d, -0.3d, 0.3d, 0d),
+ }), input.StartProjection, EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty()),
+ "input rejects start-corridor station mismatch");
+ ExpectArgumentException(() => new LateralPlanningInput(segment,
+ new StaticCorridor(new[]
+ {
+ new LateralInterval(0d, -0.1d, 0.1d, 0d),
+ new LateralInterval(2d, -0.1d, 0.1d, 0d),
+ }), new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0.2d, 0d, 0d),
+ EmTerminalType.Goal, CreateVehicle(), EmPlannerConfiguration.CreateDefault(), Array.Empty()),
+ "input rejects start projection outside first hard interval");
+ }
+
+ private static void VerifiesLateralResultPublicationContract()
+ {
+ LateralPath unvalidated = new LateralPath(new[] { CreatePathPoint(0d) }, false);
+ LateralPath validated = new LateralPath(new[] { CreatePathPoint(0d), CreatePathPoint(1d) }, true);
+
+ ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.Success, unvalidated, string.Empty),
+ "success requires an independently validated path");
+ ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback,
+ new LateralPath(Array.Empty(), true), string.Empty),
+ "fallback requires a non-empty path");
+ ExpectArgumentException(() => new LateralPlanningResult(EmPlanningStatus.LateralInfeasible, validated, string.Empty),
+ "failed result has no candidate");
+ LateralPlanningResult result = new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, validated, "fallback");
+ Verification.Equal(validated, result.Path, "fallback path is preserved");
+ }
+
+ private static DirectionSegmentView CreateStraightSegment()
+ {
+ var points = new List
+ {
+ 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 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.1d,
+ WidthMeters = 0.1d,
+ SafetyMarginMeters = 0d,
+ MaximumCurvaturePerMeter = 1d,
+ };
+ }
+
+ private static LateralPathPoint CreatePathPoint(double referenceS)
+ {
+ return new LateralPathPoint(referenceS, referenceS, 0d, 0d, 0d, 0d, referenceS, 0d, 0d, 0d, 0d, 0d);
+ }
+
+ private static LateralVariableLayout CreateLayout(int stationCount)
+ {
+ return new LateralVariableLayout(stationCount);
+ }
+
+ private static void ExpectArgumentException(Action action, string name)
+ {
+ try
+ {
+ action();
+ }
+ catch (ArgumentException)
+ {
+ return;
+ }
+ throw new InvalidOperationException(name + " did not throw ArgumentException.");
+ }
+}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
index 1915e73..4dbd108 100644
--- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
@@ -8,9 +8,9 @@ internal static class Program
{
if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
- args[0] != "all-foundation"))
+ args[0] != "all-foundation" && args[0] != "lateral-model"))
{
- Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation");
+ Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model");
return 2;
}
@@ -50,6 +50,11 @@ internal static class Program
{
MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.RunProbe();
}
+ if (args[0] == "lateral-model")
+ {
+ MultiWheelC.TrajectoryPlanning.EMPlanner.LateralModelChecks.Run();
+ Console.WriteLine("PASS lateral-model");
+ }
return 0;
}
catch (Exception exception)