chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -195,10 +195,14 @@ public sealed class TrajectoryObservationController
    private EmPlanningCoordinator coordinator;
    private TrajectoryExecutor executor;
    private EmTrajectory previousTrajectoryForVisualization;
+    private EmTrajectory activeFullDirectionTrajectory;
+    private EmPlanningCoordinator pendingFullDirectionCoordinator;
    private readonly string sessionId;
    private long cycleId;
    private int plannedSegmentIndex;
+    private int pendingFullDirectionSegmentIndex = -1;
    private bool planAttemptedForActiveSegment;
+    private bool pendingFullDirectionPlanAttempted;

    public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId)
@@ -226,7 +230,9 @@ public sealed class TrajectoryObservationController
        this.sessionId = sessionId;
    }

-    public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory;
+    public EmTrajectory PublishedTrajectory => settings.PlanningScope == EmPlanningScope.FullDirectionSegment
+        ? activeFullDirectionTrajectory
+        : coordinator.PublishedTrajectory;

    public DirectionSegmentView ActiveSegment => bootstrap.Segments[segmentTracker.State.ActiveSegmentIndex];

@@ -253,37 +259,97 @@ public sealed class TrajectoryObservationController
                plannedSegmentIndex = ActiveSegment.SegmentIndex;
                planAttemptedForActiveSegment = false;
            }
+            if (TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex))
+            {
+                if (pendingFullDirectionSegmentIndex != pendingSegmentIndex)
+                    return true;
+                return !pendingFullDirectionPlanAttempted;
+            }
            return !planAttemptedForActiveSegment;
        }
        return coordinator.ShouldStartCycle(now);
    }

-    public Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
+    public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
        CancellationToken cancellationToken)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));

-        planAttemptedForActiveSegment = true;
-        plannedSegmentIndex = ActiveSegment.SegmentIndex;
+        int targetSegmentIndex = ActiveSegment.SegmentIndex;
+        int pendingSegmentIndex = -1;
+        bool planningPendingFullDirection = settings.PlanningScope == EmPlanningScope.FullDirectionSegment &&
+            TryGetPendingFullDirectionSegmentIndex(out pendingSegmentIndex);
+        EmPlanningCoordinator targetCoordinator;
+        EmTrajectory previousTrajectory;
+        if (planningPendingFullDirection)
+        {
+            targetSegmentIndex = pendingSegmentIndex;
+            if (pendingFullDirectionSegmentIndex != targetSegmentIndex || pendingFullDirectionCoordinator == null)
+            {
+                pendingFullDirectionSegmentIndex = targetSegmentIndex;
+                pendingFullDirectionPlanAttempted = false;
+                pendingFullDirectionCoordinator = new EmPlanningCoordinator(planningService);
+            }
+            pendingFullDirectionPlanAttempted = true;
+            targetCoordinator = pendingFullDirectionCoordinator;
+            previousTrajectory = null;
+        }
+        else
+        {
+            planAttemptedForActiveSegment = true;
+            plannedSegmentIndex = ActiveSegment.SegmentIndex;
+            targetCoordinator = coordinator;
+            previousTrajectory = coordinator.PublishedTrajectory;
+        }
+
        long currentCycleId = Interlocked.Increment(ref cycleId);
-        EmTrajectory previousTrajectory = coordinator.PublishedTrajectory;
        var request = new EmPlanningRequest(
            bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
-            ActiveSegment.SegmentIndex, previousTrajectory, now, now,
+            targetSegmentIndex, previousTrajectory, now, now,
            sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
            previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
            EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope);
-        return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
+        PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken)
+            .ConfigureAwait(false);
+        if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment && result.Published &&
+            result.Result.Trajectory != null && !planningPendingFullDirection)
+        {
+            activeFullDirectionTrajectory = result.Result.Trajectory;
+        }
+        return result;
    }

    public bool TryAdvanceSegment(DateTimeOffset now, VehicleMotionState state)
    {
        if (state == null) throw new ArgumentNullException(nameof(state));

-        TrajectoryObservationSegmentUpdate update = segmentTracker.Update(now, state, coordinator.PublishedTrajectory);
+        if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment &&
+            segmentTracker.State.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection &&
+            !HasPendingFullDirectionTrajectory())
+        {
+            return false;
+        }
+
+        EmTrajectory activeTrajectory = PublishedTrajectory;
+        TrajectoryObservationSegmentUpdate update = segmentTracker.Update(now, state, activeTrajectory);
        if (!update.Advanced)
            return false;

