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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/DirectionSegmentView.cs

@@ -6,8 +6,16 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 平滑参考路径中单一前进或倒车方向段的只读视图；EM 规划和投影不得跨越该段的精确换向边界。
+/// </summary>
 public sealed class DirectionSegmentView
 {
+    /// <summary>
+    /// 创建一个已重基到局部 S=0 的方向段只读视图。
+    /// 参数：points 的 ArcLength 为 m 且方向必须一致，边界与 sourceStartArcLength 为 m；首点与起边界必须位于局部 S=0，终边界须在 1e-12 m 内匹配末点。
+    /// 返回：防御性复制的不可变段；索引、方向、边界或弧长不一致时引发异常。
+    /// </summary>
     public DirectionSegmentView(
         int segmentIndex,
         TravelDirection direction,
@@ -48,17 +56,38 @@ public sealed class DirectionSegmentView
         SourceStartArcLength = sourceStartArcLength;
     }
 
+    /// <summary>
+    /// 在原始平滑路径方向段序列中的零基索引。
+    /// </summary>
     public int SegmentIndex { get; }
 
+    /// <summary>
+    /// 全段唯一的行驶方向；投影和优化不得在本视图内切换方向。
+    /// </summary>
     public TravelDirection Direction { get; }
 
+    /// <summary>
+    /// 局部弧长已重基、按 S（m）非递减排列的平滑参考点只读副本。
+    /// </summary>
     public IReadOnlyList<SmoothedPathPoint> Points { get; }
 
+    /// <summary>
+    /// 局部 S=0 的起始边界，含换向离开语义（若有）。
+    /// </summary>
     public ReferenceBoundary StartBoundary { get; }
 
+    /// <summary>
+    /// 局部段末端的边界，含目标或换向接近语义（若有）。
+    /// </summary>
     public ReferenceBoundary EndBoundary { get; }
 
+    /// <summary>
+    /// 本段局部 S=0 在原始平滑路径中的累计弧长，单位 m。
+    /// </summary>
     public double SourceStartArcLength { get; }
 
+    /// <summary>
+    /// 本段从局部 S=0 到终边界的长度，单位 m。
+    /// </summary>
     public double LengthMeters { get { return EndBoundary.SegmentLocalS; } }
 }

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);

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceBoundary.cs

