diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs new file mode 100644 index 0000000..2482794 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs @@ -0,0 +1,201 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Reconstructs world geometry and actual path arc length from a lateral candidate. +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 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(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 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(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 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; } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs new file mode 100644 index 0000000..3de7692 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs @@ -0,0 +1,318 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Independently recomputes and verifies lateral world geometry before a path may be published. +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 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 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(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 stations, + IReadOnlyList 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; } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs index ea8b4fa..5539d41 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs @@ -5,6 +5,7 @@ using EMPlannerVerificationHost; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle; using MultiWheelC.TrajectoryPlanning.PathSmoothing; +using MultiWheelC.TrajectoryPlanning.Utils; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; @@ -20,6 +21,8 @@ internal static class LateralModelChecks VerifiesAllNamedCostScales(); VerifiesEmptyHardBoundIntersectionFailsBeforeSolve(); VerifiesFakeSolverCapturesTheNeutralQpBoundary(); + VerifiesNonlinearGeometryInBothDirections(); + VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths(); } private static void VerifiesDeterministicVariableLayout() @@ -275,6 +278,114 @@ internal static class LateralModelChecks Verification.NearlyEqual(2d, solver.LastWarmStart[1], "fake solver records a defensive warm-start copy"); } + private static void VerifiesNonlinearGeometryInBothDirections() + { + double[] stations = { 0d, 0.7d, 2d }; + LateralCandidate candidate = LateralCandidate.Integrate(stations, 0.1d, 0.1d, 0.05d, + new[] { 0.02d, -0.03d }); + LateralGeometryEvaluator evaluator = CreateGeometryEvaluator(); + LateralSolutionValidator validator = CreateGeometryValidator(); + + VerifyGeometryForDirection(TravelDirection.Forward, 0d, candidate, evaluator, validator); + VerifyGeometryForDirection(TravelDirection.Reverse, 0d, candidate, evaluator, validator); + VerifyGeometryForDirection(TravelDirection.Forward, 0.2d, candidate, evaluator, validator); + VerifyGeometryForDirection(TravelDirection.Reverse, 0.2d, candidate, evaluator, validator); + } + + private static void VerifiesIndependentGeometryValidationRejectsUnsafeOrTamperedPaths() + { + LateralGeometryEvaluator evaluator = CreateGeometryEvaluator(); + LateralSolutionValidator validator = CreateGeometryValidator(); + double[] stations = { 0d, 1d, 2d }; + + LateralPlanningInput singularInput = CreateGeometryInput(TravelDirection.Forward, 1d, stations, 0.81d, 0d, 10d); + LateralCandidate singular = LateralCandidate.Integrate(stations, 0.81d, 0d, 0d, new[] { 0d, 0d }); + Verification.True(!evaluator.TryEvaluate(singularInput, singular, out LateralPath singularPath, out string singularReason), + "Frenet denominator below 0.20 is rejected by reconstruction"); + Verification.True(singularPath == null && singularReason.Length > 0, "singular reconstruction has no path"); + + LateralPlanningInput curvatureInput = CreateGeometryInput(TravelDirection.Forward, 0d, stations, 0d, 0d, 1d); + LateralCandidate excessiveCurvature = LateralCandidate.Integrate(stations, 0d, 0d, 2d, new[] { 0d, 0d }); + Verification.True(evaluator.TryEvaluate(curvatureInput, excessiveCurvature, out LateralPath excessivePath, + out string excessiveReason), "curvature reconstruction remains geometric: " + excessiveReason); + Verification.True(!validator.TryValidate(curvatureInput, excessiveCurvature, excessivePath, + out LateralPath rejectedCurvature, out string curvatureReason), "curvature above vehicle limit is rejected"); + Verification.True(rejectedCurvature == null && curvatureReason.Length > 0, "curvature failure has no validated path"); + + LateralCandidate valid = LateralCandidate.Integrate(stations, 0d, 0d, 0d, new[] { 0d, 0d }); + Verification.True(evaluator.TryEvaluate(curvatureInput, valid, out LateralPath rawPath, out string rawReason), + "valid path reconstructs: " + rawReason); + var tamperedPoints = new List(rawPath.Points); + LateralPathPoint original = tamperedPoints[1]; + tamperedPoints[1] = new LateralPathPoint(original.ReferenceS, original.PathS, original.L, original.DL, + original.DDL, original.DDDL, original.X + 0.01d, original.Y, original.VehicleYaw, + original.GeometricCurvature, original.VehicleCurvature, original.VehicleCurvatureDerivative); + Verification.True(!validator.TryValidate(curvatureInput, valid, new LateralPath(tamperedPoints, false), + out LateralPath rejectedTampered, out string tamperedReason), + "validator independently rejects a world-coordinate mismatch"); + Verification.True(rejectedTampered == null && tamperedReason.Length > 0, "tampered path has no validated copy"); + + ExpectArgumentException(() => new LateralCandidate(stations, new[] { double.NaN, 0d, 0d }, + new[] { 0d, 0d, 0d }, new[] { 0d, 0d, 0d }, new[] { 0d, 0d }), "non-finite lateral values are rejected"); + ExpectArgumentException(() => new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, double.NaN, 0d, 0d, 0d, 0d, 0d), + "non-finite world geometry is rejected"); + } + + private static void VerifyGeometryForDirection(TravelDirection direction, double referenceCurvature, + LateralCandidate candidate, + LateralGeometryEvaluator evaluator, LateralSolutionValidator validator) + { + LateralPlanningInput input = CreateGeometryInput(direction, referenceCurvature, candidate.ReferenceStations, + candidate.L[0], candidate.DL[0], 10d); + Verification.True(evaluator.TryEvaluate(input, candidate, out LateralPath rawPath, out string evaluationReason), + direction + " geometry reconstructs: " + evaluationReason); + Verification.True(!rawPath.IsIndependentlyValidated, direction + " evaluator does not self-validate"); + Verification.True(validator.TryValidate(input, candidate, rawPath, out LateralPath validatedPath, + out string validationReason), direction + " geometry validates: " + validationReason); + Verification.True(validatedPath.IsIndependentlyValidated, direction + " validation creates a marked immutable path"); + + double directionSign = direction == TravelDirection.Forward ? 1d : -1d; + Verification.NearlyEqual(0d, rawPath.Points[0].PathS, direction + " PathS starts at zero"); + for (int index = 0; index < rawPath.Points.Count; index++) + { + LateralPathPoint point = rawPath.Points[index]; + FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment, point.ReferenceS); + double denominator = 1d - reference.GeometricCurvature * point.L; + double expectedX = reference.X - point.L * Math.Sin(reference.TravelYaw); + double expectedY = reference.Y + point.L * Math.Cos(reference.TravelYaw); + double expectedTravelYaw = reference.TravelYaw + Math.Atan2(point.DL, denominator); + double expectedVehicleYaw = direction == TravelDirection.Forward + ? AngleMath.NormalizeRadians(expectedTravelYaw) + : AngleMath.NormalizeRadians(expectedTravelYaw + Math.PI); + double expectedGeometricCurvature = CalculateGeometricCurvature(reference, point.L, point.DL, point.DDL, + directionSign * reference.VehicleCurvatureDerivative); + Verification.NearlyEqual(expectedX, point.X, direction + " world x " + index); + Verification.NearlyEqual(expectedY, point.Y, direction + " world y " + index); + Verification.NearlyEqual(expectedVehicleYaw, point.VehicleYaw, direction + " vehicle yaw " + index); + Verification.NearlyEqual(expectedGeometricCurvature, point.GeometricCurvature, + direction + " full Frenet curvature " + index); + Verification.NearlyEqual(directionSign * point.GeometricCurvature, point.VehicleCurvature, + direction + " vehicle curvature sign " + index); + if (index > 0) + { + LateralPathPoint previous = rawPath.Points[index - 1]; + double chord = Math.Sqrt((point.X - previous.X) * (point.X - previous.X) + + (point.Y - previous.Y) * (point.Y - previous.Y)); + Verification.NearlyEqual(previous.PathS + chord, point.PathS, direction + " actual chord PathS " + index); + Verification.True(point.PathS > previous.PathS, direction + " PathS strictly increases " + index); + } + } + + LateralPathPoint check = rawPath.Points[1]; + double signedSpeed = directionSign * 0.3d; + var trajectoryPoint = new EmTrajectoryPoint(check.X, check.Y, check.VehicleYaw, signedSpeed, 0d, + check.VehicleCurvature, 0, check.ReferenceS, check.PathS, direction, EmBoundaryType.None, 0d, 0d); + Verification.NearlyEqual(signedSpeed * check.VehicleCurvature, trajectoryPoint.YawRate, + direction + " yaw-rate identity"); + Verification.NearlyEqual(0.3d * check.GeometricCurvature, trajectoryPoint.YawRate, + direction + " travel curvature yaw-rate identity"); + } + private static DirectionSegmentView CreateStraightSegment(double referenceCurvatureDerivative = 0d, double referenceCurvature = 0d) { @@ -327,6 +438,57 @@ internal static class LateralModelChecks CreateVehicle(maximumVehicleCurvature), configuration, seed); } + private static LateralPlanningInput CreateGeometryInput(TravelDirection direction, double referenceCurvature, + IReadOnlyList stations, double startL, double startDL, double maximumVehicleCurvature) + { + var points = new List(stations.Count); + for (int index = 0; index < stations.Count; index++) + { + double s = stations[index]; + double travelYaw = referenceCurvature * s; + double x = Math.Abs(referenceCurvature) <= 1e-12d ? s : Math.Sin(travelYaw) / referenceCurvature; + double y = Math.Abs(referenceCurvature) <= 1e-12d ? 0d : (1d - Math.Cos(travelYaw)) / referenceCurvature; + double vehicleYaw = direction == TravelDirection.Forward ? travelYaw : travelYaw - Math.PI; + points.Add(new SmoothedPathPoint(x, y, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, s, direction, + referenceCurvature, direction == TravelDirection.Forward ? referenceCurvature : -referenceCurvature, + 0d, 1d, false, SmoothedPathPointSource.Anchor)); + } + double end = stations[stations.Count - 1]; + var segment = new DirectionSegmentView(0, direction, points, + new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d), + new ReferenceBoundary(0, end, EmBoundaryType.RollingSafetyStop, end), 0d); + var corridorStations = new List(stations.Count); + for (int index = 0; index < stations.Count; index++) + corridorStations.Add(new LateralInterval(stations[index], -1d, 1d, startL)); + EmPlannerConfiguration configuration = CreateUnitScaleConfiguration(); + configuration.Lateral.MaximumLateralSlope = 5d; + configuration.Lateral.MaximumLateralSecondDerivativePerMeter = 5d; + configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter = 5d; + double startDenominator = 1d - referenceCurvature * startL; + return new LateralPlanningInput(segment, new StaticCorridor(corridorStations), + new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, stations[0]), startL, + Math.Atan2(startDL, startDenominator), 0d), EmTerminalType.RollingSafetyStop, + CreateVehicle(maximumVehicleCurvature), configuration, Array.Empty()); + } + + private static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl, + double referenceCurvatureDerivative) + { + double a = 1d - reference.GeometricCurvature * l; + return (a * a * reference.GeometricCurvature + a * ddl + referenceCurvatureDerivative * l * dl + + 2d * reference.GeometricCurvature * dl * dl) / Math.Pow(a * a + dl * dl, 1.5d); + } + + private static LateralGeometryEvaluator CreateGeometryEvaluator() + { + return new LateralGeometryEvaluator(); + } + + private static LateralSolutionValidator CreateGeometryValidator() + { + return new LateralSolutionValidator(); + } + private static LateralObjectiveBuilder CreateObjectiveBuilder() { return new LateralObjectiveBuilder();