feat: 发布 EM 轨迹规划首个版本

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
+using System.Globalization;
 using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;

@@ -9,28 +10,65 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 /// <summary>Runs one deterministic EM LS/ST planning pipeline and publishes only independently validated trajectories.</summary>
 public sealed class EmPlanningService : IEmPlanningService
 {
+    private static readonly TimeSpan MaximumCycleDeadlineRemaining =
+        TimeSpan.FromMilliseconds(int.MaxValue);
    private readonly IQpSolver qpSolver;
    private readonly IEmPlannerDebugSink defaultDebugSink;
+    private readonly Func<TimeSpan> cycleElapsedForTesting;

    public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
+        : this(qpSolver, defaultDebugSink, null)
+    {
+    }
+
+    internal EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink,
+        Func<TimeSpan> cycleElapsedForTesting)
    {
        this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
        this.defaultDebugSink = defaultDebugSink;
+        this.cycleElapsedForTesting = cycleElapsedForTesting;
    }

    public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
    {
-        if (cancellationToken.IsCancellationRequested)
+        Stopwatch cycleStopwatch = cycleElapsedForTesting == null ? Stopwatch.StartNew() : null;
+        Func<TimeSpan> cycleElapsed = cycleElapsedForTesting ?? (() => cycleStopwatch.Elapsed);
+        if (CallerCancellationRequested(request, cancellationToken))
            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");
+        if (request?.CycleDeadlineRemaining is TimeSpan suppliedRemaining &&
+            (suppliedRemaining < TimeSpan.Zero || suppliedRemaining > MaximumCycleDeadlineRemaining))
+        {
+            return Failure(EmPlanningStatus.InvalidInput, request,
+                "CycleDeadlineRemaining must be between zero and " +
+                MaximumCycleDeadlineRemaining.TotalMilliseconds + " milliseconds.");
+        }
+        if (DeadlineExpired(request, cycleElapsed))
+            return CycleDeadlineFailure(request, "request", CycleRemaining(request, cycleElapsed));

        EmPlanningRequestValidationResult requestValidation = EmPlanningRequestValidator.Validate(request);
        if (!requestValidation.IsValid)
            return Failure(requestValidation.Status, request, requestValidation.FailureReason);
        EmPlannerConfiguration configuration = requestValidation.Snapshot.Configuration;
        EmitDebug(request, "request/config validation succeeded");
+        if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                "request-validation", out EmPlanningResult validationFailure))
+        {
+            return validationFailure;
+        }
+        TimeSpan expirationRemaining = CycleRemaining(request, cycleElapsed);
+        using var cycleExpiration = request.CycleDeadlineRemaining.HasValue
+            ? new CancellationTokenSource(expirationRemaining)
+            : null;
+        using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
+            cancellationToken, request.CallerCancellationToken, request.CycleDeadlineToken,
+            cycleExpiration?.Token ?? CancellationToken.None);
+        CancellationToken planningCancellationToken = linkedCancellation.Token;

        try
        {
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "projection", out EmPlanningResult deadlineFailure))
+                return deadlineFailure;
            IReadOnlyList<DirectionSegmentView> segments = ReferencePathSegmenter.Create(request.ReferencePath);
            if (request.SegmentIndex < 0 || request.SegmentIndex >= segments.Count)
                return Failure(EmPlanningStatus.InvalidReferencePath, request, "The requested direction segment is unavailable.");
@@ -59,7 +97,11 @@ public sealed class EmPlanningService : IEmPlanningService
                    "Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
            }
            EmitDebug(request, "bounded ego projection succeeded");
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "projection", out deadlineFailure))
+                return deadlineFailure;

+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
+                return deadlineFailure;
            double initialProgressSpeed = Math.Abs(request.VehicleState.SignedLongitudinalSpeedMetersPerSecond);
            double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
            var horizonSelector = new PlanningHorizonSelector();
