feat: 发布 EM 轨迹规划首个版本
This commit is contained in:
@@ -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(
|
||||
|
||||
Reference in New Issue
Block a user