112 lines
4.4 KiB
C#
112 lines
4.4 KiB
C#
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public sealed partial class EmPlannerConfiguration
|
|
{
|
|
public SchedulingConfiguration Scheduling { get; set; }
|
|
public FrenetConfiguration Frenet { get; set; }
|
|
public CorridorConfiguration Corridor { get; set; }
|
|
public LateralConfiguration Lateral { get; set; }
|
|
public LongitudinalConfiguration Longitudinal { get; set; }
|
|
public SolverConfiguration Solver { get; set; }
|
|
public ValidationConfiguration Validation { get; set; }
|
|
|
|
public static EmPlannerConfiguration CreateDefault()
|
|
{
|
|
return new EmPlannerConfiguration
|
|
{
|
|
Scheduling = new SchedulingConfiguration
|
|
{
|
|
ReplanPeriodSeconds = 0.20d,
|
|
TimeHorizonSeconds = 6d,
|
|
DistanceHorizonMeters = 5d,
|
|
OutputTimeStepSeconds = 0.05d,
|
|
SolverTimeoutSeconds = 0.10d,
|
|
HandoffLookaheadSeconds = 0.30d,
|
|
MaximumVehicleStateAgeSeconds = 0.20d,
|
|
},
|
|
Corridor = new CorridorConfiguration
|
|
{
|
|
LongitudinalSampleSpacingMeters = 0.10d,
|
|
LateralSampleSpacingMeters = 0.025d,
|
|
MaximumLateralOffsetMeters = 0.30d,
|
|
AdditionalClearanceReserveMeters = 0.02d,
|
|
MaximumCollisionCheckStepMeters = 0.025d,
|
|
},
|
|
Frenet = new FrenetConfiguration
|
|
{
|
|
MaximumProjectionDistanceMeters = 0.50d,
|
|
MinimumFrenetDenominator = 0.20d,
|
|
BoundaryAnchorToleranceMeters = 1e-8d,
|
|
},
|
|
Lateral = new LateralConfiguration
|
|
{
|
|
MaximumLateralStepPerIterationMeters = 0.05d,
|
|
MaximumLateralSlope = 0.50d,
|
|
MaximumLateralSecondDerivativePerMeter = 1d,
|
|
MaximumLateralThirdDerivativePerSquareMeter = 2d,
|
|
Weights = new LateralWeights
|
|
{
|
|
ReferenceOffset = 10d,
|
|
HeadingDeviation = 1d,
|
|
SecondDerivative = 5d,
|
|
ThirdDerivative = 10d,
|
|
Curvature = 5d,
|
|
CurvatureVariation = 20d,
|
|
PreviousTrajectory = 5d,
|
|
RollingTerminal = 10d,
|
|
},
|
|
},
|
|
Longitudinal = new LongitudinalConfiguration
|
|
{
|
|
MaximumForwardSpeedMetersPerSecond = 0.20d,
|
|
MaximumReverseSpeedMetersPerSecond = 0.20d,
|
|
MaximumAccelerationMetersPerSecondSquared = 0.20d,
|
|
MaximumDecelerationMetersPerSecondSquared = 0.30d,
|
|
MaximumJerkMetersPerSecondCubed = 0.50d,
|
|
MaximumLateralAccelerationMetersPerSecondSquared = 0.20d,
|
|
MaximumCurvatureRatePerMeterPerSecond = 0.50d,
|
|
StopSpeedToleranceMetersPerSecond = 0.01d,
|
|
ZeroSpeedHoldSeconds = 0.20d,
|
|
Weights = new LongitudinalWeights
|
|
{
|
|
ReferenceSpeed = 10d,
|
|
Acceleration = 1d,
|
|
Jerk = 10d,
|
|
PreviousTrajectory = 5d,
|
|
TerminalAcceleration = 1d,
|
|
},
|
|
},
|
|
Solver = new SolverConfiguration
|
|
{
|
|
MaximumOuterIterations = 5,
|
|
MaximumOsqpIterations = 4000,
|
|
AbsoluteTolerance = 1e-5d,
|
|
RelativeTolerance = 1e-5d,
|
|
StrictResidualTolerance = 1e-5d,
|
|
WarmStart = true,
|
|
Polish = true,
|
|
NativeVerbose = false,
|
|
},
|
|
Validation = new ValidationConfiguration
|
|
{
|
|
SpatialToleranceMeters = 1e-8d,
|
|
KinematicTolerance = 1e-8d,
|
|
},
|
|
};
|
|
}
|
|
|
|
internal EmPlannerConfiguration Copy()
|
|
{
|
|
return new EmPlannerConfiguration
|
|
{
|
|
Scheduling = Scheduling == null ? null : Scheduling.Copy(),
|
|
Frenet = Frenet == null ? null : Frenet.Copy(),
|
|
Corridor = Corridor == null ? null : Corridor.Copy(),
|
|
Lateral = Lateral == null ? null : Lateral.Copy(),
|
|
Longitudinal = Longitudinal == null ? null : Longitudinal.Copy(),
|
|
Solver = Solver == null ? null : Solver.Copy(),
|
|
Validation = Validation == null ? null : Validation.Copy(),
|
|
};
|
|
}
|
|
}
|