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

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -58,6 +58,12 @@ public sealed class EmPlanningCoordinator
    /// <param name="cancellationToken">调用方取消令牌；会与本周期内部令牌链接。</param>
    /// <returns>完成后给出不可变 <see cref="PlanningCycleResult"/>；只有版本和身份仍当前且结果成功完整时其 <c>Published</c> 为 <see langword="true"/>。</returns>
    public Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input, CancellationToken cancellationToken)
+    {
+        return PlanLatestAsync(input, cancellationToken, null);
+    }
+
+    internal Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input,
+        CancellationToken cancellationToken, Action<EmTrajectory> publicationCommit)
    {
        if (input == null)
            throw new ArgumentNullException(nameof(input));
@@ -75,7 +81,8 @@ public sealed class EmPlanningCoordinator
            replanPeriodSeconds = input.ReplanPeriodSeconds;
        }

-        return Task.Run(() => CompleteCycle(version, input, cycleCancellation));
+        return Task.Run(() => CompleteCycle(version, input, cycleCancellation, cancellationToken,
+            publicationCommit));
    }

    /// <summary>从协调器当前已发布的不可变轨迹构建下一周期的安全交接决策。</summary>
@@ -100,7 +107,8 @@ public sealed class EmPlanningCoordinator
    /// <param name="cycleCancellation">本周期与调用方取消状态链接的令牌源。</param>
    /// <returns>包含原始或 Superseded 规划结果、发布标记和观察器诊断的周期记录。</returns>
    private PlanningCycleResult CompleteCycle(long version, PlanningCycleInput input,
-        CancellationTokenSource cycleCancellation)
+        CancellationTokenSource cycleCancellation, CancellationToken callerCancellationToken,
+        Action<EmTrajectory> publicationCommit)
    {
        EmPlanningResult planned;
        try
@@ -130,11 +138,57 @@ public sealed class EmPlanningCoordinator
            }
            else
            {
-                bool publishable = (planned.Status == EmPlanningStatus.Success || planned.Status == EmPlanningStatus.SuccessWithFallback) &&
-                    planned.Trajectory != null;
+                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);
+                {
+                    EmPlanningPublicationDecision decision = input.Request.TryAuthorizePublication(
+                        callerCancellationToken, () =>
+                        {
+                            publicationCommit?.Invoke(planned.Trajectory);
+                            publishedTrajectory = planned.Trajectory;
+                        });
+                    if (decision == EmPlanningPublicationDecision.CallerCancelled)
+                    {
+                        var cancelled = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
+                            "callerCancellation=true;phase=publication");
+                        result = new PlanningCycleResult(version, input.Identity, cancelled, false,
+                            cancelled.FailureReason);
+                    }
+                    else if (decision == EmPlanningPublicationDecision.DeadlineExpired)
+                    {
+                        var expired = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
+                            "cycleDeadlineExpired=true;phase=publication;remainingMs=0.000");
+                        result = new PlanningCycleResult(version, input.Identity, expired, false,
+                            expired.FailureReason);
+                    }
+                    else
+                    {
+                        result = new PlanningCycleResult(version, input.Identity, planned, true,
+                            planned.FailureReason);
+                    }
+                }
+                else if (input.Request.CallerCancellationToken.IsCancellationRequested ||
+                    callerCancellationToken.IsCancellationRequested)
+                {
+                    var cancelled = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
+                        "callerCancellation=true;phase=publication");
+                    result = new PlanningCycleResult(version, input.Identity, cancelled, false,
+                        cancelled.FailureReason);
+                }
+                else if (input.Request.CycleDeadlineToken.IsCancellationRequested ||
+                    input.Request.IsCycleDeadlineExpired())
+                {
+                    var expired = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
+                        "cycleDeadlineExpired=true;phase=publication;remainingMs=0.000");
+                    result = new PlanningCycleResult(version, input.Identity, expired, false,
+                        expired.FailureReason);
+                }
+                else
+                {
+                    result = new PlanningCycleResult(version, input.Identity, planned, false,
+                        planned.FailureReason);
+                }
            }
        }