feat: track observed direction segments

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationSegmentTracker.cs

@@ -0,0 +1,226 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+public enum TrajectoryObservationSegmentPhase
+{
+    Planning,
+    WaitingForStop,
+    WaitingForDirection,
+    Completed,
+}
+
+public sealed class TrajectoryObservationSegmentState
+{
+    internal TrajectoryObservationSegmentState(int activeSegmentIndex, TrajectoryObservationSegmentPhase phase,
+        TravelDirection? expectedDirection, int confirmedDirectionSamples)
+    {
+        ActiveSegmentIndex = activeSegmentIndex;
+        Phase = phase;
+        ExpectedDirection = expectedDirection;
+        ConfirmedDirectionSamples = confirmedDirectionSamples;
+    }
+
+    public int ActiveSegmentIndex { get; }
+
+    public TrajectoryObservationSegmentPhase Phase { get; }
+
+    public TravelDirection? ExpectedDirection { get; }
+
+    public int ConfirmedDirectionSamples { get; }
+}
+
+public sealed class TrajectoryObservationSegmentUpdate
+{
+    internal TrajectoryObservationSegmentUpdate(bool advanced, bool completed, string diagnostic,
+        TrajectoryObservationSegmentState state)
+    {
+        Advanced = advanced;
+        Completed = completed;
+        Diagnostic = diagnostic ?? string.Empty;
+        State = state ?? throw new ArgumentNullException(nameof(state));
+    }
+
+    public bool Advanced { get; }
+
+    public bool Completed { get; }
+
+    public string Diagnostic { get; }
+
+    public TrajectoryObservationSegmentState State { get; }
+}
+
+public sealed class TrajectoryObservationSegmentTracker
+{
+    private readonly IReadOnlyList<DirectionSegmentView> segments;
+    private readonly TrajectoryObservationSettings settings;
+    private readonly double stopSpeedTolerance;
+    private readonly FrenetProjector projector = new FrenetProjector();
+    private long? lastSequenceId;
+    private DateTimeOffset? lastUpdateAtUtc;
+    private DateTimeOffset? stopHoldStartedAtUtc;
+    private int confirmedDirectionSamples;
+
+    public TrajectoryObservationSegmentTracker(IReadOnlyList<DirectionSegmentView> segments,
+        TrajectoryObservationSettings settings, double stopSpeedTolerance)
+    {
+        if (segments == null || segments.Count == 0)
+            throw new ArgumentException("At least one direction segment is required.", nameof(segments));
+        if (settings == null) throw new ArgumentNullException(nameof(settings));
+        if (!IsFinite(stopSpeedTolerance) || stopSpeedTolerance <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(stopSpeedTolerance));
+
+        var copy = new List<DirectionSegmentView>(segments.Count);
+        for (int index = 0; index < segments.Count; index++)
+        {
+            DirectionSegmentView segment = segments[index] ?? throw new ArgumentException(
+                "Direction segments cannot contain null values.", nameof(segments));
+            if (segment.SegmentIndex != index)
+                throw new ArgumentException("Direction segments must be ordered with consecutive indexes.", nameof(segments));
+            copy.Add(segment);
+        }
+
+        this.segments = new ReadOnlyCollection<DirectionSegmentView>(copy);
+        this.settings = settings.CreateValidatedSnapshot();
+        this.stopSpeedTolerance = stopSpeedTolerance;
+        State = CreateState(0, TrajectoryObservationSegmentPhase.Planning, 0);
+    }
+
+    public TrajectoryObservationSegmentState State { get; private set; }
+
+    public TrajectoryObservationSegmentUpdate Update(DateTimeOffset now, VehicleMotionState measuredState,
+        EmTrajectory trajectory)
+    {
+        if (measuredState == null) throw new ArgumentNullException(nameof(measuredState));
+
+        if (!HasStrictlyNewEvidence(now, measuredState.SequenceId))
+            return ResetConfirmation("换向确认已重置：状态序列或观察时间不连续。");
+
+        lastSequenceId = measuredState.SequenceId;
+        lastUpdateAtUtc = now;
+        if (State.Phase == TrajectoryObservationSegmentPhase.Completed)
+            return CreateUpdate(false, true, "方向段已完成，忽略后续观察样本。");
+
+        DirectionSegmentView current = segments[State.ActiveSegmentIndex];
+        if (!IsEligibleGearSwitchTerminal(now, measuredState, trajectory, current))
+        {
+            if (State.Phase == TrajectoryObservationSegmentPhase.Planning)
+                return CreateUpdate(false, false, "等待当前方向段的有效换向终点轨迹。");
+            return ResetConfirmation("换向确认已重置：轨迹、终点或连接点投影不再满足条件。");
+        }
+
+        if (State.ActiveSegmentIndex + 1 >= segments.Count)
+        {
+            State = CreateState(State.ActiveSegmentIndex, TrajectoryObservationSegmentPhase.Completed, 0);
+            return CreateUpdate(false, true, "方向段观察完成：已到达最后一个方向段末端。");
+        }
+
+        double speed = measuredState.SignedLongitudinalSpeedMetersPerSecond;
+        switch (State.Phase)
+        {
+            case TrajectoryObservationSegmentPhase.Planning:
+                if (Math.Abs(speed) > stopSpeedTolerance)
+                    return CreateUpdate(false, false, "等待真实速度连续停车后确认换向。");
+                stopHoldStartedAtUtc = now;
+                confirmedDirectionSamples = 0;
+                State = CreateState(State.ActiveSegmentIndex, TrajectoryObservationSegmentPhase.WaitingForStop, 0);
+                return CreateUpdate(false, false, "已检测到停车样本，开始换向停车保持。");
+
+            case TrajectoryObservationSegmentPhase.WaitingForStop:
+                if (Math.Abs(speed) > stopSpeedTolerance)
+                    return ResetConfirmation("换向确认已重置：停车保持期间速度不为零。");
+                if (now - stopHoldStartedAtUtc.Value < TimeSpan.FromSeconds(settings.GearSwitchStopHoldSeconds))
+                    return CreateUpdate(false, false, "等待连续停车保持完成。");
+                State = CreateState(State.ActiveSegmentIndex, TrajectoryObservationSegmentPhase.WaitingForDirection, 0);
+                return CreateUpdate(false, false, "停车保持完成，等待下一方向的连续速度样本。");
+
+            case TrajectoryObservationSegmentPhase.WaitingForDirection:
+                TravelDirection expectedDirection = segments[State.ActiveSegmentIndex + 1].Direction;
+                if (!MatchesExpectedDirection(speed, expectedDirection))
+                    return ResetConfirmation("换向确认已重置：下一方向速度样本符号或幅值无效。");
+                confirmedDirectionSamples++;
+                if (confirmedDirectionSamples < settings.DirectionConfirmationSamples)
+                {
+                    State = CreateState(State.ActiveSegmentIndex,
+                        TrajectoryObservationSegmentPhase.WaitingForDirection, confirmedDirectionSamples);
+                    return CreateUpdate(false, false, "已确认下一方向速度样本 " + confirmedDirectionSamples + "/" +
+                        settings.DirectionConfirmationSamples + "。" );
+                }
+                int nextIndex = State.ActiveSegmentIndex + 1;
+                State = CreateState(nextIndex, TrajectoryObservationSegmentPhase.Planning, 0);
+                stopHoldStartedAtUtc = null;
+                confirmedDirectionSamples = 0;
+                return CreateUpdate(true, false, "换向确认完成：方向段 " + (nextIndex - 1) + " 已切换到 " + nextIndex + "。");
+
+            default:
+                throw new InvalidOperationException("Unexpected direction-segment phase.");
+        }
+    }
+
+    private bool HasStrictlyNewEvidence(DateTimeOffset now, long sequenceId)
+    {
+        return (!lastSequenceId.HasValue || sequenceId > lastSequenceId.Value) &&
+            (!lastUpdateAtUtc.HasValue || now >= lastUpdateAtUtc.Value);
+    }
+
+    private bool IsEligibleGearSwitchTerminal(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)
+            return false;
+
+        EmTrajectoryPoint terminalPoint = trajectory.Points[trajectory.Points.Count - 1];
+        if (terminalPoint.SegmentIndex != current.SegmentIndex || terminalPoint.Direction != current.Direction ||
+            now < trajectory.Metadata.EffectiveAtUtc + TimeSpan.FromSeconds(terminalPoint.TimeFromStart))
+            return false;
+
+        if (!projector.TryProject(measuredState.Pose, current, current.LengthMeters, current.LengthMeters,
+                settings.GearSwitchProjectionToleranceMeters, current.LengthMeters, out _))
+            return false;
+        if (State.ActiveSegmentIndex + 1 >= segments.Count)
+            return true;
+
+        DirectionSegmentView next = segments[State.ActiveSegmentIndex + 1];
+        return projector.TryProject(measuredState.Pose, next, 0d, 0d,
+            settings.GearSwitchProjectionToleranceMeters, 0d, out _);
+    }
+
+    private bool MatchesExpectedDirection(double signedSpeed, TravelDirection expectedDirection)
+    {
+        if (!IsFinite(signedSpeed) || Math.Abs(signedSpeed) < settings.DirectionConfirmationSpeedMetersPerSecond)
+            return false;
+        return expectedDirection == TravelDirection.Forward ? signedSpeed > 0d : signedSpeed < 0d;
+    }
+
+    private TrajectoryObservationSegmentUpdate ResetConfirmation(string diagnostic)
+    {
+        stopHoldStartedAtUtc = null;
+        confirmedDirectionSamples = 0;
+        State = CreateState(State.ActiveSegmentIndex, TrajectoryObservationSegmentPhase.Planning, 0);
+        return CreateUpdate(false, false, diagnostic);
+    }
+
+    private TrajectoryObservationSegmentState CreateState(int activeSegmentIndex,
+        TrajectoryObservationSegmentPhase phase, int samples)
+    {
+        TravelDirection? expectedDirection = activeSegmentIndex + 1 < segments.Count
+            ? segments[activeSegmentIndex + 1].Direction
+            : null;
+        return new TrajectoryObservationSegmentState(activeSegmentIndex, phase, expectedDirection, samples);
+    }
+
+    private TrajectoryObservationSegmentUpdate CreateUpdate(bool advanced, bool completed, string diagnostic)
+    {
+        return new TrajectoryObservationSegmentUpdate(advanced, completed, diagnostic, State);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -18,6 +18,7 @@ internal static class TrajectoryObservationChecks
     public static void Run()
     {
         TrajectoryObservationSettingsChecks.Run();
+        TrajectoryObservationSegmentChecks.Run();
         VerifiesObservationSourceHasNoActuatorCalls();
         VerifiesObservationSourceUsesRequiredOperatorText();
         VerifiesOperatorDocumentationUsesExactUiEntry();

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs

@@ -0,0 +1,187 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+namespace EMPlannerVerificationHost;
+
+internal static class TrajectoryObservationSegmentChecks
+{
+    public static void Run()
+    {
+        AdvancesOnlyAfterContinuousStopAndStableNextDirection();
+        RejectsNonTerminalOrMismatchedGearTrajectory();
+        ResetsConfirmationForInvalidDirectionEvidence();
+        CompletesOneSegmentWithoutIndexingPastTheEnd();
+    }
+
+    private static void AdvancesOnlyAfterContinuousStopAndStableNextDirection()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 0, 0, 0, TimeSpan.Zero);
+        var tracker = CreateTracker();
+        EmTrajectory trajectory = GearTerminal(t0);
+
+        tracker.Update(t0, StateAtSwitch(0d, t0, 1L), trajectory);
+        Verification.Equal(TrajectoryObservationSegmentPhase.WaitingForStop, tracker.State.Phase,
+            "first zero sample begins stop hold");
+        tracker.Update(t0.AddSeconds(0.21d), StateAtSwitch(0d, t0.AddSeconds(0.21d), 2L), trajectory);
+        Verification.Equal(TrajectoryObservationSegmentPhase.WaitingForDirection, tracker.State.Phase,
+            "continuous stop arms next direction");
+        tracker.Update(t0.AddSeconds(0.25d), StateAtSwitch(-0.03d, t0.AddSeconds(0.25d), 3L), trajectory);
+        tracker.Update(t0.AddSeconds(0.30d), StateAtSwitch(-0.03d, t0.AddSeconds(0.30d), 4L), trajectory);
+        TrajectoryObservationSegmentUpdate advanced = tracker.Update(
+            t0.AddSeconds(0.35d), StateAtSwitch(-0.03d, t0.AddSeconds(0.35d), 5L), trajectory);
+
+        Verification.True(advanced.Advanced, "three stable reverse samples advance exactly one segment");
+        Verification.Equal(1, tracker.State.ActiveSegmentIndex, "tracker advances only from N to N+1");
+        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase,
+            "advanced tracker resumes planning on next segment");
+    }
+
+    private static void RejectsNonTerminalOrMismatchedGearTrajectory()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 1, 0, 0, TimeSpan.Zero);
+        var tracker = CreateTracker();
+
+        tracker.Update(t0, StateAtSwitch(0d, t0, 1L), GearTerminal(t0, EmTerminalType.Goal));
+        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase,
+            "non-gear terminal cannot begin confirmation");
+
+        tracker.Update(t0.AddSeconds(0.01d), StateAtSwitch(0d, t0.AddSeconds(0.01d), 2L),
+            GearTerminal(t0, EmTerminalType.GearSwitch, TravelDirection.Reverse));
+        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase,
+            "wrong trajectory direction cannot begin confirmation");
+
+        tracker.Update(t0.AddSeconds(0.02d), StateAtSwitch(0d, t0.AddSeconds(0.02d), 3L),
+            GearTerminal(t0, EmTerminalType.GearSwitch, TravelDirection.Forward, 0.10d));
+        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase,
+            "trajectory before its absolute terminal cannot begin confirmation");
+    }
+
+    private static void ResetsConfirmationForInvalidDirectionEvidence()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 2, 0, 0, TimeSpan.Zero);
+        AssertConfirmationReset(t0, StateAtSwitch(0.03d, t0.AddSeconds(0.25d), 3L), GearTerminal(t0),
+            "wrong signed direction resets confirmation");
+        AssertConfirmationReset(t0, StateAtSwitch(0d, t0.AddSeconds(0.25d), 3L), GearTerminal(t0),
+            "zero speed resets confirmation");
+        AssertConfirmationReset(t0, StateAt(2d, 0d, -0.03d, t0.AddSeconds(0.25d), 3L), GearTerminal(t0),
+            "excessive switch projection distance resets confirmation");
+        AssertConfirmationReset(t0, StateAtSwitch(-0.03d, t0.AddSeconds(0.25d), 3L),
+            GearTerminal(t0, EmTerminalType.Goal), "non-gear trajectory resets confirmation");
+
+        var repeated = CreateTracker();
+        ArmWaitingForDirection(repeated, t0, GearTerminal(t0));
+        repeated.Update(t0.AddSeconds(0.25d), StateAtSwitch(-0.03d, t0.AddSeconds(0.25d), 2L), GearTerminal(t0));
+        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, repeated.State.Phase,
+            "repeated sequence resets confirmation");
+
+        var discontinuous = CreateTracker();
+        ArmWaitingForDirection(discontinuous, t0, GearTerminal(t0));
+        discontinuous.Update(t0.AddSeconds(0.20d), StateAtSwitch(-0.03d, t0.AddSeconds(0.20d), 3L), GearTerminal(t0));
+        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, discontinuous.State.Phase,
+            "time discontinuity resets confirmation");
+    }
+
+    private static void CompletesOneSegmentWithoutIndexingPastTheEnd()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 3, 0, 0, TimeSpan.Zero);
+        var tracker = new TrajectoryObservationSegmentTracker(new[] { CreateSegments()[0] },
+            CreateSettings(), 0.01d);
+
+        TrajectoryObservationSegmentUpdate completed = tracker.Update(
+            t0, StateAtSwitch(0d, t0, 1L), GearTerminal(t0));
+
+        Verification.True(completed.Completed, "single segment update reports completion");
+        Verification.Equal(TrajectoryObservationSegmentPhase.Completed, tracker.State.Phase,
+            "single segment reaches completed instead of indexing past end");
+        Verification.Equal(0, tracker.State.ActiveSegmentIndex, "completed tracker retains final segment index");
+    }
+
+    private static void AssertConfirmationReset(DateTimeOffset t0, VehicleMotionState invalidState,
+        EmTrajectory invalidTrajectory, string name)
+    {
+        var tracker = CreateTracker();
+        ArmWaitingForDirection(tracker, t0, GearTerminal(t0));
+        tracker.Update(invalidState.CapturedAtUtc, invalidState, invalidTrajectory);
+        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase, name);
+    }
+
+    private static void ArmWaitingForDirection(TrajectoryObservationSegmentTracker tracker, DateTimeOffset t0,
+        EmTrajectory trajectory)
+    {
+        tracker.Update(t0, StateAtSwitch(0d, t0, 1L), trajectory);
+        tracker.Update(t0.AddSeconds(0.21d), StateAtSwitch(0d, t0.AddSeconds(0.21d), 2L), trajectory);
+        Verification.Equal(TrajectoryObservationSegmentPhase.WaitingForDirection, tracker.State.Phase,
+            "fixture arms direction confirmation");
+    }
+
+    private static TrajectoryObservationSegmentTracker CreateTracker()
+    {
+        return new TrajectoryObservationSegmentTracker(CreateSegments(), CreateSettings(), 0.01d);
+    }
+
+    private static TrajectoryObservationSettings CreateSettings()
+    {
+        return new TrajectoryObservationSettings
+        {
+            DirectionConfirmationSpeedMetersPerSecond = 0.02d,
+            DirectionConfirmationSamples = 3,
+            GearSwitchProjectionToleranceMeters = 0.50d,
+            GearSwitchStopHoldSeconds = 0.20d,
+        }.CreateValidatedSnapshot();
+    }
+
+    private static IReadOnlyList<DirectionSegmentView> CreateSegments()
+    {
+        return new[]
+        {
+            CreateSegment(0, TravelDirection.Forward, 0d, 1d, false, 0d,
+                EmBoundaryType.None, EmBoundaryType.GearSwitchApproach),
+            CreateSegment(1, TravelDirection.Reverse, 1d, 0d, true, 1d,
+                EmBoundaryType.GearSwitchDeparture, EmBoundaryType.Goal),
+        };
+    }
+
+    private static DirectionSegmentView CreateSegment(int index, TravelDirection direction, double startX,
+        double endX, bool startIsGearSwitch, double sourceStartS, EmBoundaryType startBoundary,
+        EmBoundaryType endBoundary)
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            new SmoothedPathPoint(startX, 0d, 0d, 0d, 0d, direction, 0d, 0d, 1d,
+                startIsGearSwitch, SmoothedPathPointSource.Anchor),
+            new SmoothedPathPoint(endX, 0d, 0d, 0d, 1d, direction, 0d, 0d, 1d,
+                false, SmoothedPathPointSource.Anchor),
+        };
+        return new DirectionSegmentView(index, direction, points,
+            new ReferenceBoundary(index, 0d, startBoundary, sourceStartS),
+            new ReferenceBoundary(index, 1d, endBoundary, sourceStartS + 1d), sourceStartS);
+    }
+
+    private static VehicleMotionState StateAtSwitch(double speed, DateTimeOffset time, long sequenceId)
+    {
+        return StateAt(1d, 0d, speed, time, sequenceId);
+    }
+
+    private static VehicleMotionState StateAt(double x, double y, double speed, DateTimeOffset time, long sequenceId)
+    {
+        return new VehicleMotionState(new Pose2D(x, y, 0d), speed, null, time, sequenceId);
+    }
+
+    private static EmTrajectory GearTerminal(DateTimeOffset effectiveAtUtc,
+        EmTerminalType terminalType = EmTerminalType.GearSwitch, TravelDirection direction = TravelDirection.Forward,
+        double finalTimeFromStart = 0d)
+    {
+        var metadata = new EmTrajectoryMetadata("gear-terminal-" + effectiveAtUtc.Ticks, effectiveAtUtc,
+            effectiveAtUtc, 1L, "segment-check", 1L, string.Empty, 0, direction, terminalType,
+            EmLongitudinalMode.ExactStopAtBoundary);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(1d, 0d, 0d, 0d, finalTimeFromStart, 0d, 0, 1d, 1d,
+                direction, EmBoundaryType.GearSwitchApproach, 0d, 0d),
+        });
+    }
+}