From 0a9c34dd58d28f26027ca6f781d80c1139e58a06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 6 Aug 2026 10:20:46 +0800 Subject: [PATCH] feat: track observed direction segments --- .../TrajectoryObservationSegmentTracker.cs | 226 ++++++++++++++++++ .../TrajectoryObservationChecks.cs | 1 + .../TrajectoryObservationSegmentChecks.cs | 187 +++++++++++++++ 3 files changed, 414 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationSegmentTracker.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationSegmentTracker.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationSegmentTracker.cs new file mode 100644 index 0000000..7606d69 --- /dev/null +++ b/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 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 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(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(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); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index deaa55b..eb8e094 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -18,6 +18,7 @@ internal static class TrajectoryObservationChecks public static void Run() { TrajectoryObservationSettingsChecks.Run(); + TrajectoryObservationSegmentChecks.Run(); VerifiesObservationSourceHasNoActuatorCalls(); VerifiesObservationSourceUsesRequiredOperatorText(); VerifiesOperatorDocumentationUsesExactUiEntry(); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs new file mode 100644 index 0000000..d578638 --- /dev/null +++ b/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 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 + { + 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), + }); + } +}