@@ -84,18 +126,31 @@ public sealed class EmPlanningService : IEmPlanningService
                return Failure(EmPlanningStatus.CorridorInfeasible, request, corridorReason);
            }
            EmitDebug(request, "static connected corridor succeeded");
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
+                return deadlineFailure;

+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
+                return deadlineFailure;
+            TimeSpan lateralBudget = SmallerBudget(
+                TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds),
+                CycleRemaining(request, cycleElapsed));
+            EmPlannerConfiguration lateralConfiguration = configuration.Copy();
+            lateralConfiguration.Scheduling.SolverTimeoutSeconds = lateralBudget.TotalSeconds;
            var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
-                request.Vehicle, configuration, previousSeed);
-            TimeSpan totalSolveBudget = TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds);
+                request.Vehicle, lateralConfiguration, previousSeed);
+            TimeSpan totalSolveBudget = lateralBudget;
            var solveBudgetStopwatch = Stopwatch.StartNew();
-            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
+            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, planningCancellationToken);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
+                return deadlineFailure;
            if (!IsSuccess(lateral.Status))
                return Failure(lateral.Status, request, lateral.FailureReason);
            if (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
            EmitDebug(request, "LS optimization and validation succeeded");

+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
+                return deadlineFailure;
            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
                initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
                out string envelopeReason);
@@ -119,9 +174,15 @@ public sealed class EmPlanningService : IEmPlanningService
                new LongitudinalPreviousTrajectorySeedBuilder().Build(
                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
                    segment.SegmentIndex, segment.Direction);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
+                return deadlineFailure;
            TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
+            remainingSolveBudget = SmallerBudget(remainingSolveBudget, CycleRemaining(request, cycleElapsed));
            if (remainingSolveBudget <= TimeSpan.Zero)
            {
+                if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                        "longitudinal", out deadlineFailure))
+                    return deadlineFailure;
                return Failure(EmPlanningStatus.SolverTimedOut, request,
                    "LS/ST optimization exhausted the shared solve budget before ST optimization.");
            }
@@ -136,13 +197,22 @@ public sealed class EmPlanningService : IEmPlanningService
                return Failure(envelopeStatus, request, envelopeReason);
            EmitDebug(request, "PathS speed envelope succeeded");

-            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "longitudinal", out deadlineFailure))
+                return deadlineFailure;
+            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(
+                longitudinalInput, planningCancellationToken);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "longitudinal", out deadlineFailure))
+                return deadlineFailure;
            if (!IsSuccess(longitudinal.Status))
                return Failure(longitudinal.Status, request, longitudinal.FailureReason);
            if (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
            EmitDebug(request, "ST optimization and validation succeeded");

+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
+                return deadlineFailure;
            var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
                segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode,
@@ -151,10 +221,15 @@ public sealed class EmPlanningService : IEmPlanningService
                longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
            if (assemblyStatus != EmPlanningStatus.Success)
                return Failure(assemblyStatus, request, assemblyFailure);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
+                return deadlineFailure;
            if (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
            EmitDebug(request, "trajectory assembly succeeded");

+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "world-validation", out deadlineFailure))
+                return deadlineFailure;
            EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
            Pose2D terminalPose = IsRealTerminalBoundary(terminalBoundary)
                ? TerminalPose(lateral.Path)
@@ -167,8 +242,13 @@ public sealed class EmPlanningService : IEmPlanningService
                return Failure(MapPublicationFailure(publication.Failure), request,
                    publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
            }
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "world-validation", out deadlineFailure))
+                return deadlineFailure;
            EmitDebug(request, "world-space publication validation succeeded");

+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
+                return deadlineFailure;
            if (cancellationToken.IsCancellationRequested)
                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");

@@ -176,11 +256,25 @@ public sealed class EmPlanningService : IEmPlanningService
                longitudinal.Status == EmPlanningStatus.SuccessWithFallback
                ? EmPlanningStatus.SuccessWithFallback
                : EmPlanningStatus.Success;
