feat: reuse prior trajectory in longitudinal planning
This commit is contained in:
梁薄云
+250
@@ -0,0 +1,250 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Resamples a compatible published trajectory onto the current ST knots as soft longitudinal references.</summary>
|
||||
public sealed class LongitudinalPreviousTrajectorySeed
|
||||
{
|
||||
private static readonly LongitudinalPreviousTrajectorySeed empty = new LongitudinalPreviousTrajectorySeed(
|
||||
Array.Empty<double>(), Array.Empty<double>());
|
||||
|
||||
public LongitudinalPreviousTrajectorySeed(IReadOnlyList<double> pathS,
|
||||
IReadOnlyList<double> progressSpeedMetersPerSecond)
|
||||
{
|
||||
if (pathS == null)
|
||||
throw new ArgumentNullException(nameof(pathS));
|
||||
if (progressSpeedMetersPerSecond == null)
|
||||
throw new ArgumentNullException(nameof(progressSpeedMetersPerSecond));
|
||||
if (pathS.Count != progressSpeedMetersPerSecond.Count)
|
||||
throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.");
|
||||
|
||||
var copiedPathS = new List<double>(pathS.Count);
|
||||
var copiedSpeed = new List<double>(progressSpeedMetersPerSecond.Count);
|
||||
for (int index = 0; index < pathS.Count; index++)
|
||||
{
|
||||
if (!IsFinite(pathS[index]) || pathS[index] < 0d ||
|
||||
!IsFinite(progressSpeedMetersPerSecond[index]) || progressSpeedMetersPerSecond[index] < 0d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(pathS));
|
||||
}
|
||||
copiedPathS.Add(pathS[index]);
|
||||
copiedSpeed.Add(progressSpeedMetersPerSecond[index]);
|
||||
}
|
||||
|
||||
PathS = new ReadOnlyCollection<double>(copiedPathS);
|
||||
ProgressSpeedMetersPerSecond = new ReadOnlyCollection<double>(copiedSpeed);
|
||||
}
|
||||
|
||||
public IReadOnlyList<double> PathS { get; }
|
||||
|
||||
public IReadOnlyList<double> ProgressSpeedMetersPerSecond { get; }
|
||||
|
||||
public static LongitudinalPreviousTrajectorySeed Empty { get { return empty; } }
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>Builds monotone PathS and progress-speed soft references from a prior published trajectory.</summary>
|
||||
public sealed class LongitudinalPreviousTrajectorySeedBuilder
|
||||
{
|
||||
private const double ProjectionTolerance = 1e-10d;
|
||||
|
||||
public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
|
||||
DateTimeOffset newEffectiveAtUtc, IReadOnlyList<double> newKnotTimes, int segmentIndex,
|
||||
TravelDirection direction)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (!IsCompatible(previous, currentPath, newKnotTimes, segmentIndex, direction))
|
||||
return LongitudinalPreviousTrajectorySeed.Empty;
|
||||
|
||||
var pathS = new List<double>(newKnotTimes.Count);
|
||||
var progressSpeed = new List<double>(newKnotTimes.Count);
|
||||
double previousProjectedPathS = double.NegativeInfinity;
|
||||
for (int index = 0; index < newKnotTimes.Count; index++)
|
||||
{
|
||||
DateTimeOffset sampleUtc = newEffectiveAtUtc.AddSeconds(newKnotTimes[index]);
|
||||
double previousTimeSeconds = (sampleUtc - previous.Metadata.EffectiveAtUtc).TotalSeconds;
|
||||
if (!TryInterpolate(previous.Points, previousTimeSeconds, out InterpolatedPreviousSample sample) ||
|
||||
!TryProjectMonotonically(currentPath, sample.X, sample.Y, previousProjectedPathS,
|
||||
out double projectedPathS))
|
||||
{
|
||||
return LongitudinalPreviousTrajectorySeed.Empty;
|
||||
}
|
||||
|
||||
pathS.Add(projectedPathS);
|
||||
progressSpeed.Add(Math.Abs(sample.SignedSpeedMetersPerSecond));
|
||||
previousProjectedPathS = projectedPathS;
|
||||
}
|
||||
|
||||
return new LongitudinalPreviousTrajectorySeed(pathS, progressSpeed);
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LongitudinalPreviousTrajectorySeed.Empty;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IsCompatible(EmTrajectory previous, LateralPath currentPath,
|
||||
IReadOnlyList<double> newKnotTimes, int segmentIndex, TravelDirection direction)
|
||||
{
|
||||
if (previous == null || currentPath == null || newKnotTimes == null || segmentIndex < 0 ||
|
||||
!Enum.IsDefined(typeof(TravelDirection), direction) || !currentPath.IsIndependentlyValidated ||
|
||||
currentPath.Points.Count < 2 || previous.Metadata == null || previous.Points.Count < 2 ||
|
||||
previous.Metadata.SegmentIndex != segmentIndex || previous.Metadata.Direction != direction)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double previousKnotTime = double.NegativeInfinity;
|
||||
for (int index = 0; index < newKnotTimes.Count; index++)
|
||||
{
|
||||
if (!IsFinite(newKnotTimes[index]) || newKnotTimes[index] < 0d ||
|
||||
newKnotTimes[index] <= previousKnotTime)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
previousKnotTime = newKnotTimes[index];
|
||||
}
|
||||
if (newKnotTimes.Count == 0)
|
||||
return false;
|
||||
|
||||
double previousTime = double.NegativeInfinity;
|
||||
for (int index = 0; index < previous.Points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint point = previous.Points[index];
|
||||
if (point == null || point.Direction != direction || !IsFinite(point.TimeFromStart) ||
|
||||
!IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.SignedLongitudinalVelocity) ||
|
||||
point.TimeFromStart <= previousTime)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
previousTime = point.TimeFromStart;
|
||||
}
|
||||
|
||||
double previousPathS = double.NegativeInfinity;
|
||||
for (int index = 0; index < currentPath.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint point = currentPath.Points[index];
|
||||
if (point == null || !IsFinite(point.PathS) || !IsFinite(point.X) || !IsFinite(point.Y) ||
|
||||
point.PathS <= previousPathS)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
previousPathS = point.PathS;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryInterpolate(IReadOnlyList<EmTrajectoryPoint> points, double sampleTimeSeconds,
|
||||
out InterpolatedPreviousSample sample)
|
||||
{
|
||||
sample = default;
|
||||
if (!IsFinite(sampleTimeSeconds) || sampleTimeSeconds < points[0].TimeFromStart - ProjectionTolerance ||
|
||||
sampleTimeSeconds > points[points.Count - 1].TimeFromStart + ProjectionTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (sampleTimeSeconds <= points[0].TimeFromStart + ProjectionTolerance)
|
||||
{
|
||||
sample = InterpolatedPreviousSample.From(points[0]);
|
||||
return true;
|
||||
}
|
||||
|
||||
for (int index = 1; index < points.Count; index++)
|
||||
{
|
||||
EmTrajectoryPoint right = points[index];
|
||||
if (sampleTimeSeconds <= right.TimeFromStart + ProjectionTolerance)
|
||||
{
|
||||
EmTrajectoryPoint left = points[index - 1];
|
||||
double ratio = (sampleTimeSeconds - left.TimeFromStart) /
|
||||
(right.TimeFromStart - left.TimeFromStart);
|
||||
ratio = Math.Max(0d, Math.Min(1d, ratio));
|
||||
sample = new InterpolatedPreviousSample(
|
||||
Linear(left.X, right.X, ratio),
|
||||
Linear(left.Y, right.Y, ratio),
|
||||
Linear(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, ratio));
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool TryProjectMonotonically(LateralPath path, double x, double y, double minimumPathS,
|
||||
out double projectedPathS)
|
||||
{
|
||||
projectedPathS = 0d;
|
||||
double bestDistanceSquared = double.PositiveInfinity;
|
||||
bool found = false;
|
||||
for (int index = 1; index < path.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint left = path.Points[index - 1];
|
||||
LateralPathPoint right = path.Points[index];
|
||||
double dx = right.X - left.X;
|
||||
double dy = right.Y - left.Y;
|
||||
double lengthSquared = dx * dx + dy * dy;
|
||||
if (!IsFinite(lengthSquared) || lengthSquared <= ProjectionTolerance)
|
||||
continue;
|
||||
|
||||
double ratio = ((x - left.X) * dx + (y - left.Y) * dy) / lengthSquared;
|
||||
ratio = Math.Max(0d, Math.Min(1d, ratio));
|
||||
double candidatePathS = Linear(left.PathS, right.PathS, ratio);
|
||||
if (candidatePathS + ProjectionTolerance < minimumPathS)
|
||||
continue;
|
||||
|
||||
double projectedX = Linear(left.X, right.X, ratio);
|
||||
double projectedY = Linear(left.Y, right.Y, ratio);
|
||||
double distanceSquared = (x - projectedX) * (x - projectedX) + (y - projectedY) * (y - projectedY);
|
||||
if (!found || distanceSquared < bestDistanceSquared - ProjectionTolerance ||
|
||||
(Math.Abs(distanceSquared - bestDistanceSquared) <= ProjectionTolerance && candidatePathS < projectedPathS))
|
||||
{
|
||||
projectedPathS = candidatePathS;
|
||||
bestDistanceSquared = distanceSquared;
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found)
|
||||
return false;
|
||||
if (minimumPathS > double.NegativeInfinity)
|
||||
projectedPathS = Math.Max(minimumPathS, projectedPathS);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double Linear(double left, double right, double ratio)
|
||||
{
|
||||
return left + (right - left) * ratio;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private readonly struct InterpolatedPreviousSample
|
||||
{
|
||||
public InterpolatedPreviousSample(double x, double y, double signedSpeedMetersPerSecond)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
SignedSpeedMetersPerSecond = signedSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double SignedSpeedMetersPerSecond { get; }
|
||||
|
||||
public static InterpolatedPreviousSample From(EmTrajectoryPoint point)
|
||||
{
|
||||
return new InterpolatedPreviousSample(point.X, point.Y, point.SignedLongitudinalVelocity);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user