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
@@ -319,7 +319,7 @@ internal static class TrajectoryObservationMovementTestRunner
            sessionId, bootstrap, controller, settings, obstacleSnapshotVersion);
        var dynamicSnapshotBuilder = new TrajectoryObservationDynamicSnapshotBuilder();
        long visualizationSequence = 0L;
-        bool gearSwitchWaitingPrinted = false;
+        int lastLoggedWaitingSegmentIndex = -1;

        while (true)
        {
@@ -335,18 +335,17 @@ internal static class TrajectoryObservationMovementTestRunner
            TrajectoryObservationCharts charts = observation.PublishedTrajectory == null
                ? null
                : TrajectoryObservationCharts.Build(observation.PublishedTrajectory, segment,
-                    settings.MapPaddingMeters);
-            TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(
-                now, observation.PublishedTrajectory);
+                    effectiveConfiguration.Frenet.MaximumProjectionDistanceMeters);
+            TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(controller.SegmentState);
            DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic);
            webPublisher?.TryPublish(now, () => dynamicSnapshotBuilder.Build(
                Interlocked.Increment(ref visualizationSequence), tick, controller.ActiveSegment,
                controller.PreviousTrajectoryForVisualization, bootstrap.Vehicle,
                controller.CreateEffectiveConfigurationSnapshot()));
-            if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted)
+            if (runtimeState.WaitingAtGearSwitch && segment.SegmentIndex != lastLoggedWaitingSegmentIndex)
            {
                LogIfCurrent(sessionId, runtimeState.WorldNotice);
-                gearSwitchWaitingPrinted = true;
+                lastLoggedWaitingSegmentIndex = segment.SegmentIndex;
            }

            if (tick.PlanningStarted)
@@ -360,6 +359,11 @@ internal static class TrajectoryObservationMovementTestRunner
                        TrajectoryObservationDiagnostics.CreateCurveReport(charts, observation.PublishedTrajectory).Text);
                }
            }
+            if (tick.SegmentState.Phase == TrajectoryObservationSegmentPhase.Completed)
+            {
+                LogIfCurrent(sessionId, "all direction segments completed; observation session ended.");
+                break;
+            }
        }
    }

@@ -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)
@@ -106,7 +106,7 @@ public sealed class TrajectoryObservationSegmentTracker
            return CreateUpdate(false, true, "方向段已完成，忽略后续观察样本。");

        DirectionSegmentView current = segments[State.ActiveSegmentIndex];
-        if (!IsEligibleGearSwitchTerminal(now, measuredState, trajectory, current))
+        if (!IsEligibleTerminal(now, measuredState, trajectory, current))
        {
            if (State.Phase == TrajectoryObservationSegmentPhase.Planning)
                return CreateUpdate(false, false, "等待当前方向段的有效换向终点轨迹。");
@@ -167,11 +167,13 @@ public sealed class TrajectoryObservationSegmentTracker
            (!lastUpdateAtUtc.HasValue || now >= lastUpdateAtUtc.Value);
    }

-    private bool IsEligibleGearSwitchTerminal(DateTimeOffset now, VehicleMotionState measuredState,
+    private bool IsEligibleTerminal(DateTimeOffset now, VehicleMotionState measuredState,
        EmTrajectory trajectory, DirectionSegmentView current)
    {
        if (trajectory == null || trajectory.Metadata.SegmentIndex != current.SegmentIndex ||
-            trajectory.Metadata.Direction != current.Direction || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
+            trajectory.Metadata.Direction != current.Direction ||
+            (trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch &&
+                trajectory.Metadata.TerminalType != EmTerminalType.Goal))
            return false;

        EmTrajectoryPoint terminalPoint = trajectory.Points[trajectory.Points.Count - 1];
@@ -182,6 +184,8 @@ public sealed class TrajectoryObservationSegmentTracker
        if (!projector.TryProject(measuredState.Pose, current, current.LengthMeters, current.LengthMeters,
                settings.GearSwitchProjectionToleranceMeters, current.LengthMeters, out _))
            return false;
+        if (trajectory.Metadata.TerminalType == EmTerminalType.Goal)
+            return State.ActiveSegmentIndex + 1 >= segments.Count;
        if (State.ActiveSegmentIndex + 1 >= segments.Count)
            return true;