-            return new EmPlanningResult(finalStatus, trajectory,
-                DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType + ";publication=validated");
+            string publicationDiagnostic = DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType +
+                ";publication=validated";
+            if (lateral.Status == EmPlanningStatus.SuccessWithFallback)
+                publicationDiagnostic += ";lateralFallback=" + lateral.FailureReason;
+            if (!string.IsNullOrWhiteSpace(longitudinal.FailureReason))
+            {
+                publicationDiagnostic += longitudinal.Status == EmPlanningStatus.SuccessWithFallback
+                    ? ";longitudinalFallback=" + longitudinal.FailureReason
+                    : ";longitudinal=" + longitudinal.FailureReason;
+            }
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
+                return deadlineFailure;
+            return new EmPlanningResult(finalStatus, trajectory, publicationDiagnostic);
        }
        catch (OperationCanceledException)
        {
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "operation", out EmPlanningResult deadlineFailure))
+                return deadlineFailure;
            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled.");
        }
        catch (ArgumentException exception)
@@ -193,6 +287,71 @@ public sealed class EmPlanningService : IEmPlanningService
        }
    }

+    private static bool TryTerminalFailure(EmPlanningRequest request, CancellationToken cancellationToken,
+        Func<TimeSpan> elapsed, string phase, out EmPlanningResult failure)
+    {
+        if (CallerCancellationRequested(request, cancellationToken))
+        {
+            failure = Failure(EmPlanningStatus.Cancelled, request,
+                "callerCancellation=true;phase=" + phase);
+            return true;
+        }
+        TimeSpan remaining = CycleRemaining(request, elapsed);
+        if (!DeadlineExpired(request, elapsed))
+        {
+            failure = null;
+            return false;
+        }
+        failure = CycleDeadlineFailure(request, phase, remaining);
+        return true;
+    }
+
+    private static bool CallerCancellationRequested(EmPlanningRequest request,
+        CancellationToken cancellationToken)
+    {
+        if (request?.CallerCancellationToken.IsCancellationRequested == true)
+            return true;
+        if (!cancellationToken.IsCancellationRequested)
+            return false;
+        return request == null || !request.CycleDeadlineToken.IsCancellationRequested ||
+            !request.CallerCancellationToken.CanBeCanceled;
+    }
+
+    private static bool DeadlineExpired(EmPlanningRequest request, Func<TimeSpan> elapsed)
+    {
+        return request != null && (request.CycleDeadlineToken.IsCancellationRequested ||
+            request.IsCycleDeadlineExpired() ||
+            request.CycleDeadlineRemaining.HasValue && CycleRemaining(request, elapsed) <= TimeSpan.Zero);
+    }
+
+    private static TimeSpan CycleRemaining(EmPlanningRequest request, Func<TimeSpan> elapsed)
+    {
+        TimeSpan snapshotRemaining = TimeSpan.MaxValue;
+        if (request.CycleDeadlineRemaining.HasValue)
+        {
+            snapshotRemaining = request.CycleDeadlineRemaining.Value - elapsed();
+            if (snapshotRemaining < TimeSpan.Zero)
+                snapshotRemaining = TimeSpan.Zero;
+        }
+        TimeSpan? dynamicRemaining = request.DynamicCycleDeadlineRemaining();
+        if (!dynamicRemaining.HasValue)
+            return snapshotRemaining;
+        return dynamicRemaining.Value < snapshotRemaining ? dynamicRemaining.Value : snapshotRemaining;
+    }
+
+    private static TimeSpan SmallerBudget(TimeSpan configured, TimeSpan cycleRemaining)
+    {
+        return configured < cycleRemaining ? configured : cycleRemaining;
+    }
+
+    private static EmPlanningResult CycleDeadlineFailure(EmPlanningRequest request, string phase,
+        TimeSpan remaining)
+    {
+        return Failure(EmPlanningStatus.CycleDeadlineExpired, request,
+            "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
+            remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
+    }
+
    private void EmitDebug(EmPlanningRequest request, string message)
    {
        if (defaultDebugSink == null || request == null || request.Configuration == null || request.Configuration.Solver == null ||