70 lines
3.1 KiB
C#
70 lines
3.1 KiB
C#
|
using System;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using System.Collections.ObjectModel;
|
|||
|
|
|
|||
|
|
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// 一条经连续碰撞检查的恒曲率运动原语。
|
|||
|
|
/// 位置和长度单位为 m，航向单位为 rad，曲率单位为 1/m；<see cref="Points"/> 不包含起点，只包含按积分顺序产生的后续点。
|
|||
|
|
/// </summary>
|
|||
|
|
public sealed class MotionPrimitive
|
|||
|
|
{
|
|||
|
|
/// <summary>
|
|||
|
|
/// 创建不可变运动原语。
|
|||
|
|
/// 参数：start 为原语起点；direction 为行驶方向；curvaturePerMeter 为恒定曲率；actualLengthMeters 为已实际行驶长度；
|
|||
|
|
/// points 与 bodyClearancesMeters 按同一索引保存内部积分位姿和对应的车体净空；isGoalTruncation 表示末点是否首次命中目标容差。
|
|||
|
|
/// </summary>
|
|||
|
|
public MotionPrimitive(
|
|||
|
|
Pose2D start,
|
|||
|
|
TravelDirection direction,
|
|||
|
|
double curvaturePerMeter,
|
|||
|
|
double actualLengthMeters,
|
|||
|
|
IEnumerable<Pose2D> points,
|
|||
|
|
IEnumerable<double> bodyClearancesMeters,
|
|||
|
|
bool isGoalTruncation)
|
|||
|
|
{
|
|||
|
|
if (start == null) throw new ArgumentNullException(nameof(start));
|
|||
|
|
if (points == null) throw new ArgumentNullException(nameof(points));
|
|||
|
|
if (bodyClearancesMeters == null) throw new ArgumentNullException(nameof(bodyClearancesMeters));
|
|||
|
|
|
|||
|
|
var copiedPoints = new List<Pose2D>(points);
|
|||
|
|
var copiedClearances = new List<double>(bodyClearancesMeters);
|
|||
|
|
if (copiedPoints.Count != copiedClearances.Count)
|
|||
|
|
throw new ArgumentException("The point and clearance counts must match.", nameof(bodyClearancesMeters));
|
|||
|
|
|
|||
|
|
Start = start;
|
|||
|
|
Direction = direction;
|
|||
|
|
CurvaturePerMeter = curvaturePerMeter;
|
|||
|
|
ActualLengthMeters = actualLengthMeters;
|
|||
|
|
Points = new ReadOnlyCollection<Pose2D>(copiedPoints);
|
|||
|
|
BodyClearancesMeters = new ReadOnlyCollection<double>(copiedClearances);
|
|||
|
|
End = copiedPoints.Count == 0 ? start : copiedPoints[copiedPoints.Count - 1];
|
|||
|
|
IsGoalTruncation = isGoalTruncation;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>原语起始车辆几何中心位姿。</summary>
|
|||
|
|
public Pose2D Start { get; }
|
|||
|
|
|
|||
|
|
/// <summary>原语最后一个积分点；零长度终点候选时等于 <see cref="Start"/>。</summary>
|
|||
|
|
public Pose2D End { get; }
|
|||
|
|
|
|||
|
|
/// <summary>原语对应的行驶方向。</summary>
|
|||
|
|
public TravelDirection Direction { get; }
|
|||
|
|
|
|||
|
|
/// <summary>原语全程采用的恒定车辆曲率，单位 1/m。</summary>
|
|||
|
|
public double CurvaturePerMeter { get; }
|
|||
|
|
|
|||
|
|
/// <summary>从 <see cref="Start"/> 到 <see cref="End"/> 的实际行驶弧长，单位 m。</summary>
|
|||
|
|
public double ActualLengthMeters { get; }
|
|||
|
|
|
|||
|
|
/// <summary>不含起点的连续积分位姿，只读且按行驶顺序排列。</summary>
|
|||
|
|
public IReadOnlyList<Pose2D> Points { get; }
|
|||
|
|
|
|||
|
|
/// <summary>与 <see cref="Points"/> 一一对应的扩大车体保守净空下界，单位 m。</summary>
|
|||
|
|
public IReadOnlyList<double> BodyClearancesMeters { get; }
|
|||
|
|
|
|||
|
|
/// <summary>末点是否因首次满足目标位置、航向和方向约束而截断。</summary>
|
|||
|
|
public bool IsGoalTruncation { get; }
|
|||
|
|
}
|