554 lines
23 KiB
C#
554 lines
23 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
public sealed class TrajectoryObservationBootstrapResult
|
|
{
|
|
private readonly VehicleParameters vehicle;
|
|
|
|
private TrajectoryObservationBootstrapResult(CoarsePathPlanningJob job, CoarsePathPlanningJobResult coarse,
|
|
PathSmoothingResult smoothedPath, IReadOnlyList<DirectionSegmentView> segments, string failureReason)
|
|
{
|
|
Job = job ?? throw new ArgumentNullException(nameof(job));
|
|
vehicle = CopyVehicle(job.Vehicle);
|
|
CoarseResult = coarse;
|
|
SmoothedPath = smoothedPath;
|
|
Segments = CopySegments(segments);
|
|
FailureReason = failureReason ?? string.Empty;
|
|
Succeeded = coarse != null && smoothedPath != null && Segments.Count > 0 && FailureReason.Length == 0;
|
|
}
|
|
|
|
public bool Succeeded { get; }
|
|
|
|
public CoarsePathPlanningJob Job { get; }
|
|
|
|
public VehicleParameters Vehicle => CopyVehicle(vehicle);
|
|
|
|
public CoarsePathPlanningJobResult CoarseResult { get; }
|
|
|
|
public PathSmoothingResult SmoothedPath { get; }
|
|
|
|
public IReadOnlyList<DirectionSegmentView> Segments { get; }
|
|
|
|
public string FailureReason { get; }
|
|
|
|
public PlanningGridMap Map => CoarseResult?.MapResult?.Map;
|
|
|
|
public static TrajectoryObservationBootstrapResult FromFailure(CoarsePathPlanningJob job,
|
|
CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath, string failureReason)
|
|
{
|
|
return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath,
|
|
Array.Empty<DirectionSegmentView>(), string.IsNullOrEmpty(failureReason) ? "Planning bootstrap failed." : failureReason);
|
|
}
|
|
|
|
public static TrajectoryObservationBootstrapResult Success(CoarsePathPlanningJob job,
|
|
CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath,
|
|
IReadOnlyList<DirectionSegmentView> segments)
|
|
{
|
|
if (coarse == null) throw new ArgumentNullException(nameof(coarse));
|
|
if (smoothedPath == null) throw new ArgumentNullException(nameof(smoothedPath));
|
|
if (segments == null || segments.Count == 0)
|
|
throw new ArgumentException("A successful bootstrap requires reference segments.", nameof(segments));
|
|
return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath, segments, string.Empty);
|
|
}
|
|
|
|
private static IReadOnlyList<DirectionSegmentView> CopySegments(IReadOnlyList<DirectionSegmentView> source)
|
|
{
|
|
var copy = new List<DirectionSegmentView>(source == null ? 0 : source.Count);
|
|
if (source != null)
|
|
{
|
|
for (int index = 0; index < source.Count; index++)
|
|
copy.Add(source[index]);
|
|
}
|
|
return new ReadOnlyCollection<DirectionSegmentView>(copy);
|
|
}
|
|
|
|
private static VehicleParameters CopyVehicle(VehicleParameters source)
|
|
{
|
|
if (source == null) return null;
|
|
return new VehicleParameters
|
|
{
|
|
LengthMeters = source.LengthMeters,
|
|
WidthMeters = source.WidthMeters,
|
|
SafetyMarginMeters = source.SafetyMarginMeters,
|
|
MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
|
|
MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
|
|
};
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationBootstrapper
|
|
{
|
|
private readonly CoarsePathPlanningService coarseService;
|
|
private readonly PathSmoothingService smoothingService;
|
|
|
|
public TrajectoryObservationBootstrapper()
|
|
: this(new CoarsePathPlanningService(), new PathSmoothingService())
|
|
{
|
|
}
|
|
|
|
public TrajectoryObservationBootstrapper(CoarsePathPlanningService coarseService,
|
|
PathSmoothingService smoothingService)
|
|
{
|
|
this.coarseService = coarseService ?? throw new ArgumentNullException(nameof(coarseService));
|
|
this.smoothingService = smoothingService ?? throw new ArgumentNullException(nameof(smoothingService));
|
|
}
|
|
|
|
public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
if (job == null) throw new ArgumentNullException(nameof(job));
|
|
|
|
CoarsePathPlanningJobResult coarse = coarseService.Plan(job, cancellationToken);
|
|
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
|
return TrajectoryObservationBootstrapResult.FromFailure(
|
|
job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);
|
|
|
|
var smoothingRequest = new PathSmoothingRequest(
|
|
CopyFiniteClearance(coarse.PlanningResult.Path, coarse.MapResult.Map),
|
|
coarse.PlanningResult.Segments, coarse.MapResult.Map, job.Vehicle,
|
|
new PathSmoothingConfiguration());
|
|
PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, cancellationToken);
|
|
if (!IsPublishedSmoothingStatus(smooth.Status))
|
|
return TrajectoryObservationBootstrapResult.FromFailure(
|
|
job, coarse, smooth, smooth.Diagnostics.TerminationReason);
|
|
return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
|
|
}
|
|
|
|
private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
|
|
PlanningGridMap map)
|
|
{
|
|
if (path == null) throw new ArgumentNullException(nameof(path));
|
|
if (map == null) throw new ArgumentNullException(nameof(map));
|
|
|
|
double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
|
|
double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
|
|
double mapDiagonalMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
|
|
var copy = new List<CoarsePathPoint>(path.Count);
|
|
for (int index = 0; index < path.Count; index++)
|
|
{
|
|
CoarsePathPoint point = path[index] ?? throw new ArgumentException(
|
|
"The coarse path cannot contain null points.", nameof(path));
|
|
double clearance = double.IsPositiveInfinity(point.BodyClearance)
|
|
? mapDiagonalMeters
|
|
: point.BodyClearance;
|
|
copy.Add(new CoarsePathPoint(point.X, point.Y, point.Heading, point.UnwrappedHeading,
|
|
point.ArcLength, point.Direction, point.VehicleCurvature, clearance,
|
|
point.IsGearSwitchPoint, point.Source));
|
|
}
|
|
return new ReadOnlyCollection<CoarsePathPoint>(copy);
|
|
}
|
|
|
|
private static bool IsPublishedSmoothingStatus(PathSmoothingStatus status)
|
|
{
|
|
return status == PathSmoothingStatus.Complete ||
|
|
status == PathSmoothingStatus.PartialImprovement ||
|
|
status == PathSmoothingStatus.NotNeeded ||
|
|
status == PathSmoothingStatus.Unchanged;
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationObservation
|
|
{
|
|
internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
|
|
EmTrajectory publishedTrajectory, EmTrajectoryPoint selectedPoint, TrajectoryControlCommand command,
|
|
TrajectoryExecutionState executorState)
|
|
{
|
|
ObservedAtUtc = observedAtUtc;
|
|
VehicleState = vehicleState ?? throw new ArgumentNullException(nameof(vehicleState));
|
|
PublishedTrajectory = publishedTrajectory;
|
|
SelectedPoint = selectedPoint;
|
|
Command = command;
|
|
ExecutorState = executorState;
|
|
}
|
|
|
|
public DateTimeOffset ObservedAtUtc { get; }
|
|
|
|
public VehicleMotionState VehicleState { get; }
|
|
|
|
public EmTrajectory PublishedTrajectory { get; }
|
|
|
|
public EmTrajectoryPoint SelectedPoint { get; }
|
|
|
|
public TrajectoryControlCommand Command { get; }
|
|
|
|
public TrajectoryExecutionState ExecutorState { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryObservationController
|
|
{
|
|
private readonly TrajectoryObservationBootstrapResult bootstrap;
|
|
private readonly TrajectoryObservationSettings settings;
|
|
private readonly EmPlannerConfiguration configuration;
|
|
private readonly IEmPlanningService planningService;
|
|
private readonly TrajectoryObservationSegmentTracker segmentTracker;
|
|
private EmPlanningCoordinator coordinator;
|
|
private TrajectoryExecutor executor;
|
|
private EmTrajectory previousTrajectoryForVisualization;
|
|
private readonly string sessionId;
|
|
private long cycleId;
|
|
private int plannedSegmentIndex;
|
|
private bool planAttemptedForActiveSegment;
|
|
|
|
public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap,
|
|
TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId)
|
|
{
|
|
this.bootstrap = bootstrap ?? throw new ArgumentNullException(nameof(bootstrap));
|
|
if (!bootstrap.Succeeded)
|
|
throw new ArgumentException("A successful planning bootstrap is required.", nameof(bootstrap));
|
|
if (settings == null) throw new ArgumentNullException(nameof(settings));
|
|
if (planningService == null) throw new ArgumentNullException(nameof(planningService));
|
|
if (string.IsNullOrWhiteSpace(sessionId)) throw new ArgumentException("A session ID is required.", nameof(sessionId));
|
|
|
|
TrajectoryObservationSettings snapshot = settings.CreateValidatedSnapshot();
|
|
this.settings = snapshot;
|
|
configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.ReplanPeriodSeconds = snapshot.ReplanPeriodSeconds;
|
|
configuration.Scheduling.SolverTimeoutSeconds = snapshot.SolverTimeoutSeconds;
|
|
configuration.Solver.MaximumOsqpIterations = snapshot.MaximumOsqpIterations;
|
|
configuration.Scheduling.TimeHorizonSeconds = snapshot.TimeHorizonSeconds;
|
|
configuration.Scheduling.OutputTimeStepSeconds = snapshot.OutputTimeStepSeconds;
|
|
this.planningService = planningService;
|
|
coordinator = new EmPlanningCoordinator(planningService);
|
|
executor = new TrajectoryExecutor(configuration);
|
|
segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, snapshot,
|
|
configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
|
|
this.sessionId = sessionId;
|
|
}
|
|
|
|
public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory;
|
|
|
|
public DirectionSegmentView ActiveSegment => bootstrap.Segments[segmentTracker.State.ActiveSegmentIndex];
|
|
|
|
public TrajectoryObservationSegmentState SegmentState => segmentTracker.State;
|
|
|
|
public EmTrajectory PreviousTrajectoryForVisualization => previousTrajectoryForVisualization;
|
|
|
|
internal EmPlannerConfiguration CreateEffectiveConfigurationSnapshot()
|
|
{
|
|
return configuration.Copy();
|
|
}
|
|
|
|
public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
|
|
{
|
|
return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
|
|
}
|
|
|
|
public bool ShouldStartCycle(DateTimeOffset now)
|
|
{
|
|
if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
|
{
|
|
if (plannedSegmentIndex != ActiveSegment.SegmentIndex)
|
|
{
|
|
plannedSegmentIndex = ActiveSegment.SegmentIndex;
|
|
planAttemptedForActiveSegment = false;
|
|
}
|
|
return !planAttemptedForActiveSegment;
|
|
}
|
|
return coordinator.ShouldStartCycle(now);
|
|
}
|
|
|
|
public Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (state == null) throw new ArgumentNullException(nameof(state));
|
|
|
|
planAttemptedForActiveSegment = true;
|
|
plannedSegmentIndex = ActiveSegment.SegmentIndex;
|
|
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,
|
|
sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
|
|
previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
|
|
EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope);
|
|
return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
|
|
}
|
|
|
|
public bool TryAdvanceSegment(DateTimeOffset now, VehicleMotionState state)
|
|
{
|
|
if (state == null) throw new ArgumentNullException(nameof(state));
|
|
|
|
TrajectoryObservationSegmentUpdate update = segmentTracker.Update(now, state, coordinator.PublishedTrajectory);
|
|
if (!update.Advanced)
|
|
return false;
|
|
|
|
previousTrajectoryForVisualization = coordinator.PublishedTrajectory;
|
|
coordinator = new EmPlanningCoordinator(planningService);
|
|
executor = new TrajectoryExecutor(configuration);
|
|
plannedSegmentIndex = segmentTracker.State.ActiveSegmentIndex;
|
|
planAttemptedForActiveSegment = false;
|
|
return true;
|
|
}
|
|
|
|
public TrajectoryObservationObservation Observe(DateTimeOffset now, VehicleMotionState state)
|
|
{
|
|
if (state == null) throw new ArgumentNullException(nameof(state));
|
|
|
|
EmTrajectory trajectory = coordinator.PublishedTrajectory;
|
|
if (trajectory == null)
|
|
return new TrajectoryObservationObservation(now, state, null, null, null, null);
|
|
|
|
TrajectoryObservationSegmentState segmentState = segmentTracker.State;
|
|
TravelDirection currentDirection = ActiveSegment.Direction;
|
|
bool waitingForDirection = segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForStop ||
|
|
segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection;
|
|
TravelDirection desiredDirection = waitingForDirection && segmentState.ExpectedDirection.HasValue
|
|
? segmentState.ExpectedDirection.Value
|
|
: currentDirection;
|
|
TrajectoryControlCommand command = executor.UpdateCommand(now, state, trajectory,
|
|
desiredDirection, currentDirection, !waitingForDirection);
|
|
TrajectoryExecutionState executorState = executor.State;
|
|
return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint,
|
|
command, executorState);
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationLoopTick
|
|
{
|
|
internal TrajectoryObservationLoopTick(TrajectoryObservationObservation observation,
|
|
PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed, bool planningInFlight,
|
|
bool planningStarted, bool planningCompleted, bool segmentAdvanced,
|
|
TrajectoryObservationSegmentState segmentState)
|
|
{
|
|
Observation = observation ?? throw new ArgumentNullException(nameof(observation));
|
|
LatestCycle = latestCycle;
|
|
LatestPlanningElapsed = latestPlanningElapsed;
|
|
PlanningInFlight = planningInFlight;
|
|
PlanningStarted = planningStarted;
|
|
PlanningCompleted = planningCompleted;
|
|
SegmentAdvanced = segmentAdvanced;
|
|
SegmentState = segmentState ?? throw new ArgumentNullException(nameof(segmentState));
|
|
}
|
|
|
|
public TrajectoryObservationObservation Observation { get; }
|
|
|
|
public PlanningCycleResult LatestCycle { get; }
|
|
|
|
public TimeSpan LatestPlanningElapsed { get; }
|
|
|
|
public bool PlanningInFlight { get; }
|
|
|
|
public bool PlanningStarted { get; }
|
|
|
|
public bool PlanningCompleted { get; }
|
|
|
|
public bool SegmentAdvanced { get; }
|
|
|
|
public TrajectoryObservationSegmentState SegmentState { get; }
|
|
|
|
public bool ShouldLog => true;
|
|
}
|
|
|
|
public sealed class TrajectoryObservationLoop
|
|
{
|
|
private readonly TrajectoryObservationController controller;
|
|
private Task<PlanningCycleResult> planningTask;
|
|
private DateTimeOffset planningStartedAtUtc;
|
|
private PlanningCycleResult latestCycle;
|
|
private TimeSpan latestPlanningElapsed;
|
|
|
|
public TrajectoryObservationLoop(TrajectoryObservationController controller)
|
|
{
|
|
this.controller = controller ?? throw new ArgumentNullException(nameof(controller));
|
|
}
|
|
|
|
public TrajectoryObservationLoopTick Tick(DateTimeOffset now, VehicleMotionState state,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (state == null) throw new ArgumentNullException(nameof(state));
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
bool planningCompleted = ConsumeCompletedPlanning(now);
|
|
bool segmentAdvanced = planningTask == null && controller.TryAdvanceSegment(now, state);
|
|
if (segmentAdvanced)
|
|
latestCycle = null;
|
|
bool planningStarted = false;
|
|
if (planningTask == null && controller.ShouldStartCycle(now))
|
|
{
|
|
planningStarted = true;
|
|
planningStartedAtUtc = now;
|
|
planningTask = controller.StartCycle(now, state, cancellationToken);
|
|
planningCompleted |= ConsumeCompletedPlanning(now);
|
|
}
|
|
|
|
TrajectoryObservationObservation observation = controller.Observe(now, state);
|
|
return new TrajectoryObservationLoopTick(observation, latestCycle, latestPlanningElapsed,
|
|
planningTask != null, planningStarted, planningCompleted, segmentAdvanced, controller.SegmentState);
|
|
}
|
|
|
|
private bool ConsumeCompletedPlanning(DateTimeOffset observedAtUtc)
|
|
{
|
|
Task<PlanningCycleResult> completed = planningTask;
|
|
if (completed == null || !completed.IsCompleted) return false;
|
|
|
|
latestCycle = completed.GetAwaiter().GetResult();
|
|
TimeSpan elapsed = observedAtUtc - planningStartedAtUtc;
|
|
latestPlanningElapsed = elapsed < TimeSpan.Zero ? TimeSpan.Zero : elapsed;
|
|
planningTask = null;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
public enum TrajectoryObservationSessionEndReason
|
|
{
|
|
BootstrapFailure,
|
|
RuntimeFault,
|
|
Cancellation,
|
|
}
|
|
|
|
public static class TrajectoryObservationSessionLifecycle
|
|
{
|
|
public static bool ShouldClearLayers(TrajectoryObservationSessionEndReason reason)
|
|
{
|
|
switch (reason)
|
|
{
|
|
case TrajectoryObservationSessionEndReason.BootstrapFailure:
|
|
return false;
|
|
case TrajectoryObservationSessionEndReason.RuntimeFault:
|
|
case TrajectoryObservationSessionEndReason.Cancellation:
|
|
return true;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(reason));
|
|
}
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationRuntimeState
|
|
{
|
|
public const string GearSwitchWaitingNotice =
|
|
"等待真实档位/方向确认；观察模式不会推进下一方向段";
|
|
|
|
private TrajectoryObservationRuntimeState(bool waitingAtGearSwitch)
|
|
{
|
|
WaitingAtGearSwitch = waitingAtGearSwitch;
|
|
WorldNotice = waitingAtGearSwitch ? GearSwitchWaitingNotice : string.Empty;
|
|
}
|
|
|
|
public bool WaitingAtGearSwitch { get; }
|
|
|
|
public string WorldNotice { get; }
|
|
|
|
public static TrajectoryObservationRuntimeState Create(DateTimeOffset now, EmTrajectory trajectory)
|
|
{
|
|
if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
|
|
return new TrajectoryObservationRuntimeState(false);
|
|
|
|
EmTrajectoryPoint finalPoint = trajectory.Points[trajectory.Points.Count - 1];
|
|
bool waiting = (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds >= finalPoint.TimeFromStart;
|
|
return new TrajectoryObservationRuntimeState(waiting);
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationLsSample
|
|
{
|
|
public TrajectoryObservationLsSample(double pathS, double lateralOffset)
|
|
{
|
|
PathS = pathS;
|
|
LateralOffset = lateralOffset;
|
|
}
|
|
|
|
public double PathS { get; }
|
|
|
|
public double LateralOffset { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryObservationStSample
|
|
{
|
|
public TrajectoryObservationStSample(double timeFromStart, double pathS)
|
|
{
|
|
TimeFromStart = timeFromStart;
|
|
PathS = pathS;
|
|
}
|
|
|
|
public double TimeFromStart { get; }
|
|
|
|
public double PathS { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryObservationSpeedSample
|
|
{
|
|
public TrajectoryObservationSpeedSample(double timeFromStart, double signedLongitudinalVelocity)
|
|
{
|
|
TimeFromStart = timeFromStart;
|
|
SignedLongitudinalVelocity = signedLongitudinalVelocity;
|
|
}
|
|
|
|
public double TimeFromStart { get; }
|
|
|
|
public double SignedLongitudinalVelocity { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryObservationCharts
|
|
{
|
|
private TrajectoryObservationCharts(IReadOnlyList<TrajectoryObservationLsSample> lsSamples,
|
|
IReadOnlyList<TrajectoryObservationStSample> stSamples,
|
|
IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples, int failedProjectionCount)
|
|
{
|
|
LsSamples = new ReadOnlyCollection<TrajectoryObservationLsSample>(
|
|
new List<TrajectoryObservationLsSample>(lsSamples));
|
|
StSamples = new ReadOnlyCollection<TrajectoryObservationStSample>(
|
|
new List<TrajectoryObservationStSample>(stSamples));
|
|
SpeedSamples = new ReadOnlyCollection<TrajectoryObservationSpeedSample>(
|
|
new List<TrajectoryObservationSpeedSample>(speedSamples));
|
|
FailedProjectionCount = failedProjectionCount;
|
|
}
|
|
|
|
public IReadOnlyList<TrajectoryObservationLsSample> LsSamples { get; }
|
|
|
|
public IReadOnlyList<TrajectoryObservationStSample> StSamples { get; }
|
|
|
|
public IReadOnlyList<TrajectoryObservationSpeedSample> SpeedSamples { get; }
|
|
|
|
public int FailedProjectionCount { get; }
|
|
|
|
public static TrajectoryObservationCharts Build(EmTrajectory trajectory, DirectionSegmentView segment,
|
|
double maximumProjectionDistanceMeters)
|
|
{
|
|
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
|
|
if (segment == null) throw new ArgumentNullException(nameof(segment));
|
|
if (double.IsNaN(maximumProjectionDistanceMeters) || double.IsInfinity(maximumProjectionDistanceMeters) ||
|
|
maximumProjectionDistanceMeters < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(maximumProjectionDistanceMeters));
|
|
|
|
var ls = new List<TrajectoryObservationLsSample>(trajectory.Points.Count);
|
|
var st = new List<TrajectoryObservationStSample>(trajectory.Points.Count);
|
|
var speed = new List<TrajectoryObservationSpeedSample>(trajectory.Points.Count);
|
|
var projector = new FrenetProjector();
|
|
double seedReferenceS = 0d;
|
|
int failedProjectionCount = 0;
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
var pose = new Pose2D(point.X, point.Y, point.Yaw);
|
|
if (projector.TryProject(pose, segment, 0d, segment.LengthMeters,
|
|
maximumProjectionDistanceMeters, seedReferenceS, out FrenetProjection projection))
|
|
{
|
|
ls.Add(new TrajectoryObservationLsSample(
|
|
segment.SourceStartArcLength + projection.ReferenceS, projection.LateralOffset));
|
|
seedReferenceS = projection.ReferenceS;
|
|
}
|
|
else
|
|
{
|
|
failedProjectionCount++;
|
|
}
|
|
|
|
st.Add(new TrajectoryObservationStSample(point.TimeFromStart, point.PathS));
|
|
speed.Add(new TrajectoryObservationSpeedSample(point.TimeFromStart, point.SignedLongitudinalVelocity));
|
|
}
|
|
|
|
return new TrajectoryObservationCharts(ls, st, speed, failedProjectionCount);
|
|
}
|
|
}
|