feat: add lateral optimization model
This commit is contained in:
梁薄云
@@ -0,0 +1,114 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>One discrete lateral iterate expressed against exact reference-S stations.</summary>
|
||||
public sealed class LateralCandidate
|
||||
{
|
||||
public LateralCandidate(IReadOnlyList<double> referenceStations, IReadOnlyList<double> l, IReadOnlyList<double> dl,
|
||||
IReadOnlyList<double> ddl, IReadOnlyList<double> 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<double> ReferenceStations { get; }
|
||||
|
||||
public IReadOnlyList<double> L { get; }
|
||||
|
||||
public IReadOnlyList<double> DL { get; }
|
||||
|
||||
public IReadOnlyList<double> DDL { get; }
|
||||
|
||||
public IReadOnlyList<double> DDDL { get; }
|
||||
|
||||
public static LateralCandidate Integrate(IReadOnlyList<double> referenceStations, double initialL, double initialDL,
|
||||
double initialDDL, IReadOnlyList<double> dddl)
|
||||
{
|
||||
IReadOnlyList<double> stations = CopyStations(referenceStations);
|
||||
if (!IsFinite(initialL) || !IsFinite(initialDL) || !IsFinite(initialDDL))
|
||||
throw new ArgumentOutOfRangeException(nameof(initialL));
|
||||
IReadOnlyList<double> 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<double> CopyStations(IReadOnlyList<double> values)
|
||||
{
|
||||
if (values == null || values.Count < 2)
|
||||
throw new ArgumentException("At least two reference-S stations are required.", nameof(values));
|
||||
|
||||
var copy = new List<double>(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<double>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> 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<double>(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<double>(copy);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable reconstructed lateral path, marked only after independent validation.</summary>
|
||||
public sealed class LateralPath
|
||||
{
|
||||
public LateralPath(IReadOnlyList<LateralPathPoint> points, bool independentlyValidated)
|
||||
{
|
||||
if (points == null)
|
||||
throw new ArgumentNullException(nameof(points));
|
||||
|
||||
var copy = new List<LateralPathPoint>(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<LateralPathPoint>(copy);
|
||||
IsIndependentlyValidated = independentlyValidated;
|
||||
}
|
||||
|
||||
public IReadOnlyList<LateralPathPoint> Points { get; }
|
||||
|
||||
public bool IsIndependentlyValidated { get; }
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable world-space sample reconstructed from one lateral candidate.</summary>
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
/// <summary>Immutable inputs for one lateral solve over a single direction segment.</summary>
|
||||
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<FrenetProjection> 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<double> ReferenceStations { get; }
|
||||
|
||||
public FrenetProjection StartProjection { get; }
|
||||
|
||||
public EmTerminalType TerminalType { get; }
|
||||
|
||||
public VehicleParameters Vehicle { get; }
|
||||
|
||||
public EmPlannerConfiguration Configuration { get; }
|
||||
|
||||
public IReadOnlyList<FrenetProjection> PreviousTrajectorySeed { get; }
|
||||
|
||||
private static StaticCorridor CopyCorridor(StaticCorridor source)
|
||||
{
|
||||
var stations = new List<LateralInterval>(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<double> CopyStationValues(IReadOnlyList<LateralInterval> stations)
|
||||
{
|
||||
if (stations.Count < 2)
|
||||
throw new ArgumentException("A lateral planning input requires at least two corridor stations.", nameof(stations));
|
||||
|
||||
var copy = new List<double>(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<double>(copy);
|
||||
}
|
||||
|
||||
private static void ValidateCorridorMatchesInput(DirectionSegmentView segment, IReadOnlyList<LateralInterval> 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<FrenetProjection> CopySeed(IReadOnlyList<FrenetProjection> seed,
|
||||
DirectionSegmentView segment)
|
||||
{
|
||||
var copy = new List<FrenetProjection>(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<FrenetProjection>(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,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Result of the lateral stage; only independently validated paths may be successful.</summary>
|
||||
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; }
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Deterministic variable ranges for one lateral QP discretized on N stations.</summary>
|
||||
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));
|
||||
}
|
||||
}
|
||||
@@ -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<FrenetProjection>()),
|
||||
"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<FrenetProjection>()),
|
||||
"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<FrenetProjection>()),
|
||||
"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<FrenetProjection>()),
|
||||
"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<LateralPathPoint>(), 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<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 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.");
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user