feat: 发布 EM 轨迹规划首个版本

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs

@@ -4,9 +4,16 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Reference-distance terminal chosen before LS without crossing the current direction segment.</summary>
+/// <summary>
+/// 在纵横向优化前选定的参考距离终端，保证不会跨越当前方向段。
+/// 所有参考站量以当前段局部 S（m）表示；边界类型和纵向模式明确滚动延续、接近停车或精确停车语义。
+/// </summary>
 public sealed class PlanningHorizonSelection
 {
+    /// <summary>
+    /// 创建仅供本程序集发布的窗口与停车边界选择。
+    /// 参数：两个参考站均为局部 S（m），boundary/terminal/mode 为已判定枚举；hasStopBoundary 表示终端是否受真实停车边界约束。
+    /// </summary>
     internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
         EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
         double stopBoundaryReferenceS, bool hasStopBoundary)
@@ -19,25 +26,58 @@ public sealed class PlanningHorizonSelection
         HasStopBoundary = hasStopBoundary;
     }
 
+    /// <summary>
+    /// 本次优化窗口末端的局部参考弧长 S，单位 m。
+    /// </summary>
     public double WindowEndReferenceS { get; }
 
+    /// <summary>
+    /// 规划输出的终端参考弧长 S，当前与窗口末端相同，单位 m。
+    /// </summary>
     public double TerminalReferenceS => WindowEndReferenceS;
 
+    /// <summary>
+    /// 窗口末端的边界语义；中途截断时为滚动安全停车边界。
+    /// </summary>
     public EmBoundaryType WindowEndBoundaryType { get; }
 
+    /// <summary>
+    /// 供终端约束使用的目标、换向或滚动终端类型。
+    /// </summary>
     public EmTerminalType TerminalType { get; }
 
+    /// <summary>
+    /// 纵向候选应滚动延续、接近停车边界还是在边界精确停车的模式。
+    /// </summary>
     public EmLongitudinalMode LongitudinalMode { get; }
 
+    /// <summary>
+    /// 实际目标或换向停车边界的局部参考弧长 S，单位 m；无此边界时仍保存段末。
+    /// </summary>
     public double StopBoundaryReferenceS { get; }
 
+    /// <summary>
+    /// 是否存在必须在当前方向段内处理的目标或换向停车边界。
+    /// </summary>
     public bool HasStopBoundary { get; }
 }
 
+/// <summary>
+/// 根据段边界、初始纵向状态和调度窗口选择 EM 优化地平线。
+/// 输入速度为 m/s、加速度为 m/s²、距离为 m；无效输入、越界初态或不足以停车的段会以明确状态拒绝。
+/// </summary>
 public sealed class PlanningHorizonSelector
 {
+    /// <summary>
+    /// 段终点、初态限值和停车距离比较使用的局部 S/距离容差，单位 m。
+    /// </summary>
     private const double BoundaryTolerance = 1e-8d;
 
+    /// <summary>
+    /// 为当前方向段选择不跨界的参考距离窗口和纵向终端模式。
+    /// 参数：currentSegmentReferenceS 为 m，初速为 m/s，初加速度为 m/s²，configuration 提供 m、s 制限值；planningScope 必须为定义的范围枚举。
+    /// 返回：成功时给出 selection；配置/初态非法返回 InvalidInput，停车距离越段返回 StoppingDistanceInsufficient，非法全段终边界返回 InvalidReferencePath。
+    /// </summary>
     public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
         double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
         EmPlanningScope planningScope, EmPlannerConfiguration configuration, out PlanningHorizonSelection selection,
@@ -138,11 +178,19 @@ public sealed class PlanningHorizonSelector
         return EmPlanningStatus.Success;
     }
 
+    /// <summary>
+    /// 判断边界是否要求车辆在当前段内停止。
+    /// 参数：boundaryType 为边界枚举；返回：Goal 或 GearSwitchApproach 时为 true，其余边界允许滚动延续。
+    /// </summary>
     private static bool IsStopBoundary(EmBoundaryType boundaryType)
     {
         return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
     }
 
+    /// <summary>
+    /// 将参考边界语义映射为纵向终端类型。
+    /// 参数：boundaryType 为边界枚举；返回：目标、任一换向边界分别映射到 Goal、GearSwitch，其余映射为 RollingSafetyStop。
+    /// </summary>
     private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
     {
         return boundaryType == EmBoundaryType.Goal
@@ -152,11 +200,19 @@ public sealed class PlanningHorizonSelector
                 : EmTerminalType.RollingSafetyStop;
     }
 
+    /// <summary>
+    /// 判断配置中的正量是否可用于时间、距离或动力学计算。
+    /// 参数：value 为对应单位的标量；返回：仅有限且严格大于零时为 true。
+    /// </summary>
     private static bool IsPositiveFinite(double value)
     {
         return IsFinite(value) && value > 0d;
     }
 
+    /// <summary>
+    /// 判断规划输入是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 与无穷均返回 false。
+    /// </summary>
     private static bool IsFinite(double value)
     {
         return !double.IsNaN(value) && !double.IsInfinity(value);