feat: track observed direction segments
This commit is contained in:
梁薄云
+226
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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