feat: publish one-shot EM trajectories

2026-08-04 11:55:20 +08:00
parent 7410654e68
commit 019b89645a
5 changed files with 730 additions and 1 deletions
@@ -0,0 +1,367 @@
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Threading;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class EmPlanningServiceChecks
+{
+    public static void Run()
+    {
+        VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
+        VerifiesRequestAndStateFailuresPublishNoTrajectory();
+        VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
+        VerifiesTimeoutFallbackAndCancellationSemantics();
+        VerifiesPublicationFailureAndDebugIsolation();
+    }
+
+    private static void VerifiesForwardReverseAndBoundarySuccessesAreDeterministic()
+    {
+        EmPlanningRequest forwardRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        var forwardService = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success));
+        EmPlanningResult firstForward = forwardService.Plan(forwardRequest, CancellationToken.None);
+        EmPlanningResult secondForward = forwardService.Plan(forwardRequest, CancellationToken.None);
+        VerifySuccess(firstForward, forwardRequest, EmTerminalType.Goal, "forward");
+        VerifySuccess(secondForward, forwardRequest, EmTerminalType.Goal, "forward repeat");
+        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);
+        EmPlanningResult reverse = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(reverseRequest,
+            CancellationToken.None);
+        VerifySuccess(reverse, reverseRequest, EmTerminalType.Goal, "reverse");
+        Verification.True(reverse.Trajectory.Points[1].SignedLongitudinalVelocity < 0d,
+            "reverse service publishes negative signed velocity");
+
+        EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0d, true, false);
+        EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(gearRequest,
+            CancellationToken.None);
+        VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear switch");
+
+        EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0d, false, true);
+        EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(rollingRequest,
+            CancellationToken.None);
+        VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "rolling stop");
+    }
+
+    private static void VerifiesRequestAndStateFailuresPublishNoTrajectory()
+    {
+        EmPlanningRequest invalidSmoothing = CreateRequest(TravelDirection.Forward, 0d, false, false,
+            PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics()));
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(invalidSmoothing,
+            CancellationToken.None), EmPlanningStatus.InvalidReferencePath, "invalid smoothing");
+
+        EmPlanningRequest stale = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        stale = ReplaceState(stale, new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, null,
+            stale.RequestedAtUtc.AddSeconds(-1d), stale.VehicleState.SequenceId));
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stale,
+            CancellationToken.None), EmPlanningStatus.StaleVehicleState, "stale state");
+
+        EmPlanningRequest directionMismatch = CreateRequest(TravelDirection.Reverse, 0.02d, false, false);
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(directionMismatch,
+            CancellationToken.None), EmPlanningStatus.StateDirectionMismatch, "state direction mismatch");
+    }
+
+    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.RequestedAtUtc, projectionFailure.VehicleState.SequenceId));
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(projectionFailure,
+            CancellationToken.None), EmPlanningStatus.ProjectionFailed, "bounded projection failure");
+
+        EmPlanningRequest corridorFailure = CreateRequest(TravelDirection.Forward, 0d, false, false,
+            referencePath: null, map: CreateMap(true));
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(corridorFailure,
+            CancellationToken.None), EmPlanningStatus.CorridorInfeasible, "corridor infeasible");
+
+        EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false);
+        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");
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.SolverUnavailable)).Plan(regular,
+            CancellationToken.None), EmPlanningStatus.SolverUnavailable, "solver unavailable");
+    }
+
+    private static void VerifiesTimeoutFallbackAndCancellationSemantics()
+    {
+        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.TimeoutWithoutFallback)).Plan(request,
+            CancellationToken.None), EmPlanningStatus.SolverTimedOut, "timeout without fallback");
+
+        EmPlanningResult fallback = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.TimeoutWithFallback)).Plan(
+            request, CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, fallback.Status, "timeout uses only strict fallback");
+        Verification.True(fallback.Trajectory != null && fallback.Trajectory.Points.Count > 0,
+            "timeout fallback publishes a complete trajectory");
+
+        using (var cancellation = new CancellationTokenSource())
+        {
+            cancellation.Cancel();
+            VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(request,
+                cancellation.Token), EmPlanningStatus.Cancelled, "cancellation");
+        }
+    }
+
+    private static void VerifiesPublicationFailureAndDebugIsolation()
+    {
+        EmPlanningRequest validationFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.PublicationValidationFailure,
+            validationFailure.Map)).Plan(validationFailure, CancellationToken.None), EmPlanningStatus.ValidationFailed,
+            "publication validation failure");
+
+        EmPlanningRequest debugRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        debugRequest.Configuration.Solver.NativeVerbose = true;
+        EmPlanningResult debugIsolated = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success),
+            new ThrowingDebugSink()).Plan(debugRequest, CancellationToken.None);
+        VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation");
+    }
+
+    private static void VerifySuccess(EmPlanningResult result, EmPlanningRequest request, EmTerminalType terminalType,
+        string name)
+    {
+        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
+            name + " successful status: " + result.FailureReason);
+        Verification.True(result.Trajectory != null && result.Trajectory.Metadata.TerminalType == terminalType,
+            name + " terminal type");
+        string identifiers = "map=" + request.Map.SnapshotId + ";reference=" + request.ReferencePathId + ";state=" +
+            request.VehicleState.SequenceId + ";previous=" + request.PreviousTrajectoryId + ";segment=" + request.SegmentIndex;
+        Verification.True(result.FailureReason.IndexOf(identifiers, StringComparison.Ordinal) >= 0,
+            name + " preserves request identifiers in deterministic diagnostics");
+    }
+
+    private static void VerifyFailure(EmPlanningResult result, EmPlanningStatus expected, string name)
+    {
+        Verification.Equal(expected, result.Status, name + " status: " + result.FailureReason);
+        Verification.True(result.Trajectory == null, name + " publishes no partial trajectory");
+    }
+
+    private static void VerifySameTrajectory(EmPlanningResult left, EmPlanningResult right, string name)
+    {
+        Verification.Equal(left.Status, right.Status, name + " status");
+        Verification.Equal(left.Trajectory.Points.Count, right.Trajectory.Points.Count, name + " point count");
+        for (int index = 0; index < left.Trajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint first = left.Trajectory.Points[index];
+            EmTrajectoryPoint second = right.Trajectory.Points[index];
+            Verification.NearlyEqual(first.X, second.X, name + " X " + index);
+            Verification.NearlyEqual(first.Y, second.Y, name + " Y " + index);
+            Verification.NearlyEqual(first.TimeFromStart, second.TimeFromStart, name + " time " + index);
+            Verification.NearlyEqual(first.SignedLongitudinalVelocity, second.SignedLongitudinalVelocity,
+                name + " signed speed " + index);
+        }
+    }
+
+    private static EmPlanningRequest CreateRequest(TravelDirection direction, double signedSpeed, bool endsAtGearSwitch,
+        bool rolling, PathSmoothingResult? referencePath = null, PlanningGridMap? map = null)
+    {
+        DateTimeOffset requestedAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Solver.MaximumOuterIterations = 2;
+        configuration.Scheduling.SolverTimeoutSeconds = 1d;
+        if (rolling)
+            configuration.Scheduling.DistanceHorizonMeters = 0.003d;
+        return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false),
+            new VehicleParameters
+            {
+                LengthMeters = 0.10d,
+                WidthMeters = 0.10d,
+                SafetyMarginMeters = 0d,
+                MaximumCurvaturePerMeter = 1d,
+            },
+            new VehicleMotionState(new Pose2D(0d, 0d, 0d), signedSpeed, 0d, requestedAtUtc, 7L), configuration, 0,
+            null, requestedAtUtc, requestedAtUtc, "output-trajectory", "reference-42", "prior-42",
+            EmMotionModel.NonholonomicForwardReverse);
+    }
+
+    private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state)
+    {
+        return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, state, source.Configuration,
+            source.SegmentIndex, source.PreviousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
+            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
+    }
+
+    private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch)
+    {
+        double endX = direction == TravelDirection.Forward ? 0.0055d : -0.0055d;
+        var points = new List<SmoothedPathPoint>
+        {
+            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,
+                endsAtGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor),
+        };
+        var segments = new List<SmoothedPathSegment>
+        {
+            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);
+        return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
+            new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
+    }
+
+    private static PlanningGridMap CreateMap(bool blockStart)
+    {
+        IMapObstacleSource[] sources = blockStart
+            ? new IMapObstacleSource[] { new ManualObstacleSource("service-obstacle", 1L, true,
+                new IMapObstacle[] { new AxisAlignedRectangleObstacle(-20f, 20f, -20f, 20f) }) }
+            : Array.Empty<IMapObstacleSource>();
+        PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
+            ResolutionMm = 20f,
+            ObstacleSources = sources,
+            AllowExplicitEmptyMap = !blockStart,
+        });
+        Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
+            "service map builds: " + result.FailureReason);
+        return result.Map!;
+    }
+
+    private enum PipelineSolverMode
+    {
+        Success,
+        LateralInfeasible,
+        LongitudinalInfeasible,
+        SolverUnavailable,
+        TimeoutWithoutFallback,
+        TimeoutWithFallback,
+        PublicationValidationFailure,
+    }
+
+    private sealed class ScriptedPipelineSolver : IQpSolver
+    {
+        private readonly PipelineSolverMode mode;
+        private readonly PlanningGridMap? mapToCorrupt;
+        private int longitudinalCallCount;
+
+        public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null)
+        {
+            this.mode = mode;
+            this.mapToCorrupt = mapToCorrupt;
+        }
+
+        public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
+            CancellationToken cancellationToken)
+        {
+            bool longitudinal = problem.VariableCount > 100;
+            if (mode == PipelineSolverMode.SolverUnavailable)
+                return Result(QpSolveStatus.SolverUnavailable, Array.Empty<double>());
+            if (!longitudinal)
+            {
+                if (mode == PipelineSolverMode.LateralInfeasible)
+                    return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
+                if (mode == PipelineSolverMode.TimeoutWithoutFallback)
+                    return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
+                return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
+            }
+            if (mode == PipelineSolverMode.LongitudinalInfeasible)
+                return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
+            if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
+                CorruptMapAtOrigin(mapToCorrupt);
+            if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
+                return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
+            longitudinalCallCount++;
+            return Result(QpSolveStatus.Solved, CreateStrictLongitudinalPrimal(problem));
+        }
+
+        private static void CorruptMapAtOrigin(PlanningGridMap? map)
+        {
+            if (map == null || !map.TryWorldToGrid(0d, 0d, out int row, out int column))
+                throw new InvalidOperationException("Unable to corrupt the publication test map.");
+            FieldInfo occupiedField = typeof(PlanningGridMap).GetField("_occupied", BindingFlags.Instance | BindingFlags.NonPublic)!
+                ?? throw new InvalidOperationException("Planning map occupancy storage was unavailable.");
+            FieldInfo distanceField = typeof(PlanningGridMap).GetField("_conservativeDistances",
+                BindingFlags.Instance | BindingFlags.NonPublic) ?? throw new InvalidOperationException("Planning map distance storage was unavailable.");
+            byte[] occupied = (byte[])occupiedField.GetValue(map)!;
+            double[] distances = (double[])distanceField.GetValue(map)!;
+            int index = row * map.Cols + column;
+            occupied[index] = 1;
+            distances[index] = 0d;
+        }
+
+        private static double[] CreateStrictLongitudinalPrimal(QuadraticProgram problem)
+        {
+            int variableCount = problem.VariableCount;
+            int knotCount = (variableCount + 1) / 4;
+            var layout = new LongitudinalVariableLayout(knotCount);
+            var jerk = new double[knotCount - 1];
+            const int rampIntervals = 5;
+            for (int index = 0; index < rampIntervals; index++) jerk[index] = 1d;
+            for (int index = rampIntervals; index < 3 * rampIntervals; index++) jerk[index] = -1d;
+            for (int index = 3 * rampIntervals; index < 4 * rampIntervals; index++) jerk[index] = 1d;
+            var times = new double[knotCount];
+            for (int index = 0; index < times.Length; index++) times[index] = index * 0.05d;
+            LongitudinalCandidate baseCandidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
+            double terminalPathS = ReadFixedVariable(problem, layout.S(knotCount - 1));
+            double scale = terminalPathS / baseCandidate.S[baseCandidate.S.Count - 1];
+            for (int index = 0; index < jerk.Length; index++) jerk[index] *= scale;
+            LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
+            var primal = new double[layout.VariableCount];
+            for (int index = 0; index < knotCount; index++)
+            {
+                primal[layout.S(index)] = candidate.S[index];
+                primal[layout.U(index)] = candidate.U[index];
+                primal[layout.A(index)] = candidate.A[index];
+            }
+            for (int index = 0; index < jerk.Length; index++) primal[layout.J(index)] = candidate.J[index];
+            for (int index = 4 * rampIntervals; index < knotCount; index++)
+            {
+                primal[layout.S(index)] = terminalPathS;
+                primal[layout.U(index)] = 0d;
+                primal[layout.A(index)] = 0d;
+            }
+            return primal;
+        }
+
+        private static double ReadFixedVariable(QuadraticProgram problem, int variable)
+        {
+            for (int row = 0; row < problem.ConstraintCount; row++)
+            {
+                int entryCount = 0;
+                double coefficient = 0d;
+                for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
+                {
+                    for (int index = problem.ConstraintMatrix.ColumnPointers[column];
+                        index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
+                    {
+                        if (problem.ConstraintMatrix.RowIndices[index] != row)
+                            continue;
+                        entryCount++;
+                        if (column == variable)
+                            coefficient = problem.ConstraintMatrix.Values[index];
+                    }
+                }
+                if (entryCount == 1 && Math.Abs(coefficient) > 1e-12d &&
+                    Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d)
+                {
+                    return problem.LowerBounds[row] / coefficient;
+                }
+            }
+            throw new InvalidOperationException("Expected a fixed ST variable constraint.");
+        }
+
+        private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal)
+        {
+            return new QpSolveResult(status, primal, 0d, 0d, 0d, 1, TimeSpan.Zero, status.ToString(), string.Empty);
+        }
+    }
+
+    private sealed class ThrowingDebugSink : IEmPlannerDebugSink
+    {
+        public void Write(string message)
+        {
+            throw new InvalidOperationException("debug sink failure");
+        }
+    }
+}
@@ -11,7 +11,8 @@ 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] != "trajectory"))
+            args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" &&
+            args[0] != "em-planning-service" && args[0] != "em-core-all"))
        {
            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;
@@ -93,6 +94,20 @@ internal static class Program
                MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
                Console.WriteLine("PASS trajectory");
            }
+            if (args[0] == "em-planning-service" || args[0] == "em-core-all")
+            {
+                if (args[0] == "em-core-all")
+                {
+                    MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
+                    Console.WriteLine("PASS longitudinal-model");
+                    MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.Run();
+                    Console.WriteLine("PASS longitudinal-integration");
+                    MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
+                    Console.WriteLine("PASS trajectory");
+                }
+                MultiWheelC.TrajectoryPlanning.EMPlanner.EmPlanningServiceChecks.Run();
+                Console.WriteLine("PASS em-planning-service");
+            }
            return 0;
        }
        catch (Exception exception)