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.Collections.ObjectModel;
+using System.Globalization;
 using System.Threading;
 using System.Threading.Tasks;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
@@ -12,6 +13,7 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

 namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

+/// <summary>一次观察会话启动的不可变结果，成功时同时携带冻结的粗路径、平滑路径、地图和方向段。</summary>
 public sealed class TrajectoryObservationBootstrapResult
 {
    private readonly VehicleParameters vehicle;
@@ -87,6 +89,7 @@ public sealed class TrajectoryObservationBootstrapResult
    }
 }

+/// <summary>只在会话开始阶段构造并冻结规划输入；后续观察 tick 不会重新读取或改写该快照。</summary>
 public sealed class TrajectoryObservationBootstrapper
 {
    private readonly CoarsePathPlanningService coarseService;
@@ -109,22 +112,110 @@ public sealed class TrajectoryObservationBootstrapper
    {
        if (job == null) throw new ArgumentNullException(nameof(job));

-        CoarsePathPlanningJobResult coarse = coarseService.Plan(job, cancellationToken);
+        TimeSpan configuredLimit = job.Configuration == null
+            ? TimeSpan.FromSeconds(5d)
+            : job.Configuration.SearchTimeout;
+        using var deadline = new TrajectoryObservationPlanningDeadline(configuredLimit, cancellationToken);
+        return Bootstrap(job, deadline, cancellationToken);
+    }
+
+    public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
+        TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken = default)
+    {
+        if (job == null) throw new ArgumentNullException(nameof(job));
+        if (deadline == null) throw new ArgumentNullException(nameof(deadline));
+        if (DeadlineExpired(deadline))
+            return DeadlineFailure(job, null, null, deadline, "coarse");
+
+        CoarsePathPlanningJob effectiveJob = CopyJobWithDeadline(job, deadline);
+        bool coarseUsesCycleRemainder = job.Configuration != null &&
+            effectiveJob.Configuration.SearchTimeout < job.Configuration.SearchTimeout;
+        using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
+            deadline.Token, cancellationToken);
+
+        CoarsePathPlanningJobResult coarse = coarseService.Plan(effectiveJob, linkedCancellation.Token);
+        if (DeadlineExpired(deadline) ||
+            (coarseUsesCycleRemainder && coarse.PlanningResult.Status == PlanningStatus.SearchTimeout))
+        {
+            return DeadlineFailure(job, coarse, null, deadline, "coarse");
+        }
        if (coarse.PlanningResult.Status != PlanningStatus.Success)
            return TrajectoryObservationBootstrapResult.FromFailure(
                job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);

+        if (DeadlineExpired(deadline))
+            return DeadlineFailure(job, coarse, null, deadline, "smoothing");
+
        var smoothingRequest = new PathSmoothingRequest(
            CopyFiniteClearance(coarse.PlanningResult.Path, coarse.MapResult.Map),
            coarse.PlanningResult.Segments, coarse.MapResult.Map, job.Vehicle,
            new PathSmoothingConfiguration());
-        PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, cancellationToken);
+        PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, linkedCancellation.Token);
+        if (DeadlineExpired(deadline))
+            return DeadlineFailure(job, coarse, smooth, deadline, "smoothing");
        if (!IsPublishedSmoothingStatus(smooth.Status))
            return TrajectoryObservationBootstrapResult.FromFailure(
                job, coarse, smooth, smooth.Diagnostics.TerminationReason);
        return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
    }

+    private static bool DeadlineExpired(TrajectoryObservationPlanningDeadline deadline)
+    {
+        return deadline.IsExpired && !deadline.CallerCancellationRequested;
+    }
+
+    private static TrajectoryObservationBootstrapResult DeadlineFailure(CoarsePathPlanningJob job,
+        CoarsePathPlanningJobResult coarse, PathSmoothingResult smooth,
+        TrajectoryObservationPlanningDeadline deadline, string phase)
+    {
+        string diagnostic = "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
+            deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture);
+        return TrajectoryObservationBootstrapResult.FromFailure(job, coarse, smooth, diagnostic);
+    }
+
+    private static CoarsePathPlanningJob CopyJobWithDeadline(CoarsePathPlanningJob source,
+        TrajectoryObservationPlanningDeadline deadline)
+    {
+        return new CoarsePathPlanningJob
+        {
+            MapRequest = source.MapRequest,
+            Start = source.Start,
+            Goal = source.Goal,
+            Vehicle = source.Vehicle,
+            Configuration = CopyConfigurationWithDeadline(source.Configuration, deadline),
+            StartVehicleCurvature = source.StartVehicleCurvature,
+            StartDirection = source.StartDirection,
+            GoalDirection = source.GoalDirection,
+            DebugOptions = source.DebugOptions,
+        };
+    }
+
+    private static HybridAStarConfiguration CopyConfigurationWithDeadline(HybridAStarConfiguration source,
+        TrajectoryObservationPlanningDeadline deadline)
+    {
+        if (source == null) return null;
+        return new HybridAStarConfiguration
+        {
+            PrimitiveLengthMeters = source.PrimitiveLengthMeters,
+            IntegrationStepMeters = source.IntegrationStepMeters,
+            MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
+            HeadingResolutionRadians = source.HeadingResolutionRadians,
+            CurvatureLevelCount = source.CurvatureLevelCount,
+            GoalPositionToleranceMeters = source.GoalPositionToleranceMeters,
+            GoalHeadingToleranceRadians = source.GoalHeadingToleranceRadians,
+            MaximumExpandedNodes = source.MaximumExpandedNodes,
+            SearchTimeout = deadline.Clamp(source.SearchTimeout),
+            HeuristicWeight = source.HeuristicWeight,
+            ReverseCostMultiplier = source.ReverseCostMultiplier,
+            GearSwitchPenaltyMeters = source.GearSwitchPenaltyMeters,
+            CurvatureMagnitudeWeight = source.CurvatureMagnitudeWeight,
+            CurvatureChangePenaltyMetersPerLevel = source.CurvatureChangePenaltyMetersPerLevel,
+            ClearanceCostWeight = source.ClearanceCostWeight,
+            ClearanceCostDistanceMeters = source.ClearanceCostDistanceMeters,
+            AllowReverse = source.AllowReverse,
+        };
+    }
+
    private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
        PlanningGridMap map)
    {
@@ -158,6 +249,7 @@ public sealed class TrajectoryObservationBootstrapper
    }
 }

