diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs index be7be41..cee5b16 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs @@ -82,9 +82,15 @@ public sealed class EmPlanningService : IEmPlanningService return Failure(lateral.Status, request, lateral.FailureReason); EmitDebug(request, "LS optimization and validation succeeded"); + IReadOnlyList knotTimes = LongitudinalCandidate.CreateKnotTimes( + configuration.Scheduling.TimeHorizonSeconds, configuration.Scheduling.OutputTimeStepSeconds); + LongitudinalPreviousTrajectorySeed previousLongitudinalSeed = + new LongitudinalPreviousTrajectorySeedBuilder().Build( + request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotTimes, + segment.SegmentIndex, segment.Direction); var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed, initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration, - Array.Empty(), Array.Empty()); + previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond); EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason); if (envelopeStatus != EmPlanningStatus.Success) return Failure(envelopeStatus, request, envelopeReason); diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs new file mode 100644 index 0000000..dfbdebc --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs @@ -0,0 +1,250 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Resamples a compatible published trajectory onto the current ST knots as soft longitudinal references. +public sealed class LongitudinalPreviousTrajectorySeed +{ + private static readonly LongitudinalPreviousTrajectorySeed empty = new LongitudinalPreviousTrajectorySeed( + Array.Empty(), Array.Empty()); + + public LongitudinalPreviousTrajectorySeed(IReadOnlyList pathS, + IReadOnlyList progressSpeedMetersPerSecond) + { + if (pathS == null) + throw new ArgumentNullException(nameof(pathS)); + if (progressSpeedMetersPerSecond == null) + throw new ArgumentNullException(nameof(progressSpeedMetersPerSecond)); + if (pathS.Count != progressSpeedMetersPerSecond.Count) + throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts."); + + var copiedPathS = new List(pathS.Count); + var copiedSpeed = new List(progressSpeedMetersPerSecond.Count); + for (int index = 0; index < pathS.Count; index++) + { + if (!IsFinite(pathS[index]) || pathS[index] < 0d || + !IsFinite(progressSpeedMetersPerSecond[index]) || progressSpeedMetersPerSecond[index] < 0d) + { + throw new ArgumentOutOfRangeException(nameof(pathS)); + } + copiedPathS.Add(pathS[index]); + copiedSpeed.Add(progressSpeedMetersPerSecond[index]); + } + + PathS = new ReadOnlyCollection(copiedPathS); + ProgressSpeedMetersPerSecond = new ReadOnlyCollection(copiedSpeed); + } + + public IReadOnlyList PathS { get; } + + public IReadOnlyList ProgressSpeedMetersPerSecond { get; } + + public static LongitudinalPreviousTrajectorySeed Empty { get { return empty; } } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} + +/// Builds monotone PathS and progress-speed soft references from a prior published trajectory. +public sealed class LongitudinalPreviousTrajectorySeedBuilder +{ + private const double ProjectionTolerance = 1e-10d; + + public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath, + DateTimeOffset newEffectiveAtUtc, IReadOnlyList newKnotTimes, int segmentIndex, + TravelDirection direction) + { + try + { + if (!IsCompatible(previous, currentPath, newKnotTimes, segmentIndex, direction)) + return LongitudinalPreviousTrajectorySeed.Empty; + + var pathS = new List(newKnotTimes.Count); + var progressSpeed = new List(newKnotTimes.Count); + double previousProjectedPathS = double.NegativeInfinity; + for (int index = 0; index < newKnotTimes.Count; index++) + { + DateTimeOffset sampleUtc = newEffectiveAtUtc.AddSeconds(newKnotTimes[index]); + double previousTimeSeconds = (sampleUtc - previous.Metadata.EffectiveAtUtc).TotalSeconds; + if (!TryInterpolate(previous.Points, previousTimeSeconds, out InterpolatedPreviousSample sample) || + !TryProjectMonotonically(currentPath, sample.X, sample.Y, previousProjectedPathS, + out double projectedPathS)) + { + return LongitudinalPreviousTrajectorySeed.Empty; + } + + pathS.Add(projectedPathS); + progressSpeed.Add(Math.Abs(sample.SignedSpeedMetersPerSecond)); + previousProjectedPathS = projectedPathS; + } + + return new LongitudinalPreviousTrajectorySeed(pathS, progressSpeed); + } + catch + { + return LongitudinalPreviousTrajectorySeed.Empty; + } + } + + private static bool IsCompatible(EmTrajectory previous, LateralPath currentPath, + IReadOnlyList newKnotTimes, int segmentIndex, TravelDirection direction) + { + if (previous == null || currentPath == null || newKnotTimes == null || segmentIndex < 0 || + !Enum.IsDefined(typeof(TravelDirection), direction) || !currentPath.IsIndependentlyValidated || + currentPath.Points.Count < 2 || previous.Metadata == null || previous.Points.Count < 2 || + previous.Metadata.SegmentIndex != segmentIndex || previous.Metadata.Direction != direction) + { + return false; + } + + double previousKnotTime = double.NegativeInfinity; + for (int index = 0; index < newKnotTimes.Count; index++) + { + if (!IsFinite(newKnotTimes[index]) || newKnotTimes[index] < 0d || + newKnotTimes[index] <= previousKnotTime) + { + return false; + } + previousKnotTime = newKnotTimes[index]; + } + if (newKnotTimes.Count == 0) + return false; + + double previousTime = double.NegativeInfinity; + for (int index = 0; index < previous.Points.Count; index++) + { + EmTrajectoryPoint point = previous.Points[index]; + if (point == null || point.Direction != direction || !IsFinite(point.TimeFromStart) || + !IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.SignedLongitudinalVelocity) || + point.TimeFromStart <= previousTime) + { + return false; + } + previousTime = point.TimeFromStart; + } + + double previousPathS = double.NegativeInfinity; + for (int index = 0; index < currentPath.Points.Count; index++) + { + LateralPathPoint point = currentPath.Points[index]; + if (point == null || !IsFinite(point.PathS) || !IsFinite(point.X) || !IsFinite(point.Y) || + point.PathS <= previousPathS) + { + return false; + } + previousPathS = point.PathS; + } + return true; + } + + private static bool TryInterpolate(IReadOnlyList points, double sampleTimeSeconds, + out InterpolatedPreviousSample sample) + { + sample = default; + if (!IsFinite(sampleTimeSeconds) || sampleTimeSeconds < points[0].TimeFromStart - ProjectionTolerance || + sampleTimeSeconds > points[points.Count - 1].TimeFromStart + ProjectionTolerance) + { + return false; + } + + if (sampleTimeSeconds <= points[0].TimeFromStart + ProjectionTolerance) + { + sample = InterpolatedPreviousSample.From(points[0]); + return true; + } + + for (int index = 1; index < points.Count; index++) + { + EmTrajectoryPoint right = points[index]; + if (sampleTimeSeconds <= right.TimeFromStart + ProjectionTolerance) + { + EmTrajectoryPoint left = points[index - 1]; + double ratio = (sampleTimeSeconds - left.TimeFromStart) / + (right.TimeFromStart - left.TimeFromStart); + ratio = Math.Max(0d, Math.Min(1d, ratio)); + sample = new InterpolatedPreviousSample( + Linear(left.X, right.X, ratio), + Linear(left.Y, right.Y, ratio), + Linear(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, ratio)); + return true; + } + } + + return false; + } + + private static bool TryProjectMonotonically(LateralPath path, double x, double y, double minimumPathS, + out double projectedPathS) + { + projectedPathS = 0d; + double bestDistanceSquared = double.PositiveInfinity; + bool found = false; + for (int index = 1; index < path.Points.Count; index++) + { + LateralPathPoint left = path.Points[index - 1]; + LateralPathPoint right = path.Points[index]; + double dx = right.X - left.X; + double dy = right.Y - left.Y; + double lengthSquared = dx * dx + dy * dy; + if (!IsFinite(lengthSquared) || lengthSquared <= ProjectionTolerance) + continue; + + double ratio = ((x - left.X) * dx + (y - left.Y) * dy) / lengthSquared; + ratio = Math.Max(0d, Math.Min(1d, ratio)); + double candidatePathS = Linear(left.PathS, right.PathS, ratio); + if (candidatePathS + ProjectionTolerance < minimumPathS) + continue; + + double projectedX = Linear(left.X, right.X, ratio); + double projectedY = Linear(left.Y, right.Y, ratio); + double distanceSquared = (x - projectedX) * (x - projectedX) + (y - projectedY) * (y - projectedY); + if (!found || distanceSquared < bestDistanceSquared - ProjectionTolerance || + (Math.Abs(distanceSquared - bestDistanceSquared) <= ProjectionTolerance && candidatePathS < projectedPathS)) + { + projectedPathS = candidatePathS; + bestDistanceSquared = distanceSquared; + found = true; + } + } + + if (!found) + return false; + if (minimumPathS > double.NegativeInfinity) + projectedPathS = Math.Max(minimumPathS, projectedPathS); + return true; + } + + private static double Linear(double left, double right, double ratio) + { + return left + (right - left) * ratio; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } + + private readonly struct InterpolatedPreviousSample + { + public InterpolatedPreviousSample(double x, double y, double signedSpeedMetersPerSecond) + { + X = x; + Y = y; + SignedSpeedMetersPerSecond = signedSpeedMetersPerSecond; + } + + public double X { get; } + public double Y { get; } + public double SignedSpeedMetersPerSecond { get; } + + public static InterpolatedPreviousSample From(EmTrajectoryPoint point) + { + return new InterpolatedPreviousSample(point.X, point.Y, point.SignedLongitudinalVelocity); + } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs index 4652c1a..4fe53a9 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs @@ -13,6 +13,7 @@ internal static class EmPlanningServiceChecks { public static void Run() { + VerifiesPreviousTrajectoryIsALongitudinalSoftReference(); VerifiesForwardReverseAndBoundarySuccessesAreDeterministic(); VerifiesRequestAndStateFailuresPublishNoTrajectory(); VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory(); @@ -31,7 +32,7 @@ internal static class EmPlanningServiceChecks VerifySameTrajectory(firstForward, secondForward, "forward deterministic result"); Verification.Equal(2, forwardRequest.ReferencePath.Path.Count, "request-owned reference list remains unchanged"); - EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, 0d, false, false); + EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, -0.01d, false, false); EmPlanningResult reverse = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(reverseRequest, CancellationToken.None); VerifySuccess(reverse, reverseRequest, EmTerminalType.Goal, "reverse"); @@ -70,7 +71,7 @@ internal static class EmPlanningServiceChecks private static void VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory() { EmPlanningRequest projectionFailure = CreateRequest(TravelDirection.Forward, 0d, false, false); - projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, + projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(3d, 0d, 0d), 0d, null, projectionFailure.RequestedAtUtc, projectionFailure.VehicleState.SequenceId)); VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(projectionFailure, CancellationToken.None), EmPlanningStatus.ProjectionFailed, "bounded projection failure"); @@ -80,15 +81,20 @@ internal static class EmPlanningServiceChecks VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(corridorFailure, CancellationToken.None), EmPlanningStatus.CorridorInfeasible, "corridor infeasible"); - EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false); + EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false, + referencePath: CreateReferencePath(TravelDirection.Forward, false, 0.0055d)); VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stoppingFailure, CancellationToken.None), EmPlanningStatus.StoppingDistanceInsufficient, "stopping distance insufficient"); EmPlanningRequest regular = CreateRequest(TravelDirection.Forward, 0d, false, false); VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LateralInfeasible)).Plan(regular, CancellationToken.None), EmPlanningStatus.LateralInfeasible, "lateral infeasible"); - VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular, - CancellationToken.None), EmPlanningStatus.LongitudinalInfeasible, "longitudinal infeasible"); + EmPlanningResult longitudinalFallback = new EmPlanningService( + new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular, CancellationToken.None); + Verification.Equal(EmPlanningStatus.SuccessWithFallback, longitudinalFallback.Status, + "longitudinal infeasible uses the validated fallback seed"); + Verification.True(longitudinalFallback.Trajectory != null, + "longitudinal fallback still publishes a complete trajectory"); VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.SolverUnavailable)).Plan(regular, CancellationToken.None), EmPlanningStatus.SolverUnavailable, "solver unavailable"); } @@ -127,6 +133,56 @@ internal static class EmPlanningServiceChecks VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation"); } + private static void VerifiesPreviousTrajectoryIsALongitudinalSoftReference() + { + EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false); + + var withoutPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success); + EmPlanningResult withoutPrevious = new EmPlanningService(withoutPreviousSolver).Plan(request, CancellationToken.None); + VerifySuccess(withoutPrevious, request, EmTerminalType.Goal, "no previous longitudinal seed"); + + EmTrajectory validPrevious = CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Forward, + request.SegmentIndex); + EmPlanningRequest withPreviousRequest = ReplacePreviousTrajectory(request, validPrevious); + var withPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success); + EmPlanningResult withPrevious = new EmPlanningService(withPreviousSolver).Plan(withPreviousRequest, + CancellationToken.None); + VerifySuccess(withPrevious, withPreviousRequest, EmTerminalType.Goal, "valid previous longitudinal seed"); + + QuadraticProgram withoutPreviousProblem = withoutPreviousSolver.LastLongitudinalProblem + ?? throw new InvalidOperationException("The no-seed longitudinal QP was not captured."); + QuadraticProgram withPreviousProblem = withPreviousSolver.LastLongitudinalProblem + ?? throw new InvalidOperationException("The seeded longitudinal QP was not captured."); + var layout = new LongitudinalVariableLayout((withPreviousProblem.VariableCount + 1) / 4); + Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)) > + MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)), + "valid previous seed adds a nonzero previous-S soft-reference Hessian term"); + Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)) > + MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)), + "valid previous seed adds a nonzero previous-U soft-reference Hessian term"); + Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.S(1)] - + withoutPreviousProblem.LinearCost[layout.S(1)]) > 1e-12d, + "valid previous seed adds a nonzero previous-S soft-reference linear term"); + Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.U(1)] - + withoutPreviousProblem.LinearCost[layout.U(1)]) > 1e-12d, + "valid previous seed adds a nonzero previous-U soft-reference linear term"); + + EmPlanningRequest incompatibleRequest = ReplacePreviousTrajectory(request, + CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Reverse, request.SegmentIndex)); + var incompatibleSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success); + EmPlanningResult incompatible = new EmPlanningService(incompatibleSolver).Plan(incompatibleRequest, + CancellationToken.None); + VerifySuccess(incompatible, incompatibleRequest, EmTerminalType.Goal, "incompatible previous seed"); + QuadraticProgram incompatibleProblem = incompatibleSolver.LastLongitudinalProblem + ?? throw new InvalidOperationException("The incompatible-seed longitudinal QP was not captured."); + Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)), + MatrixValue(incompatibleProblem.UpperTriangularP, layout.S(1), layout.S(1)), + "incompatible previous seed omits previous-S soft-reference term"); + Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)), + MatrixValue(incompatibleProblem.UpperTriangularP, layout.U(1), layout.U(1)), + "incompatible previous seed omits previous-U soft-reference term"); + } + private static void VerifySuccess(EmPlanningResult result, EmPlanningRequest request, EmTerminalType terminalType, string name) { @@ -170,7 +226,7 @@ internal static class EmPlanningServiceChecks configuration.Solver.MaximumOuterIterations = 2; configuration.Scheduling.SolverTimeoutSeconds = 1d; if (rolling) - configuration.Scheduling.DistanceHorizonMeters = 0.003d; + configuration.Scheduling.DistanceHorizonMeters = 0.30d; return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false), new VehicleParameters { @@ -191,21 +247,55 @@ internal static class EmPlanningServiceChecks source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel); } - private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch) + private static EmPlanningRequest ReplacePreviousTrajectory(EmPlanningRequest source, EmTrajectory previousTrajectory) { - double endX = direction == TravelDirection.Forward ? 0.0055d : -0.0055d; + return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, source.VehicleState, + source.Configuration, source.SegmentIndex, previousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc, + source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel); + } + + private static EmTrajectory CreateLongitudinalPreviousTrajectory(DateTimeOffset effectiveAtUtc, + TravelDirection direction, int segmentIndex) + { + var metadata = new EmTrajectoryMetadata("previous-service", effectiveAtUtc, effectiveAtUtc, 1L, + "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop, + EmLongitudinalMode.RollingContinuation); + double sign = direction == TravelDirection.Forward ? 1d : -1d; + return new EmTrajectory(metadata, new[] + { + new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 0d, 0d, segmentIndex, 0.001d, 0.001d, + direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 6d, 0d, segmentIndex, 0.001d, 0.001d, + direction, EmBoundaryType.None, 0d, 0d), + }); + } + + private static double MatrixValue(SparseCscMatrix matrix, int row, int column) + { + for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++) + { + if (matrix.RowIndices[index] == row) + return matrix.Values[index]; + } + return 0d; + } + + private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch, + double lengthMeters = 2d) + { + double endX = direction == TravelDirection.Forward ? lengthMeters : -lengthMeters; var points = new List { new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, direction, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), - new SmoothedPathPoint(endX, 0d, 0d, 0d, 0.0055d, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch, + new SmoothedPathPoint(endX, 0d, 0d, 0d, lengthMeters, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch, endsAtGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor), }; var segments = new List { new SmoothedPathSegment(0, direction, 0, 1, false, endsAtGearSwitch), }; - var metrics = new PathQualityMetrics(true, 0.0055d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d); + var metrics = new PathQualityMetrics(true, lengthMeters, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d); return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments, new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List()); } @@ -218,7 +308,7 @@ internal static class EmPlanningServiceChecks : Array.Empty(); PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest { - Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f), + Bounds = new MapBoundsMm(-3000f, 3000f, -1000f, 1000f), ResolutionMm = 20f, ObstacleSources = sources, AllowExplicitEmptyMap = !blockStart, @@ -245,6 +335,8 @@ internal static class EmPlanningServiceChecks private readonly PlanningGridMap? mapToCorrupt; private int longitudinalCallCount; + public QuadraticProgram? LastLongitudinalProblem { get; private set; } + public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null) { this.mode = mode; @@ -265,6 +357,7 @@ internal static class EmPlanningServiceChecks return Result(QpSolveStatus.TimeLimit, Array.Empty()); return Result(QpSolveStatus.Solved, new double[problem.VariableCount]); } + LastLongitudinalProblem = problem; if (mode == PipelineSolverMode.LongitudinalInfeasible) return Result(QpSolveStatus.PrimalInfeasible, Array.Empty()); if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0) @@ -272,7 +365,7 @@ internal static class EmPlanningServiceChecks if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1) return Result(QpSolveStatus.TimeLimit, Array.Empty()); longitudinalCallCount++; - return Result(QpSolveStatus.Solved, CreateStrictLongitudinalPrimal(problem)); + return Result(QpSolveStatus.Solved, warmStart); } private static void CorruptMapAtOrigin(PlanningGridMap? map) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index 25bca36..3cfa641 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -20,6 +20,7 @@ internal static class LongitudinalModelChecks VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime(); VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints(); VerifiesModeSpecificSolutionValidation(); + VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically(); } private static void VerifiesJerkLimitedStoppingProfileEndsAtRest() @@ -454,6 +455,34 @@ internal static class LongitudinalModelChecks "approach failure identifies the jerk-limited stoppable set"); } + private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically() + { + LateralPath path = CreateStraightPath(1d); + DateTimeOffset previousEffectiveAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d); + EmTrajectory previous = CreatePreviousTrajectory(previousEffectiveAtUtc, TravelDirection.Forward, 3); + var builder = new LongitudinalPreviousTrajectorySeedBuilder(); + LongitudinalPreviousTrajectorySeed seed = builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d), + new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Forward); + + Verification.Equal(3, seed.PathS.Count, "previous seed path-S count"); + Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count, "previous seed speed count"); + Verification.NearlyEqual(0.20d, seed.PathS[0], "previous seed begins at new absolute effective time"); + Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1], + "previous seed progress is monotone"); + Verification.NearlyEqual(0.10d, seed.ProgressSpeedMetersPerSecond[0], + "previous seed uses absolute progress speed"); + + Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d), + new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Reverse).PathS.Count, + "different direction returns an empty seed"); + Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d), + new[] { 0d, 0.10d, 0.20d }, 4, TravelDirection.Forward).PathS.Count, + "different segment returns an empty seed"); + Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.40d), + new[] { 0d, 0.10d }, 3, TravelDirection.Forward).PathS.Count, + "out-of-range absolute sampling returns an empty seed"); + } + private static LateralPath CreatePath(IReadOnlyList fixtures) { var points = new List(fixtures.Count); @@ -466,6 +495,28 @@ internal static class LongitudinalModelChecks return new LateralPath(points, true); } + private static EmTrajectory CreatePreviousTrajectory(DateTimeOffset effectiveAtUtc, TravelDirection direction, + int segmentIndex) + { + var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L, + "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop, + EmLongitudinalMode.RollingContinuation); + double sign = direction == TravelDirection.Forward ? 1d : -1d; + return new EmTrajectory(metadata, new[] + { + new EmTrajectoryPoint(0d, 0d, 0d, sign * 0.10d, 0d, 0d, segmentIndex, 0d, 0d, + direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(sign * 0.10d, 0d, 0d, sign * 0.10d, 0.10d, 0d, segmentIndex, 0.10d, 0.10d, + direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(sign * 0.20d, 0d, 0d, sign * 0.10d, 0.20d, 0d, segmentIndex, 0.20d, 0.20d, + direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(sign * 0.30d, 0d, 0d, sign * 0.10d, 0.30d, 0d, segmentIndex, 0.30d, 0.30d, + direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(sign * 0.40d, 0d, 0d, sign * 0.10d, 0.40d, 0d, segmentIndex, 0.40d, 0.40d, + direction, EmBoundaryType.None, 0d, 0d), + }); + } + private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType) { var points = new List