diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjection.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjection.cs new file mode 100644 index 0000000..6aa1609 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjection.cs @@ -0,0 +1,33 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// World pose projected onto one bounded direction segment. +public sealed class FrenetProjection +{ + public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError, + double squaredDistanceMeters) + { + if (referencePoint == null) + throw new ArgumentNullException(nameof(referencePoint)); + if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d) + throw new ArgumentOutOfRangeException(nameof(lateralOffset)); + + ReferencePoint = referencePoint; + ReferenceS = referencePoint.ReferenceS; + LateralOffset = lateralOffset; + HeadingError = headingError; + SquaredDistanceMeters = squaredDistanceMeters; + } + + public FrenetReferencePoint ReferencePoint { get; } + public double ReferenceS { get; } + public double LateralOffset { get; } + public double HeadingError { get; } + public double SquaredDistanceMeters { get; } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjector.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjector.cs new file mode 100644 index 0000000..cfa5848 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjector.cs @@ -0,0 +1,131 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Deterministically projects a world pose only inside the supplied direction segment and S window. +public sealed class FrenetProjector +{ + private const double TieTolerance = 1e-14d; + + public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS, + double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection) + { + return TryProject(worldPose, segment, minimumReferenceS, maximumReferenceS, maximumDistanceMeters, + minimumReferenceS, out projection); + } + + public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS, + double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection) + { + projection = null; + if (worldPose == null || segment == null || !IsFinite(worldPose.X) || !IsFinite(worldPose.Y) || + !IsFinite(worldPose.Heading) || !IsFinite(minimumReferenceS) || !IsFinite(maximumReferenceS) || + !IsFinite(maximumDistanceMeters) || !IsFinite(seedReferenceS) || maximumDistanceMeters < 0d) + return false; + + double lowerBound = Math.Max(0d, minimumReferenceS); + double upperBound = Math.Min(segment.LengthMeters, maximumReferenceS); + if (lowerBound > upperBound) + return false; + + Candidate best = null; + for (int index = 0; index + 1 < segment.Points.Count; index++) + { + double startS = Math.Max(lowerBound, segment.Points[index].ArcLength); + double endS = Math.Min(upperBound, segment.Points[index + 1].ArcLength); + if (startS > endS) + continue; + + FrenetReferencePoint start = ReferencePathInterpolator.Interpolate(segment, startS); + FrenetReferencePoint end = ReferencePathInterpolator.Interpolate(segment, endS); + ConsiderLine(worldPose, start, end, seedReferenceS, ref best); + } + + if (best == null && Math.Abs(lowerBound - upperBound) <= TieTolerance) + { + FrenetReferencePoint point = ReferencePathInterpolator.Interpolate(segment, lowerBound); + ConsiderPoint(worldPose, point, seedReferenceS, ref best); + } + + if (best == null || best.SquaredDistanceMeters > maximumDistanceMeters * maximumDistanceMeters) + return false; + + FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, best.ReferenceS); + double dx = worldPose.X - reference.X; + double dy = worldPose.Y - reference.Y; + double travelYaw = reference.TravelYaw; + double lateralOffset = -dx * Math.Sin(travelYaw) + dy * Math.Cos(travelYaw); + double egoTravelYaw = FrenetTransform.GetTravelYaw(worldPose.Heading, segment.Direction); + double headingError = AngleMath.NormalizeRadians(egoTravelYaw - travelYaw); + projection = new FrenetProjection(reference, lateralOffset, headingError, best.SquaredDistanceMeters); + return true; + } + + private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end, + double seedReferenceS, ref Candidate best) + { + double dx = end.X - start.X; + double dy = end.Y - start.Y; + double lengthSquared = dx * dx + dy * dy; + if (lengthSquared <= TieTolerance) + { + ConsiderPoint(worldPose, start, seedReferenceS, ref best); + ConsiderPoint(worldPose, end, seedReferenceS, ref best); + return; + } + + double fraction = ((worldPose.X - start.X) * dx + (worldPose.Y - start.Y) * dy) / lengthSquared; + fraction = Math.Max(0d, Math.Min(1d, fraction)); + double referenceS = start.ReferenceS + (end.ReferenceS - start.ReferenceS) * fraction; + double x = start.X + dx * fraction; + double y = start.Y + dy * fraction; + Consider(worldPose, referenceS, x, y, seedReferenceS, ref best); + } + + private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS, + ref Candidate best) + { + Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best); + } + + private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS, + ref Candidate best) + { + double dx = worldPose.X - x; + double dy = worldPose.Y - y; + double squaredDistance = dx * dx + dy * dy; + var candidate = new Candidate(referenceS, squaredDistance, Math.Abs(referenceS - seedReferenceS)); + if (best == null || candidate.IsPreferredTo(best)) + best = candidate; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } + + private sealed class Candidate + { + public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance) + { + ReferenceS = referenceS; + SquaredDistanceMeters = squaredDistanceMeters; + SeedDistance = seedDistance; + } + + public double ReferenceS { get; } + public double SquaredDistanceMeters { get; } + public double SeedDistance { get; } + + public bool IsPreferredTo(Candidate other) + { + if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true; + if (SquaredDistanceMeters > other.SquaredDistanceMeters + TieTolerance) return false; + if (SeedDistance < other.SeedDistance - TieTolerance) return true; + if (SeedDistance > other.SeedDistance + TieTolerance) return false; + return ReferenceS < other.ReferenceS; + } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetReferencePoint.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetReferencePoint.cs new file mode 100644 index 0000000..043b70a --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetReferencePoint.cs @@ -0,0 +1,58 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable interpolated reference sample in a single direction segment. +public sealed class FrenetReferencePoint +{ + public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw, + TravelDirection direction, double geometricCurvature, double vehicleCurvature, + double vehicleCurvatureDerivative, double bodyClearance) + { + RequireFinite(referenceS, nameof(referenceS)); + RequireFinite(x, nameof(x)); + RequireFinite(y, nameof(y)); + RequireFinite(vehicleYaw, nameof(vehicleYaw)); + RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw)); + RequireFinite(geometricCurvature, nameof(geometricCurvature)); + RequireFinite(vehicleCurvature, nameof(vehicleCurvature)); + RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative)); + RequireFinite(bodyClearance, nameof(bodyClearance)); + + ReferenceS = referenceS; + X = x; + Y = y; + VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw); + UnwrappedVehicleYaw = unwrappedVehicleYaw; + Direction = direction; + GeometricCurvature = geometricCurvature; + VehicleCurvature = vehicleCurvature; + VehicleCurvatureDerivative = vehicleCurvatureDerivative; + BodyClearance = bodyClearance; + } + + public double ReferenceS { get; } + public double X { get; } + public double Y { get; } + public double VehicleYaw { get; } + public double UnwrappedVehicleYaw { get; } + public TravelDirection Direction { get; } + public double GeometricCurvature { get; } + public double VehicleCurvature { get; } + public double VehicleCurvatureDerivative { get; } + public double BodyClearance { get; } + + /// Unwrapped direction of travel, used internally for geometry. + public double TravelYaw + { + get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; } + } + + private static void RequireFinite(double value, string name) + { + if (double.IsNaN(value) || double.IsInfinity(value)) + throw new ArgumentOutOfRangeException(name); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetTransform.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetTransform.cs new file mode 100644 index 0000000..3ca1d29 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetTransform.cs @@ -0,0 +1,45 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Coordinate conversion that keeps Frenet lateral sign relative to travel direction. +public static class FrenetTransform +{ + public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative, + double minimumFrenetDenominator, out Pose2D pose) + { + pose = null; + if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) || + !IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d) + return false; + + double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset; + if (!IsFinite(denominator) || denominator < minimumFrenetDenominator) + return false; + + double travelYaw = referencePoint.TravelYaw; + double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw); + double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw); + double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator); + double vehicleYaw = referencePoint.Direction == TravelDirection.Forward + ? AngleMath.NormalizeRadians(optimizedTravelYaw) + : AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI); + if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw)) + return false; + + pose = new Pose2D(x, y, vehicleYaw); + return true; + } + + internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction) + { + return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/ReferencePathInterpolator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/ReferencePathInterpolator.cs new file mode 100644 index 0000000..c4ce493 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/ReferencePathInterpolator.cs @@ -0,0 +1,64 @@ +using System; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Interpolates reference geometry inside exactly one direction segment. +public static class ReferencePathInterpolator +{ + private const double Epsilon = 1e-12d; + + public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS) + { + if (segment == null) + throw new ArgumentNullException(nameof(segment)); + if (!IsFinite(referenceS) || referenceS < -Epsilon || referenceS > segment.LengthMeters + Epsilon) + throw new ArgumentOutOfRangeException(nameof(referenceS)); + + double clampedS = Math.Max(0d, Math.Min(segment.LengthMeters, referenceS)); + for (int index = 0; index < segment.Points.Count; index++) + { + SmoothedPathPoint upper = segment.Points[index]; + if (Math.Abs(upper.ArcLength - clampedS) <= Epsilon) + return FromPoint(upper); + if (upper.ArcLength > clampedS) + { + SmoothedPathPoint lower = segment.Points[index - 1]; + double span = upper.ArcLength - lower.ArcLength; + if (span <= Epsilon) + throw new ArgumentException("Reference points must have positive interpolation spans.", nameof(segment)); + double fraction = (clampedS - lower.ArcLength) / span; + return new FrenetReferencePoint( + clampedS, + Lerp(lower.X, upper.X, fraction), + Lerp(lower.Y, upper.Y, fraction), + AngleMath.NormalizeRadians(Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction)), + Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction), + segment.Direction, + Lerp(lower.GeometricCurvature, upper.GeometricCurvature, fraction), + Lerp(lower.VehicleCurvature, upper.VehicleCurvature, fraction), + Lerp(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction), + Lerp(lower.BodyClearance, upper.BodyClearance, fraction)); + } + } + return FromPoint(segment.Points[segment.Points.Count - 1]); + } + + private static FrenetReferencePoint FromPoint(SmoothedPathPoint point) + { + return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading, + point.Direction, point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative, + point.BodyClearance); + } + + private static double Lerp(double lower, double upper, double fraction) + { + return lower + (upper - lower) * fraction; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/FrenetChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/FrenetChecks.cs new file mode 100644 index 0000000..fd2c7a8 --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/FrenetChecks.cs @@ -0,0 +1,136 @@ +using System; +using System.Collections.Generic; +using EMPlannerVerificationHost; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal static class FrenetChecks +{ + public static void Run() + { + VerifiesForwardProjectionAndReconstruction(); + VerifiesReverseProjectionAcrossYawWrap(); + VerifiesBoundedProjectionOnALoop(); + } + + private static void VerifiesForwardProjectionAndReconstruction() + { + DirectionSegmentView segment = CreateStraightSegment(TravelDirection.Forward, 0d, 0d); + var world = new Pose2D(1.5d, 0.2d, 0d); + var projector = new FrenetProjector(); + + Verification.True(projector.TryProject(world, segment, 0d, 4d, 0.5d, out FrenetProjection projection), + "forward projection succeeds"); + Verification.NearlyEqual(1.5d, projection.ReferenceS, "forward reference s"); + Verification.NearlyEqual(0.2d, projection.LateralOffset, "forward positive l is travel-left"); + Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d, + out Pose2D reconstructed), "forward reconstruction succeeds"); + Verification.NearlyEqual(world.X, reconstructed.X, "forward reconstructed x"); + Verification.NearlyEqual(world.Y, reconstructed.Y, "forward reconstructed y"); + Verification.NearlyEqual(0d, reconstructed.Heading, "forward reconstructed vehicle yaw"); + } + + private static void VerifiesReverseProjectionAcrossYawWrap() + { + const double vehicleYaw = -Math.PI + 0.01d; + const double travelYaw = 0.01d; + DirectionSegmentView segment = CreateStraightSegment(TravelDirection.Reverse, vehicleYaw, travelYaw); + double expectedS = 1.5d; + double expectedL = 0.2d; + var world = new Pose2D( + expectedS * Math.Cos(travelYaw) - expectedL * Math.Sin(travelYaw), + expectedS * Math.Sin(travelYaw) + expectedL * Math.Cos(travelYaw), + vehicleYaw); + var projector = new FrenetProjector(); + + Verification.True(projector.TryProject(world, segment, 0d, 4d, 0.5d, out FrenetProjection projection), + "reverse projection succeeds"); + Verification.NearlyEqual(expectedS, projection.ReferenceS, "reverse reference s"); + Verification.NearlyEqual(expectedL, projection.LateralOffset, "reverse positive l is travel-left and body-right"); + Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d, + out Pose2D reconstructed), "reverse reconstruction succeeds"); + Verification.NearlyEqual(world.X, reconstructed.X, "reverse reconstructed x"); + Verification.NearlyEqual(world.Y, reconstructed.Y, "reverse reconstructed y"); + Verification.NearlyEqual(AngleMath.NormalizeRadians(vehicleYaw), reconstructed.Heading, + "reverse reconstructed vehicle yaw"); + + DirectionSegmentView wrapped = CreateWrappedForwardSegment(); + FrenetReferencePoint wrappedPoint = ReferencePathInterpolator.Interpolate(wrapped, 1.5d); + Verification.NearlyEqual(3.18d, wrappedPoint.UnwrappedVehicleYaw, "interpolated yaw remains unwrapped internally"); + Verification.NearlyEqual(AngleMath.NormalizeRadians(3.18d), wrappedPoint.VehicleYaw, + "interpolated public yaw is normalized"); + Verification.True(!FrenetTransform.TryReconstruct(new FrenetReferencePoint(0d, 0d, 0d, 0d, 0d, + TravelDirection.Forward, 1d, 0d, 0d, 0d), 1d, 0d, 0.2d, out _), + "singular Frenet reconstruction is rejected"); + } + + private static void VerifiesBoundedProjectionOnALoop() + { + DirectionSegmentView segment = CreateLoopSegment(); + var projector = new FrenetProjector(); + var world = new Pose2D(1d, 0.2d, Math.PI); + + Verification.True(projector.TryProject(world, segment, 2.2d, 4.2d, 0.1d, out FrenetProjection projection), + "bounded loop projection succeeds"); + Verification.NearlyEqual(3.2d, projection.ReferenceS, "bounded loop picks the local upper branch"); + Verification.True(projection.ReferenceS >= 2.2d && projection.ReferenceS <= 4.2d, + "projection remains in supplied reference-s interval"); + Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d, + out Pose2D reconstructed), "loop reconstruction succeeds"); + Verification.NearlyEqual(world.X, reconstructed.X, "loop reconstructed x"); + Verification.NearlyEqual(world.Y, reconstructed.Y, "loop reconstructed y"); + } + + private static DirectionSegmentView CreateStraightSegment(TravelDirection direction, double vehicleYaw, + double travelYaw) + { + var points = new List + { + Point(0d, 0d, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, 0d, direction), + Point(4d * Math.Cos(travelYaw), 4d * Math.Sin(travelYaw), AngleMath.NormalizeRadians(vehicleYaw), + vehicleYaw, 4d, direction), + }; + return CreateSegment(direction, points); + } + + private static DirectionSegmentView CreateWrappedForwardSegment() + { + var points = new List + { + Point(0d, 0d, AngleMath.NormalizeRadians(3.12d), 3.12d, 0d, TravelDirection.Forward), + Point(1d, 0d, AngleMath.NormalizeRadians(3.16d), 3.16d, 1d, TravelDirection.Forward), + Point(2d, 0d, AngleMath.NormalizeRadians(3.20d), 3.20d, 2d, TravelDirection.Forward), + }; + return CreateSegment(TravelDirection.Forward, points); + } + + private static DirectionSegmentView CreateLoopSegment() + { + var points = new List + { + Point(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward), + Point(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward), + Point(2d, 0.2d, Math.PI / 2d, Math.PI / 2d, 2.2d, TravelDirection.Forward), + Point(0d, 0.2d, Math.PI, Math.PI, 4.2d, TravelDirection.Forward), + }; + return CreateSegment(TravelDirection.Forward, points); + } + + private static DirectionSegmentView CreateSegment(TravelDirection direction, IReadOnlyList points) + { + double length = points[points.Count - 1].ArcLength; + return new DirectionSegmentView(0, direction, points, + new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d), + new ReferenceBoundary(0, length, EmBoundaryType.Goal, length), 0d); + } + + private static SmoothedPathPoint Point(double x, double y, double heading, double unwrappedHeading, double s, + TravelDirection direction) + { + return new SmoothedPathPoint(x, y, heading, unwrappedHeading, s, direction, 0d, 0d, 0d, 1d, + false, SmoothedPathPointSource.Anchor); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index a769551..79aaf36 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -6,9 +6,9 @@ internal static class Program { private static int Main(string[] args) { - if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation")) + if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet")) { - Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation"); + Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet"); return 2; } @@ -19,11 +19,16 @@ internal static class Program MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run(); Console.WriteLine("PASS foundation"); } - else + else if (args[0] == "segmentation") { MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run(); Console.WriteLine("PASS segmentation"); } + else + { + MultiWheelC.TrajectoryPlanning.EMPlanner.FrenetChecks.Run(); + Console.WriteLine("PASS frenet"); + } return 0; } catch (Exception exception)