+/// <summary>单个观察 tick 的只读结果，供状态文本、报告和可视化共同消费。</summary>
 public sealed class TrajectoryObservationObservation
 {
    internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
@@ -185,6 +277,7 @@ public sealed class TrajectoryObservationObservation
    public TrajectoryExecutionState ExecutorState { get; }
 }

+/// <summary>协调一次冻结会话中的 EM 规划、轨迹采样和诊断；从不将预测命令发送给车辆。</summary>
 public sealed class TrajectoryObservationController
 {
    private readonly TrajectoryObservationBootstrapResult bootstrap;
@@ -272,8 +365,19 @@ public sealed class TrajectoryObservationController

    public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
        CancellationToken cancellationToken)
+    {
+        using var deadline = new TrajectoryObservationPlanningDeadline(
+            TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), cancellationToken);
+        return await StartCycle(now, state, deadline, cancellationToken).ConfigureAwait(false);
+    }
+
+    public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
+        TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));
+        if (deadline == null) throw new ArgumentNullException(nameof(deadline));
+        using var callerCancellation = CancellationTokenSource.CreateLinkedTokenSource(
+            deadline.CallerToken, cancellationToken);

        int targetSegmentIndex = ActiveSegment.SegmentIndex;
        int pendingSegmentIndex = -1;
@@ -303,18 +407,53 @@ public sealed class TrajectoryObservationController
        }

        long currentCycleId = Interlocked.Increment(ref cycleId);
+        EmPlannerConfiguration effectiveConfiguration = configuration.Copy();
+        TimeSpan cycleRemaining = deadline.Remaining;
+        TimeSpan configuredSolverLimit = TimeSpan.FromSeconds(
+            configuration.Scheduling.SolverTimeoutSeconds);
+        if (cycleRemaining > TimeSpan.Zero &&
+            cycleRemaining < configuredSolverLimit - TimeSpan.FromMilliseconds(1d))
+        {
+            effectiveConfiguration.Scheduling.SolverTimeoutSeconds = cycleRemaining.TotalSeconds;
+        }
        var request = new EmPlanningRequest(
-            bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
+            bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, effectiveConfiguration,
            targetSegmentIndex, previousTrajectory, now, now,
            sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
            previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
-            EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope);
-        PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken)
-            .ConfigureAwait(false);
-        if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment && result.Published &&
-            result.Result.Trajectory != null && !planningPendingFullDirection)
+            EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope, cycleRemaining,
+            callerCancellation.Token, deadline.DeadlineToken,
+            deadline.PublicationAuthorization);
+        if (callerCancellation.IsCancellationRequested)
        {
-            activeFullDirectionTrajectory = result.Result.Trajectory;
+            var cancelledResult = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
+                "callerCancellation=true;phase=request");
+            return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
+                cancelledResult, false, cancelledResult.FailureReason);
+        }
+        if (deadline.DeadlineToken.IsCancellationRequested)
+        {
+            var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
+                "cycleDeadlineExpired=true;phase=request;remainingMs=" +
+                cycleRemaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
+            return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
+                expiredResult, false, expiredResult.FailureReason);
+        }
+        Action<EmTrajectory> fullDirectionCommit =
+            settings.PlanningScope == EmPlanningScope.FullDirectionSegment && !planningPendingFullDirection
+                ? trajectory => activeFullDirectionTrajectory = trajectory
+                : null;
+        PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(
+            new PlanningCycleInput(request, now), callerCancellation.Token,
+            fullDirectionCommit).ConfigureAwait(false);
+        if (!result.Published && deadline.IsExpired && !callerCancellation.IsCancellationRequested &&
+            result.Result.Status == EmPlanningStatus.Cancelled)
+        {
+            var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
+                "cycleDeadlineExpired=true;phase=planning;remainingMs=" +
+                deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
+            result = new PlanningCycleResult(result.Version, result.Identity, expiredResult, false,
+                expiredResult.FailureReason);
        }
        return result;
    }
@@ -454,6 +593,7 @@ public sealed class TrajectoryObservationLoopTick
    public bool ShouldLog => true;
 }

+/// <summary>按观察周期捕获新鲜只读状态并发布快照的循环；规划耗时不能改变观察安全边界。</summary>
 public sealed class TrajectoryObservationLoop
 {
    private readonly TrajectoryObservationController controller;
@@ -511,6 +651,7 @@ public enum TrajectoryObservationSessionEndReason
    Cancellation,
 }

+/// <summary>集中定义观察会话的开始、停止和故障清理语义，避免留下旧会话可视化。</summary>
 public static class TrajectoryObservationSessionLifecycle
 {
    public static bool ShouldClearLayers(TrajectoryObservationSessionEndReason reason)