chore: save current workspace progress
This commit is contained in:
@@ -0,0 +1,63 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>最终粗路径上的一个稠密连续点;长度、净空和位置使用 m,航向使用 rad,曲率使用 1/m。</summary>
|
||||
public sealed class CoarsePathPoint
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建粗路径点。
|
||||
/// 参数:xMeters、yMeters 为世界坐标 m;headingRadians 与 unwrappedHeadingRadians 为航向 rad;arcLengthMeters 和 bodyClearanceMeters 为 m;vehicleCurvaturePerMeter 为 1/m。
|
||||
/// </summary>
|
||||
public CoarsePathPoint(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double arcLengthMeters,
|
||||
TravelDirection direction,
|
||||
double vehicleCurvaturePerMeter,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
CoarsePathPointSource source)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
Heading = headingRadians;
|
||||
UnwrappedHeading = unwrappedHeadingRadians;
|
||||
ArcLength = arcLengthMeters;
|
||||
Direction = direction;
|
||||
VehicleCurvature = vehicleCurvaturePerMeter;
|
||||
BodyClearance = bodyClearanceMeters;
|
||||
IsGearSwitchPoint = isGearSwitchPoint;
|
||||
Source = source;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标,单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标,单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>归一化后可用于几何查询的车头航向,单位 rad。</summary>
|
||||
public double Heading { get; }
|
||||
|
||||
/// <summary>跨越 ±π 后仍连续的车头航向,单位 rad。</summary>
|
||||
public double UnwrappedHeading { get; }
|
||||
|
||||
/// <summary>从路径起点累计的弧长,单位 m;必须非负且不递减。</summary>
|
||||
public double ArcLength { get; }
|
||||
|
||||
/// <summary>从上一点运动到当前点所在方向段的行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>车辆在此点采用的恒曲率原语曲率,单位 1/m。</summary>
|
||||
public double VehicleCurvature { get; }
|
||||
|
||||
/// <summary>扩大车体到障碍物的保守净空下界,单位 m。</summary>
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>此点是否为新方向段开始的换向点。</summary>
|
||||
public bool IsGearSwitchPoint { get; }
|
||||
|
||||
/// <summary>此点由起点、普通原语或终点截断产生的来源。</summary>
|
||||
public CoarsePathPointSource Source { get; }
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>粗路径点在搜索与原语重建中的产生来源。</summary>
|
||||
public enum CoarsePathPointSource
|
||||
{
|
||||
/// <summary>请求提供的起始位姿。</summary>
|
||||
Start,
|
||||
|
||||
/// <summary>未截断恒曲率运动原语的内部积分点。</summary>
|
||||
MotionPrimitive,
|
||||
|
||||
/// <summary>首次满足目标容差而在原语内部截断的终点积分点。</summary>
|
||||
GoalTruncation,
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>终点候选最后一个连续运动段的方向约束。</summary>
|
||||
public enum GoalDirectionConstraint
|
||||
{
|
||||
/// <summary>不限制进入终点的运动方向。</summary>
|
||||
Any,
|
||||
|
||||
/// <summary>必须以前进方向进入终点。</summary>
|
||||
Forward,
|
||||
|
||||
/// <summary>必须以倒车方向进入终点。</summary>
|
||||
Reverse,
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid A* 粗路径搜索的可调配置。
|
||||
/// 长度和距离使用 m,航向使用 rad,曲率使用 1/m;各项有效范围由规划器在执行前校验。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarConfiguration
|
||||
{
|
||||
/// <summary>创建采用 P0 固定安全边界和代价权重的默认配置。</summary>
|
||||
public HybridAStarConfiguration()
|
||||
{
|
||||
PrimitiveLengthMeters = 0.50d;
|
||||
IntegrationStepMeters = 0.05d;
|
||||
MaximumCollisionCheckStepMeters = 0.025d;
|
||||
HeadingResolutionRadians = Math.PI / 36d;
|
||||
CurvatureLevelCount = 5;
|
||||
GoalPositionToleranceMeters = 0.15d;
|
||||
GoalHeadingToleranceRadians = Math.PI / 36d;
|
||||
MaximumExpandedNodes = 200000;
|
||||
SearchTimeout = TimeSpan.FromSeconds(5d);
|
||||
HeuristicWeight = 1d;
|
||||
ReverseCostMultiplier = 1.5d;
|
||||
GearSwitchPenaltyMeters = 1d;
|
||||
CurvatureMagnitudeWeight = 0.10d;
|
||||
CurvatureChangePenaltyMetersPerLevel = 0.05d;
|
||||
ClearanceCostWeight = 0.20d;
|
||||
ClearanceCostDistanceMeters = 0.50d;
|
||||
AllowReverse = true;
|
||||
}
|
||||
|
||||
/// <summary>单个恒曲率原语的最大行驶长度,单位 m。</summary>
|
||||
public double PrimitiveLengthMeters { get; set; }
|
||||
|
||||
/// <summary>原语内部输出积分点之间允许的最大弧长,单位 m。</summary>
|
||||
public double IntegrationStepMeters { get; set; }
|
||||
|
||||
/// <summary>连续碰撞检查允许的最大车辆中心位移,单位 m;实际值还受地图分辨率限制。</summary>
|
||||
public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
|
||||
/// <summary>搜索状态离散所使用的航向格宽,单位 rad。</summary>
|
||||
public double HeadingResolutionRadians { get; set; }
|
||||
|
||||
/// <summary>从最大负曲率到最大正曲率的离散曲率等级数。</summary>
|
||||
public int CurvatureLevelCount { get; set; }
|
||||
|
||||
/// <summary>终点位置允许的欧氏距离误差,单位 m。</summary>
|
||||
public double GoalPositionToleranceMeters { get; set; }
|
||||
|
||||
/// <summary>终点车头航向允许的最小环形角度误差,单位 rad。</summary>
|
||||
public double GoalHeadingToleranceRadians { get; set; }
|
||||
|
||||
/// <summary>单次搜索允许扩展的最大节点数。</summary>
|
||||
public int MaximumExpandedNodes { get; set; }
|
||||
|
||||
/// <summary>单次搜索允许消耗的最长时间;超出后返回 <see cref="PlanningStatus.SearchTimeout"/>。</summary>
|
||||
public TimeSpan SearchTimeout { get; set; }
|
||||
|
||||
/// <summary>二维绕障启发式的权重;1 表示不额外放大。</summary>
|
||||
public double HeuristicWeight { get; set; }
|
||||
|
||||
/// <summary>倒车原语长度代价相对前进原语的倍率。</summary>
|
||||
public double ReverseCostMultiplier { get; set; }
|
||||
|
||||
/// <summary>相邻原语发生换向时增加的等效距离代价,单位 m。</summary>
|
||||
public double GearSwitchPenaltyMeters { get; set; }
|
||||
|
||||
/// <summary>曲率绝对值对应的无量纲代价权重。</summary>
|
||||
public double CurvatureMagnitudeWeight { get; set; }
|
||||
|
||||
/// <summary>相邻曲率等级每变化一级增加的等效距离代价,单位 m。</summary>
|
||||
public double CurvatureChangePenaltyMetersPerLevel { get; set; }
|
||||
|
||||
/// <summary>车体保守净空不足时增加的无量纲代价权重。</summary>
|
||||
public double ClearanceCostWeight { get; set; }
|
||||
|
||||
/// <summary>计算净空代价时视为足够安全的车体保守净空,单位 m。</summary>
|
||||
public double ClearanceCostDistanceMeters { get; set; }
|
||||
|
||||
/// <summary>是否允许生成倒车原语;false 时搜索只生成前进原语。</summary>
|
||||
public bool AllowReverse { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>粗路径中方向一致的一段连续点范围;索引两端均包含在段内。</summary>
|
||||
public sealed class PathSegment
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建方向分段。
|
||||
/// 参数:segmentIndex 为从零开始的段序号;startIndex 与 endIndex 为 <see cref="PlanningResult.Path"/> 的包含式索引;两个换向标记描述段首或段尾是否位于换向对。
|
||||
/// </summary>
|
||||
public PathSegment(int segmentIndex, TravelDirection direction, int startIndex, int endIndex, bool startsAtGearSwitch, bool endsAtGearSwitch)
|
||||
{
|
||||
SegmentIndex = segmentIndex;
|
||||
Direction = direction;
|
||||
StartIndex = startIndex;
|
||||
EndIndex = endIndex;
|
||||
StartsAtGearSwitch = startsAtGearSwitch;
|
||||
EndsAtGearSwitch = endsAtGearSwitch;
|
||||
}
|
||||
|
||||
/// <summary>从零开始的方向段序号。</summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>此段所有连续运动点对应的行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>此段在 <see cref="PlanningResult.Path"/> 中的起始索引,包含该点。</summary>
|
||||
public int StartIndex { get; }
|
||||
|
||||
/// <summary>此段在 <see cref="PlanningResult.Path"/> 中的结束索引,包含该点。</summary>
|
||||
public int EndIndex { get; }
|
||||
|
||||
/// <summary>此段首点是否为换向后保留的新方向点。</summary>
|
||||
public bool StartsAtGearSwitch { get; }
|
||||
|
||||
/// <summary>此段尾点是否紧邻下一方向段的换向对。</summary>
|
||||
public bool EndsAtGearSwitch { get; }
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>一次规划的只读统计与终止说明;长度和净空单位为 m。</summary>
|
||||
public sealed class PlanningDiagnostics
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建规划统计快照。
|
||||
/// 参数中的节点数量均为非负计数;pathLengthMeters 和 minimumBodyClearanceMeters 单位为 m;elapsed 为总耗时;pathSearchElapsed 为地图和起终点预检通过后的路径产出耗时;terminationReason 为可读终止说明,可为 null。
|
||||
/// </summary>
|
||||
public PlanningDiagnostics(
|
||||
int expandedNodeCount = 0,
|
||||
int generatedNodeCount = 0,
|
||||
int reopenedNodeCount = 0,
|
||||
int staleOpenListEntryCount = 0,
|
||||
int peakOpenListCount = 0,
|
||||
double pathLengthMeters = 0d,
|
||||
double minimumBodyClearanceMeters = 0d,
|
||||
TimeSpan elapsed = default(TimeSpan),
|
||||
string terminationReason = null,
|
||||
TimeSpan pathSearchElapsed = default(TimeSpan))
|
||||
{
|
||||
ExpandedNodeCount = expandedNodeCount;
|
||||
GeneratedNodeCount = generatedNodeCount;
|
||||
ReopenedNodeCount = reopenedNodeCount;
|
||||
StaleOpenListEntryCount = staleOpenListEntryCount;
|
||||
PeakOpenListCount = peakOpenListCount;
|
||||
PathLengthMeters = pathLengthMeters;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
Elapsed = elapsed;
|
||||
PathSearchElapsed = pathSearchElapsed;
|
||||
TerminationReason = terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>从 Open List 取出并真正扩展的节点数量。</summary>
|
||||
public int ExpandedNodeCount { get; }
|
||||
|
||||
/// <summary>生成并尝试加入搜索状态的节点数量。</summary>
|
||||
public int GeneratedNodeCount { get; }
|
||||
|
||||
/// <summary>以严格更小代价到达同一离散键而重新打开的节点数量。</summary>
|
||||
public int ReopenedNodeCount { get; }
|
||||
|
||||
/// <summary>从 Open List 取出后因已有更优条目而丢弃的陈旧堆条目数量。</summary>
|
||||
public int StaleOpenListEntryCount { get; }
|
||||
|
||||
/// <summary>搜索期间 Open List 同时容纳的最大有效或待丢弃条目数量。</summary>
|
||||
public int PeakOpenListCount { get; }
|
||||
|
||||
/// <summary>成功路径的累计弧长,单位 m;失败结果通常为 0。</summary>
|
||||
public double PathLengthMeters { get; }
|
||||
|
||||
/// <summary>成功路径所有点中车体保守净空下界的最小值,单位 m;失败结果通常为 0。</summary>
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
|
||||
/// <summary>从规划入口到返回结果的总耗时。</summary>
|
||||
public TimeSpan Elapsed { get; }
|
||||
|
||||
/// <summary>地图和起终点预检通过后,二维启发式、Hybrid A*、回溯、装配和最终复核的耗时;不含建图,搜索前失败时为零。</summary>
|
||||
public TimeSpan PathSearchElapsed { get; }
|
||||
|
||||
/// <summary>面向调用方的终止原因;成功时可为空字符串。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 已建规划地图上的一次 Hybrid A* 请求。
|
||||
/// 本对象只接受不可变 <see cref="PlanningGridMap"/> 快照,不包含地图构建器、传感器、UI 或调试对象。
|
||||
/// </summary>
|
||||
public sealed class PlanningRequest
|
||||
{
|
||||
/// <summary>创建空规划请求。调用规划器前必须提供地图、起点、终点、车辆和配置。</summary>
|
||||
public PlanningRequest()
|
||||
{
|
||||
StartVehicleCurvature = 0d;
|
||||
GoalDirection = GoalDirectionConstraint.Any;
|
||||
}
|
||||
|
||||
/// <summary>本次搜索唯一允许查询的不可变规划地图快照;位置查询单位为 m。</summary>
|
||||
public PlanningGridMap Map { get; set; }
|
||||
|
||||
/// <summary>车辆几何中心的起始位姿;位置单位 m,航向单位 rad。</summary>
|
||||
public Pose2D Start { get; set; }
|
||||
|
||||
/// <summary>车辆几何中心的目标位姿;位置单位 m,航向单位 rad。</summary>
|
||||
public Pose2D Goal { get; set; }
|
||||
|
||||
/// <summary>车辆几何、余量和曲率限制;尺寸单位 m,曲率单位 1/m。</summary>
|
||||
public VehicleParameters Vehicle { get; set; }
|
||||
|
||||
/// <summary>搜索步长、离散、终点容差、代价和资源上限配置。</summary>
|
||||
public HybridAStarConfiguration Configuration { get; set; }
|
||||
|
||||
/// <summary>车辆起步时的转向曲率,单位 1/m;默认值为 0。</summary>
|
||||
public double StartVehicleCurvature { get; set; }
|
||||
|
||||
/// <summary>起步方向约束;null 表示可从前进或倒车开始。</summary>
|
||||
public TravelDirection? StartDirection { get; set; }
|
||||
|
||||
/// <summary>目标进入方向约束;默认值为 <see cref="GoalDirectionConstraint.Any"/>。</summary>
|
||||
public GoalDirectionConstraint GoalDirection { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 一次粗路径规划的最终不可变结果。
|
||||
/// 只有 <see cref="Status"/> 为 <see cref="PlanningStatus.Success"/> 时才携带非空路径与方向分段;其他状态始终返回空只读集合。
|
||||
/// </summary>
|
||||
public sealed class PlanningResult
|
||||
{
|
||||
private static readonly IReadOnlyList<CoarsePathPoint> EmptyPath = new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
|
||||
private static readonly IReadOnlyList<PathSegment> EmptySegments = new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
|
||||
|
||||
private PlanningResult(PlanningStatus status, PlanningDiagnostics diagnostics, IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments)
|
||||
{
|
||||
Status = status;
|
||||
Diagnostics = diagnostics ?? new PlanningDiagnostics(terminationReason: "未提供诊断信息。");
|
||||
Path = path;
|
||||
Segments = segments;
|
||||
}
|
||||
|
||||
/// <summary>规划最终状态;只有 <see cref="PlanningStatus.Success"/> 可以发布路径。</summary>
|
||||
public PlanningStatus Status { get; }
|
||||
|
||||
/// <summary>节点、耗时、路径长度、净空和终止原因统计;始终非空。</summary>
|
||||
public PlanningDiagnostics Diagnostics { get; }
|
||||
|
||||
/// <summary>成功时的稠密粗路径;失败时为不可修改的空集合。</summary>
|
||||
public IReadOnlyList<CoarsePathPoint> Path { get; }
|
||||
|
||||
/// <summary>成功时覆盖 <see cref="Path"/> 的包含式方向分段;失败时为不可修改的空集合。</summary>
|
||||
public IReadOnlyList<PathSegment> Segments { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 创建成功结果。
|
||||
/// 参数:path 与 segments 必须均为非空;diagnostics 为本次规划的统计快照。参数不符合要求时抛出 <see cref="ArgumentException"/>,防止以成功状态发布不完整路径。
|
||||
/// </summary>
|
||||
public static PlanningResult Success(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, PlanningDiagnostics diagnostics)
|
||||
{
|
||||
if (path == null || path.Count == 0)
|
||||
throw new ArgumentException("Successful planning results require a non-empty path.", nameof(path));
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("Successful planning results require non-empty segments.", nameof(segments));
|
||||
return new PlanningResult(PlanningStatus.Success, diagnostics, CopyReadOnly(path), CopyReadOnly(segments));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建失败、取消或资源受限结果。
|
||||
/// 参数:status 不能为 <see cref="PlanningStatus.Success"/>;diagnostics 会原样保留。返回结果的路径与分段始终为空只读集合。
|
||||
/// </summary>
|
||||
public static PlanningResult Failure(PlanningStatus status, PlanningDiagnostics diagnostics)
|
||||
{
|
||||
if (status == PlanningStatus.Success)
|
||||
throw new ArgumentException("Use Success to create a successful planning result.", nameof(status));
|
||||
return new PlanningResult(status, diagnostics, EmptyPath, EmptySegments);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>粗路径规划的最终状态;除 <see cref="Success"/> 外均不发布路径或方向分段。</summary>
|
||||
public enum PlanningStatus
|
||||
{
|
||||
/// <summary>已得到并通过最终连续碰撞复核的完整粗路径。</summary>
|
||||
Success,
|
||||
|
||||
/// <summary>在搜索扩展检查点收到取消请求。</summary>
|
||||
Cancelled,
|
||||
|
||||
/// <summary>请求对象或其必要成员为空,或包含不符合基本契约的值。</summary>
|
||||
InvalidRequest,
|
||||
|
||||
/// <summary>请求提供的地图对象不满足规划器的结构要求。</summary>
|
||||
InvalidMap,
|
||||
|
||||
/// <summary>地图快照尚未准备好参与规划;应查看地图的阻止原因。</summary>
|
||||
MapNotReady,
|
||||
|
||||
/// <summary>车辆尺寸、安全余量或曲率限制无效。</summary>
|
||||
InvalidVehicleParameters,
|
||||
|
||||
/// <summary>原语、离散、代价、容差或资源限制配置无效。</summary>
|
||||
InvalidCurvatureConfiguration,
|
||||
|
||||
/// <summary>起始车辆几何中心或扩大车体不在地图范围内。</summary>
|
||||
StartOutsideMap,
|
||||
|
||||
/// <summary>起始扩大车体与地图障碍物相交或擦边。</summary>
|
||||
StartInCollision,
|
||||
|
||||
/// <summary>目标车辆几何中心或扩大车体不在地图范围内。</summary>
|
||||
GoalOutsideMap,
|
||||
|
||||
/// <summary>目标扩大车体与地图障碍物相交或擦边。</summary>
|
||||
GoalInCollision,
|
||||
|
||||
/// <summary>搜索达到 <see cref="HybridAStarConfiguration.SearchTimeout"/> 限制。</summary>
|
||||
SearchTimeout,
|
||||
|
||||
/// <summary>搜索达到 <see cref="HybridAStarConfiguration.MaximumExpandedNodes"/> 限制。</summary>
|
||||
SearchNodeLimitExceeded,
|
||||
|
||||
/// <summary>Open List 已耗尽,或二维启发式证明目标不可达。</summary>
|
||||
NoFeasiblePath,
|
||||
|
||||
/// <summary>搜索成功节点无法按父链回溯为完整路径。</summary>
|
||||
BacktrackingFailed,
|
||||
|
||||
/// <summary>回溯路径未通过连续碰撞、终点或输出不变量复核。</summary>
|
||||
FinalValidationFailed,
|
||||
|
||||
/// <summary>规划内部发生未预期错误;不会发布部分路径。</summary>
|
||||
InternalError,
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 粗路径规划使用的二维连续位姿。
|
||||
/// 位置以世界坐标 m 表示,航向以 rad 表示;此值对象不在构造时归一化航向,调用方可保留展开航向。
|
||||
/// </summary>
|
||||
public sealed class Pose2D
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建二维位姿。
|
||||
/// 参数:xMeters、yMeters 为世界坐标,单位 m;headingRadians 为车头航向,单位 rad。
|
||||
/// </summary>
|
||||
public Pose2D(double xMeters, double yMeters, double headingRadians)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
Heading = headingRadians;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标,单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标,单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>车头航向,单位 rad;可以是已展开的连续航向。</summary>
|
||||
public double Heading { get; }
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>连续运动段的行驶方向。</summary>
|
||||
public enum TravelDirection
|
||||
{
|
||||
/// <summary>沿车辆车头方向前进。</summary>
|
||||
Forward,
|
||||
|
||||
/// <summary>与车辆车头方向相反地倒车。</summary>
|
||||
Reverse,
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 车辆几何与运动学参数。
|
||||
/// 所有几何尺寸均以车辆几何中心为 <see cref="Pose2D"/> 参考点,单位为 m;曲率单位为 1/m。
|
||||
/// </summary>
|
||||
public sealed class VehicleParameters
|
||||
{
|
||||
/// <summary>创建空车辆参数。调用规划器前必须填写有效的几何尺寸和至少一种曲率限制。</summary>
|
||||
public VehicleParameters()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>车辆本体长度,单位 m;不含 <see cref="SafetyMarginMeters"/>。</summary>
|
||||
public double LengthMeters { get; set; }
|
||||
|
||||
/// <summary>车辆本体宽度,单位 m;不含 <see cref="SafetyMarginMeters"/>。</summary>
|
||||
public double WidthMeters { get; set; }
|
||||
|
||||
/// <summary>碰撞检查时加在车体四周的安全余量,单位 m;不会写入地图。</summary>
|
||||
public double SafetyMarginMeters { get; set; }
|
||||
|
||||
/// <summary>车辆允许的最大绝对曲率,单位 1/m;null 表示由 <see cref="MinimumTurningRadiusMeters"/> 提供限制。</summary>
|
||||
public double? MaximumCurvaturePerMeter { get; set; }
|
||||
|
||||
/// <summary>车辆允许的最小转弯半径,单位 m;null 表示由 <see cref="MaximumCurvaturePerMeter"/> 提供限制。</summary>
|
||||
public double? MinimumTurningRadiusMeters { get; set; }
|
||||
}
|
||||
Reference in New Issue
Block a user