feat: validate lateral path geometry
This commit is contained in:
梁薄云
@@ -0,0 +1,201 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Reconstructs world geometry and actual path arc length from a lateral candidate.</summary>
|
||||
public sealed class LateralGeometryEvaluator
|
||||
{
|
||||
public bool TryEvaluate(LateralPlanningInput input, LateralCandidate candidate, out LateralPath path,
|
||||
out string failureReason)
|
||||
{
|
||||
path = null;
|
||||
failureReason = string.Empty;
|
||||
if (!HasMatchingStations(input, candidate, out failureReason))
|
||||
return false;
|
||||
|
||||
try
|
||||
{
|
||||
List<GeometrySample> samples = Reconstruct(input, candidate, out failureReason);
|
||||
if (samples == null)
|
||||
return false;
|
||||
CalculateActualPathSAndCurvatureDerivative(samples, out failureReason);
|
||||
if (failureReason.Length != 0)
|
||||
return false;
|
||||
|
||||
var points = new List<LateralPathPoint>(samples.Count);
|
||||
for (int index = 0; index < samples.Count; index++)
|
||||
{
|
||||
GeometrySample sample = samples[index];
|
||||
double dddl = candidate.DDDL[Math.Min(index, candidate.DDDL.Count - 1)];
|
||||
points.Add(new LateralPathPoint(sample.ReferenceS, sample.PathS, sample.L, sample.DL, sample.DDL, dddl,
|
||||
sample.X, sample.Y, sample.VehicleYaw, sample.GeometricCurvature, sample.VehicleCurvature,
|
||||
sample.VehicleCurvatureDerivative));
|
||||
}
|
||||
path = new LateralPath(points, false);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static List<GeometrySample> Reconstruct(LateralPlanningInput input, LateralCandidate candidate,
|
||||
out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
double minimumDenominator = input.Configuration.Frenet.MinimumFrenetDenominator;
|
||||
if (!IsFinite(minimumDenominator) || minimumDenominator <= 0d)
|
||||
{
|
||||
failureReason = "The minimum Frenet denominator is invalid.";
|
||||
return null;
|
||||
}
|
||||
|
||||
double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
var samples = new List<GeometrySample>(candidate.ReferenceStations.Count);
|
||||
for (int index = 0; index < candidate.ReferenceStations.Count; index++)
|
||||
{
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
|
||||
candidate.ReferenceStations[index]);
|
||||
double l = candidate.L[index];
|
||||
double dl = candidate.DL[index];
|
||||
double ddl = candidate.DDL[index];
|
||||
double denominator = 1d - reference.GeometricCurvature * l;
|
||||
if (!IsFinite(denominator) || denominator < minimumDenominator)
|
||||
{
|
||||
failureReason = "Frenet denominator is below the hard minimum at station " + index + ".";
|
||||
return null;
|
||||
}
|
||||
|
||||
double travelYaw = reference.TravelYaw + Math.Atan2(dl, denominator);
|
||||
double vehicleYaw = input.ReferenceSegment.Direction == TravelDirection.Forward
|
||||
? AngleMath.NormalizeRadians(travelYaw)
|
||||
: AngleMath.NormalizeRadians(travelYaw + Math.PI);
|
||||
double x = reference.X - l * Math.Sin(reference.TravelYaw);
|
||||
double y = reference.Y + l * Math.Cos(reference.TravelYaw);
|
||||
double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
|
||||
directionSign * reference.VehicleCurvatureDerivative);
|
||||
double vehicleCurvature = directionSign * geometricCurvature;
|
||||
if (!IsFinite(travelYaw) || !IsFinite(vehicleYaw) || !IsFinite(x) || !IsFinite(y) ||
|
||||
!IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature))
|
||||
{
|
||||
failureReason = "Reconstructed lateral geometry is non-finite at station " + index + ".";
|
||||
return null;
|
||||
}
|
||||
samples.Add(new GeometrySample(candidate.ReferenceStations[index], l, dl, ddl, x, y, travelYaw, vehicleYaw,
|
||||
geometricCurvature, vehicleCurvature));
|
||||
}
|
||||
return samples;
|
||||
}
|
||||
|
||||
private static void CalculateActualPathSAndCurvatureDerivative(IReadOnlyList<GeometrySample> samples,
|
||||
out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
samples[0].PathS = 0d;
|
||||
for (int index = 1; index < samples.Count; index++)
|
||||
{
|
||||
double dx = samples[index].X - samples[index - 1].X;
|
||||
double dy = samples[index].Y - samples[index - 1].Y;
|
||||
double chord = Math.Sqrt(dx * dx + dy * dy);
|
||||
if (!IsFinite(chord) || chord <= 0d)
|
||||
{
|
||||
failureReason = "Reconstructed path S is not strictly increasing at station " + index + ".";
|
||||
return;
|
||||
}
|
||||
samples[index].PathS = samples[index - 1].PathS + chord;
|
||||
}
|
||||
for (int index = 0; index < samples.Count; index++)
|
||||
{
|
||||
int lower = index == 0 ? 0 : index - 1;
|
||||
int upper = index == samples.Count - 1 ? samples.Count - 1 : index + 1;
|
||||
double span = samples[upper].PathS - samples[lower].PathS;
|
||||
if (!IsFinite(span) || span <= 0d)
|
||||
{
|
||||
failureReason = "Path-S curvature derivative span is invalid at station " + index + ".";
|
||||
return;
|
||||
}
|
||||
double derivative = (samples[upper].VehicleCurvature - samples[lower].VehicleCurvature) / span;
|
||||
if (!IsFinite(derivative))
|
||||
{
|
||||
failureReason = "Vehicle curvature derivative is non-finite at station " + index + ".";
|
||||
return;
|
||||
}
|
||||
samples[index].VehicleCurvatureDerivative = derivative;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool HasMatchingStations(LateralPlanningInput input, LateralCandidate candidate, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
if (input == null || candidate == null)
|
||||
{
|
||||
failureReason = "Lateral input and candidate are required.";
|
||||
return false;
|
||||
}
|
||||
if (candidate.ReferenceStations.Count != input.ReferenceStations.Count)
|
||||
{
|
||||
failureReason = "Candidate station count does not match the lateral input.";
|
||||
return false;
|
||||
}
|
||||
for (int index = 0; index < input.ReferenceStations.Count; index++)
|
||||
{
|
||||
if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > 1e-12d)
|
||||
{
|
||||
failureReason = "Candidate stations do not match the lateral input.";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
internal static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
|
||||
double referenceCurvatureDerivative)
|
||||
{
|
||||
double a = 1d - reference.GeometricCurvature * l;
|
||||
double denominatorSquared = a * a + dl * dl;
|
||||
double numerator = a * a * reference.GeometricCurvature + a * ddl +
|
||||
referenceCurvatureDerivative * l * dl + 2d * reference.GeometricCurvature * dl * dl;
|
||||
return numerator / (denominatorSquared * Math.Sqrt(denominatorSquared));
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private sealed class GeometrySample
|
||||
{
|
||||
public GeometrySample(double referenceS, double l, double dl, double ddl, double x, double y, double travelYaw,
|
||||
double vehicleYaw, double geometricCurvature, double vehicleCurvature)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
L = l;
|
||||
DL = dl;
|
||||
DDL = ddl;
|
||||
X = x;
|
||||
Y = y;
|
||||
TravelYaw = travelYaw;
|
||||
VehicleYaw = vehicleYaw;
|
||||
GeometricCurvature = geometricCurvature;
|
||||
VehicleCurvature = vehicleCurvature;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double L { get; }
|
||||
public double DL { get; }
|
||||
public double DDL { get; }
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double TravelYaw { get; }
|
||||
public double VehicleYaw { get; }
|
||||
public double GeometricCurvature { get; }
|
||||
public double VehicleCurvature { get; }
|
||||
public double PathS { get; set; }
|
||||
public double VehicleCurvatureDerivative { get; set; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,318 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Independently recomputes and verifies lateral world geometry before a path may be published.</summary>
|
||||
public sealed class LateralSolutionValidator
|
||||
{
|
||||
public bool TryValidate(LateralPlanningInput input, LateralCandidate candidate, LateralPath path,
|
||||
out LateralPath validatedPath, out string failureReason)
|
||||
{
|
||||
validatedPath = null;
|
||||
failureReason = string.Empty;
|
||||
if (input == null || candidate == null || path == null)
|
||||
{
|
||||
failureReason = "Lateral input, candidate, and reconstructed path are required.";
|
||||
return false;
|
||||
}
|
||||
if (path.Points.Count != input.ReferenceStations.Count || candidate.ReferenceStations.Count != path.Points.Count)
|
||||
{
|
||||
failureReason = "Lateral path point count does not match the candidate stations.";
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
double spatialTolerance = RequireNonnegative(input.Configuration.Validation.SpatialToleranceMeters,
|
||||
"spatial tolerance");
|
||||
double kinematicTolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance,
|
||||
"kinematic tolerance");
|
||||
double residualTolerance = RequireNonnegative(input.Configuration.Solver.StrictResidualTolerance,
|
||||
"strict residual tolerance");
|
||||
if (!ValidateCandidateConstraints(input, candidate, spatialTolerance, kinematicTolerance, residualTolerance,
|
||||
out failureReason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
List<ExpectedSample> expected = ReconstructIndependently(input, candidate, out failureReason);
|
||||
if (expected == null)
|
||||
return false;
|
||||
if (!ComparePath(path, candidate, input.ReferenceStations, expected, spatialTolerance, kinematicTolerance,
|
||||
out failureReason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
validatedPath = new LateralPath(path.Points, true);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool ValidateCandidateConstraints(LateralPlanningInput input, LateralCandidate candidate,
|
||||
double spatialTolerance, double kinematicTolerance, double residualTolerance, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
if (candidate.ReferenceStations.Count != input.ReferenceStations.Count)
|
||||
{
|
||||
failureReason = "Candidate station count does not match the input.";
|
||||
return false;
|
||||
}
|
||||
if (!candidate.SatisfiesExactDiscreteDynamics(residualTolerance))
|
||||
{
|
||||
failureReason = "Candidate violates exact lateral dynamics.";
|
||||
return false;
|
||||
}
|
||||
|
||||
LateralConfiguration lateral = input.Configuration.Lateral;
|
||||
double maximumCurvature = GetMaximumVehicleCurvature(input.Vehicle);
|
||||
for (int index = 0; index < input.ReferenceStations.Count; index++)
|
||||
{
|
||||
if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > spatialTolerance)
|
||||
{
|
||||
failureReason = "Candidate reference-S does not match the input at station " + index + ".";
|
||||
return false;
|
||||
}
|
||||
LateralInterval corridor = input.Corridor.Stations[index];
|
||||
double l = candidate.L[index];
|
||||
double dl = candidate.DL[index];
|
||||
double ddl = candidate.DDL[index];
|
||||
if (!IsFinite(l) || !IsFinite(dl) || !IsFinite(ddl) || l < corridor.MinimumL - spatialTolerance ||
|
||||
l > corridor.MaximumL + spatialTolerance || Math.Abs(l) > input.Configuration.Corridor.MaximumLateralOffsetMeters + spatialTolerance ||
|
||||
Math.Abs(dl) > lateral.MaximumLateralSlope + kinematicTolerance ||
|
||||
Math.Abs(ddl) > lateral.MaximumLateralSecondDerivativePerMeter + kinematicTolerance)
|
||||
{
|
||||
failureReason = "Candidate violates lateral corridor or derivative limits at station " + index + ".";
|
||||
return false;
|
||||
}
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
|
||||
input.ReferenceStations[index]);
|
||||
double denominator = 1d - reference.GeometricCurvature * l;
|
||||
if (!IsFinite(denominator) || denominator < input.Configuration.Frenet.MinimumFrenetDenominator - kinematicTolerance)
|
||||
{
|
||||
failureReason = "Candidate violates the Frenet denominator at station " + index + ".";
|
||||
return false;
|
||||
}
|
||||
double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
|
||||
(input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d) * reference.VehicleCurvatureDerivative);
|
||||
double vehicleCurvature = (input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d) *
|
||||
geometricCurvature;
|
||||
if (!IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature) ||
|
||||
Math.Abs(vehicleCurvature) > maximumCurvature + kinematicTolerance)
|
||||
{
|
||||
failureReason = "Candidate violates vehicle curvature at station " + index + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for (int index = 0; index < candidate.DDDL.Count; index++)
|
||||
{
|
||||
if (!IsFinite(candidate.DDDL[index]) ||
|
||||
Math.Abs(candidate.DDDL[index]) > lateral.MaximumLateralThirdDerivativePerSquareMeter + kinematicTolerance)
|
||||
{
|
||||
failureReason = "Candidate violates third-derivative limits at interval " + index + ".";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
double startDenominator = 1d - input.StartProjection.ReferencePoint.GeometricCurvature *
|
||||
input.StartProjection.LateralOffset;
|
||||
double expectedStartDL = startDenominator * Math.Tan(input.StartProjection.HeadingError);
|
||||
if (!IsFinite(expectedStartDL) || Math.Abs(candidate.L[0] - input.StartProjection.LateralOffset) > spatialTolerance ||
|
||||
Math.Abs(candidate.DL[0] - expectedStartDL) > kinematicTolerance)
|
||||
{
|
||||
failureReason = "Candidate violates the lateral start state.";
|
||||
return false;
|
||||
}
|
||||
if (input.TerminalType != EmTerminalType.RollingSafetyStop &&
|
||||
(Math.Abs(candidate.L[candidate.L.Count - 1]) > spatialTolerance ||
|
||||
Math.Abs(candidate.DL[candidate.DL.Count - 1]) > kinematicTolerance))
|
||||
{
|
||||
failureReason = "Candidate violates the exact terminal lateral state.";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static List<ExpectedSample> ReconstructIndependently(LateralPlanningInput input, LateralCandidate candidate,
|
||||
out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
double minimumDenominator = input.Configuration.Frenet.MinimumFrenetDenominator;
|
||||
double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
var samples = new List<ExpectedSample>(candidate.ReferenceStations.Count);
|
||||
for (int index = 0; index < candidate.ReferenceStations.Count; index++)
|
||||
{
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
|
||||
candidate.ReferenceStations[index]);
|
||||
double l = candidate.L[index];
|
||||
double dl = candidate.DL[index];
|
||||
double ddl = candidate.DDL[index];
|
||||
double denominator = 1d - reference.GeometricCurvature * l;
|
||||
if (!IsFinite(denominator) || denominator < minimumDenominator)
|
||||
{
|
||||
failureReason = "Independent reconstruction found a Frenet denominator violation at station " + index + ".";
|
||||
return null;
|
||||
}
|
||||
double travelYaw = reference.TravelYaw + Math.Atan2(dl, denominator);
|
||||
double vehicleYaw = input.ReferenceSegment.Direction == TravelDirection.Forward
|
||||
? AngleMath.NormalizeRadians(travelYaw)
|
||||
: AngleMath.NormalizeRadians(travelYaw + Math.PI);
|
||||
double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
|
||||
directionSign * reference.VehicleCurvatureDerivative);
|
||||
double vehicleCurvature = directionSign * geometricCurvature;
|
||||
double x = reference.X - l * Math.Sin(reference.TravelYaw);
|
||||
double y = reference.Y + l * Math.Cos(reference.TravelYaw);
|
||||
if (!IsFinite(x) || !IsFinite(y) || !IsFinite(travelYaw) || !IsFinite(vehicleYaw) ||
|
||||
!IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature))
|
||||
{
|
||||
failureReason = "Independent reconstruction produced non-finite geometry at station " + index + ".";
|
||||
return null;
|
||||
}
|
||||
samples.Add(new ExpectedSample(candidate.ReferenceStations[index], x, y, vehicleYaw, geometricCurvature,
|
||||
vehicleCurvature));
|
||||
}
|
||||
|
||||
samples[0].PathS = 0d;
|
||||
for (int index = 1; index < samples.Count; index++)
|
||||
{
|
||||
double dx = samples[index].X - samples[index - 1].X;
|
||||
double dy = samples[index].Y - samples[index - 1].Y;
|
||||
double chord = Math.Sqrt(dx * dx + dy * dy);
|
||||
if (!IsFinite(chord) || chord <= 0d)
|
||||
{
|
||||
failureReason = "Independent reconstruction found non-increasing PathS at station " + index + ".";
|
||||
return null;
|
||||
}
|
||||
samples[index].PathS = samples[index - 1].PathS + chord;
|
||||
}
|
||||
for (int index = 0; index < samples.Count; index++)
|
||||
{
|
||||
int lower = index == 0 ? 0 : index - 1;
|
||||
int upper = index == samples.Count - 1 ? samples.Count - 1 : index + 1;
|
||||
double span = samples[upper].PathS - samples[lower].PathS;
|
||||
if (!IsFinite(span) || span <= 0d)
|
||||
{
|
||||
failureReason = "Independent curvature derivative span is invalid at station " + index + ".";
|
||||
return null;
|
||||
}
|
||||
samples[index].VehicleCurvatureDerivative =
|
||||
(samples[upper].VehicleCurvature - samples[lower].VehicleCurvature) / span;
|
||||
}
|
||||
return samples;
|
||||
}
|
||||
|
||||
private static bool ComparePath(LateralPath path, LateralCandidate candidate, IReadOnlyList<double> stations,
|
||||
IReadOnlyList<ExpectedSample> expected, double spatialTolerance, double kinematicTolerance,
|
||||
out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
for (int index = 0; index < path.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint actual = path.Points[index];
|
||||
ExpectedSample sample = expected[index];
|
||||
double dddl = candidate.DDDL[Math.Min(index, candidate.DDDL.Count - 1)];
|
||||
if (!AreClose(actual.ReferenceS, stations[index], spatialTolerance) ||
|
||||
!AreClose(actual.PathS, sample.PathS, spatialTolerance) ||
|
||||
!AreClose(actual.L, candidate.L[index], spatialTolerance) ||
|
||||
!AreClose(actual.DL, candidate.DL[index], kinematicTolerance) ||
|
||||
!AreClose(actual.DDL, candidate.DDL[index], kinematicTolerance) ||
|
||||
!AreClose(actual.DDDL, dddl, kinematicTolerance) ||
|
||||
!AreClose(actual.X, sample.X, spatialTolerance) || !AreClose(actual.Y, sample.Y, spatialTolerance) ||
|
||||
Math.Abs(AngleMath.NormalizeRadians(actual.VehicleYaw - sample.VehicleYaw)) > kinematicTolerance ||
|
||||
!AreClose(actual.GeometricCurvature, sample.GeometricCurvature, kinematicTolerance) ||
|
||||
!AreClose(actual.VehicleCurvature, sample.VehicleCurvature, kinematicTolerance) ||
|
||||
!AreClose(actual.VehicleCurvatureDerivative, sample.VehicleCurvatureDerivative, kinematicTolerance))
|
||||
{
|
||||
failureReason = "Independent lateral geometry validation failed at station " + index + ".";
|
||||
return false;
|
||||
}
|
||||
if (index == 0 && Math.Abs(actual.PathS) > spatialTolerance)
|
||||
{
|
||||
failureReason = "PathS must start at zero.";
|
||||
return false;
|
||||
}
|
||||
if (index > 0 && actual.PathS <= path.Points[index - 1].PathS + spatialTolerance)
|
||||
{
|
||||
failureReason = "PathS must be strictly increasing.";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
|
||||
double referenceCurvatureDerivative)
|
||||
{
|
||||
double a = 1d - reference.GeometricCurvature * l;
|
||||
double denominatorSquared = a * a + dl * dl;
|
||||
double numerator = a * a * reference.GeometricCurvature + a * ddl +
|
||||
referenceCurvatureDerivative * l * dl + 2d * reference.GeometricCurvature * dl * dl;
|
||||
return numerator / (denominatorSquared * Math.Sqrt(denominatorSquared));
|
||||
}
|
||||
|
||||
private static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
|
||||
{
|
||||
if (vehicle == null)
|
||||
throw new ArgumentNullException(nameof(vehicle));
|
||||
if (vehicle.MaximumCurvaturePerMeter.HasValue)
|
||||
return RequirePositive(vehicle.MaximumCurvaturePerMeter.Value, "vehicle maximum curvature");
|
||||
if (vehicle.MinimumTurningRadiusMeters.HasValue)
|
||||
return 1d / RequirePositive(vehicle.MinimumTurningRadiusMeters.Value, "vehicle minimum turning radius");
|
||||
throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
|
||||
}
|
||||
|
||||
private static bool AreClose(double actual, double expected, double tolerance)
|
||||
{
|
||||
return IsFinite(actual) && IsFinite(expected) && Math.Abs(actual - expected) <= tolerance;
|
||||
}
|
||||
|
||||
private static double RequirePositive(double value, string name)
|
||||
{
|
||||
if (!IsFinite(value) || value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(name);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static double RequireNonnegative(double value, string name)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(name);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private sealed class ExpectedSample
|
||||
{
|
||||
public ExpectedSample(double referenceS, double x, double y, double vehicleYaw, double geometricCurvature,
|
||||
double vehicleCurvature)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
X = x;
|
||||
Y = y;
|
||||
VehicleYaw = vehicleYaw;
|
||||
GeometricCurvature = geometricCurvature;
|
||||
VehicleCurvature = vehicleCurvature;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double VehicleYaw { get; }
|
||||
public double GeometricCurvature { get; }
|
||||
public double VehicleCurvature { get; }
|
||||
public double PathS { get; set; }
|
||||
public double VehicleCurvatureDerivative { get; set; }
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user