@@ -2,8 +2,16 @@ using System;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 方向段的不可变参考边界标识，携带局部与源路径弧长以及边界语义。
+/// 两个弧长均以 m 计；相等性仅代表同一段、同一局部位置和同一类型，不以源弧长参与判等。
+/// </summary>
 public sealed class ReferenceBoundary : IEquatable<ReferenceBoundary>
 {
+    /// <summary>
+    /// 创建已验证的参考边界。
+    /// 参数：segmentIndex 必须非负，segmentLocalS 与 sourceArcLength 为非负有限 m 制弧长，boundaryType 必须是已定义枚举；非法输入会引发异常。
+    /// </summary>
     public ReferenceBoundary(int segmentIndex, double segmentLocalS, EmBoundaryType boundaryType, double sourceArcLength)
     {
         if (segmentIndex < 0)
@@ -21,22 +29,46 @@ public sealed class ReferenceBoundary : IEquatable<ReferenceBoundary>
         SourceArcLength = sourceArcLength;
     }
 
+    /// <summary>
+    /// 边界所属方向段在路径段序列中的零基索引。
+    /// </summary>
     public int SegmentIndex { get; }
 
+    /// <summary>
+    /// 边界相对该方向段起点的局部参考弧长 S，单位 m。
+    /// </summary>
     public double SegmentLocalS { get; }
 
+    /// <summary>
+    /// 边界的目标、换向或普通边界语义。
+    /// </summary>
     public EmBoundaryType BoundaryType { get; }
 
+    /// <summary>
+    /// 边界在未重基原始平滑路径中的累计弧长，单位 m。
+    /// </summary>
     public double SourceArcLength { get; }
 
+    /// <summary>
+    /// 比较两个边界在优化语义上是否相同。
+    /// 参数：other 可为 null；返回：仅段索引、局部 S（精确 double 比较）和边界类型都相同时为 true，源弧长不参与比较。
+    /// </summary>
     public bool Equals(ReferenceBoundary other)
     {
         return other != null && SegmentIndex == other.SegmentIndex && SegmentLocalS.Equals(other.SegmentLocalS) &&
             BoundaryType == other.BoundaryType;
     }
 
+    /// <summary>
+    /// 比较任意对象是否为同一参考边界。
+    /// 参数：obj 可为空或非边界对象；返回：仅可转换为 <see cref="ReferenceBoundary"/> 且满足强类型相等性时为 true。
+    /// </summary>
     public override bool Equals(object obj) { return Equals(obj as ReferenceBoundary); }
 
+    /// <summary>
+    /// 计算与边界相等性一致的哈希值。
+    /// 返回：由段索引、局部 S 与边界类型组成的哈希；源弧长不参与以保持与 <see cref="Equals(ReferenceBoundary)"/> 一致。
+    /// </summary>
     public override int GetHashCode()
     {
         unchecked

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceHorizonSlicer.cs

@@ -6,8 +6,15 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 当前方向段内供一次 EM 优化使用的参考路径切片，保留全局参考站与切片局部弧长的对应关系。
+/// </summary>
 public sealed class ReferenceHorizonSlice
 {
+    /// <summary>
+    /// 创建一个以当前段起点为基准的参考地平线切片。
+    /// 参数：points 为局部 S（m）有序的参考点，terminalBoundary 必须属于 segment；返回：points 的防御性只读副本，空输入或边界不匹配会被拒绝。
+    /// </summary>
     public ReferenceHorizonSlice(DirectionSegmentView segment, IReadOnlyList<SmoothedPathPoint> points,
         ReferenceBoundary terminalBoundary)
     {
@@ -25,17 +32,38 @@ public sealed class ReferenceHorizonSlice
         TerminalBoundary = terminalBoundary;
     }
 
+    /// <summary>
+    /// 切片所属且不会跨越的单一方向段。
+    /// </summary>
     public DirectionSegmentView Segment { get; }
 
+    /// <summary>
+    /// 从段起点至精确终端点的参考点只读副本，局部 ArcLength 单位为 m。
+    /// </summary>
     public IReadOnlyList<SmoothedPathPoint> Points { get; }
 
+    /// <summary>
+    /// 切片末端的精确参考边界；中途截断时具有滚动安全停车语义。
+    /// </summary>
     public ReferenceBoundary TerminalBoundary { get; }
 }
 
+/// <summary>
+/// 按局部参考距离截取单一方向段，并在请求终端精确插值参考点。
+/// 切片只包含本段坐标；S、原始弧长与 X/Y 使用 m，航向使用 rad，终端附近以 1e-12 m 判定相等。
+/// </summary>
 public static class ReferenceHorizonSlicer
 {
+    /// <summary>
+    /// 端点复制、精确节点匹配和原段终点判定使用的局部 S 容差，单位 m。
+    /// </summary>
     private const double Epsilon = 1e-12d;
 
+    /// <summary>
+    /// 将方向段截取到请求的局部终端 S，并追加该终端的精确点。
+    /// 参数：requestedEndSegmentLocalS 为非负有限 m 制局部 S；超过段长的请求夹紧到段末。
+    /// 返回：段末保留原边界，中途截断创建 RollingSafetyStop 边界；空段或非法 S 会引发异常。
+    /// </summary>
     public static ReferenceHorizonSlice Slice(DirectionSegmentView segment, double requestedEndSegmentLocalS)
     {
         if (segment == null)
@@ -60,6 +88,10 @@ public static class ReferenceHorizonSlicer
         return new ReferenceHorizonSlice(segment, points, terminal);
     }
 
+    /// <summary>
+    /// 取得给定局部 S 的精确已有点或其相邻点之间的插值点。
+    /// 参数：terminalS 为已夹紧的 m 制局部 S；返回：与节点相差不超过 1e-12 m 时复用节点，否则在正跨度内线性插值。
+    /// </summary>
     private static SmoothedPathPoint GetExactTerminalPoint(DirectionSegmentView segment, double terminalS)
     {
         for (int index = 0; index < segment.Points.Count; index++)
@@ -76,6 +108,10 @@ public static class ReferenceHorizonSlicer
         return segment.Points[segment.Points.Count - 1];
     }
 
+    /// <summary>
+    /// 在线性弧长区间内插值一个终端平滑路径点。
+    /// 参数：lower/upper 的 ArcLength 为 m，localS 为其间的 m 制位置；返回：位置、展开航向、曲率和净空线性插值，标记为 Interpolated。
+    /// </summary>
     private static SmoothedPathPoint Interpolate(SmoothedPathPoint lower, SmoothedPathPoint upper, double localS)
     {
         double interval = upper.ArcLength - lower.ArcLength;
@@ -98,6 +134,10 @@ public static class ReferenceHorizonSlicer
             SmoothedPathPointSource.Interpolated);
     }
 
+    /// <summary>
+    /// 计算同单位标量的线性插值。
+    /// 参数：lower、upper 为端点，fraction 为无单位比例；返回：未夹紧的线性结果。
+    /// </summary>
     private static double Interpolate(double lower, double upper, double fraction)
     {
         return lower + (upper - lower) * fraction;

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferencePathSegmenter.cs

@@ -5,8 +5,16 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 将已发布的平滑参考路径拆分为不会跨越换向点的局部方向段视图。
+/// 每个输出段以局部 S=0 重基（m），同时保留原始累计弧长（m）及目标/换向边界语义；索引覆盖或弧长不连续会被拒绝。
+/// </summary>
 public static class ReferencePathSegmenter
 {
+    /// <summary>
+    /// 从已验证的平滑路径及其方向分段创建局部方向段视图。
+    /// 参数：referencePath 必须含完整 Path 和 Segments，路径弧长为有限 m 制量；返回：按源段索引排列的只读视图，任何漏覆盖、方向或严格递增弧长错误均引发异常。
+    /// </summary>
     public static IReadOnlyList<DirectionSegmentView> Create(PathSmoothingResult referencePath)
     {
         if (referencePath == null || referencePath.Path == null || referencePath.Segments == null ||
@@ -65,6 +73,10 @@ public static class ReferencePathSegmenter
         return new ReadOnlyCollection<DirectionSegmentView>(result);
     }
 
+    /// <summary>
+    /// 复制一个源参考点并将其弧长重基到当前方向段局部坐标。
+    /// 参数：source 保存世界 X/Y（m）、航向（rad）和曲率量，localS 为非负段局部 m 制弧长；返回：除 ArcLength 外保持源点几何和来源不变的副本。
+    /// </summary>
     internal static SmoothedPathPoint CloneAtLocalS(SmoothedPathPoint source, double localS)
     {
         return new SmoothedPathPoint(