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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetReferencePoint.cs

@@ -4,9 +4,17 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
+/// <summary>
+/// 单一方向段内的不可变插值参考样本，连接世界几何与 Frenet 坐标。
+/// X/Y 与参考 S 为 m，航向为 rad，曲率为 1/m、曲率导数为 1/m²；倒车段仍以车辆航向保存几何。
+/// </summary>
 public sealed class FrenetReferencePoint
 {
+    /// <summary>
+    /// 创建已验证的方向段参考样本。
+    /// 参数：位置采用世界 X/Y（m），referenceS 为段局部弧长（m），航向为 rad，曲率量遵循 1/m 与 1/m²；所有数值必须有限。
+    /// 返回：车辆航向会规范到 [-π, π]；任一非有限输入会引发 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
     public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
         TravelDirection direction, double geometricCurvature, double vehicleCurvature,
         double vehicleCurvatureDerivative, double bodyClearance)
@@ -33,23 +41,69 @@ public sealed class FrenetReferencePoint
         BodyClearance = bodyClearance;
     }
 
+    /// <summary>
+    /// 样本在当前方向段的局部参考弧长 S，单位 m。
+    /// </summary>
     public double ReferenceS { get; }
+
+    /// <summary>
+    /// 参考中心线点的世界 X 坐标，单位 m。
+    /// </summary>
     public double X { get; }
+
+    /// <summary>
+    /// 参考中心线点的世界 Y 坐标，单位 m。
+    /// </summary>
     public double Y { get; }
+
+    /// <summary>
+    /// 归一化后的车辆车身航向，单位 rad。
+    /// </summary>
     public double VehicleYaw { get; }
+
+    /// <summary>
+    /// 连续展开的车辆车身航向，单位 rad，供跨点几何插值使用。
+    /// </summary>
     public double UnwrappedVehicleYaw { get; }
+
+    /// <summary>
+    /// 该参考样本的行驶方向，决定 Frenet 横向正负号和行驶航向。
+    /// </summary>
     public TravelDirection Direction { get; }
+
+    /// <summary>
+    /// 中心线几何曲率，单位 1/m。
+    /// </summary>
     public double GeometricCurvature { get; }
+
+    /// <summary>
+    /// 满足车辆模型后的车辆曲率，单位 1/m。
+    /// </summary>
     public double VehicleCurvature { get; }
+
+    /// <summary>
+    /// 车辆曲率相对弧长的导数，单位 1/m²。
+    /// </summary>
     public double VehicleCurvatureDerivative { get; }
+
+    /// <summary>
+    /// 参考点记录的车体净空或可用裕度，单位 m。
+    /// </summary>
     public double BodyClearance { get; }
 
-    /// <summary>Unwrapped direction of travel, used internally for geometry.</summary>
+    /// <summary>
+    /// 获取与当前行驶方向一致的连续航向。
+    /// 返回：前进时为车辆展开航向，倒车时加 π；单位 rad，仅供 Frenet 几何计算，不重新归一化。
+    /// </summary>
     public double TravelYaw
     {
         get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
     }
 
+    /// <summary>
+    /// 拒绝不能安全保存为参考几何的数值。
+    /// 参数：value 是待验证的任意单位标量，name 是异常参数名；NaN 或无穷会引发 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
     private static void RequireFinite(double value, string name)
     {
         if (double.IsNaN(value) || double.IsInfinity(value))