diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs new file mode 100644 index 0000000..402b6d2 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs @@ -0,0 +1,84 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Builds immutable world-space trajectory points from validated LS/ST results. +public sealed class EmTrajectoryAssembler +{ + private readonly double outputTimeStepSeconds; + private readonly double zeroSpeedHoldSeconds; + + public EmTrajectoryAssembler() + : this(EmPlannerConfiguration.CreateDefault()) + { + } + + public EmTrajectoryAssembler(EmPlannerConfiguration configuration) + { + if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null) + throw new ArgumentNullException(nameof(configuration)); + outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds; + zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds; + if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) || + zeroSpeedHoldSeconds < 0d) + { + throw new ArgumentOutOfRangeException(nameof(configuration)); + } + } + + public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata) + { + if (longitudinal == null || longitudinal.Candidate == null || + (longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback)) + { + throw new ArgumentException("Trajectory assembly requires a successful longitudinal result.", nameof(longitudinal)); + } + if (metadata == null) + throw new ArgumentNullException(nameof(metadata)); + + var interpolator = new LateralPathInterpolator(path); + var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds); + double terminalPathS = path.Points[path.Points.Count - 1].PathS; + var points = new List(schedule.Samples.Count); + double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d; + + for (int index = 0; index < schedule.Samples.Count; index++) + { + TrajectorySample sample = schedule.Samples[index]; + if (sample.PathS > terminalPathS + 1e-10d) + throw new ArgumentException("Longitudinal PathS exceeds the assembled lateral path.", nameof(longitudinal)); + + InterpolatedLateralPathPoint geometry = interpolator.Interpolate(sample.PathS); + bool isTerminalAnchor = index == longitudinal.Candidate.KnotTimes.Count - 1; + EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None; + double signedSpeed = directionSign * sample.ProgressSpeed; + points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart, + geometry.VehicleCurvature, metadata.SegmentIndex, sample.PathS, sample.PathS, metadata.Direction, + boundaryType, sample.Acceleration, sample.Jerk)); + } + + return new EmTrajectory(metadata, points); + } + + private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType) + { + switch (terminalType) + { + case EmTerminalType.RollingSafetyStop: + return EmBoundaryType.RollingSafetyStop; + case EmTerminalType.GearSwitch: + return EmBoundaryType.GearSwitchApproach; + case EmTerminalType.Goal: + return EmBoundaryType.Goal; + default: + throw new ArgumentOutOfRangeException(nameof(terminalType)); + } + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/LateralPathInterpolator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/LateralPathInterpolator.cs new file mode 100644 index 0000000..44eed00 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/LateralPathInterpolator.cs @@ -0,0 +1,99 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal sealed class LateralPathInterpolator +{ + private const double BoundaryTolerance = 1e-10d; + private readonly LateralPath path; + + public LateralPathInterpolator(LateralPath path) + { + if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2) + throw new ArgumentException("Trajectory assembly requires an independently validated lateral path.", nameof(path)); + + double previousPathS = double.NegativeInfinity; + for (int index = 0; index < path.Points.Count; index++) + { + LateralPathPoint point = path.Points[index]; + if (point == null || point.PathS <= previousPathS) + throw new ArgumentException("Lateral path points must have strictly increasing PathS.", nameof(path)); + previousPathS = point.PathS; + } + + this.path = path; + } + + public InterpolatedLateralPathPoint Interpolate(double pathS) + { + if (!IsFinite(pathS)) + throw new ArgumentOutOfRangeException(nameof(pathS)); + + LateralPathPoint first = path.Points[0]; + LateralPathPoint last = path.Points[path.Points.Count - 1]; + if (pathS < first.PathS - BoundaryTolerance || pathS > last.PathS + BoundaryTolerance) + throw new ArgumentOutOfRangeException(nameof(pathS)); + if (pathS <= first.PathS + BoundaryTolerance) + return From(first); + if (pathS >= last.PathS - BoundaryTolerance) + return From(last); + + for (int index = 1; index < path.Points.Count; index++) + { + LateralPathPoint right = path.Points[index]; + if (pathS <= right.PathS) + { + LateralPathPoint left = path.Points[index - 1]; + double ratio = (pathS - left.PathS) / (right.PathS - left.PathS); + return new InterpolatedLateralPathPoint( + Linear(left.X, right.X, ratio), + Linear(left.Y, right.Y, ratio), + NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)), + Linear(left.VehicleCurvature, right.VehicleCurvature, ratio)); + } + } + + throw new InvalidOperationException("A PathS value inside the lateral path was not bracketed."); + } + + private static InterpolatedLateralPathPoint From(LateralPathPoint point) + { + return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature); + } + + private static double Linear(double left, double right, double ratio) + { + return left + ratio * (right - left); + } + + internal static double NormalizeYaw(double yaw) + { + double normalized = yaw % (2d * Math.PI); + if (normalized >= Math.PI) + normalized -= 2d * Math.PI; + if (normalized < -Math.PI) + normalized += 2d * Math.PI; + return normalized; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} + +internal sealed class InterpolatedLateralPathPoint +{ + public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature) + { + X = x; + Y = y; + Yaw = yaw; + VehicleCurvature = vehicleCurvature; + } + + public double X { get; } + public double Y { get; } + public double Yaw { get; } + public double VehicleCurvature { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs new file mode 100644 index 0000000..75782e2 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs @@ -0,0 +1,74 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal sealed class TrajectorySampleSchedule +{ + private const double ZeroTolerance = 1e-12d; + + public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds) + { + if (candidate == null) + throw new ArgumentNullException(nameof(candidate)); + if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d) + throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds)); + if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d) + throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds)); + if (Math.Abs(candidate.U[candidate.U.Count - 1]) > ZeroTolerance) + throw new ArgumentException("A publishable trajectory requires an exact zero-speed terminal candidate.", nameof(candidate)); + + var samples = new List(candidate.KnotTimes.Count + 4); + double previousPathS = double.NegativeInfinity; + for (int index = 0; index < candidate.KnotTimes.Count; index++) + { + if (candidate.S[index] < previousPathS) + throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate)); + if (candidate.U[index] < -ZeroTolerance) + throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate)); + + samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]), + candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false)); + previousPathS = candidate.S[index]; + } + + TrajectorySample terminal = samples[samples.Count - 1]; + double holdElapsed = 0d; + while (holdElapsed < holdDurationSeconds - ZeroTolerance) + { + holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds); + samples.Add(new TrajectorySample(terminal.TimeFromStart + holdElapsed, terminal.PathS, 0d, 0d, 0d, true)); + } + + Samples = new ReadOnlyCollection(samples); + } + + public IReadOnlyList Samples { get; } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} + +internal sealed class TrajectorySample +{ + public TrajectorySample(double timeFromStart, double pathS, double progressSpeed, double acceleration, double jerk, + bool isHoldSample) + { + TimeFromStart = timeFromStart; + PathS = pathS; + ProgressSpeed = progressSpeed; + Acceleration = acceleration; + Jerk = jerk; + IsHoldSample = isHoldSample; + } + + public double TimeFromStart { get; } + public double PathS { get; } + public double ProgressSpeed { get; } + public double Acceleration { get; } + public double Jerk { get; } + public bool IsHoldSample { get; } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 6b1980f..8acb1e0 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -11,7 +11,7 @@ internal static class Program args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" && args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" && args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" && - args[0] != "longitudinal-real-osqp-probe")) + args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory")) { Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration"); return 2; @@ -88,6 +88,11 @@ internal static class Program MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.RunRealOsqp(); Console.WriteLine("PASS longitudinal-real-osqp"); } + if (args[0] == "trajectory") + { + MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run(); + Console.WriteLine("PASS trajectory"); + } return 0; } catch (Exception exception) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs new file mode 100644 index 0000000..b3e2d8b --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs @@ -0,0 +1,128 @@ +using System; +using System.Collections.Generic; +using EMPlannerVerificationHost; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal static class TrajectoryChecks +{ + public static void Run() + { + VerifiesForwardFieldsExactTerminalAndHold(); + VerifiesReverseTravelVelocityAndUnwrappedYaw(); + VerifiesPublishedListsAreImmutable(); + } + + private static void VerifiesForwardFieldsExactTerminalAndHold() + { + EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble( + CreatePath(TravelDirection.Forward, 0d, Math.PI / 2d), + CreateLongitudinalResult(), + CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal)); + + VerifyKinematicFields(trajectory, TravelDirection.Forward, "forward"); + VerifyTerminalAndHold(trajectory, EmBoundaryType.Goal, "forward"); + } + + private static void VerifiesReverseTravelVelocityAndUnwrappedYaw() + { + EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble( + CreatePath(TravelDirection.Reverse, 3.10d, -3.10d), + CreateLongitudinalResult(), + CreateMetadata(TravelDirection.Reverse, EmTerminalType.GearSwitch)); + + VerifyKinematicFields(trajectory, TravelDirection.Reverse, "reverse"); + VerifyTerminalAndHold(trajectory, EmBoundaryType.GearSwitchApproach, "reverse"); + EmTrajectoryPoint moving = trajectory.Points[1]; + Verification.True(moving.SignedLongitudinalVelocity < 0d, "reverse signed velocity is negative"); + Verification.True(moving.VelocityX * Math.Cos(moving.Yaw) + moving.VelocityY * Math.Sin(moving.Yaw) < 0d, + "reverse world velocity points opposite the vehicle yaw"); + Verification.True(Math.Abs(Math.Abs(moving.Yaw) - Math.PI) < 0.1d, + "reverse yaw interpolation unwraps across the pi boundary"); + } + + private static void VerifiesPublishedListsAreImmutable() + { + EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble( + CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(), + CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop)); + Verification.True(!(trajectory.Points is IList mutable) || mutable.IsReadOnly, + "trajectory public point list is immutable"); + } + + private static void VerifyKinematicFields(EmTrajectory trajectory, TravelDirection direction, string name) + { + double directionSign = direction == TravelDirection.Forward ? 1d : -1d; + double previousTime = double.NegativeInfinity; + double previousPathS = double.NegativeInfinity; + for (int index = 0; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint point = trajectory.Points[index]; + Verification.NearlyEqual(Math.Abs(point.SignedLongitudinalVelocity), point.Speed, + name + " speed field " + index); + Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Cos(point.Yaw), point.VelocityX, + name + " velocity X field " + index); + Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Sin(point.Yaw), point.VelocityY, + name + " velocity Y field " + index); + Verification.NearlyEqual(point.SignedLongitudinalVelocity * point.VehicleCurvature, point.YawRate, + name + " yaw-rate field " + index); + if (index < 4) + Verification.NearlyEqual(directionSign * CreateLongitudinalResult().Candidate.U[index], + point.SignedLongitudinalVelocity, name + " signed speed field " + index); + Verification.True(point.TimeFromStart > previousTime, name + " time strictly increases " + index); + Verification.True(point.PathS >= previousPathS, name + " PathS never decreases " + index); + previousTime = point.TimeFromStart; + previousPathS = point.PathS; + } + } + + private static void VerifyTerminalAndHold(EmTrajectory trajectory, EmBoundaryType terminalBoundary, string name) + { + const int terminalIndex = 3; + EmTrajectoryPoint terminal = trajectory.Points[terminalIndex]; + Verification.Equal(terminalBoundary, terminal.BoundaryType, name + " exact terminal boundary type"); + Verification.NearlyEqual(0d, terminal.SignedLongitudinalVelocity, name + " exact terminal signed speed"); + Verification.NearlyEqual(0d, terminal.YawRate, name + " exact terminal yaw rate"); + Verification.NearlyEqual(0.137d, terminal.TimeFromStart, name + " exact non-regular terminal time"); + Verification.Equal(terminalIndex + 5, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples"); + + for (int index = terminalIndex + 1; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint hold = trajectory.Points[index]; + Verification.NearlyEqual(terminal.TimeFromStart + (index - terminalIndex) * 0.05d, + hold.TimeFromStart, name + " hold timing " + index); + Verification.NearlyEqual(terminal.X, hold.X, name + " hold X " + index); + Verification.NearlyEqual(terminal.Y, hold.Y, name + " hold Y " + index); + Verification.NearlyEqual(terminal.Yaw, hold.Yaw, name + " hold yaw " + index); + Verification.NearlyEqual(0d, hold.SignedLongitudinalVelocity, name + " hold signed speed " + index); + Verification.NearlyEqual(0d, hold.YawRate, name + " hold yaw rate " + index); + } + } + + private static LateralPath CreatePath(TravelDirection direction, double firstYaw, double lastYaw) + { + return new LateralPath(new[] + { + new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, firstYaw, 0d, 0.5d, 0d), + new LateralPathPoint(1d, 0.12d, 0d, 0d, 0d, 0d, 0.12d, 0d, lastYaw, 0d, 0.5d, 0d), + }, true); + } + + private static LongitudinalPlanningResult CreateLongitudinalResult() + { + var candidate = new LongitudinalCandidate( + new[] { 0d, 0.05d, 0.10d, 0.137d }, + new[] { 0d, 0.04d, 0.08d, 0.12d }, + new[] { 0.8d, 0.8d, 0.8d, 0d }, + new[] { 0d, 0d, 0d, 0d }, + new[] { 0d, 0d, 0d }); + return new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty); + } + + private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType) + { + return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L, + "reference", 4L, string.Empty, 2, direction, terminalType); + } +}