85 lines
3.8 KiB
C#
85 lines
3.8 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Builds immutable world-space trajectory points from validated LS/ST results.</summary>
|
|
public sealed class EmTrajectoryAssembler
|
|
{
|
|
private readonly double outputTimeStepSeconds;
|
|
private readonly double zeroSpeedHoldSeconds;
|
|
|
|
public EmTrajectoryAssembler()
|
|
: this(EmPlannerConfiguration.CreateDefault())
|
|
{
|
|
}
|
|
|
|
public EmTrajectoryAssembler(EmPlannerConfiguration configuration)
|
|
{
|
|
if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null)
|
|
throw new ArgumentNullException(nameof(configuration));
|
|
outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
|
|
zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
|
|
if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
|
|
zeroSpeedHoldSeconds < 0d)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(configuration));
|
|
}
|
|
}
|
|
|
|
public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata)
|
|
{
|
|
if (longitudinal == null || longitudinal.Candidate == null ||
|
|
(longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback))
|
|
{
|
|
throw new ArgumentException("Trajectory assembly requires a successful longitudinal result.", nameof(longitudinal));
|
|
}
|
|
if (metadata == null)
|
|
throw new ArgumentNullException(nameof(metadata));
|
|
|
|
var interpolator = new LateralPathInterpolator(path);
|
|
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds);
|
|
double terminalPathS = path.Points[path.Points.Count - 1].PathS;
|
|
var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
|
|
double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;
|
|
|
|
for (int index = 0; index < schedule.Samples.Count; index++)
|
|
{
|
|
TrajectorySample sample = schedule.Samples[index];
|
|
if (sample.PathS > terminalPathS + 1e-10d)
|
|
throw new ArgumentException("Longitudinal PathS exceeds the assembled lateral path.", nameof(longitudinal));
|
|
|
|
InterpolatedLateralPathPoint geometry = interpolator.Interpolate(sample.PathS);
|
|
bool isTerminalAnchor = index == longitudinal.Candidate.KnotTimes.Count - 1;
|
|
EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None;
|
|
double signedSpeed = directionSign * sample.ProgressSpeed;
|
|
points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart,
|
|
geometry.VehicleCurvature, metadata.SegmentIndex, sample.PathS, sample.PathS, metadata.Direction,
|
|
boundaryType, sample.Acceleration, sample.Jerk));
|
|
}
|
|
|
|
return new EmTrajectory(metadata, points);
|
|
}
|
|
|
|
private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType)
|
|
{
|
|
switch (terminalType)
|
|
{
|
|
case EmTerminalType.RollingSafetyStop:
|
|
return EmBoundaryType.RollingSafetyStop;
|
|
case EmTerminalType.GearSwitch:
|
|
return EmBoundaryType.GearSwitchApproach;
|
|
case EmTerminalType.Goal:
|
|
return EmBoundaryType.Goal;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(terminalType));
|
|
}
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|