ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// EM 规划器的完整可变配置树；所有子配置均须非空，建议通过 <see cref="CreateDefault"/> 创建协调且彼此独立的默认快照。
/// </summary>
public sealed partial class EmPlannerConfiguration
{
    /// <summary>
    /// 重规划节奏、时间窗口和资源上限配置；无直接单位，必须为非空的 <see cref="SchedulingConfiguration"/>，默认值为新的调度配置对象。
    /// </summary>
    public SchedulingConfiguration Scheduling { get; set; }
    /// <summary>
    /// 世界坐标至 Frenet 坐标的投影容差配置；无直接单位，必须为非空的 <see cref="FrenetConfiguration"/>，默认值为新的 Frenet 配置对象。
    /// </summary>
    public FrenetConfiguration Frenet { get; set; }
    /// <summary>
    /// 静态无碰撞走廊的采样与净空配置；无直接单位，必须为非空的 <see cref="CorridorConfiguration"/>，默认值为新的走廊配置对象。
    /// </summary>
    public CorridorConfiguration Corridor { get; set; }
    /// <summary>
    /// LS 横向优化的约束与权重配置；无直接单位，必须为非空的 <see cref="LateralConfiguration"/>，默认值为新的横向配置对象。
    /// </summary>
    public LateralConfiguration Lateral { get; set; }
    /// <summary>
    /// ST 纵向优化的速度、舒适性与权重配置；无直接单位，必须为非空的 <see cref="LongitudinalConfiguration"/>，默认值为新的纵向配置对象。
    /// </summary>
    public LongitudinalConfiguration Longitudinal { get; set; }
    /// <summary>
    /// QP 求解器的迭代次数、容差和运行选项；无直接单位，必须为非空的 <see cref="SolverConfiguration"/>，默认值为新的求解器配置对象。
    /// </summary>
    public SolverConfiguration Solver { get; set; }
    /// <summary>
    /// 发布前几何、运动学和终端姿态校验容差；无直接单位，必须为非空的 <see cref="ValidationConfiguration"/>，默认值为新的校验配置对象。
    /// </summary>
    public ValidationConfiguration Validation { get; set; }

    /// <summary>
    /// 创建用于低速泊车的全套默认配置；每次调用均构造新的子配置和权重对象，因此调用方可修改返回值而不影响其他默认快照。
    /// </summary>
    public static EmPlannerConfiguration CreateDefault()
    {
        return new EmPlannerConfiguration
        {
            Scheduling = new SchedulingConfiguration
            {
                ReplanPeriodSeconds = 0.20d,
                TimeHorizonSeconds = 6d,                // 单次规划的时间窗口，单位 s。
                DistanceHorizonMeters = 500d,           // 单次规划的最大前向探索距离，单位 m。
                OutputTimeStepSeconds = 0.05d,
                SolverTimeoutSeconds = 0.10d,
                HandoffLookaheadSeconds = 0.30d,
                MaximumVehicleStateAgeSeconds = 0.20d,
                MaximumOptimizationTimeStepSeconds = 0.20d,
                MaximumOptimizationSpatialStepMeters = 0.10d,
                MaximumOptimizationKnotCount = 401,
                MaximumPublishedSampleCount = 5001,
            },
            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 = 1d,
                MaximumReverseSpeedMetersPerSecond = 0.5d,
                DesiredForwardSpeedMetersPerSecond = 1d,
                DesiredReverseSpeedMetersPerSecond = 0.5d,
                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-5d,
                TerminalPositionToleranceMeters = 0d,
                TerminalYawToleranceRadians = 0d,
            },
        };
    }

    /// <summary>
    /// 深复制当前配置树；输入为当前实例状态，输出与当前实例不共享非空嵌套对象的可修改快照，原本为 <c>null</c> 的子配置保持为 <c>null</c> 且不产生失败状态。
    /// </summary>
    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(),
        };
    }
}