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

This commit is contained in:
2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -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(