feat: track observed direction segments
This commit is contained in:
梁薄云
@@ -18,6 +18,7 @@ internal static class TrajectoryObservationChecks
|
||||
public static void Run()
|
||||
{
|
||||
TrajectoryObservationSettingsChecks.Run();
|
||||
TrajectoryObservationSegmentChecks.Run();
|
||||
VerifiesObservationSourceHasNoActuatorCalls();
|
||||
VerifiesObservationSourceUsesRequiredOperatorText();
|
||||
VerifiesOperatorDocumentationUsesExactUiEntry();
|
||||
|
||||
@@ -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),
|
||||
});
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user