85 lines
3.3 KiB
C#
85 lines
3.3 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public sealed class EmTrajectoryPoint
|
|
{
|
|
public EmTrajectoryPoint(
|
|
double x,
|
|
double y,
|
|
double yaw,
|
|
double signedLongitudinalVelocity,
|
|
double timeFromStart,
|
|
double vehicleCurvature,
|
|
int segmentIndex,
|
|
double segmentLocalS,
|
|
double pathS,
|
|
TravelDirection direction,
|
|
EmBoundaryType boundaryType,
|
|
double longitudinalAcceleration,
|
|
double longitudinalJerk)
|
|
{
|
|
ContractNumeric.RequireFinite(x, nameof(x));
|
|
ContractNumeric.RequireFinite(y, nameof(y));
|
|
ContractNumeric.RequireFinite(yaw, nameof(yaw));
|
|
ContractNumeric.RequireFinite(signedLongitudinalVelocity, nameof(signedLongitudinalVelocity));
|
|
ContractNumeric.RequireFinite(timeFromStart, nameof(timeFromStart));
|
|
ContractNumeric.RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
|
|
ContractNumeric.RequireFinite(segmentLocalS, nameof(segmentLocalS));
|
|
ContractNumeric.RequireFinite(pathS, nameof(pathS));
|
|
ContractNumeric.RequireFinite(longitudinalAcceleration, nameof(longitudinalAcceleration));
|
|
ContractNumeric.RequireFinite(longitudinalJerk, nameof(longitudinalJerk));
|
|
if (segmentIndex < 0)
|
|
throw new ArgumentOutOfRangeException(nameof(segmentIndex));
|
|
if (timeFromStart < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(timeFromStart));
|
|
if (segmentLocalS < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(segmentLocalS));
|
|
if (pathS < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(pathS));
|
|
if (!Enum.IsDefined(typeof(TravelDirection), direction))
|
|
throw new ArgumentOutOfRangeException(nameof(direction));
|
|
if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType))
|
|
throw new ArgumentOutOfRangeException(nameof(boundaryType));
|
|
|
|
X = x;
|
|
Y = y;
|
|
Yaw = yaw;
|
|
SignedLongitudinalVelocity = signedLongitudinalVelocity;
|
|
Speed = Math.Abs(signedLongitudinalVelocity);
|
|
VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
|
|
VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
|
|
YawRate = signedLongitudinalVelocity * vehicleCurvature;
|
|
TimeFromStart = timeFromStart;
|
|
VehicleCurvature = vehicleCurvature;
|
|
SegmentIndex = segmentIndex;
|
|
SegmentLocalS = segmentLocalS;
|
|
PathS = pathS;
|
|
Direction = direction;
|
|
BoundaryType = boundaryType;
|
|
LongitudinalAcceleration = longitudinalAcceleration;
|
|
LongitudinalJerk = longitudinalJerk;
|
|
}
|
|
|
|
public double X { get; }
|
|
public double Y { get; }
|
|
public double Yaw { get; }
|
|
public double SignedLongitudinalVelocity { get; }
|
|
public double Speed { get; }
|
|
public double VelocityX { get; }
|
|
public double VelocityY { get; }
|
|
public double YawRate { get; }
|
|
public double TimeFromStart { get; }
|
|
public double VehicleCurvature { get; }
|
|
public int SegmentIndex { get; }
|
|
public double SegmentLocalS { get; }
|
|
public double PathS { get; }
|
|
public TravelDirection Direction { get; }
|
|
public EmBoundaryType BoundaryType { get; }
|
|
|
|
internal double LongitudinalAcceleration { get; }
|
|
|
|
internal double LongitudinalJerk { get; }
|
|
}
|