75 lines
3.0 KiB
C#
75 lines
3.0 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal sealed class TrajectorySampleSchedule
|
|
{
|
|
private const double ZeroTolerance = 1e-12d;
|
|
|
|
public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds)
|
|
{
|
|
if (candidate == null)
|
|
throw new ArgumentNullException(nameof(candidate));
|
|
if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds));
|
|
if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds));
|
|
if (Math.Abs(candidate.U[candidate.U.Count - 1]) > ZeroTolerance)
|
|
throw new ArgumentException("A publishable trajectory requires an exact zero-speed terminal candidate.", nameof(candidate));
|
|
|
|
var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
|
|
double previousPathS = double.NegativeInfinity;
|
|
for (int index = 0; index < candidate.KnotTimes.Count; index++)
|
|
{
|
|
if (candidate.S[index] < previousPathS)
|
|
throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
|
|
if (candidate.U[index] < -ZeroTolerance)
|
|
throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
|
|
|
|
samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]),
|
|
candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false));
|
|
previousPathS = candidate.S[index];
|
|
}
|
|
|
|
TrajectorySample terminal = samples[samples.Count - 1];
|
|
double holdElapsed = 0d;
|
|
while (holdElapsed < holdDurationSeconds - ZeroTolerance)
|
|
{
|
|
holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds);
|
|
samples.Add(new TrajectorySample(terminal.TimeFromStart + holdElapsed, terminal.PathS, 0d, 0d, 0d, true));
|
|
}
|
|
|
|
Samples = new ReadOnlyCollection<TrajectorySample>(samples);
|
|
}
|
|
|
|
public IReadOnlyList<TrajectorySample> Samples { get; }
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
}
|
|
|
|
internal sealed class TrajectorySample
|
|
{
|
|
public TrajectorySample(double timeFromStart, double pathS, double progressSpeed, double acceleration, double jerk,
|
|
bool isHoldSample)
|
|
{
|
|
TimeFromStart = timeFromStart;
|
|
PathS = pathS;
|
|
ProgressSpeed = progressSpeed;
|
|
Acceleration = acceleration;
|
|
Jerk = jerk;
|
|
IsHoldSample = isHoldSample;
|
|
}
|
|
|
|
public double TimeFromStart { get; }
|
|
public double PathS { get; }
|
|
public double ProgressSpeed { get; }
|
|
public double Acceleration { get; }
|
|
public double Jerk { get; }
|
|
public bool IsHoldSample { get; }
|
|
}
|