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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetTransform.cs

@@ -4,9 +4,17 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
+/// <summary>
+/// 在世界坐标与 Frenet 横向约定间转换，并始终以实际行驶方向确定 L 的正负。
+/// 世界位置使用 X/Y（m）、航向使用 rad；倒车时车辆车身航向与行驶航向相差 π。
+/// </summary>
 public static class FrenetTransform
 {
+    /// <summary>
+    /// 将参考点及其横向状态重建为世界车辆中心位姿。
+    /// 参数：lateralOffset 为 L（m），lateralDerivative 为 dL/dS（无量纲），minimumFrenetDenominator 为正的奇异性下界；参考点采用世界 X/Y（m）与 rad 航向。
+    /// 返回：当 1-κL 有限且不小于下界、重建位姿有限时返回 true；空参考点、非法输入或接近 Frenet 奇异点时返回 false。
+    /// </summary>
     public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
         double minimumFrenetDenominator, out Pose2D pose)
     {
@@ -33,11 +41,19 @@ public static class FrenetTransform
         return true;
     }
 
+    /// <summary>
+    /// 从车辆车身航向取得对应实际行驶的连续航向。
+    /// 参数：vehicleYaw 为 rad，direction 指定前进或倒车；返回：前进原样、倒车加 π，结果不归一化。
+    /// </summary>
     internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
     {
         return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
     }
 
+    /// <summary>
+    /// 判定重建中间量是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 或正负无穷时为 false。
+    /// </summary>
     private static bool IsFinite(double value)
     {
         return !double.IsNaN(value) && !double.IsInfinity(value);