feat: assemble complete EM trajectories
This commit is contained in:
梁薄云
@@ -0,0 +1,84 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal sealed class LateralPathInterpolator
|
||||
{
|
||||
private const double BoundaryTolerance = 1e-10d;
|
||||
private readonly LateralPath path;
|
||||
|
||||
public LateralPathInterpolator(LateralPath path)
|
||||
{
|
||||
if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
|
||||
throw new ArgumentException("Trajectory assembly requires an independently validated lateral path.", nameof(path));
|
||||
|
||||
double previousPathS = double.NegativeInfinity;
|
||||
for (int index = 0; index < path.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint point = path.Points[index];
|
||||
if (point == null || point.PathS <= previousPathS)
|
||||
throw new ArgumentException("Lateral path points must have strictly increasing PathS.", nameof(path));
|
||||
previousPathS = point.PathS;
|
||||
}
|
||||
|
||||
this.path = path;
|
||||
}
|
||||
|
||||
public InterpolatedLateralPathPoint Interpolate(double pathS)
|
||||
{
|
||||
if (!IsFinite(pathS))
|
||||
throw new ArgumentOutOfRangeException(nameof(pathS));
|
||||
|
||||
LateralPathPoint first = path.Points[0];
|
||||
LateralPathPoint last = path.Points[path.Points.Count - 1];
|
||||
if (pathS < first.PathS - BoundaryTolerance || pathS > last.PathS + BoundaryTolerance)
|
||||
throw new ArgumentOutOfRangeException(nameof(pathS));
|
||||
if (pathS <= first.PathS + BoundaryTolerance)
|
||||
return From(first);
|
||||
if (pathS >= last.PathS - BoundaryTolerance)
|
||||
return From(last);
|
||||
|
||||
for (int index = 1; index < path.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint right = path.Points[index];
|
||||
if (pathS <= right.PathS)
|
||||
{
|
||||
LateralPathPoint left = path.Points[index - 1];
|
||||
double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
|
||||
return new InterpolatedLateralPathPoint(
|
||||
Linear(left.X, right.X, ratio),
|
||||
Linear(left.Y, right.Y, ratio),
|
||||
NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
|
||||
Linear(left.VehicleCurvature, right.VehicleCurvature, ratio));
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidOperationException("A PathS value inside the lateral path was not bracketed.");
|
||||
}
|
||||
|
||||
private static InterpolatedLateralPathPoint From(LateralPathPoint point)
|
||||
{
|
||||
return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
|
||||
}
|
||||
|
||||
private static double Linear(double left, double right, double ratio)
|
||||
{
|
||||
return left + ratio * (right - left);
|
||||
}
|
||||
|
||||
internal static double NormalizeYaw(double yaw)
|
||||
{
|
||||
double normalized = yaw % (2d * Math.PI);
|
||||
if (normalized >= Math.PI)
|
||||
normalized -= 2d * Math.PI;
|
||||
if (normalized < -Math.PI)
|
||||
normalized += 2d * Math.PI;
|
||||
return normalized;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class InterpolatedLateralPathPoint
|
||||
{
|
||||
public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
Yaw = yaw;
|
||||
VehicleCurvature = vehicleCurvature;
|
||||
}
|
||||
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double Yaw { get; }
|
||||
public double VehicleCurvature { get; }
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
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; }
|
||||
}
|
||||
Reference in New Issue
Block a user