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

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -0,0 +1,226 @@
+using System;
+using System.Diagnostics;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+/// <summary>Owns one monotonic time budget shared by every phase of an observation planning cycle.</summary>
+public sealed class TrajectoryObservationPlanningDeadline : IDisposable
+{
+    private static readonly TimeSpan MaximumTimerDelay = TimeSpan.FromMilliseconds(uint.MaxValue - 1d);
+    private readonly object authorizationGate = new object();
+    private readonly TimeSpan limit;
+    private readonly Stopwatch stopwatch;
+    private readonly TimeSpan? injectedElapsed;
+    private readonly CancellationToken callerToken;
+    private readonly CancellationTokenSource expirationCancellation;
+    private readonly CancellationTokenSource linkedCancellation;
+    private readonly Timer expirationTimer;
+    private readonly IEmPlanningPublicationAuthorization publicationAuthorization;
+    private bool callerCancellationObserved;
+    private bool deadlineExpired;
+    private bool disposed;
+
+    public TrajectoryObservationPlanningDeadline(TimeSpan limit, CancellationToken callerToken = default)
+        : this(limit, null, callerToken)
+    {
+    }
+
+    private TrajectoryObservationPlanningDeadline(TimeSpan limit, TimeSpan? injectedElapsed,
+        CancellationToken callerToken)
+    {
+        if (limit < TimeSpan.Zero)
+            throw new ArgumentOutOfRangeException(nameof(limit), "The planning deadline cannot be negative.");
+        if (injectedElapsed.HasValue && injectedElapsed.Value < TimeSpan.Zero)
+            throw new ArgumentOutOfRangeException(nameof(injectedElapsed));
+
+        this.limit = limit;
+        this.injectedElapsed = injectedElapsed;
+        this.callerToken = callerToken;
+        stopwatch = injectedElapsed.HasValue ? null : Stopwatch.StartNew();
+        expirationCancellation = new CancellationTokenSource();
+        TimeSpan initialRemaining = ComputeRemaining();
+        deadlineExpired = initialRemaining <= TimeSpan.Zero;
+        if (deadlineExpired)
+            expirationCancellation.Cancel();
+        linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
+            callerToken, expirationCancellation.Token);
+        publicationAuthorization = new DeadlinePublicationAuthorization(this);
+        if (!deadlineExpired)
+        {
+            TimeSpan timerDelay = initialRemaining < MaximumTimerDelay
+                ? initialRemaining
+                : MaximumTimerDelay;
+            expirationTimer = new Timer(ExpireFromTimer, null, timerDelay, Timeout.InfiniteTimeSpan);
+        }
+    }
+
+    public TimeSpan Remaining
+    {
+        get
+        {
+            bool cancelExpiration = false;
+            TimeSpan remaining;
+            lock (authorizationGate)
+            {
+                remaining = ComputeRemaining();
+                if (!deadlineExpired && remaining <= TimeSpan.Zero)
+                {
+                    deadlineExpired = true;
+                    cancelExpiration = true;
+                }
+                if (deadlineExpired)
+                    remaining = TimeSpan.Zero;
+            }
+            if (cancelExpiration)
+                CancelExpirationToken();
+            return remaining;
+        }
+    }
+
+    public bool IsExpired => callerToken.IsCancellationRequested || DeadlineExpired;
+
+    public CancellationToken Token => linkedCancellation.Token;
+
+    internal bool CallerCancellationRequested => callerToken.IsCancellationRequested;
+
+    internal CancellationToken CallerToken => callerToken;
+
+    internal CancellationToken DeadlineToken => expirationCancellation.Token;
+
+    internal IEmPlanningPublicationAuthorization PublicationAuthorization => publicationAuthorization;
+
+    internal static TrajectoryObservationPlanningDeadline CreateForTesting(TimeSpan limit,
+        TimeSpan elapsed, CancellationToken callerToken)
+    {
+        return new TrajectoryObservationPlanningDeadline(limit, elapsed, callerToken);
+    }
+
+    public TimeSpan Clamp(TimeSpan configuredLimit)
+    {
+        if (configuredLimit < TimeSpan.Zero)
+            throw new ArgumentOutOfRangeException(nameof(configuredLimit));
+        TimeSpan remaining = Remaining;
+        return configuredLimit < remaining ? configuredLimit : remaining;
+    }
+
+    public void Dispose()
+    {
+        lock (authorizationGate)
+            disposed = true;
+        expirationTimer?.Dispose();
+        linkedCancellation.Dispose();
+        expirationCancellation.Dispose();
+    }
+
+    private bool DeadlineExpired
+    {
+        get
+        {
+            _ = Remaining;
+            lock (authorizationGate)
+                return deadlineExpired;
+        }
+    }
+
+    private TimeSpan ComputeRemaining()
+    {
+        TimeSpan elapsed = injectedElapsed ?? stopwatch.Elapsed;
+        TimeSpan remaining = limit - elapsed;
+        return remaining > TimeSpan.Zero ? remaining : TimeSpan.Zero;
+    }
+
+    private EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
+        CancellationToken coordinatorCallerCancellationToken, Action publish)
+    {
+        if (publish == null)
+            throw new ArgumentNullException(nameof(publish));
+
+        bool cancelExpiration = false;
+        using CancellationTokenRegistration ownerCallerRegistration =
+            callerToken.Register(ObserveCallerCancellation);
+        using CancellationTokenRegistration requestCallerRegistration =
+            requestCallerCancellationToken.Register(ObserveCallerCancellation);
+        using CancellationTokenRegistration coordinatorCallerRegistration =
+            coordinatorCallerCancellationToken.Register(ObserveCallerCancellation);
+        EmPlanningPublicationDecision decision;
+        lock (authorizationGate)
+        {
+            if (callerToken.IsCancellationRequested ||
+                requestCallerCancellationToken.IsCancellationRequested ||
+                coordinatorCallerCancellationToken.IsCancellationRequested)
+            {
+                callerCancellationObserved = true;
+            }
+            if (!deadlineExpired && ComputeRemaining() <= TimeSpan.Zero)
+            {
+                deadlineExpired = true;
+                cancelExpiration = true;
+            }
+            if (callerCancellationObserved)
+                decision = EmPlanningPublicationDecision.CallerCancelled;
+            else if (deadlineExpired || disposed)
+                decision = EmPlanningPublicationDecision.DeadlineExpired;
+            else
+            {
+                publish();
+                decision = EmPlanningPublicationDecision.Published;
+            }
+        }
+        if (cancelExpiration)
+            CancelExpirationToken();
+        return decision;
+    }
+
+    private void ObserveCallerCancellation()
+    {
+        lock (authorizationGate)
+            callerCancellationObserved = true;
+    }
+
+    private void ExpireFromTimer(object state)
+    {
+        bool cancelExpiration = false;
+        lock (authorizationGate)
+        {
+            if (!disposed && !deadlineExpired)
+            {
+                deadlineExpired = true;
+                cancelExpiration = true;
+            }
+        }
+        if (cancelExpiration)
+            CancelExpirationToken();
+    }
+
+    private void CancelExpirationToken()
+    {
+        try
+        {
+            expirationCancellation.Cancel();
+        }
+        catch (ObjectDisposedException)
+        {
+            // Disposal can race a timer callback only after the owning cycle has completed.
+        }
+    }
+
+    private sealed class DeadlinePublicationAuthorization : IEmPlanningPublicationAuthorization
+    {
+        private readonly TrajectoryObservationPlanningDeadline owner;
+
+        public DeadlinePublicationAuthorization(TrajectoryObservationPlanningDeadline owner)
+        {
+            this.owner = owner;
+        }
+
+        public TimeSpan Remaining => owner.Remaining;
+
+        public bool IsExpired => owner.DeadlineExpired;
+
+        public EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
+            CancellationToken coordinatorCallerCancellationToken, Action publish) =>
+            owner.TryPublish(requestCallerCancellationToken, coordinatorCallerCancellationToken, publish);
+    }
+}