feat: coordinate rolling EM replans

2026-08-04 12:47:39 +08:00
parent 49109ec835
commit d75380cf8f
7 changed files with 497 additions and 1 deletions
@@ -0,0 +1,124 @@
+using System;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// Latest-wins rolling coordinator around the pure one-shot planning service.
+/// It owns no localization, hardware, UI, or wall-clock source; callers supply each cycle's state and time.
+/// </summary>
+public sealed class EmPlanningCoordinator
+{
+    private readonly IEmPlanningService planningService;
+    private readonly IEmPlanningCycleSink sink;
+    private readonly object publicationGate = new object();
+    private long latestCycleVersion;
+    private PlanningCycleIdentity latestIdentity;
+    private CancellationTokenSource latestCancellation;
+    private EmTrajectory publishedTrajectory;
+    private DateTimeOffset? lastCycleStartedAtUtc;
+    private double replanPeriodSeconds = 0.20d;
+
+    public EmPlanningCoordinator(IEmPlanningService planningService, IEmPlanningCycleSink sink = null)
+    {
+        this.planningService = planningService ?? throw new ArgumentNullException(nameof(planningService));
+        this.sink = sink;
+    }
+
+    public EmTrajectory PublishedTrajectory
+    {
+        get
+        {
+            lock (publicationGate)
+                return publishedTrajectory;
+        }
+    }
+
+    /// <summary>Returns whether a caller-supplied time is due for another rolling cycle.</summary>
+    public bool ShouldStartCycle(DateTimeOffset now)
+    {
+        lock (publicationGate)
+        {
+            return !lastCycleStartedAtUtc.HasValue ||
+                now - lastCycleStartedAtUtc.Value >= TimeSpan.FromSeconds(replanPeriodSeconds);
+        }
+    }
+
+    public Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input, CancellationToken cancellationToken)
+    {
+        if (input == null)
+            throw new ArgumentNullException(nameof(input));
+
+        long version;
+        CancellationTokenSource cycleCancellation;
+        lock (publicationGate)
+        {
+            latestCancellation?.Cancel();
+            latestCancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
+            cycleCancellation = latestCancellation;
+            version = ++latestCycleVersion;
+            latestIdentity = input.Identity;
+            lastCycleStartedAtUtc = input.Now;
+            replanPeriodSeconds = input.ReplanPeriodSeconds;
+        }
+
+        return Task.Run(() => CompleteCycle(version, input, cycleCancellation));
+    }
+
+    private PlanningCycleResult CompleteCycle(long version, PlanningCycleInput input,
+        CancellationTokenSource cycleCancellation)
+    {
+        EmPlanningResult planned;
+        try
+        {
+            planned = planningService.Plan(input.Request, cycleCancellation.Token);
+            if (planned == null)
+                planned = new EmPlanningResult(EmPlanningStatus.Failed, null, "planning service returned no result");
+        }
+        catch (OperationCanceledException)
+        {
+            planned = new EmPlanningResult(EmPlanningStatus.Cancelled, null, "planning cycle cancelled");
+        }
+        catch (Exception exception)
+        {
+            planned = new EmPlanningResult(EmPlanningStatus.Failed, null, "planning service exception: " + exception.Message);
+        }
+
+        PlanningCycleResult result;
+        lock (publicationGate)
+        {
+            bool current = version == latestCycleVersion && input.Identity.Equals(latestIdentity);
+            if (!current)
+            {
+                var superseded = new EmPlanningResult(EmPlanningStatus.Superseded, null,
+                    "rolling cycle superseded before publication");
+                result = new PlanningCycleResult(version, input.Identity, superseded, false, superseded.FailureReason);
+            }
+            else
+            {
+                bool publishable = (planned.Status == EmPlanningStatus.Success || planned.Status == EmPlanningStatus.SuccessWithFallback) &&
+                    planned.Trajectory != null;
+                if (publishable)
+                    publishedTrajectory = planned.Trajectory;
+                result = new PlanningCycleResult(version, input.Identity, planned, publishable, planned.FailureReason);
+            }
+        }
+
+        if (sink == null)
+            return result;
+
+        try
+        {
+            sink.OnCycleCompleted(result);
+            return result;
+        }
+        catch (Exception exception)
+        {
+            string diagnostic = string.IsNullOrEmpty(result.Diagnostic)
+                ? "cycle sink exception: " + exception.Message
+                : result.Diagnostic + "; cycle sink exception: " + exception.Message;
+            return result.WithDiagnostic(diagnostic);
+        }
+    }
+}
@@ -0,0 +1,7 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Optional observer for completed rolling planning cycles.</summary>
+public interface IEmPlanningCycleSink
+{
+    void OnCycleCompleted(PlanningCycleResult result);
+}
@@ -0,0 +1,75 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable identity bound to one rolling planning cycle.</summary>
+public sealed class PlanningCycleIdentity : IEquatable<PlanningCycleIdentity>
+{
+    public PlanningCycleIdentity(long mapSnapshotId, string referencePathId, long vehicleStateSequenceId,
+        string previousTrajectoryId, int segmentIndex)
+    {
+        if (mapSnapshotId < 0)
+            throw new ArgumentOutOfRangeException(nameof(mapSnapshotId));
+        if (string.IsNullOrWhiteSpace(referencePathId))
+            throw new ArgumentException("A reference path ID is required.", nameof(referencePathId));
+        if (vehicleStateSequenceId < 0)
+            throw new ArgumentOutOfRangeException(nameof(vehicleStateSequenceId));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+
+        MapSnapshotId = mapSnapshotId;
+        ReferencePathId = referencePathId;
+        VehicleStateSequenceId = vehicleStateSequenceId;
+        PreviousTrajectoryId = previousTrajectoryId ?? string.Empty;
+        SegmentIndex = segmentIndex;
+    }
+
+    public long MapSnapshotId { get; }
+
+    public string ReferencePathId { get; }
+
+    public long VehicleStateSequenceId { get; }
+
+    public string PreviousTrajectoryId { get; }
+
+    public int SegmentIndex { get; }
+
+    public static PlanningCycleIdentity FromRequest(EmPlanningRequest request)
+    {
+        if (request == null)
+            throw new ArgumentNullException(nameof(request));
+        if (request.Map == null)
+            throw new ArgumentException("A planning map is required for a rolling cycle.", nameof(request));
+        if (request.VehicleState == null)
+            throw new ArgumentException("A vehicle state is required for a rolling cycle.", nameof(request));
+
+        return new PlanningCycleIdentity(request.Map.SnapshotId, request.ReferencePathId,
+            request.VehicleState.SequenceId, request.PreviousTrajectoryId, request.SegmentIndex);
+    }
+
+    public bool Equals(PlanningCycleIdentity other)
+    {
+        return other != null && MapSnapshotId == other.MapSnapshotId &&
+            string.Equals(ReferencePathId, other.ReferencePathId, StringComparison.Ordinal) &&
+            VehicleStateSequenceId == other.VehicleStateSequenceId &&
+            string.Equals(PreviousTrajectoryId, other.PreviousTrajectoryId, StringComparison.Ordinal) &&
+            SegmentIndex == other.SegmentIndex;
+    }
+
+    public override bool Equals(object obj)
+    {
+        return Equals(obj as PlanningCycleIdentity);
+    }
+
+    public override int GetHashCode()
+    {
+        unchecked
+        {
+            int hash = MapSnapshotId.GetHashCode();
+            hash = (hash * 397) ^ StringComparer.Ordinal.GetHashCode(ReferencePathId);
+            hash = (hash * 397) ^ VehicleStateSequenceId.GetHashCode();
+            hash = (hash * 397) ^ StringComparer.Ordinal.GetHashCode(PreviousTrajectoryId);
+            return (hash * 397) ^ SegmentIndex;
+        }
+    }
+}
@@ -0,0 +1,29 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Caller-captured request and clock value for one rolling cycle.</summary>
+public sealed class PlanningCycleInput
+{
+    public PlanningCycleInput(EmPlanningRequest request, DateTimeOffset now)
+    {
+        Request = request ?? throw new ArgumentNullException(nameof(request));
+        Identity = PlanningCycleIdentity.FromRequest(request);
+        Now = now;
+        ReplanPeriodSeconds = ReadReplanPeriod(request.Configuration);
+    }
+
+    public EmPlanningRequest Request { get; }
+
+    public PlanningCycleIdentity Identity { get; }
+
+    public DateTimeOffset Now { get; }
+
+    public double ReplanPeriodSeconds { get; }
+
+    private static double ReadReplanPeriod(EmPlannerConfiguration configuration)
+    {
+        double configured = configuration?.Scheduling?.ReplanPeriodSeconds ?? 0.20d;
+        return !double.IsNaN(configured) && !double.IsInfinity(configured) && configured > 0d ? configured : 0.20d;
+    }
+}
@@ -0,0 +1,34 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable completion record for one rolling planning cycle.</summary>
+public sealed class PlanningCycleResult
+{
+    public PlanningCycleResult(long version, PlanningCycleIdentity identity, EmPlanningResult result, bool published,
+        string diagnostic)
+    {
+        if (version <= 0)
+            throw new ArgumentOutOfRangeException(nameof(version));
+        Version = version;
+        Identity = identity ?? throw new ArgumentNullException(nameof(identity));
+        Result = result ?? throw new ArgumentNullException(nameof(result));
+        Published = published;
+        Diagnostic = diagnostic ?? string.Empty;
+    }
+
+    public long Version { get; }
+
+    public PlanningCycleIdentity Identity { get; }
+
+    public EmPlanningResult Result { get; }
+
+    public bool Published { get; }
+
+    public string Diagnostic { get; }
+
+    internal PlanningCycleResult WithDiagnostic(string diagnostic)
+    {
+        return new PlanningCycleResult(Version, Identity, Result, Published, diagnostic);
+    }
+}