+        if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment)
+        {
+            EmTrajectory pendingTrajectory = pendingFullDirectionCoordinator.PublishedTrajectory;
+            previousTrajectoryForVisualization = activeTrajectory;
+            activeFullDirectionTrajectory = RebaseEffectiveAt(pendingTrajectory, now);
+            coordinator = pendingFullDirectionCoordinator;
+            pendingFullDirectionCoordinator = null;
+            pendingFullDirectionSegmentIndex = -1;
+            pendingFullDirectionPlanAttempted = false;
+            executor = new TrajectoryExecutor(configuration);
+            plannedSegmentIndex = segmentTracker.State.ActiveSegmentIndex;
+            planAttemptedForActiveSegment = true;
+            return true;
+        }
+
        previousTrajectoryForVisualization = coordinator.PublishedTrajectory;
        coordinator = new EmPlanningCoordinator(planningService);
        executor = new TrajectoryExecutor(configuration);
@@ -296,7 +362,7 @@ public sealed class TrajectoryObservationController
    {
        if (state == null) throw new ArgumentNullException(nameof(state));

-        EmTrajectory trajectory = coordinator.PublishedTrajectory;
+        EmTrajectory trajectory = PublishedTrajectory;
        if (trajectory == null)
            return new TrajectoryObservationObservation(now, state, null, null, null, null);

@@ -313,6 +379,43 @@ public sealed class TrajectoryObservationController
        return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint,
            command, executorState);
    }
+
+    private bool TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex)
+    {
+        pendingSegmentIndex = -1;
+        if (settings.PlanningScope != EmPlanningScope.FullDirectionSegment ||
+            segmentTracker.State.Phase != TrajectoryObservationSegmentPhase.WaitingForDirection ||
+            segmentTracker.State.ActiveSegmentIndex + 1 >= bootstrap.Segments.Count)
+        {
+            return false;
+        }
+        pendingSegmentIndex = segmentTracker.State.ActiveSegmentIndex + 1;
+        return true;
+    }
+
+    private bool HasPendingFullDirectionTrajectory()
+    {
+        if (!TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex) ||
+            pendingFullDirectionSegmentIndex != pendingSegmentIndex || pendingFullDirectionCoordinator == null)
+        {
+            return false;
+        }
+
+        EmTrajectory pending = pendingFullDirectionCoordinator.PublishedTrajectory;
+        DirectionSegmentView segment = bootstrap.Segments[pendingSegmentIndex];
+        return pending != null && pending.Metadata.SegmentIndex == segment.SegmentIndex &&
+            pending.Metadata.Direction == segment.Direction;
+    }
+
+    private static EmTrajectory RebaseEffectiveAt(EmTrajectory source, DateTimeOffset effectiveAtUtc)
+    {
+        if (source == null) throw new ArgumentNullException(nameof(source));
+        EmTrajectoryMetadata metadata = source.Metadata;
+        return new EmTrajectory(new EmTrajectoryMetadata(metadata.TrajectoryId, metadata.GeneratedAtUtc, effectiveAtUtc,
+            metadata.MapSnapshotId, metadata.ReferencePathId, metadata.VehicleStateSequenceId,
+            metadata.PreviousTrajectoryId, metadata.SegmentIndex, metadata.Direction, metadata.TerminalType,
+            metadata.LongitudinalMode, metadata.PlanningScope), source.Points);
+    }
 }

 public sealed class TrajectoryObservationLoopTick
@@ -428,7 +531,7 @@ public static class TrajectoryObservationSessionLifecycle
 public sealed class TrajectoryObservationRuntimeState
 {
    public const string GearSwitchWaitingNotice =
-        "等待真实档位/方向确认；观察模式不会推进下一方向段";
+        "等待真实档位/方向确认；观察模式不会激活下一方向段";

    private TrajectoryObservationRuntimeState(bool waitingAtGearSwitch)
    {
@@ -440,6 +543,14 @@ public sealed class TrajectoryObservationRuntimeState

    public string WorldNotice { get; }

+    public static TrajectoryObservationRuntimeState Create(TrajectoryObservationSegmentState segmentState)
+    {
+        if (segmentState == null) throw new ArgumentNullException(nameof(segmentState));
+        bool waiting = segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForStop ||
+            segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection;
+        return new TrajectoryObservationRuntimeState(waiting);
+    }
+
    public static TrajectoryObservationRuntimeState Create(DateTimeOffset now, EmTrajectory trajectory)
    {
        if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)