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; }
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 一次从地图输入到 Hybrid A* 粗路径输出的完整业务请求。
|
||||
/// 地图边界、分辨率和障碍物几何位于 <see cref="MapRequest"/> 中并使用 mm；位姿和车辆几何使用 m，航向使用 rad，曲率使用 1/m。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathPlanningJob
|
||||
{
|
||||
/// <summary>创建默认目标方向、起步曲率和关闭调试旁路的业务请求。</summary>
|
||||
public CoarsePathPlanningJob()
|
||||
{
|
||||
StartVehicleCurvature = 0d;
|
||||
GoalDirection = GoalDirectionConstraint.Any;
|
||||
DebugOptions = new PlanningDebugOptions();
|
||||
}
|
||||
|
||||
/// <summary>本次唯一建图输入；边界、分辨率和障碍物几何均遵循 Map 模块的 mm 契约。</summary>
|
||||
public PlanningMapRequest MapRequest { 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; }
|
||||
|
||||
/// <summary>可选调试旁路配置；默认关闭且使用空接收器，不参与地图或路径计算。</summary>
|
||||
public PlanningDebugOptions DebugOptions { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 一次粗路径业务编排的不可变结果。
|
||||
/// 始终同时保留地图构建结果和规划结果；调试旁路消息只用于诊断，不会修改地图或规划内容。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathPlanningJobResult
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建业务编排结果。
|
||||
/// 参数：mapResult 和 planningResult 均不能为空；debugDiagnostics 可为 null，返回时会转换为只读字符串集合。
|
||||
/// </summary>
|
||||
public CoarsePathPlanningJobResult(PlanningMapBuildResult mapResult, PlanningResult planningResult,
|
||||
IReadOnlyList<string> debugDiagnostics = null)
|
||||
{
|
||||
MapResult = mapResult ?? throw new ArgumentNullException(nameof(mapResult));
|
||||
PlanningResult = planningResult ?? throw new ArgumentNullException(nameof(planningResult));
|
||||
DebugDiagnostics = CopyDiagnostics(debugDiagnostics);
|
||||
}
|
||||
|
||||
/// <summary>本次调用的地图创建结果；失败时读取 <see cref="PlanningMapBuildResult.FailureReason"/>。</summary>
|
||||
public PlanningMapBuildResult MapResult { get; }
|
||||
|
||||
/// <summary>本次调用的粗路径结果；地图创建失败时为无路径的失败状态。</summary>
|
||||
public PlanningResult PlanningResult { get; }
|
||||
|
||||
/// <summary>调试旁路的非致命诊断信息；为空时表示未启用、未发生异常或无额外调试消息。</summary>
|
||||
public IReadOnlyList<string> DebugDiagnostics { get; }
|
||||
|
||||
private static IReadOnlyList<string> CopyDiagnostics(IReadOnlyList<string> source)
|
||||
{
|
||||
if (source == null || source.Count == 0)
|
||||
return new ReadOnlyCollection<string>(new List<string>());
|
||||
|
||||
var copy = new List<string>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index] ?? string.Empty);
|
||||
return new ReadOnlyCollection<string>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,108 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 从 <see cref="PlanningMapRequest"/> 到 Hybrid A* 粗路径的一次调用服务。
|
||||
/// 服务生命周期内长期持有同一个地图工厂和规划器，以保留地图缓存并避免将 UI、传感器或调试依赖带入规划核心。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathPlanningService
|
||||
{
|
||||
private readonly PlanningMapFactory _mapFactory;
|
||||
private readonly HybridAStarPlanner _planner;
|
||||
|
||||
/// <summary>创建长期复用默认地图工厂与 Hybrid A* 规划器的服务。</summary>
|
||||
public CoarsePathPlanningService()
|
||||
: this(new PlanningMapFactory(), new HybridAStarPlanner())
|
||||
{
|
||||
}
|
||||
|
||||
internal CoarsePathPlanningService(PlanningMapFactory mapFactory, HybridAStarPlanner planner)
|
||||
{
|
||||
_mapFactory = mapFactory ?? throw new ArgumentNullException(nameof(mapFactory));
|
||||
_planner = planner ?? throw new ArgumentNullException(nameof(planner));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按固定顺序创建地图并执行一次 Hybrid A* 粗路径规划。
|
||||
/// 参数：job 的地图输入使用 mm，位姿和车辆尺寸使用 m，航向使用 rad，曲率使用 1/m；cancellationToken 会传递给搜索阶段。
|
||||
/// 返回：始终同时保留地图创建结果和规划结果；地图创建失败时不会启动搜索，并返回 <see cref="PlanningStatus.InvalidMap"/> 的空路径结果。
|
||||
/// </summary>
|
||||
public CoarsePathPlanningJobResult Plan(CoarsePathPlanningJob job,
|
||||
CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
PlanningOperationBudget budget = CreateBudget(job, cancellationToken);
|
||||
PlanningMapBuildResult mapResult = _mapFactory.Create(job == null ? null : job.MapRequest, budget);
|
||||
if (!mapResult.Succeeded || mapResult.Map == null)
|
||||
{
|
||||
PlanningResult mapFailure = PlanningResult.Failure(MapFailureStatus(mapResult),
|
||||
new PlanningDiagnostics(elapsed: budget.Elapsed, terminationReason: BuildMapFailureReason(mapResult)));
|
||||
return PublishDebug(job, mapResult, mapFailure);
|
||||
}
|
||||
|
||||
var request = new PlanningRequest
|
||||
{
|
||||
Map = mapResult.Map,
|
||||
Start = job.Start,
|
||||
Goal = job.Goal,
|
||||
Vehicle = job.Vehicle,
|
||||
Configuration = job.Configuration,
|
||||
StartVehicleCurvature = job.StartVehicleCurvature,
|
||||
StartDirection = job.StartDirection,
|
||||
GoalDirection = job.GoalDirection,
|
||||
};
|
||||
PlanningResult planningResult = _planner.Plan(request, budget);
|
||||
return PublishDebug(job, mapResult, planningResult);
|
||||
}
|
||||
|
||||
private static PlanningOperationBudget CreateBudget(CoarsePathPlanningJob job, CancellationToken cancellationToken)
|
||||
{
|
||||
HybridAStarConfiguration configuration = job == null ? null : job.Configuration;
|
||||
return configuration != null && configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? new PlanningOperationBudget(cancellationToken, configuration.SearchTimeout)
|
||||
: PlanningOperationBudget.Unlimited(cancellationToken);
|
||||
}
|
||||
|
||||
private static PlanningStatus MapFailureStatus(PlanningMapBuildResult mapResult)
|
||||
{
|
||||
if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.Cancelled) return PlanningStatus.Cancelled;
|
||||
if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.TimedOut) return PlanningStatus.SearchTimeout;
|
||||
return PlanningStatus.InvalidMap;
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJobResult PublishDebug(CoarsePathPlanningJob job, PlanningMapBuildResult mapResult,
|
||||
PlanningResult planningResult)
|
||||
{
|
||||
var diagnostics = new List<string>();
|
||||
PlanningDebugOptions options = job == null ? null : job.DebugOptions;
|
||||
if (options != null && options.Enabled)
|
||||
{
|
||||
IPlanningDebugSink sink = options.Sink ?? NullPlanningDebugSink.Instance;
|
||||
try
|
||||
{
|
||||
sink.Publish(mapResult, planningResult);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
diagnostics.Add("调试旁路发布失败：" + exception.GetType().Name + "。" + exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
return new CoarsePathPlanningJobResult(mapResult, planningResult, diagnostics);
|
||||
}
|
||||
|
||||
private static string BuildMapFailureReason(PlanningMapBuildResult mapResult)
|
||||
{
|
||||
if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.Cancelled)
|
||||
return "规划地图创建已取消，未启动 Hybrid A* 搜索。";
|
||||
if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.TimedOut)
|
||||
return "规划地图创建已超时，未启动 Hybrid A* 搜索。";
|
||||
string reason = mapResult == null ? string.Empty : mapResult.FailureReason;
|
||||
return string.IsNullOrEmpty(reason) ? "规划地图创建失败，未启动 Hybrid A* 搜索。" :
|
||||
"规划地图创建失败，未启动 Hybrid A* 搜索：" + reason;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 接收一次地图构建和粗路径规划完成后的旁路调试数据。
|
||||
/// 实现不得修改输入对象；服务会隔离实现抛出的异常，调试失败不会改变规划结果。
|
||||
/// </summary>
|
||||
public interface IPlanningDebugSink
|
||||
{
|
||||
/// <summary>
|
||||
/// 发布本次编排得到的地图和规划结果。
|
||||
/// 参数：mapResult 为地图创建结果；planningResult 为对应的完整或失败规划结果；二者均不可由接收方修改。
|
||||
/// </summary>
|
||||
void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult);
|
||||
}
|
||||
|
||||
internal sealed class NullPlanningDebugSink : IPlanningDebugSink
|
||||
{
|
||||
internal static readonly NullPlanningDebugSink Instance = new NullPlanningDebugSink();
|
||||
|
||||
private NullPlanningDebugSink()
|
||||
{
|
||||
}
|
||||
|
||||
public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
/// <summary>
|
||||
/// 一次粗路径编排的可选调试旁路配置。
|
||||
/// 调试开关和接收器不参与地图输入指纹、缓存键、搜索状态或路径结果。
|
||||
/// </summary>
|
||||
public sealed class PlanningDebugOptions
|
||||
{
|
||||
/// <summary>创建默认关闭并使用空接收器的调试配置。</summary>
|
||||
public PlanningDebugOptions()
|
||||
{
|
||||
Enabled = false;
|
||||
Sink = NullPlanningDebugSink.Instance;
|
||||
}
|
||||
|
||||
/// <summary>是否在本次编排结束后向 <see cref="Sink"/> 发布旁路数据；默认值为 false。</summary>
|
||||
public bool Enabled { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 接收地图和规划结果的旁路对象；默认为空实现。赋值为 null 时服务仍会使用空实现，且不会影响主流程。
|
||||
/// </summary>
|
||||
public IPlanningDebugSink Sink { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,264 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
/// <summary>
|
||||
/// 已建 <see cref="PlanningGridMap"/> 上 Hybrid A* 粗路径规划的下层门面。
|
||||
/// 本类不建图、不读取 UI 或传感器；只有路径经回溯、装配和最终连续复核后才发布成功结果。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarPlanner
|
||||
{
|
||||
private readonly HybridAStarSearch _search;
|
||||
private readonly PathBacktracker _backtracker;
|
||||
private readonly CoarsePathAssembler _assembler;
|
||||
private readonly CoarsePathValidator _validator;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认搜索、回溯、装配和最终复核组件的规划器。</summary>
|
||||
public HybridAStarPlanner()
|
||||
: this(new HybridAStarSearch(), new PathBacktracker(), new CoarsePathAssembler(), new CoarsePathValidator(),
|
||||
new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
internal HybridAStarPlanner(HybridAStarSearch search, PathBacktracker backtracker, CoarsePathAssembler assembler,
|
||||
CoarsePathValidator validator, FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_search = search ?? throw new ArgumentNullException(nameof(search));
|
||||
_backtracker = backtracker ?? throw new ArgumentNullException(nameof(backtracker));
|
||||
_assembler = assembler ?? throw new ArgumentNullException(nameof(assembler));
|
||||
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在请求提供的不可变规划地图上执行一次 Hybrid A* 粗路径规划。
|
||||
/// 参数：request 的位置单位为 m、航向单位为 rad、曲率单位为 1/m；cancellationToken 会在搜索扩展检查点取消。
|
||||
/// 返回：输入、边界、碰撞、搜索、回溯或最终复核失败均返回空路径；只有 <see cref="PlanningStatus.Success"/> 携带完整路径和分段。
|
||||
/// </summary>
|
||||
public PlanningResult Plan(PlanningRequest request, CancellationToken cancellationToken = default(CancellationToken))
|
||||
{
|
||||
PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? new PlanningOperationBudget(cancellationToken, request.Configuration.SearchTimeout)
|
||||
: PlanningOperationBudget.Unlimited(cancellationToken);
|
||||
return Plan(request, budget);
|
||||
}
|
||||
|
||||
/// <summary>使用门面传入的共享预算执行预检、搜索和最终路径复核。</summary>
|
||||
internal PlanningResult Plan(PlanningRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
Stopwatch pathSearchStopwatch = null;
|
||||
try
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return Failure(ToPlanningStatus(stopReason), budget, "规划在开始前已停止。", null);
|
||||
|
||||
PlanningStatus preflightStatus = ValidatePreflight(request, out string preflightReason);
|
||||
if (preflightStatus != PlanningStatus.Success)
|
||||
return Failure(preflightStatus, budget, preflightReason, null);
|
||||
|
||||
if (!IsFootprintInsideMap(request.Start, request.Map, request.Vehicle))
|
||||
return Failure(PlanningStatus.StartOutsideMap, budget, "起始扩大车体不完全位于地图边界内。", null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Start, request.Map, request.Vehicle, 0d, out _))
|
||||
return Failure(PlanningStatus.StartInCollision, budget, "起始扩大车体与障碍物相交或擦边。", null);
|
||||
if (!IsFootprintInsideMap(request.Goal, request.Map, request.Vehicle))
|
||||
return Failure(PlanningStatus.GoalOutsideMap, budget, "目标扩大车体不完全位于地图边界内。", null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Goal, request.Map, request.Vehicle, 0d, out _))
|
||||
return Failure(PlanningStatus.GoalInCollision, budget, "目标扩大车体与障碍物相交或擦边。", null);
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return Failure(ToPlanningStatus(stopReason), budget, "规划在搜索前已停止。", null);
|
||||
pathSearchStopwatch = Stopwatch.StartNew();
|
||||
HybridAStarSearchResult searchResult = _search.Search(request, budget);
|
||||
if (searchResult == null)
|
||||
return Failure(PlanningStatus.InternalError, budget, "搜索器未返回结果。", null, pathSearchStopwatch);
|
||||
if (searchResult.Status != PlanningStatus.Success)
|
||||
return Failure(searchResult.Status, budget,
|
||||
BuildSearchFailureReason(searchResult, request.Configuration), searchResult, pathSearchStopwatch);
|
||||
|
||||
if (!_backtracker.TryBacktrack(searchResult, request, out BacktrackedPath backtrackedPath, out string backtrackingReason))
|
||||
return Failure(PlanningStatus.BacktrackingFailed, budget, backtrackingReason, searchResult, pathSearchStopwatch);
|
||||
if (!_assembler.TryAssemble(backtrackedPath, request, out var path, out var segments, out string assemblyReason))
|
||||
return Failure(PlanningStatus.FinalValidationFailed, budget, assemblyReason, searchResult, pathSearchStopwatch);
|
||||
if (!_validator.TryValidate(path, segments, request, out double minimumClearanceMeters, out string validationReason))
|
||||
return Failure(PlanningStatus.FinalValidationFailed, budget, validationReason, searchResult, pathSearchStopwatch);
|
||||
|
||||
double pathLengthMeters = path[path.Count - 1].ArcLength;
|
||||
return PlanningResult.Success(path, segments, CreateDiagnostics(searchResult, budget.Elapsed, pathLengthMeters,
|
||||
minimumClearanceMeters, string.Empty, pathSearchStopwatch.Elapsed));
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
string reason = "规划内部错误：" + exception.GetType().Name +
|
||||
(string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
|
||||
return Failure(PlanningStatus.InternalError, budget ?? PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
reason, null, pathSearchStopwatch);
|
||||
}
|
||||
}
|
||||
|
||||
private static PlanningStatus ValidatePreflight(PlanningRequest request, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
|
||||
!IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
|
||||
!IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
|
||||
{
|
||||
failureReason = "规划请求缺少必要对象或包含非法数值。";
|
||||
return PlanningStatus.InvalidRequest;
|
||||
}
|
||||
|
||||
PlanningGridMap map = request.Map;
|
||||
if (map.Bounds == null || map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters))
|
||||
{
|
||||
failureReason = "规划地图结构无效。";
|
||||
return PlanningStatus.InvalidMap;
|
||||
}
|
||||
if (!map.PlanningReady)
|
||||
{
|
||||
failureReason = string.IsNullOrEmpty(map.PlanningBlockReason) ? "规划地图尚未就绪。" : map.PlanningBlockReason;
|
||||
return PlanningStatus.MapNotReady;
|
||||
}
|
||||
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
|
||||
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
{
|
||||
failureReason = "车辆尺寸、安全余量或曲率限制无效。";
|
||||
return PlanningStatus.InvalidVehicleParameters;
|
||||
}
|
||||
|
||||
HybridAStarConfiguration configuration = request.Configuration;
|
||||
if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
|
||||
(request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
|
||||
{
|
||||
failureReason = "Hybrid A* 曲率、离散、代价或资源配置无效。";
|
||||
return PlanningStatus.InvalidCurvatureConfiguration;
|
||||
}
|
||||
|
||||
return PlanningStatus.Success;
|
||||
}
|
||||
|
||||
private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
|
||||
{
|
||||
return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
|
||||
configuration.HeadingResolutionRadians <= 2d * Math.PI && configuration.CurvatureLevelCount >= 3 &&
|
||||
configuration.CurvatureLevelCount % 2 == 1 && NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) &&
|
||||
configuration.GoalPositionToleranceMeters >= 0d && NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) &&
|
||||
configuration.GoalHeadingToleranceRadians >= 0d && configuration.MaximumExpandedNodes >= 0 &&
|
||||
configuration.SearchTimeout >= TimeSpan.Zero && NumericGuard.IsFinite(configuration.HeuristicWeight) &&
|
||||
configuration.HeuristicWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
|
||||
NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) &&
|
||||
configuration.CurvatureChangePenaltyMetersPerLevel >= 0d && NumericGuard.IsFinite(configuration.ClearanceCostWeight) &&
|
||||
configuration.ClearanceCostWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
|
||||
}
|
||||
|
||||
private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
|
||||
{
|
||||
if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
|
||||
|
||||
double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double longitudinalX = Math.Cos(pose.Heading);
|
||||
double longitudinalY = Math.Sin(pose.Heading);
|
||||
double lateralX = -longitudinalY;
|
||||
double lateralY = longitudinalX;
|
||||
for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
|
||||
for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
|
||||
{
|
||||
double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
|
||||
double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
|
||||
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static string BuildSearchFailureReason(HybridAStarSearchResult searchResult,
|
||||
HybridAStarConfiguration configuration)
|
||||
{
|
||||
string reason = string.IsNullOrEmpty(searchResult.TerminationReason)
|
||||
? "Hybrid A* 搜索以 " + searchResult.Status + " 状态终止。"
|
||||
: searchResult.TerminationReason;
|
||||
string resourceLimit = string.Empty;
|
||||
if (searchResult.Status == PlanningStatus.SearchTimeout)
|
||||
{
|
||||
resourceLimit = "总预算=" + configuration.SearchTimeout.TotalSeconds.ToString(
|
||||
"F3", CultureInfo.InvariantCulture) + "s；";
|
||||
}
|
||||
else if (searchResult.Status == PlanningStatus.SearchNodeLimitExceeded)
|
||||
{
|
||||
resourceLimit = "节点上限=" + configuration.MaximumExpandedNodes.ToString(
|
||||
CultureInfo.InvariantCulture) + "；";
|
||||
}
|
||||
|
||||
return reason + resourceLimit +
|
||||
"扩展=" + searchResult.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||||
"生成=" + searchResult.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||||
"重开=" + searchResult.ReopenedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||||
"陈旧条目=" + searchResult.StaleOpenListEntryCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||||
"Open List峰值=" + searchResult.PeakOpenListCount.ToString(CultureInfo.InvariantCulture) + "。";
|
||||
}
|
||||
|
||||
private static PlanningResult Failure(PlanningStatus status, PlanningOperationBudget budget, string reason,
|
||||
HybridAStarSearchResult searchResult, Stopwatch pathSearchStopwatch = null)
|
||||
{
|
||||
TimeSpan pathSearchElapsed = pathSearchStopwatch == null ? TimeSpan.Zero : pathSearchStopwatch.Elapsed;
|
||||
return PlanningResult.Failure(status, CreateDiagnostics(searchResult, budget.Elapsed, 0d, 0d,
|
||||
reason, pathSearchElapsed));
|
||||
}
|
||||
|
||||
private static PlanningDiagnostics CreateDiagnostics(HybridAStarSearchResult searchResult, TimeSpan elapsed,
|
||||
double pathLengthMeters, double minimumClearanceMeters, string reason, TimeSpan pathSearchElapsed)
|
||||
{
|
||||
return new PlanningDiagnostics(
|
||||
searchResult == null ? 0 : searchResult.ExpandedNodeCount,
|
||||
searchResult == null ? 0 : searchResult.GeneratedNodeCount,
|
||||
searchResult == null ? 0 : searchResult.ReopenedNodeCount,
|
||||
searchResult == null ? 0 : searchResult.StaleOpenListEntryCount,
|
||||
searchResult == null ? 0 : searchResult.PeakOpenListCount,
|
||||
pathLengthMeters,
|
||||
minimumClearanceMeters,
|
||||
elapsed,
|
||||
reason,
|
||||
pathSearchElapsed);
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
|
||||
private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
|
||||
if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
|
||||
throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,216 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 将回溯原语装配为调用方可消费的稠密路径和包含式方向分段。
|
||||
/// 装配器保留原语边界的换向双点，其余相邻重复位姿会被删除。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathAssembler
|
||||
{
|
||||
private const double DuplicateTolerance = 1e-8d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认连续车体检查器的路径装配器。</summary>
|
||||
public CoarsePathAssembler()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车体检查器的路径装配器。</summary>
|
||||
public CoarsePathAssembler(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从已回溯的恒曲率原语构造稠密路径。
|
||||
/// 参数：backtrackedPath 提供原语顺序；request 提供地图和车辆；path、segments 为成功时的只读输出。
|
||||
/// 返回：首点可通过连续车体检查、每个原语积分点数据一致且方向分段完整覆盖时为 true；否则返回 false 且输出为空。
|
||||
/// </summary>
|
||||
public bool TryAssemble(BacktrackedPath backtrackedPath, PlanningRequest request,
|
||||
out IReadOnlyList<CoarsePathPoint> path, out IReadOnlyList<PathSegment> segments, out string failureReason)
|
||||
{
|
||||
path = EmptyPath();
|
||||
segments = EmptySegments();
|
||||
failureReason = string.Empty;
|
||||
if (backtrackedPath == null || request == null || request.Map == null || request.Vehicle == null ||
|
||||
!IsFinitePose(backtrackedPath.Start) || !IsTravelDirection(backtrackedPath.StartDirection) ||
|
||||
!NumericGuard.IsFinite(backtrackedPath.StartCurvaturePerMeter))
|
||||
{
|
||||
failureReason = "路径装配输入无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_collisionChecker.IsPoseCollisionFree(backtrackedPath.Start, request.Map, request.Vehicle, 0d, out double startClearanceMeters))
|
||||
{
|
||||
failureReason = "回溯路径起点未通过连续车体检查。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var points = new List<CoarsePathPoint>();
|
||||
TravelDirection currentDirection = backtrackedPath.Primitives.Count > 0
|
||||
? backtrackedPath.Primitives[0].Direction
|
||||
: backtrackedPath.StartDirection;
|
||||
double currentCurvature = request.StartVehicleCurvature;
|
||||
double normalizedStartHeading = AngleMath.NormalizeRadians(backtrackedPath.Start.Heading);
|
||||
if (!NumericGuard.IsFinite(normalizedStartHeading))
|
||||
{
|
||||
failureReason = "回溯路径起点航向无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
points.Add(new CoarsePathPoint(backtrackedPath.Start.X, backtrackedPath.Start.Y, normalizedStartHeading,
|
||||
backtrackedPath.Start.Heading, 0d, currentDirection, currentCurvature, startClearanceMeters, false,
|
||||
CoarsePathPointSource.Start));
|
||||
|
||||
for (int primitiveIndex = 0; primitiveIndex < backtrackedPath.Primitives.Count; primitiveIndex++)
|
||||
{
|
||||
MotionPrimitive primitive = backtrackedPath.Primitives[primitiveIndex];
|
||||
if (!IsValidPrimitive(primitive))
|
||||
{
|
||||
failureReason = "回溯路径包含无效原语。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint lastPoint = points[points.Count - 1];
|
||||
if (primitive.Direction != lastPoint.Direction)
|
||||
{
|
||||
// 换向处的旧方向终点和新方向起点必须共存，二者位置、航向、弧长完全相同。
|
||||
points.Add(new CoarsePathPoint(lastPoint.X, lastPoint.Y, lastPoint.Heading, lastPoint.UnwrappedHeading,
|
||||
lastPoint.ArcLength, primitive.Direction, primitive.CurvaturePerMeter, lastPoint.BodyClearance,
|
||||
true, CoarsePathPointSource.MotionPrimitive));
|
||||
}
|
||||
|
||||
Pose2D previousPose = primitive.Start;
|
||||
for (int pointIndex = 0; pointIndex < primitive.Points.Count; pointIndex++)
|
||||
{
|
||||
Pose2D pose = primitive.Points[pointIndex];
|
||||
double bodyClearanceMeters = primitive.BodyClearancesMeters[pointIndex];
|
||||
if (!IsFinitePose(pose) || !IsValidClearance(bodyClearanceMeters))
|
||||
{
|
||||
failureReason = "原语积分点或净空无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint previousPoint = points[points.Count - 1];
|
||||
double arcIncrementMeters = CalculateArcIncrement(previousPose, pose, primitive.CurvaturePerMeter);
|
||||
if (!NumericGuard.IsFinite(arcIncrementMeters) || arcIncrementMeters <= 0d)
|
||||
{
|
||||
failureReason = "原语积分点未产生正弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (IsSamePose(previousPoint, pose))
|
||||
{
|
||||
// 非换向情况下不允许重复采样点泄露到对外路径。
|
||||
previousPose = pose;
|
||||
continue;
|
||||
}
|
||||
|
||||
double normalizedHeading = AngleMath.NormalizeRadians(pose.Heading);
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(previousPoint.Heading, normalizedHeading);
|
||||
if (!NumericGuard.IsFinite(normalizedHeading) || !NumericGuard.IsFinite(headingDelta))
|
||||
{
|
||||
failureReason = "原语积分点航向无法展开。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPointSource source = primitive.IsGoalTruncation && pointIndex == primitive.Points.Count - 1
|
||||
? CoarsePathPointSource.GoalTruncation
|
||||
: CoarsePathPointSource.MotionPrimitive;
|
||||
points.Add(new CoarsePathPoint(pose.X, pose.Y, normalizedHeading,
|
||||
previousPoint.UnwrappedHeading + headingDelta, previousPoint.ArcLength + arcIncrementMeters,
|
||||
primitive.Direction, primitive.CurvaturePerMeter, bodyClearanceMeters, false, source));
|
||||
previousPose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
if (points.Count == 0)
|
||||
{
|
||||
failureReason = "路径装配未产生起点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
path = new ReadOnlyCollection<CoarsePathPoint>(points);
|
||||
segments = BuildSegments(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> BuildSegments(IReadOnlyList<CoarsePathPoint> points)
|
||||
{
|
||||
var segments = new List<PathSegment>();
|
||||
int startIndex = 0;
|
||||
TravelDirection direction = points[0].Direction;
|
||||
for (int index = 1; index < points.Count; index++)
|
||||
{
|
||||
if (points[index].Direction == direction) continue;
|
||||
segments.Add(new PathSegment(segments.Count, direction, startIndex, index - 1,
|
||||
points[startIndex].IsGearSwitchPoint, true));
|
||||
startIndex = index;
|
||||
direction = points[index].Direction;
|
||||
}
|
||||
|
||||
segments.Add(new PathSegment(segments.Count, direction, startIndex, points.Count - 1,
|
||||
points[startIndex].IsGearSwitchPoint, false));
|
||||
return new ReadOnlyCollection<PathSegment>(segments);
|
||||
}
|
||||
|
||||
private static double CalculateArcIncrement(Pose2D from, Pose2D to, double curvaturePerMeter)
|
||||
{
|
||||
if (!IsFinitePose(from) || !IsFinitePose(to) || !NumericGuard.IsFinite(curvaturePerMeter)) return double.NaN;
|
||||
if (Math.Abs(curvaturePerMeter) < 1e-12d)
|
||||
{
|
||||
double deltaX = to.X - from.X;
|
||||
double deltaY = to.Y - from.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
|
||||
return Math.Abs(headingDelta / curvaturePerMeter);
|
||||
}
|
||||
|
||||
private static bool IsValidPrimitive(MotionPrimitive primitive)
|
||||
{
|
||||
return primitive != null && primitive.Start != null && primitive.Points != null && primitive.BodyClearancesMeters != null &&
|
||||
primitive.Points.Count == primitive.BodyClearancesMeters.Count && primitive.Points.Count > 0 &&
|
||||
IsTravelDirection(primitive.Direction) && NumericGuard.IsFinite(primitive.CurvaturePerMeter) &&
|
||||
NumericGuard.IsPositiveFinite(primitive.ActualLengthMeters);
|
||||
}
|
||||
|
||||
private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
|
||||
{
|
||||
return Math.Abs(point.X - pose.X) <= DuplicateTolerance && Math.Abs(point.Y - pose.Y) <= DuplicateTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= DuplicateTolerance;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsValidClearance(double clearanceMeters)
|
||||
{
|
||||
return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> EmptyPath()
|
||||
{
|
||||
return new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> EmptySegments()
|
||||
{
|
||||
return new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,245 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 对准备发布的粗路径执行独立的连续安全与输出契约复核。
|
||||
/// 复核失败的路径不得被包装为 <see cref="PlanningStatus.Success"/>。
|
||||
/// </summary>
|
||||
public sealed class CoarsePathValidator
|
||||
{
|
||||
private const double NumericTolerance = 1e-6d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认连续车体检查器的最终路径复核器。</summary>
|
||||
public CoarsePathValidator()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车体检查器的最终路径复核器。</summary>
|
||||
public CoarsePathValidator(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复核路径数值、曲率、连续扫掠碰撞、终点约束、累计弧长和方向分段。
|
||||
/// 参数：path 与 segments 为待发布输出；request 必须是生成该路径的请求；minimumBodyClearanceMeters 返回沿途的保守净空下界。
|
||||
/// 返回：所有规则通过时为 true；否则返回 false、写入失败原因，调用方必须丢弃 path 和 segments。
|
||||
/// </summary>
|
||||
public bool TryValidate(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments,
|
||||
PlanningRequest request, out double minimumBodyClearanceMeters, out string failureReason)
|
||||
{
|
||||
minimumBodyClearanceMeters = 0d;
|
||||
failureReason = string.Empty;
|
||||
if (path == null || segments == null || request == null || request.Map == null || request.Vehicle == null ||
|
||||
request.Configuration == null || path.Count == 0 || segments.Count == 0)
|
||||
{
|
||||
failureReason = "最终路径或复核请求为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvaturePerMeter))
|
||||
{
|
||||
failureReason = "车辆曲率约束无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
CoarsePathPoint first = path[0];
|
||||
if (!IsValidPoint(first) || first.Source != CoarsePathPointSource.Start || first.IsGearSwitchPoint ||
|
||||
Math.Abs(first.ArcLength) > NumericTolerance || !IsSamePose(first, request.Start))
|
||||
{
|
||||
failureReason = "最终路径首点不符合起点契约。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double minimumClearance = double.PositiveInfinity;
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint current = path[index];
|
||||
if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvaturePerMeter + NumericTolerance)
|
||||
{
|
||||
failureReason = "最终路径包含非法数值或超限曲率。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var currentPose = new Pose2D(current.X, current.Y, current.Heading);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(currentPose, request.Map, request.Vehicle, 0d, out double poseClearanceMeters) ||
|
||||
IsClearanceOverclaimed(current.BodyClearance, poseClearanceMeters))
|
||||
{
|
||||
failureReason = "最终路径点未通过连续车体碰撞复核。";
|
||||
return false;
|
||||
}
|
||||
|
||||
minimumClearance = Math.Min(minimumClearance, current.BodyClearance);
|
||||
if (index == 0) continue;
|
||||
|
||||
CoarsePathPoint previous = path[index - 1];
|
||||
if (current.ArcLength + NumericTolerance < previous.ArcLength ||
|
||||
!IsUnwrappedHeadingContinuous(previous, current))
|
||||
{
|
||||
failureReason = "最终路径的弧长或展开航向不连续。";
|
||||
return false;
|
||||
}
|
||||
|
||||
bool isDuplicate = IsDuplicatePoseAndArcLength(previous, current);
|
||||
if (isDuplicate)
|
||||
{
|
||||
if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
|
||||
{
|
||||
failureReason = "相邻重复点不是合法换向对。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + NumericTolerance ||
|
||||
!IsArcIncrementConsistent(previous, current))
|
||||
{
|
||||
failureReason = "非换向路径点的弧长增量不一致。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
|
||||
if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, request.Map, request.Vehicle,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters, out double sweptClearanceMeters))
|
||||
{
|
||||
failureReason = "最终路径相邻点之间的车体扫掠碰撞复核失败。";
|
||||
return false;
|
||||
}
|
||||
|
||||
minimumClearance = Math.Min(minimumClearance, sweptClearanceMeters);
|
||||
}
|
||||
|
||||
CoarsePathPoint last = path[path.Count - 1];
|
||||
if (!GoalToleranceChecker.IsSatisfied(new Pose2D(last.X, last.Y, last.Heading), request.Goal, request.Configuration,
|
||||
last.Direction, request.GoalDirection) || (path.Count > 1 && last.Source != CoarsePathPointSource.GoalTruncation))
|
||||
{
|
||||
failureReason = "最终路径末点未满足终点容差、方向或来源契约。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!AreSegmentsValid(path, segments, out failureReason)) return false;
|
||||
minimumBodyClearanceMeters = minimumClearance;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool AreSegmentsValid(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
int expectedStartIndex = 0;
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment segment = segments[segmentIndex];
|
||||
if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
|
||||
segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
|
||||
segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
|
||||
{
|
||||
failureReason = "方向分段索引或起始换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
|
||||
{
|
||||
if (path[pointIndex].Direction != segment.Direction)
|
||||
{
|
||||
failureReason = "方向分段包含不同方向的路径点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool hasNextSegment = segmentIndex + 1 < segments.Count;
|
||||
bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
|
||||
path[segment.EndIndex + 1].IsGearSwitchPoint;
|
||||
if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
|
||||
{
|
||||
failureReason = "方向分段末尾换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
expectedStartIndex = segment.EndIndex + 1;
|
||||
}
|
||||
|
||||
if (expectedStartIndex != path.Count)
|
||||
{
|
||||
failureReason = "方向分段未完整覆盖路径点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(CoarsePathPoint point)
|
||||
{
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
|
||||
!NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!NumericGuard.IsFinite(point.VehicleCurvature) || !IsTravelDirection(point.Direction) ||
|
||||
!IsValidClearance(point.BodyClearance))
|
||||
return false;
|
||||
|
||||
return Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
|
||||
{
|
||||
return point != null && pose != null && Math.Abs(point.X - pose.X) <= NumericTolerance &&
|
||||
Math.Abs(point.Y - pose.Y) <= NumericTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsUnwrappedHeadingContinuous(CoarsePathPoint previous, CoarsePathPoint current)
|
||||
{
|
||||
double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
||||
return NumericGuard.IsFinite(expectedDelta) &&
|
||||
Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsDuplicatePoseAndArcLength(CoarsePathPoint previous, CoarsePathPoint current)
|
||||
{
|
||||
return Math.Abs(previous.X - current.X) <= NumericTolerance && Math.Abs(previous.Y - current.Y) <= NumericTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= NumericTolerance &&
|
||||
Math.Abs(previous.ArcLength - current.ArcLength) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsArcIncrementConsistent(CoarsePathPoint previous, CoarsePathPoint current)
|
||||
{
|
||||
double expectedIncrement;
|
||||
if (Math.Abs(current.VehicleCurvature) < 1e-12d)
|
||||
{
|
||||
double deltaX = current.X - previous.X;
|
||||
double deltaY = current.Y - previous.Y;
|
||||
expectedIncrement = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
else
|
||||
{
|
||||
double headingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
||||
expectedIncrement = Math.Abs(headingDelta / current.VehicleCurvature);
|
||||
}
|
||||
|
||||
return NumericGuard.IsFinite(expectedIncrement) && expectedIncrement > 0d &&
|
||||
Math.Abs((current.ArcLength - previous.ArcLength) - expectedIncrement) <= NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsClearanceOverclaimed(double reportedClearanceMeters, double checkedClearanceMeters)
|
||||
{
|
||||
if (double.IsPositiveInfinity(reportedClearanceMeters)) return !double.IsPositiveInfinity(checkedClearanceMeters);
|
||||
return reportedClearanceMeters > checkedClearanceMeters + NumericTolerance;
|
||||
}
|
||||
|
||||
private static bool IsValidClearance(double clearanceMeters)
|
||||
{
|
||||
return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,167 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
|
||||
|
||||
/// <summary>
|
||||
/// 成功搜索父链重新生成后的原语序列。
|
||||
/// 起点位置使用 m/rad，起始曲率使用 1/m；原语集合按从起点到终点的顺序排列。
|
||||
/// </summary>
|
||||
public sealed class BacktrackedPath
|
||||
{
|
||||
internal BacktrackedPath(Pose2D start, TravelDirection startDirection, double startCurvaturePerMeter,
|
||||
IReadOnlyList<MotionPrimitive> primitives)
|
||||
{
|
||||
Start = start;
|
||||
StartDirection = startDirection;
|
||||
StartCurvaturePerMeter = startCurvaturePerMeter;
|
||||
Primitives = primitives;
|
||||
}
|
||||
|
||||
/// <summary>原始请求中的起始车辆中心位姿；位置单位 m，航向单位 rad。</summary>
|
||||
public Pose2D Start { get; }
|
||||
|
||||
/// <summary>成功父链根节点记录的起步方向。</summary>
|
||||
public TravelDirection StartDirection { get; }
|
||||
|
||||
/// <summary>成功父链根节点离散后的起始曲率，单位 1/m。</summary>
|
||||
public double StartCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>按起点到终点顺序重新积分的恒曲率原语；每项都不包含自身起点。</summary>
|
||||
public IReadOnlyList<MotionPrimitive> Primitives { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 仅依据成功节点的父索引和原语描述，确定性地重建稠密原语序列。
|
||||
/// 不复用搜索期保存的积分点，以保证输出与当前解析积分、扫掠检查规则一致。
|
||||
/// </summary>
|
||||
public sealed class PathBacktracker
|
||||
{
|
||||
private const double PoseComparisonTolerance = 1e-7d;
|
||||
private const double LengthComparisonTolerance = 1e-7d;
|
||||
private readonly MotionPrimitiveGenerator _primitiveGenerator;
|
||||
|
||||
/// <summary>创建使用默认解析积分器的回溯器。</summary>
|
||||
public PathBacktracker()
|
||||
: this(new MotionPrimitiveGenerator())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定原语生成器的回溯器。</summary>
|
||||
public PathBacktracker(MotionPrimitiveGenerator primitiveGenerator)
|
||||
{
|
||||
_primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 依据搜索成功节点重建从起点到终点的原语序列。
|
||||
/// 参数：searchResult 必须是携带成功节点索引的搜索结果；request 必须仍指向执行搜索的同一不可变地图和配置。
|
||||
/// 返回：父链无环、索引连续且每条原语按同一解析规则可重建时为 true；失败时 path 为 null 并返回可读原因。
|
||||
/// </summary>
|
||||
public bool TryBacktrack(HybridAStarSearchResult searchResult, PlanningRequest request,
|
||||
out BacktrackedPath path, out string failureReason)
|
||||
{
|
||||
path = null;
|
||||
failureReason = string.Empty;
|
||||
if (searchResult == null || request == null || searchResult.Status != PlanningStatus.Success ||
|
||||
!searchResult.SuccessNodeIndex.HasValue || searchResult.Nodes == null)
|
||||
{
|
||||
failureReason = "搜索结果不含可回溯的成功节点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
int currentIndex = searchResult.SuccessNodeIndex.Value;
|
||||
var reverseNodes = new List<HybridAStarNode>();
|
||||
var visited = new HashSet<int>();
|
||||
while (currentIndex >= 0)
|
||||
{
|
||||
if (currentIndex >= searchResult.Nodes.Count || !visited.Add(currentIndex))
|
||||
{
|
||||
failureReason = "搜索父链索引越界或存在环。";
|
||||
return false;
|
||||
}
|
||||
|
||||
HybridAStarNode current = searchResult.Nodes[currentIndex];
|
||||
if (current == null || current.NodeIndex != currentIndex || current.Pose == null)
|
||||
{
|
||||
failureReason = "搜索节点索引或连续位姿不一致。";
|
||||
return false;
|
||||
}
|
||||
|
||||
reverseNodes.Add(current);
|
||||
currentIndex = current.ParentNodeIndex;
|
||||
}
|
||||
|
||||
if (reverseNodes.Count == 0)
|
||||
{
|
||||
failureReason = "搜索父链为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
reverseNodes.Reverse();
|
||||
HybridAStarNode root = reverseNodes[0];
|
||||
if (root.ParentNodeIndex != -1 || root.IncomingPrimitive != null || !IsFinitePose(root.Pose))
|
||||
{
|
||||
failureReason = "搜索父链根节点无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var primitives = new List<MotionPrimitive>(Math.Max(0, reverseNodes.Count - 1));
|
||||
Pose2D previousPose = root.Pose;
|
||||
for (int index = 1; index < reverseNodes.Count; index++)
|
||||
{
|
||||
HybridAStarNode node = reverseNodes[index];
|
||||
MotionPrimitive descriptor = node.IncomingPrimitive;
|
||||
if (node.ParentNodeIndex != reverseNodes[index - 1].NodeIndex || descriptor == null ||
|
||||
!IsFinitePose(node.Pose) || !NumericGuard.IsFinite(descriptor.CurvaturePerMeter) ||
|
||||
!IsTravelDirection(descriptor.Direction) || descriptor.ActualLengthMeters <= 0d)
|
||||
{
|
||||
failureReason = "搜索父链中的原语描述无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
// 只把恒曲率、方向和长度描述作为真源，再走一遍相同的解析积分与连续碰撞检查。
|
||||
MotionPrimitive rebuilt = _primitiveGenerator.Generate(previousPose, descriptor.CurvaturePerMeter, descriptor.Direction, request);
|
||||
if (rebuilt == null || !IsEquivalent(descriptor, rebuilt) || !IsSamePose(rebuilt.End, node.Pose))
|
||||
{
|
||||
failureReason = "搜索原语无法按当前解析规则确定性重建。";
|
||||
return false;
|
||||
}
|
||||
|
||||
primitives.Add(rebuilt);
|
||||
previousPose = rebuilt.End;
|
||||
}
|
||||
|
||||
path = new BacktrackedPath(root.Pose, root.Direction, root.CurvaturePerMeter,
|
||||
new ReadOnlyCollection<MotionPrimitive>(primitives));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsEquivalent(MotionPrimitive expected, MotionPrimitive actual)
|
||||
{
|
||||
return expected.Direction == actual.Direction && expected.IsGoalTruncation == actual.IsGoalTruncation &&
|
||||
Math.Abs(expected.CurvaturePerMeter - actual.CurvaturePerMeter) <= PoseComparisonTolerance &&
|
||||
Math.Abs(expected.ActualLengthMeters - actual.ActualLengthMeters) <= LengthComparisonTolerance;
|
||||
}
|
||||
|
||||
private static bool IsSamePose(Pose2D first, Pose2D second)
|
||||
{
|
||||
return IsFinitePose(first) && IsFinitePose(second) &&
|
||||
Math.Abs(first.X - second.X) <= PoseComparisonTolerance &&
|
||||
Math.Abs(first.Y - second.Y) <= PoseComparisonTolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(first.Heading, second.Heading)) <= PoseComparisonTolerance;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,387 @@
|
||||
# CoarsePath 粗路径规划（P0/P1）
|
||||
|
||||
`CoarsePath` 在 `Map` 提供的不可变 `PlanningGridMap` 上执行 Hybrid A*，输出已经过连续碰撞、终点和输出不变量复核的粗路径。它只处理“能否安全地从当前车辆几何中心到达目标”的几何搜索；不读取传感器或定位，不绘制 UI，也不向底盘发送任何命令。
|
||||
|
||||
地图障碍物来源、世界坐标栅格化、距离场和缓存细节由 [Map/README.md](../Map/README.md) 说明。本模块唯一建议的业务调用入口是：
|
||||
|
||||
```csharp
|
||||
CoarsePathPlanningService.Plan(job, cancellationToken)
|
||||
```
|
||||
|
||||
## 模块说明（Module Overview）
|
||||
|
||||
| 模块 | 负责内容 | 不负责内容 |
|
||||
| --- | --- | --- |
|
||||
| `Map` | 外部障碍物快照、占据栅格、保守障碍距离、地图缓存 | 车辆足迹、运动原语、路径搜索、控制 |
|
||||
| `CoarsePath` | 车辆扩大足迹、连续碰撞检查、前进/倒车原语、Hybrid A*、路径复核与方向分段 | 传感器读取、定位读取、速度规划、路径跟踪、底盘命令 |
|
||||
| `Facade` | 将建图、搜索、取消、总预算和可选调试旁路编排为一次调用 | 修改地图内容、写入 AMR 占据、执行轨迹 |
|
||||
| `Test` | 固定回归场景、P1 手动测试入口与规划结果可视化 | 真实作业地图、实时重规划或车辆控制 |
|
||||
|
||||
安全余量只由 `VehicleParameters.SafetyMarginMeters` 扩大车辆矩形。它不会回写到地图障碍物，因此同一个 `PlanningGridMap` 可由不同车辆参数重复使用。
|
||||
|
||||
## 文件结构（File Structure）
|
||||
|
||||
```text
|
||||
CoarsePath/
|
||||
├── README.md # 本模块说明：结构、数据流、调用与测试
|
||||
├── HybridAStarPlanner.cs # 公开规划门面后的核心编排：校验、搜索、回溯和复核
|
||||
├── Contracts/
|
||||
│ ├── Pose2D.cs # 车辆几何中心位姿：m / rad
|
||||
│ ├── PlanningRequest.cs # 已有 PlanningGridMap 上的一次内部搜索请求
|
||||
│ ├── PlanningResult.cs # 不可变规划结果：成功路径或空结果
|
||||
│ ├── PlanningStatus.cs # 成功、取消、无解、输入和资源限制状态
|
||||
│ ├── CoarsePathPoint.cs # 稠密路径点、方向、曲率、净空和换向标记
|
||||
│ ├── PathSegment.cs # 前进或倒车的包含式路径索引段
|
||||
│ ├── VehicleParameters.cs # 车体尺寸、安全余量和曲率限制
|
||||
│ ├── HybridAStarConfiguration.cs # 原语、离散、代价、容差和资源上限
|
||||
│ └── GoalDirectionConstraint.cs # 目标进入方向约束
|
||||
├── Vehicle/
|
||||
│ ├── VehicleKinematics.cs # 恒曲率车辆运动学积分
|
||||
│ ├── VehicleFootprint.cs # 扩大后的车辆矩形几何
|
||||
│ ├── OrientedRectangleCellIntersection.cs # 旋转矩形与占据格相交判定
|
||||
│ └── FootprintCollisionChecker.cs # 连续扫掠的足迹碰撞检查
|
||||
├── Search/
|
||||
│ ├── BinaryMinHeap.cs # 可更新优先级的 Open List
|
||||
│ ├── GridDijkstraHeuristic.cs # 二维栅格可达性和距离启发式
|
||||
│ ├── GoalToleranceChecker.cs # 目标位置、航向和方向约束判定
|
||||
│ ├── MotionPrimitive.cs # 单个前进或倒车恒曲率原语
|
||||
│ ├── MotionPrimitiveGenerator.cs # 原语离散与连续积分点生成
|
||||
│ ├── SearchCostCalculator.cs # 长度、倒车、换向、曲率和净空代价
|
||||
│ ├── HybridAStarNode.cs # 搜索节点与父链信息
|
||||
│ ├── HybridAStarNodeKey.cs # 离散状态键
|
||||
│ └── HybridAStarSearch.cs # Hybrid A* 主搜索循环
|
||||
├── Output/
|
||||
│ ├── PathBacktracker.cs # 从终点节点安全回溯父链
|
||||
│ ├── CoarsePathAssembler.cs # 组装稠密路径和方向段
|
||||
│ └── CoarsePathValidator.cs # 对最终输出重新进行连续复核
|
||||
├── Facade/
|
||||
│ ├── CoarsePathPlanningJob.cs # 一次完整业务输入：地图请求、位姿、车辆、配置
|
||||
│ ├── CoarsePathPlanningJobResult.cs # 同时包含地图结果和规划结果的不可变输出
|
||||
│ ├── CoarsePathPlanningService.cs # 唯一业务调用门面
|
||||
│ ├── PlanningDebugOptions.cs # 可选调试旁路配置
|
||||
│ └── IPlanningDebugSink.cs # 调试旁路接收器契约
|
||||
└── Test/
|
||||
├── CoarsePathScenarioFactory.cs # 六个固定场景和手动目标演示请求工厂
|
||||
└── MovementTest.CoarsePathTest.cs # 七个 Clumsy 入口、后台取消和 Painter 绘制
|
||||
```
|
||||
|
||||
## 规划数据流（Planning Data Flow）
|
||||
|
||||
```text
|
||||
CoarsePathPlanningJob
|
||||
│ MapRequest 使用 mm；Pose2D/车辆使用 m、rad
|
||||
▼
|
||||
CoarsePathPlanningService.Plan(job, cancellationToken)
|
||||
│
|
||||
├── PlanningMapFactory.Create(job.MapRequest)
|
||||
│ │
|
||||
│ ├── 失败、取消或超时
|
||||
│ │ └── MapResult + 空路径 PlanningResult，搜索不启动
|
||||
│ │
|
||||
│ └── 成功：不可变 PlanningGridMap
|
||||
▼
|
||||
PlanningRequest
|
||||
│
|
||||
▼
|
||||
HybridAStarPlanner
|
||||
├── 车辆扩大足迹与连续碰撞检查
|
||||
├── GridDijkstraHeuristic + Hybrid A* 搜索
|
||||
├── PathBacktracker + CoarsePathAssembler
|
||||
└── CoarsePathValidator 最终复核
|
||||
▼
|
||||
PlanningResult + MapResult
|
||||
▼
|
||||
CoarsePathPlanningJobResult
|
||||
```
|
||||
|
||||
调用方只创建 `CoarsePathPlanningJob` 并消费 `CoarsePathPlanningJobResult`。`PlanningRequest`、`HybridAStarPlanner`、原语和碰撞检查器属于模块内部协作对象，不应由 UI、传感器或 MovementTest 直接拼接。
|
||||
|
||||
## 构建状态与停止（Build Status and Stop）
|
||||
|
||||
必须一起处理 `MapResult` 和 `PlanningResult`。`MapResult.Status` 的类型是 `PlanningMapBuildStatus`；地图失败时，门面返回对应的空路径结果，并且不会启动 Hybrid A*。
|
||||
|
||||
| 情况 | `MapResult` | `PlanningResult` | 调用方处理 |
|
||||
| --- | --- | --- | --- |
|
||||
| 地图和搜索成功 | `Success` 且 `Map` 非空 | `Success`，发布完整路径和方向段 | 消费粗路径；后续模块仍需自行进行平滑、速度和控制 |
|
||||
| 地图输入/来源失败 | `Failed` | `InvalidMap` | 读取 `FailureReason`，修复地图输入 |
|
||||
| 调用被取消 | `Cancelled` | `Cancelled` | 不重试为普通无解；不会发布地图或部分路径 |
|
||||
| 总预算耗尽 | `TimedOut` | `SearchTimeout` | 根据上层策略调整预算或稍后重试 |
|
||||
| 搜索无解 | 地图成功 | `NoFeasiblePath` | 当前地图、车体和运动约束下无可行路径 |
|
||||
| 节点或搜索资源受限 | 地图成功 | `SearchNodeLimitExceeded` 或 `SearchTimeout` | 读取诊断后调整配置或上层策略 |
|
||||
|
||||
除 `PlanningStatus.Success` 外，`PlanningResult.Path` 与 `PlanningResult.Segments` 始终为空。取消、超时、无解、输入错误和最终复核失败都不能作为“部分可执行路径”使用。
|
||||
|
||||
### 总预算与取消
|
||||
|
||||
`HybridAStarConfiguration.SearchTimeout` 是从门面开始计时的一次总预算，依次覆盖建图、距离场、二维启发式和 Hybrid A*。同一个 `CancellationToken` 会沿调用链传递，取消优先于超时。
|
||||
|
||||
### 总耗时与路径搜索耗时
|
||||
|
||||
`PlanningDiagnostics.Elapsed` 是从 `CoarsePathPlanningService.Plan` 开始的总耗时，包含地图来源、缓存、栅格化、距离场和路径规划。`PathSearchElapsed`（路径搜索耗时）从地图和起终点预检通过后开始，包含二维启发式、Hybrid A*、回溯、装配、方向分段和最终复核；搜索开始前失败时为零。
|
||||
|
||||
## 坐标与单位（Coordinates and Units）
|
||||
|
||||
| 数据 | 单位 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `PlanningMapRequest.Bounds`、分辨率、障碍物几何 | mm | 来自 Map 的世界坐标；边界采用 `[min, max)` |
|
||||
| `Pose2D.X`、`Pose2D.Y`、路径位置、车辆尺寸、安全余量、弧长 | m | CoarsePath 的连续世界坐标和长度 |
|
||||
| `Pose2D.Heading`、航向容差 | rad | 核心一律使用弧度 |
|
||||
| 曲率、起步曲率 | 1/m | 最大曲率或最小转弯半径至少提供一个 |
|
||||
| `PlanningGridMap` 世界查询参数 | m | 越界位置按占据处理，净距为 0 |
|
||||
| P1 的 AMR/手动目标 X/Y | mm | 仅在 UI 边界读取，进入核心前除以 1000 |
|
||||
| P1 的 AMR/手动目标航向 | deg | 仅在 UI 边界转换为 `deg * PI / 180 -> rad` |
|
||||
|
||||
起点和终点都表示车辆**几何中心**。若上游定位参考点是雷达、天线或其他安装点，必须先在上游应用安装外参；不要在 CoarsePath 内猜测偏移。车辆外扩由 `VehicleParameters.SafetyMarginMeters` 表达，不要把余量写入 Map 障碍物。
|
||||
|
||||
## 最小调用示例（Minimal Call Example）
|
||||
|
||||
以下示例明确允许空图，因而只适合算法或单位演示。真实作业必须通过 `IMapObstacleSource` 提供有效障碍物快照；如何构造来源请阅读 [Map/README.md](../Map/README.md)。
|
||||
|
||||
```csharp
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
var service = new CoarsePathPlanningService(); // 长期持有，保留地图缓存
|
||||
|
||||
var job = new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = Array.Empty<IMapObstacleSource>(),
|
||||
AllowExplicitEmptyMap = true, // 仅演示时明确允许
|
||||
},
|
||||
Start = new Pose2D(1d, 1d, 0d),
|
||||
Goal = new Pose2D(3d, 1d, 0d),
|
||||
Vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
},
|
||||
Configuration = new HybridAStarConfiguration(),
|
||||
GoalDirection = GoalDirectionConstraint.Forward,
|
||||
};
|
||||
|
||||
CoarsePathPlanningJobResult result =
|
||||
service.Plan(job, CancellationToken.None);
|
||||
|
||||
if (!result.MapResult.Succeeded)
|
||||
throw new InvalidOperationException(result.MapResult.FailureReason);
|
||||
|
||||
if (result.PlanningResult.Status != PlanningStatus.Success)
|
||||
throw new InvalidOperationException(
|
||||
result.PlanningResult.Diagnostics.TerminationReason);
|
||||
|
||||
foreach (CoarsePathPoint point in result.PlanningResult.Path)
|
||||
Console.WriteLine(point.X + "," + point.Y + "," + point.Heading);
|
||||
```
|
||||
|
||||
## 缓存与 SourceVersion（Cache and SourceVersion）
|
||||
|
||||
`CoarsePathPlanningService` 在生命周期内长期持有 `PlanningMapFactory`，因此重复调用时能够复用地图缓存。不要每次规划都新建服务，否则会失去缓存收益。
|
||||
|
||||
| 缓存层级 | 条件 | 结果 |
|
||||
| --- | --- | --- |
|
||||
| `Input` | 边界、分辨率、空图策略、来源 ID、`SourceVersion`、必需性和来源结果相同 | 返回同一个不可变 `PlanningGridMap` |
|
||||
| `Occupancy` | 输入版本变化，但最终占据栅格相同 | 复用占据/距离数组，生成新的快照元数据 |
|
||||
| `None` | 占据内容变化 | 重建规划快照和距离场 |
|
||||
|
||||
障碍来源内容改变时，调用方必须递增该来源的 `SourceVersion`。仅修改起终点、车辆、搜索配置、调试开关或调试接收器不会改变地图输入；改变障碍物却不递增版本则可能错误复用旧快照。
|
||||
|
||||
## 详细使用指南（Detailed Usage Guide）
|
||||
|
||||
本节说明调用方如何从一个地图输入得到可消费的粗路径。所有业务调用都通过 `CoarsePathPlanningService.Plan(job, cancellationToken)` 完成。
|
||||
|
||||
### 第 1 步：长期持有服务
|
||||
|
||||
服务持有地图工厂和规划器，应该作为规划业务、任务执行器或上层服务的长期字段，而不是在每次调用中创建：
|
||||
|
||||
```csharp
|
||||
private readonly CoarsePathPlanningService _coarsePathService =
|
||||
new CoarsePathPlanningService();
|
||||
```
|
||||
|
||||
### 第 2 步：准备地图请求
|
||||
|
||||
创建 `PlanningMapRequest`，其边界、分辨率和障碍物仍使用 mm。使用手工圆形/矩形或 TwoLeg 快照时，应先按 [Map/README.md](../Map/README.md) 将它们包装为 `IMapObstacleSource`，并为内容变化递增 `SourceVersion`。
|
||||
|
||||
```csharp
|
||||
var mapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = false,
|
||||
};
|
||||
```
|
||||
|
||||
`AllowExplicitEmptyMap = true` 只在调用方明确确认空地图安全时使用。未提供有效障碍物且未显式允许空图时，地图不会进入规划。
|
||||
|
||||
### 第 3 步：填写起点、终点和方向约束
|
||||
|
||||
将车辆几何中心的世界 X/Y 从 mm 转为 m，并将航向转换为 rad 后创建 `Pose2D`。`StartDirection = null` 表示允许从前进或倒车开始；`GoalDirection` 可以限制最终进入目标的方向。
|
||||
|
||||
```csharp
|
||||
var start = new Pose2D(startXmm / 1000d, startYmm / 1000d,
|
||||
startHeadingDeg * Math.PI / 180d);
|
||||
var goal = new Pose2D(goalXmm / 1000d, goalYmm / 1000d,
|
||||
goalHeadingDeg * Math.PI / 180d);
|
||||
```
|
||||
|
||||
### 第 4 步：填写车辆参数
|
||||
|
||||
车辆尺寸和安全余量全部为 m。曲率限制可填写 `MaximumCurvaturePerMeter`，或填写 `MinimumTurningRadiusMeters`；至少必须提供一个有效限制。
|
||||
|
||||
```csharp
|
||||
var vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
};
|
||||
```
|
||||
|
||||
### 第 5 步：调整搜索配置
|
||||
|
||||
默认 `HybridAStarConfiguration` 包含原语长度、积分步长、航向离散、终点容差、代价、节点上限和总超时。若业务需要覆盖默认值，应同时理解安全影响：`MaximumCollisionCheckStepMeters` 不能以牺牲连续碰撞检查精度为代价随意增大。
|
||||
|
||||
```csharp
|
||||
var configuration = new HybridAStarConfiguration
|
||||
{
|
||||
SearchTimeout = TimeSpan.FromSeconds(5d),
|
||||
MaximumExpandedNodes = 200000,
|
||||
};
|
||||
```
|
||||
|
||||
### 第 6 步：调用并消费成功结果
|
||||
|
||||
只有 `Success` 可以发布完整路径。`Path` 是稠密点序列；`Segments` 是覆盖整条路径的前进/倒车包含式索引段，可供后续的速度规划或显示模块消费。
|
||||
|
||||
```csharp
|
||||
var job = new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = mapRequest,
|
||||
Start = start,
|
||||
Goal = goal,
|
||||
Vehicle = vehicle,
|
||||
Configuration = configuration,
|
||||
GoalDirection = GoalDirectionConstraint.Any,
|
||||
};
|
||||
|
||||
CoarsePathPlanningJobResult result = _coarsePathService.Plan(job, cancellationToken);
|
||||
if (!result.MapResult.Succeeded)
|
||||
ReportMapFailure(result.MapResult.FailureReason);
|
||||
else if (result.PlanningResult.Status == PlanningStatus.Success)
|
||||
ConsumeCoarsePath(result.PlanningResult.Path, result.PlanningResult.Segments);
|
||||
else
|
||||
ReportPlanningFailure(result.PlanningResult.Diagnostics.TerminationReason);
|
||||
```
|
||||
|
||||
`CoarsePathPoint.IsGearSwitchPoint` 为 `true` 表示该点是新方向段开始处。换向位置会保留一对位置、航向与弧长相同、方向不同的相邻点；`UnwrappedHeading` 用于跨越 `-pi/pi` 时保持显示连续。
|
||||
|
||||
## P1 手动测试与可视化（P1 Manual Tests and Visualization）
|
||||
|
||||
P1 在 `Test/MovementTest.CoarsePathTest.cs` 提供只读测试入口。它们共用一个长期存活的 `CoarsePathPlanningService`，只提交规划并绘制结果；不发送底盘、速度或转向命令。
|
||||
|
||||
| MovementTest 名称 | 场景 | 预期 |
|
||||
| --- | --- | --- |
|
||||
| `粗路径规划-显式空图` | 显式允许的空图 | 前进直达成功 |
|
||||
| `粗路径规划-单矩形绕行` | 中央矩形阻断直线 | 成功绕障 |
|
||||
| `粗路径规划-多来源障碍` | 手工圆形、矩形和 TwoLeg 快照 | 证明多来源经过同一门面 |
|
||||
| `粗路径规划-缓存命中` | 重复相同地图输入 | 后续调用显示 `Input` 缓存命中 |
|
||||
| `粗路径规划-倒车换向` | 前进起步、倒车到达 | 成功路径含 `IsGearSwitchPoint` |
|
||||
| `粗路径规划-无解` | 贯穿地图的障碍带 | 返回 `NoFeasiblePath` 且不发布路径 |
|
||||
| `粗路径规划` | 当前 AMR 位姿、人工终点和可选人工障碍物 | 验证手动障碍物、边界、路径与可视化 |
|
||||
|
||||
### 固定案例的实时 AMR 锚定
|
||||
|
||||
`粗路径规划-显式空图`、`粗路径规划-单矩形绕行`、`粗路径规划-多来源障碍`、`粗路径规划-缓存命中`、`粗路径规划-倒车换向` 和 `粗路径规划-无解` 是六个固定案例。它们不再以写死的世界起点运行：共享运行器在前台仅调用一次 `DetourInterface.getCartLocation()`，校验并冻结本次 AMR 的世界 `X(mm)`、`Y(mm)` 与航向 `deg`，再创建本次规划请求；后台规划期间不会再次读取定位。
|
||||
|
||||
`Create(CoarsePathTestScenario scenario, double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)`
|
||||
|
||||
该入口把基准案例的起点映射为冻结的 AMR 位姿，并以相同的 `ΔX/ΔY` 平移地图边界、目标、圆形/矩形障碍，以及 TwoLeg 的检测原点。因此，固定案例始终在当前 AMR 附近保留原有的相对几何关系。起点航向严格使用冻结的 AMR 航向；终点航向保持基准案例的“终点航向减起点航向”差值，叠加到当前 AMR 航向后规范化到 `[-pi, pi]`。TwoLeg 只平移检测原点，`DetectionHeadingRadians` 不会因 AMR 航向发生旋转。
|
||||
|
||||
`粗路径规划-缓存命中` 只有两次运行冻结到相同的 AMR `X/Y`、从而形成相同的平移后地图输入时，才作为缓存命中场景；AMR 位置移动后，地图输入正常未命中并重建快照。仅 AMR 航向变化不会改变固定案例的地图输入，地图缓存仍可命中。若定位读取为空、抛出异常，或 `X`、`Y`、航向含有 `NaN`/无穷值，运行器不会提交后台规划，状态与 Toast 会显示以“AMR 位姿不可用”开头的诊断原因。
|
||||
|
||||
手动 `粗路径规划` 的人工终点、障碍物和超时输入流程保持不变；它不套用固定案例的整体平移规则。
|
||||
|
||||
### AMR 位姿、手动终点与障碍物
|
||||
|
||||
`CoarsePathPlanningTest` 启动时读取一次 `DetourInterface.getCartLocation()`，将当前 AMR 世界位姿冻结为起点；随后依次输入目标世界 `X(mm)`、`Y(mm)`、航向 `deg`，以及障碍物数量 `0-20`。每个障碍物再依次输入类型和几何参数：
|
||||
|
||||
| 类型输入 | 形状 | 输入参数（全部为 mm） |
|
||||
| --- | --- | --- |
|
||||
| `1` | 圆形 | 几何中心 `X`、`Y` 与半径；半径必须大于 0 |
|
||||
| `2` | 轴对齐矩形 | 几何中心 `X`、`Y`、X 向长度、Y 向宽度；两个尺寸必须大于 0 |
|
||||
|
||||
矩形只支持 `AxisAlignedRectangle`，不提供旋转角；其中心和长宽由 `ManualCoarsePathObstacle.AxisAlignedRectangle` 表达。圆形由 `ManualCoarsePathObstacle.Circle` 表达。所有无穷、NaN、非数字或不合法尺寸都会在输入阶段拒绝。
|
||||
|
||||
`CoarsePathScenarioFactory.CreateManualObstacleDemo` 在唯一边界完成转换：
|
||||
|
||||
```text
|
||||
AMR/目标 X、Y：mm / 1000 -> m
|
||||
AMR/目标航向：deg * PI / 180 -> rad
|
||||
```
|
||||
|
||||
当数量为 0 时，入口使用显式空图；`CreateManualGoalDemo` 保留为同一零障碍物场景的兼容帮助方法。数量大于 0 时，工厂将 `ManualCoarsePathObstacle` 快照封装成来源 ID 为 `manual-user-input` 的地图输入，并为每次手动快照分配新的 `SourceVersion`，避免错误复用地图缓存。规划边界覆盖起点、终点和每个障碍物的完整轮廓，再增加 8000 mm 余量并按 50 mm 对齐。
|
||||
|
||||
手动入口还要求输入一次“粗路径规划总超时”，单位为秒，只接受 `TimeSpan` 可表示范围内的有限正数秒；`0`、负数、NaN、Infinity 或溢出值都会在启动规划前拒绝。该值只覆盖本次 `CoarsePathPlanningJob.Configuration.SearchTimeout`，不会改变固定场景或全局默认值。
|
||||
|
||||
此入口使用固定演示车辆：长 `0.80 m`、宽 `0.60 m`、四周安全余量 `0.05 m`、最小转弯半径 `1.20 m`。这些值不是从现场 AMR 配置读取的，判断现场可行性前必须确认车辆参数一致。
|
||||
|
||||
`getCartLocation` 在无有效定位时可能阻塞，因此应在定位准备完成的测试环境使用。手动障碍物是测试输入，不能替代现场障碍物来源；零障碍物的显式空图也绝不代表现场不存在障碍物。
|
||||
|
||||
### 后台执行、停止与图层
|
||||
|
||||
每次测试启动时会创建独立的 `CancellationTokenSource`，以 `Task.Run` 调用门面，并先取消旧会话。`Test()` 不等待任务，也不读取 `Task.Result`；`TestStop` 取消当前令牌、使会话失效并清空图层。已取消任务完成后不会覆盖新会话，也不会显示部分路径。
|
||||
|
||||
专用世界坐标 Painter 图层为 `CoarsePathPlanningV1`。它直接读取本次 `PlanningGridMap` 的 `Bounds`、`ResolutionMm`、`SnapshotId` 和 `IsOccupied(row, col)`，因此边界、抽稀网格和占据格与实际规划快照一致，而不是重新绘制原始障碍物。
|
||||
|
||||
状态图层显示规划状态、总耗时、`PathSearchElapsed`（路径搜索耗时）、扩展/生成节点数、Open List 峰值、失败原因和固定演示车辆参数。Toast 同时显示两种耗时，并在失败时附加 `TerminationReason`，因此超时、节点上限、无解、碰撞和内部错误不会只显示成泛化失败。
|
||||
|
||||
| 颜色 | 可视化元素 |
|
||||
| --- | --- |
|
||||
| 灰白 | 地图边界与栅格网络 |
|
||||
| 暗红 | 占据格 |
|
||||
| 绿色 | 起点、前进路径和方向箭头 |
|
||||
| 橙色 | 终点、航向和位置容差圈 |
|
||||
| 天蓝 | 倒车路径和方向箭头 |
|
||||
| 紫色 | 换向点 |
|
||||
| 金色 | 已纳入安全余量的车辆检查框 |
|
||||
|
||||
自动化已检查 UI 入口的后台、取消和数据来源结构。仍需在实际 Clumsy 界面手动运行“粗路径规划-单矩形绕行”和“粗路径规划”，确认图层交互显示与停止按钮效果。
|
||||
|
||||
## 常见错误（Common Errors）
|
||||
|
||||
| 现象 | 原因 | 处理 |
|
||||
| --- | --- | --- |
|
||||
| 起点、终点或障碍物位置相差 1000 倍 | 将 mm 直接传给 `Pose2D` 或把 m 传给地图输入 | Map 输入使用 mm；`Pose2D`、车辆和路径使用 m |
|
||||
| 路径朝向错误或旋转异常 | 将 P1 的 deg 直接当作核心 rad | 在 UI/上层边界执行 `deg * PI / 180`，核心只保存 rad |
|
||||
| 障碍物已经变化却复用旧地图 | 内容变更后没有递增 `SourceVersion` | 每次来源快照内容变化后增加对应版本号 |
|
||||
| 地图创建成功但规划被阻止 | 没有有效障碍物且未显式允许空图 | 提供有效来源；仅在确认安全的演示中设置 `AllowExplicitEmptyMap = true` |
|
||||
| 无解、取消或超时后仍尝试使用路径 | 没有检查 `PlanningStatus.Success` | 仅成功时消费 `Path` 和 `Segments`；其他状态读取诊断 |
|
||||
| 将粗路径直接下发给车辆 | 粗路径不包含速度、时间、执行控制或实时安全闭环 | 在后续阶段增加平滑、时间参数化、跟踪和独立安全控制 |
|
||||
| P1 手动终点表现为空场地安全 | 手动入口使用显式空图演示 | 真实作业必须提供真实障碍物快照，不能复用空图语义 |
|
||||
|
||||
## 第一版限制（First-Version Limits）
|
||||
|
||||
当前 P0/P1 已提供安全、确定性的粗路径核心和手动结果可视化，但不包含：
|
||||
|
||||
- 路径平滑或曲率连续优化；
|
||||
- Reeds-Shepp 或 Dubins 精确终点连接；
|
||||
- 速度、加速度、时间标注、时间轨迹和路径跟踪控制；
|
||||
- 底盘命令、避障闭环、现场传感器采集或实时重规划调度；
|
||||
- 横移、蟹行或其他非前进/倒车运动原语；
|
||||
- 原地旋转；
|
||||
- 真实作业地图接入、交互式场景编辑和 Release 性能/资源/确定性基准。
|
||||
|
||||
当前只生成汽车式恒曲率前进/倒车原语，并允许在原语边界换向；未实现的 Reeds-Shepp、横移、蟹行和原地旋转是整个粗规划核心的第一版能力边界，不是 MovementTest 单独关闭。
|
||||
|
||||
因此，调用方只能把 `Success` 结果视作后续模块的粗路径输入，不能把它当作可直接下发的时间轨迹。
|
||||
@@ -0,0 +1,146 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 为 Hybrid A* Open List 提供确定性优先级的二叉最小堆。
|
||||
/// 排序严格依次比较 F、H、较大的 G 和插入序号；F、H、G 必须为有限且非负的等效米代价。
|
||||
/// </summary>
|
||||
/// <typeparam name="T">与一组搜索代价关联的节点或条目类型。</typeparam>
|
||||
public sealed class BinaryMinHeap<T>
|
||||
{
|
||||
private readonly List<HeapEntry> _entries = new List<HeapEntry>();
|
||||
private long _nextInsertionSequence;
|
||||
|
||||
/// <summary>创建空的确定性 Open List 堆。</summary>
|
||||
public BinaryMinHeap()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>当前堆内尚未出队的条目数量。</summary>
|
||||
public int Count { get { return _entries.Count; } }
|
||||
|
||||
/// <summary>
|
||||
/// 将一个条目和其搜索排序代价压入堆。
|
||||
/// 参数：item 为关联条目；f、h、g 均为有限且非负的等效米代价。
|
||||
/// 失败：任一代价无效、item 为 null（仅引用类型）或插入序号耗尽时抛出异常。
|
||||
/// </summary>
|
||||
public void Push(T item, double f, double h, double g)
|
||||
{
|
||||
if (ReferenceEquals(item, null)) throw new ArgumentNullException(nameof(item));
|
||||
ValidateCost(f, nameof(f));
|
||||
ValidateCost(h, nameof(h));
|
||||
ValidateCost(g, nameof(g));
|
||||
if (_nextInsertionSequence == long.MaxValue)
|
||||
throw new InvalidOperationException("The binary heap insertion sequence has been exhausted.");
|
||||
|
||||
var entry = new HeapEntry(item, f, h, g, _nextInsertionSequence);
|
||||
_nextInsertionSequence++;
|
||||
_entries.Add(entry);
|
||||
SiftUp(_entries.Count - 1);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 弹出当前排序最优的条目。
|
||||
/// 返回：按 F、H、较大 G 和插入序号排序后的最小条目；空堆时抛出 <see cref="InvalidOperationException"/>。
|
||||
/// </summary>
|
||||
public T Pop()
|
||||
{
|
||||
if (_entries.Count == 0) throw new InvalidOperationException("The binary heap is empty.");
|
||||
|
||||
HeapEntry result = _entries[0];
|
||||
int lastIndex = _entries.Count - 1;
|
||||
if (lastIndex == 0)
|
||||
{
|
||||
_entries.RemoveAt(0);
|
||||
return result.Item;
|
||||
}
|
||||
|
||||
_entries[0] = _entries[lastIndex];
|
||||
_entries.RemoveAt(lastIndex);
|
||||
SiftDown(0);
|
||||
return result.Item;
|
||||
}
|
||||
|
||||
/// <summary>清空尚未出队的条目；后续插入序号继续单调递增以保持整个实例内的确定性。</summary>
|
||||
public void Clear()
|
||||
{
|
||||
_entries.Clear();
|
||||
}
|
||||
|
||||
private void SiftUp(int index)
|
||||
{
|
||||
while (index > 0)
|
||||
{
|
||||
int parentIndex = (index - 1) / 2;
|
||||
if (Compare(_entries[index], _entries[parentIndex]) >= 0) return;
|
||||
Swap(index, parentIndex);
|
||||
index = parentIndex;
|
||||
}
|
||||
}
|
||||
|
||||
private void SiftDown(int index)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
int leftChildIndex = index * 2 + 1;
|
||||
if (leftChildIndex >= _entries.Count) return;
|
||||
|
||||
int bestChildIndex = leftChildIndex;
|
||||
int rightChildIndex = leftChildIndex + 1;
|
||||
if (rightChildIndex < _entries.Count && Compare(_entries[rightChildIndex], _entries[leftChildIndex]) < 0)
|
||||
bestChildIndex = rightChildIndex;
|
||||
|
||||
if (Compare(_entries[bestChildIndex], _entries[index]) >= 0) return;
|
||||
Swap(index, bestChildIndex);
|
||||
index = bestChildIndex;
|
||||
}
|
||||
}
|
||||
|
||||
private void Swap(int firstIndex, int secondIndex)
|
||||
{
|
||||
HeapEntry temporary = _entries[firstIndex];
|
||||
_entries[firstIndex] = _entries[secondIndex];
|
||||
_entries[secondIndex] = temporary;
|
||||
}
|
||||
|
||||
private static int Compare(HeapEntry left, HeapEntry right)
|
||||
{
|
||||
int comparison = left.F.CompareTo(right.F);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = left.H.CompareTo(right.H);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = right.G.CompareTo(left.G);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
return left.InsertionSequence.CompareTo(right.InsertionSequence);
|
||||
}
|
||||
|
||||
private static void ValidateCost(double value, string parameterName)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Search costs must be finite and non-negative.");
|
||||
}
|
||||
|
||||
private sealed class HeapEntry
|
||||
{
|
||||
public HeapEntry(T item, double f, double h, double g, long insertionSequence)
|
||||
{
|
||||
Item = item;
|
||||
F = f;
|
||||
H = h;
|
||||
G = g;
|
||||
InsertionSequence = insertionSequence;
|
||||
}
|
||||
|
||||
public T Item { get; }
|
||||
public double F { get; }
|
||||
public double H { get; }
|
||||
public double G { get; }
|
||||
public long InsertionSequence { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>按位置、航向和进入方向约束判定连续位姿是否可以作为目标候选。</summary>
|
||||
public static class GoalToleranceChecker
|
||||
{
|
||||
/// <summary>
|
||||
/// 判断一个位姿是否满足目标容差和进入方向约束。
|
||||
/// 参数：pose 与 goal 使用世界 m/rad；configuration 提供位置 m 和航向 rad 容差；direction 为候选末段方向；goalDirection 为目标进入方向约束。
|
||||
/// 返回：输入有限、位置距离和最小环形航向误差均不超过容差且方向匹配时为 true；无效输入保守地返回 false。
|
||||
/// </summary>
|
||||
public static bool IsSatisfied(
|
||||
Pose2D pose,
|
||||
Pose2D goal,
|
||||
HybridAStarConfiguration configuration,
|
||||
TravelDirection direction,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsFinitePose(pose) || !IsFinitePose(goal) || configuration == null ||
|
||||
!NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) ||
|
||||
!NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) ||
|
||||
configuration.GoalPositionToleranceMeters < 0d || configuration.GoalHeadingToleranceRadians < 0d ||
|
||||
!IsTravelDirection(direction) || !IsGoalDirection(goalDirection))
|
||||
return false;
|
||||
|
||||
if (!MatchesDirection(direction, goalDirection)) return false;
|
||||
|
||||
double deltaX = pose.X - goal.X;
|
||||
double deltaY = pose.Y - goal.Y;
|
||||
double positionDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsFinite(positionDistanceMeters) || positionDistanceMeters > configuration.GoalPositionToleranceMeters)
|
||||
return false;
|
||||
|
||||
double headingDifferenceRadians = Math.Abs(AngleMath.ShortestSignedDifference(pose.Heading, goal.Heading));
|
||||
return NumericGuard.IsFinite(headingDifferenceRadians) && headingDifferenceRadians <= configuration.GoalHeadingToleranceRadians;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
|
||||
private static bool MatchesDirection(TravelDirection direction, GoalDirectionConstraint constraint)
|
||||
{
|
||||
return constraint == GoalDirectionConstraint.Any ||
|
||||
(constraint == GoalDirectionConstraint.Forward && direction == TravelDirection.Forward) ||
|
||||
(constraint == GoalDirectionConstraint.Reverse && direction == TravelDirection.Reverse);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 基于不可变栅格地图的目标反向八邻域 Dijkstra 启发式。
|
||||
/// 距离单位为 m；对角移动仅在两个对应正交邻格都未占据时允许，以避免从障碍夹角穿越。
|
||||
/// </summary>
|
||||
public sealed class GridDijkstraHeuristic
|
||||
{
|
||||
private readonly PlanningGridMap _map;
|
||||
private readonly double[] _costs;
|
||||
|
||||
/// <summary>
|
||||
/// 从目标栅格预计算所有可达自由格到目标的二维最短距离。
|
||||
/// 参数：map 必须是已就绪的不可变地图；goalRow、goalCol 为地图内且未占据的目标格索引。
|
||||
/// 失败：地图为空、未就绪或目标格无效时抛出异常。
|
||||
/// </summary>
|
||||
public GridDijkstraHeuristic(PlanningGridMap map, int goalRow, int goalCol)
|
||||
{
|
||||
if (!TryCreate(map, goalRow, goalCol, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out GridDijkstraHeuristic heuristic, out _))
|
||||
throw new InvalidOperationException("Unbounded Dijkstra construction unexpectedly stopped.");
|
||||
_map = heuristic._map;
|
||||
_costs = heuristic._costs;
|
||||
}
|
||||
|
||||
private GridDijkstraHeuristic(PlanningGridMap map, double[] costs)
|
||||
{
|
||||
_map = map;
|
||||
_costs = costs;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建完整二维启发式；停止时不返回部分成本数组。</summary>
|
||||
internal static bool TryCreate(PlanningGridMap map, int goalRow, int goalCol, PlanningOperationBudget budget,
|
||||
out GridDijkstraHeuristic heuristic, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (!map.PlanningReady) throw new ArgumentException("The planning map must be ready.", nameof(map));
|
||||
if (goalRow < 0 || goalRow >= map.Rows || goalCol < 0 || goalCol >= map.Cols)
|
||||
throw new ArgumentOutOfRangeException(nameof(goalRow));
|
||||
if (map.IsOccupied(goalRow, goalCol))
|
||||
throw new ArgumentException("The goal grid cell must be free.", nameof(goalRow));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
|
||||
heuristic = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
var costs = new double[checked(map.Rows * map.Cols)];
|
||||
int workItemCount = 0;
|
||||
for (int index = 0; index < costs.Length; index++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
costs[index] = double.PositiveInfinity;
|
||||
}
|
||||
if (!TryBuild(map, costs, goalRow, goalCol, budget, ref workItemCount, out stopReason)) return false;
|
||||
heuristic = new GridDijkstraHeuristic(map, costs);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查询指定栅格到构造时目标格的二维最短距离。
|
||||
/// 参数：row、col 为从零开始的栅格索引。
|
||||
/// 返回：单位 m 的有限最短距离；自由格不可达或索引越界时返回正无穷。
|
||||
/// </summary>
|
||||
public double GetCost(int row, int col)
|
||||
{
|
||||
return row < 0 || row >= _map.Rows || col < 0 || col >= _map.Cols
|
||||
? double.PositiveInfinity
|
||||
: _costs[row * _map.Cols + col];
|
||||
}
|
||||
|
||||
private static bool TryBuild(PlanningGridMap map, double[] costs, int goalRow, int goalCol,
|
||||
PlanningOperationBudget budget, ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
var openList = new BinaryMinHeap<int>();
|
||||
int goalIndex = goalRow * map.Cols + goalCol;
|
||||
costs[goalIndex] = 0d;
|
||||
openList.Push(goalIndex, 0d, 0d, 0d);
|
||||
|
||||
while (openList.Count > 0)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
int currentIndex = openList.Pop();
|
||||
int currentRow = currentIndex / map.Cols;
|
||||
int currentCol = currentIndex % map.Cols;
|
||||
double currentCost = costs[currentIndex];
|
||||
|
||||
for (int rowOffset = -1; rowOffset <= 1; rowOffset++)
|
||||
for (int colOffset = -1; colOffset <= 1; colOffset++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
if (rowOffset == 0 && colOffset == 0) continue;
|
||||
|
||||
int nextRow = currentRow + rowOffset;
|
||||
int nextCol = currentCol + colOffset;
|
||||
if (nextRow < 0 || nextRow >= map.Rows || nextCol < 0 || nextCol >= map.Cols || map.IsOccupied(nextRow, nextCol))
|
||||
continue;
|
||||
|
||||
bool isDiagonal = rowOffset != 0 && colOffset != 0;
|
||||
if (isDiagonal && (map.IsOccupied(currentRow + rowOffset, currentCol) || map.IsOccupied(currentRow, currentCol + colOffset)))
|
||||
continue;
|
||||
|
||||
double stepCost = isDiagonal ? Math.Sqrt(2d) * map.ResolutionMeters : map.ResolutionMeters;
|
||||
double candidateCost = currentCost + stepCost;
|
||||
int nextIndex = nextRow * map.Cols + nextCol;
|
||||
if (candidateCost >= costs[nextIndex]) continue;
|
||||
|
||||
costs[nextIndex] = candidateCost;
|
||||
openList.Push(nextIndex, candidateCost, 0d, 0d);
|
||||
}
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid A* 运行期节点。
|
||||
/// 节点保留连续位姿和其离散闭集键；搜索过程中不会修改已创建节点，改进代价时会追加新节点并使旧 Open List 条目失效。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarNode
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个搜索节点。
|
||||
/// 参数：nodeIndex 为本次搜索内稳定索引；key 为离散状态；pose 为连续车辆中心位姿；parentNodeIndex 为父节点索引，根节点使用 -1；
|
||||
/// incomingPrimitive 为父节点到本节点的原语，根节点为 null；curvaturePerMeter、gCostMeters 和 hCostMeters 均使用规划器的标准单位。
|
||||
/// </summary>
|
||||
public HybridAStarNode(
|
||||
int nodeIndex,
|
||||
HybridAStarNodeKey key,
|
||||
Pose2D pose,
|
||||
int parentNodeIndex,
|
||||
MotionPrimitive incomingPrimitive,
|
||||
double curvaturePerMeter,
|
||||
double gCostMeters,
|
||||
double hCostMeters)
|
||||
: this(nodeIndex, key, pose, parentNodeIndex, incomingPrimitive, curvaturePerMeter, gCostMeters, hCostMeters, false)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建一个搜索节点，并显式指定它是否为原语内部命中的终点候选。
|
||||
/// 参数：isGoalCandidate 为 true 时，本节点不参与普通离散状态的最优 G 值支配；它仍必须在从 Open List 出队后重新通过连续终点和碰撞检查才能成功。
|
||||
/// </summary>
|
||||
public HybridAStarNode(
|
||||
int nodeIndex,
|
||||
HybridAStarNodeKey key,
|
||||
Pose2D pose,
|
||||
int parentNodeIndex,
|
||||
MotionPrimitive incomingPrimitive,
|
||||
double curvaturePerMeter,
|
||||
double gCostMeters,
|
||||
double hCostMeters,
|
||||
bool isGoalCandidate)
|
||||
{
|
||||
if (key == null) throw new ArgumentNullException(nameof(key));
|
||||
if (pose == null) throw new ArgumentNullException(nameof(pose));
|
||||
|
||||
NodeIndex = nodeIndex;
|
||||
Key = key;
|
||||
Pose = pose;
|
||||
ParentNodeIndex = parentNodeIndex;
|
||||
IncomingPrimitive = incomingPrimitive;
|
||||
CurvaturePerMeter = curvaturePerMeter;
|
||||
GCostMeters = gCostMeters;
|
||||
HCostMeters = hCostMeters;
|
||||
IsGoalCandidate = isGoalCandidate;
|
||||
}
|
||||
|
||||
/// <summary>本次搜索节点数组中的稳定索引。</summary>
|
||||
public int NodeIndex { get; }
|
||||
|
||||
/// <summary>本节点用于 Open/Closed 状态管理的离散键。</summary>
|
||||
public HybridAStarNodeKey Key { get; }
|
||||
|
||||
/// <summary>未量化的连续车辆中心位姿。</summary>
|
||||
public Pose2D Pose { get; }
|
||||
|
||||
/// <summary>父节点稳定索引；根节点为 -1。</summary>
|
||||
public int ParentNodeIndex { get; }
|
||||
|
||||
/// <summary>由父节点驶入本节点的已连续碰撞检查原语；根节点为 null。</summary>
|
||||
public MotionPrimitive IncomingPrimitive { get; }
|
||||
|
||||
/// <summary>与 <see cref="IncomingPrimitive"/> 含义相同的原语别名。</summary>
|
||||
public MotionPrimitive Primitive { get { return IncomingPrimitive; } }
|
||||
|
||||
/// <summary>当前末段采用的车辆曲率，单位 1/m。</summary>
|
||||
public double CurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>当前节点的累计等效米代价。</summary>
|
||||
public double GCostMeters { get; }
|
||||
|
||||
/// <summary>当前节点的二维启发式等效米代价。</summary>
|
||||
public double HCostMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 本节点是否由原语内部首次满足终点条件而生成。
|
||||
/// 终点候选必须保留连续位姿，不能因同一离散键的普通低代价节点而被 Open List 准入规则压制。
|
||||
/// </summary>
|
||||
public bool IsGoalCandidate { get; }
|
||||
|
||||
/// <summary>Open List 排序使用的总等效米代价。</summary>
|
||||
public double FCostMeters { get { return GCostMeters + HCostMeters; } }
|
||||
|
||||
/// <summary>本节点末段的行驶方向。</summary>
|
||||
public TravelDirection Direction { get { return Key.Direction; } }
|
||||
|
||||
/// <summary>本节点末段的曲率等级索引。</summary>
|
||||
public int CurvatureLevelIndex { get { return Key.CurvatureLevelIndex; } }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid A* 闭集使用的离散状态键。
|
||||
/// 位置使用地图行列索引；航向、行驶方向和曲率等级共同保留车辆运动学状态，避免把同一栅格中的不同可达姿态错误合并。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarNodeKey : IEquatable<HybridAStarNodeKey>
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个离散 Hybrid A* 状态键。
|
||||
/// 参数：row、column 为零开始的地图行列；headingIndex 为航向桶；direction 为末段行驶方向；curvatureLevelIndex 为末段曲率等级。
|
||||
/// </summary>
|
||||
public HybridAStarNodeKey(int row, int column, int headingIndex, TravelDirection direction, int curvatureLevelIndex)
|
||||
{
|
||||
Row = row;
|
||||
Column = column;
|
||||
HeadingIndex = headingIndex;
|
||||
Direction = direction;
|
||||
CurvatureLevelIndex = curvatureLevelIndex;
|
||||
}
|
||||
|
||||
/// <summary>车辆中心所在的零开始地图行索引。</summary>
|
||||
public int Row { get; }
|
||||
|
||||
/// <summary>车辆中心所在的零开始地图列索引。</summary>
|
||||
public int Column { get; }
|
||||
|
||||
/// <summary>与 <see cref="Column"/> 含义相同的列索引别名。</summary>
|
||||
public int Col { get { return Column; } }
|
||||
|
||||
/// <summary>根据配置航向分辨率量化后的航向桶索引。</summary>
|
||||
public int HeadingIndex { get; }
|
||||
|
||||
/// <summary>到达当前节点的最后一段行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>到达当前节点的最后一段曲率等级索引。</summary>
|
||||
public int CurvatureLevelIndex { get; }
|
||||
|
||||
/// <summary>判断另一个键是否表示完全相同的离散搜索状态。</summary>
|
||||
public bool Equals(HybridAStarNodeKey other)
|
||||
{
|
||||
return other != null && Row == other.Row && Column == other.Column && HeadingIndex == other.HeadingIndex &&
|
||||
Direction == other.Direction && CurvatureLevelIndex == other.CurvatureLevelIndex;
|
||||
}
|
||||
|
||||
/// <summary>判断另一个对象是否表示完全相同的离散搜索状态。</summary>
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
return Equals(obj as HybridAStarNodeKey);
|
||||
}
|
||||
|
||||
/// <summary>返回用于闭集字典的稳定哈希值。</summary>
|
||||
public override int GetHashCode()
|
||||
{
|
||||
unchecked
|
||||
{
|
||||
int hashCode = Row;
|
||||
hashCode = hashCode * 397 ^ Column;
|
||||
hashCode = hashCode * 397 ^ HeadingIndex;
|
||||
hashCode = hashCode * 397 ^ (int)Direction;
|
||||
return hashCode * 397 ^ CurvatureLevelIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,542 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 单次 Hybrid A* 搜索的只读结果。
|
||||
/// 即使失败也会保留已经创建的运行期节点，供上层记录诊断；仅 <see cref="PlanningStatus.Success"/> 时 <see cref="SuccessNodeIndex"/> 有值。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarSearchResult
|
||||
{
|
||||
internal HybridAStarSearchResult(
|
||||
PlanningStatus status,
|
||||
IEnumerable<HybridAStarNode> nodes,
|
||||
int expandedNodeCount,
|
||||
int generatedNodeCount,
|
||||
int reopenedNodeCount,
|
||||
int staleOpenListEntryCount,
|
||||
int peakOpenListCount,
|
||||
int? successNodeIndex,
|
||||
string terminationReason)
|
||||
{
|
||||
Status = status;
|
||||
Nodes = new ReadOnlyCollection<HybridAStarNode>(new List<HybridAStarNode>(nodes ?? Array.Empty<HybridAStarNode>()));
|
||||
ExpandedNodeCount = expandedNodeCount;
|
||||
GeneratedNodeCount = generatedNodeCount;
|
||||
ReopenedNodeCount = reopenedNodeCount;
|
||||
StaleOpenListEntryCount = staleOpenListEntryCount;
|
||||
PeakOpenListCount = peakOpenListCount;
|
||||
SuccessNodeIndex = status == PlanningStatus.Success ? successNodeIndex : null;
|
||||
TerminationReason = status == PlanningStatus.Success ? string.Empty : terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>搜索终止状态。</summary>
|
||||
public PlanningStatus Status { get; }
|
||||
|
||||
/// <summary>本次搜索已经创建的全部节点；索引与 <see cref="HybridAStarNode.NodeIndex"/> 一致。</summary>
|
||||
public IReadOnlyList<HybridAStarNode> Nodes { get; }
|
||||
|
||||
/// <summary>实际从 Open List 弹出并扩展的节点数量。</summary>
|
||||
public int ExpandedNodeCount { get; }
|
||||
|
||||
/// <summary>已进入 Open List 的节点数量，包含根节点和因改进代价追加的节点。</summary>
|
||||
public int GeneratedNodeCount { get; }
|
||||
|
||||
/// <summary>更优路径到达已关闭离散状态、并重新放回 Open List 的次数。</summary>
|
||||
public int ReopenedNodeCount { get; }
|
||||
|
||||
/// <summary>从 Open List 弹出后因已有更优普通状态而被丢弃的陈旧条目数量。</summary>
|
||||
public int StaleOpenListEntryCount { get; }
|
||||
|
||||
/// <summary>搜索期间 Open List 持有的最大条目数，包含等待惰性丢弃的旧条目。</summary>
|
||||
public int PeakOpenListCount { get; }
|
||||
|
||||
/// <summary>成功时最后一个从 Open List 弹出且满足终点条件的节点索引；失败时为 null。</summary>
|
||||
public int? SuccessNodeIndex { get; }
|
||||
|
||||
/// <summary>搜索边界记录的原始终止原因；成功时为空字符串，失败时非空。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在不可变 <see cref="PlanningGridMap"/> 上执行前进/倒车恒曲率原语的 Hybrid A* 搜索。
|
||||
/// 搜索只消费已准备好的地图快照；所有候选原语均先完成连续扩大车体碰撞检查，再参与 Open List 排序。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarSearch
|
||||
{
|
||||
private const double CostImprovementToleranceMeters = 1e-9d;
|
||||
private readonly MotionPrimitiveGenerator _primitiveGenerator;
|
||||
private readonly SearchCostCalculator _costCalculator;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认原语、代价和碰撞检查实现的搜索器。</summary>
|
||||
public HybridAStarSearch()
|
||||
: this(new MotionPrimitiveGenerator(), new SearchCostCalculator(), new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定协作对象的搜索器，便于在不引入地图或 UI 依赖的情况下测试搜索过程。</summary>
|
||||
public HybridAStarSearch(
|
||||
MotionPrimitiveGenerator primitiveGenerator,
|
||||
SearchCostCalculator costCalculator,
|
||||
FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
|
||||
_costCalculator = costCalculator ?? throw new ArgumentNullException(nameof(costCalculator));
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行一次不可变地图上的 Hybrid A* 搜索。
|
||||
/// 参数：request 提供地图、起终点、车辆和搜索配置；cancellationToken 在每次节点扩展前检查。
|
||||
/// 返回：成功仅在满足目标约束的候选已从 Open List 弹出时报告；任一失败状态均不报告成功节点。
|
||||
/// </summary>
|
||||
public HybridAStarSearchResult Search(PlanningRequest request, CancellationToken cancellationToken)
|
||||
{
|
||||
TimeSpan timeout = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? request.Configuration.SearchTimeout
|
||||
: TimeSpan.Zero;
|
||||
PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? new PlanningOperationBudget(cancellationToken, timeout)
|
||||
: PlanningOperationBudget.Unlimited(cancellationToken);
|
||||
return Search(request, budget);
|
||||
}
|
||||
|
||||
/// <summary>使用门面传入的共享预算执行搜索；预算从整次规划开始计时。</summary>
|
||||
internal HybridAStarSearchResult Search(PlanningRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
var nodes = new List<HybridAStarNode>();
|
||||
int expandedNodeCount = 0;
|
||||
int generatedNodeCount = 0;
|
||||
int reopenedNodeCount = 0;
|
||||
int staleOpenListEntryCount = 0;
|
||||
int peakOpenListCount = 0;
|
||||
|
||||
try
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
PlanningStatus validationStatus = ValidateRequest(request);
|
||||
if (validationStatus != PlanningStatus.Success)
|
||||
return CreateResult(validationStatus, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
PlanningGridMap map = request.Map;
|
||||
HybridAStarConfiguration configuration = request.Configuration;
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
|
||||
if (!IsFootprintInsideMap(request.Start, map, vehicle))
|
||||
return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Start, map, vehicle, 0d, out _))
|
||||
return CreateResult(PlanningStatus.StartInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!IsFootprintInsideMap(request.Goal, map, vehicle))
|
||||
return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Goal, map, vehicle, 0d, out _))
|
||||
return CreateResult(PlanningStatus.GoalInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (configuration.MaximumExpandedNodes == 0)
|
||||
return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
if (!map.TryWorldToGrid(request.Goal.X, request.Goal.Y, out int goalRow, out int goalColumn))
|
||||
return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!GridDijkstraHeuristic.TryCreate(map, goalRow, goalColumn, budget, out GridDijkstraHeuristic heuristic, out stopReason))
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return CreateResult(PlanningStatus.InvalidVehicleParameters, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
IReadOnlyList<double> curvatureLevels = _primitiveGenerator.GetCurvatureLevels(vehicle, configuration);
|
||||
if (curvatureLevels.Count == 0)
|
||||
return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
int headingBinCount = GetHeadingBinCount(configuration.HeadingResolutionRadians);
|
||||
int startCurvatureLevelIndex = GetNearestCurvatureLevelIndex(curvatureLevels, request.StartVehicleCurvature);
|
||||
if (startCurvatureLevelIndex < 0)
|
||||
return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
if (!map.TryWorldToGrid(request.Start.X, request.Start.Y, out int startRow, out int startColumn))
|
||||
return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
var openList = new BinaryMinHeap<int>();
|
||||
var bestStates = new Dictionary<HybridAStarNodeKey, NodeLabel>();
|
||||
foreach (TravelDirection startDirection in GetStartDirections(request, configuration))
|
||||
{
|
||||
int headingIndex = AngleMath.ToHeadingIndex(request.Start.Heading, configuration.HeadingResolutionRadians, headingBinCount);
|
||||
var key = new HybridAStarNodeKey(startRow, startColumn, headingIndex, startDirection, startCurvatureLevelIndex);
|
||||
if (bestStates.ContainsKey(key)) continue;
|
||||
|
||||
double hCostMeters = GetHeuristicCost(heuristic, startRow, startColumn, configuration);
|
||||
if (double.IsPositiveInfinity(hCostMeters)) continue;
|
||||
var node = new HybridAStarNode(nodes.Count, key, request.Start, -1, null,
|
||||
curvatureLevels[startCurvatureLevelIndex], 0d, hCostMeters);
|
||||
nodes.Add(node);
|
||||
bestStates.Add(key, new NodeLabel(node.NodeIndex, 0d, false));
|
||||
openList.Push(node.NodeIndex, node.FCostMeters, node.HCostMeters, node.GCostMeters);
|
||||
peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
|
||||
generatedNodeCount++;
|
||||
}
|
||||
|
||||
if (openList.Count == 0)
|
||||
return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null,
|
||||
"二维启发式标记起点不可达目标，或起始方向无法进入 Open List。");
|
||||
|
||||
while (openList.Count > 0)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (expandedNodeCount >= configuration.MaximumExpandedNodes)
|
||||
return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
int nodeIndex = openList.Pop();
|
||||
HybridAStarNode current = nodes[nodeIndex];
|
||||
NodeLabel currentLabel = null;
|
||||
if (!current.IsGoalCandidate)
|
||||
{
|
||||
if (!bestStates.TryGetValue(current.Key, out currentLabel) || currentLabel.NodeIndex != nodeIndex || currentLabel.IsClosed)
|
||||
{
|
||||
staleOpenListEntryCount++;
|
||||
continue;
|
||||
}
|
||||
|
||||
currentLabel.IsClosed = true;
|
||||
}
|
||||
|
||||
expandedNodeCount++;
|
||||
if (current.IsGoalCandidate)
|
||||
{
|
||||
if (!GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection) ||
|
||||
!_collisionChecker.IsPoseCollisionFree(current.Pose, map, vehicle, 0d, out _))
|
||||
continue;
|
||||
|
||||
return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
|
||||
}
|
||||
|
||||
if (GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection))
|
||||
return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
|
||||
|
||||
for (int curvatureLevelIndex = 0; curvatureLevelIndex < curvatureLevels.Count; curvatureLevelIndex++)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!MotionPrimitiveGenerator.AreCurvatureLevelsAdjacent(current.CurvatureLevelIndex, curvatureLevelIndex)) continue;
|
||||
|
||||
foreach (TravelDirection direction in GetSuccessorDirections(configuration))
|
||||
{
|
||||
MotionPrimitive primitive = _primitiveGenerator.Generate(current.Pose, curvatureLevels[curvatureLevelIndex], direction, request);
|
||||
if (primitive == null || primitive.ActualLengthMeters <= 0d) continue;
|
||||
|
||||
if (!map.TryWorldToGrid(primitive.End.X, primitive.End.Y, out int row, out int column)) continue;
|
||||
double hCostMeters = GetHeuristicCost(heuristic, row, column, configuration);
|
||||
if (double.IsPositiveInfinity(hCostMeters)) continue;
|
||||
|
||||
double bodyClearanceMeters = GetMinimumBodyClearance(primitive);
|
||||
bool isGearSwitch = current.ParentNodeIndex >= 0 && current.Direction != direction;
|
||||
int curvatureLevelDelta = curvatureLevelIndex - current.CurvatureLevelIndex;
|
||||
double incrementalCostMeters = _costCalculator.Calculate(
|
||||
primitive.ActualLengthMeters,
|
||||
direction,
|
||||
isGearSwitch,
|
||||
curvatureLevels[curvatureLevelIndex],
|
||||
maximumCurvaturePerMeter,
|
||||
curvatureLevelDelta,
|
||||
bodyClearanceMeters,
|
||||
configuration);
|
||||
double gCostMeters = current.GCostMeters + incrementalCostMeters;
|
||||
if (!NumericGuard.IsFinite(gCostMeters) || gCostMeters < 0d) continue;
|
||||
|
||||
int headingIndex = AngleMath.ToHeadingIndex(primitive.End.Heading, configuration.HeadingResolutionRadians, headingBinCount);
|
||||
var key = new HybridAStarNodeKey(row, column, headingIndex, direction, curvatureLevelIndex);
|
||||
bestStates.TryGetValue(key, out NodeLabel existingLabel);
|
||||
var successor = new HybridAStarNode(nodes.Count, key, primitive.End, current.NodeIndex, primitive,
|
||||
curvatureLevels[curvatureLevelIndex], gCostMeters, hCostMeters, primitive.IsGoalTruncation);
|
||||
if (existingLabel != null && !ShouldEnqueueSuccessor(successor, existingLabel.GCostMeters)) continue;
|
||||
|
||||
if (!successor.IsGoalCandidate && existingLabel != null && existingLabel.IsClosed) reopenedNodeCount++;
|
||||
nodes.Add(successor);
|
||||
if (!successor.IsGoalCandidate)
|
||||
bestStates[key] = new NodeLabel(successor.NodeIndex, gCostMeters, false);
|
||||
openList.Push(successor.NodeIndex, successor.FCostMeters, successor.HCostMeters, successor.GCostMeters);
|
||||
peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
|
||||
generatedNodeCount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null,
|
||||
"Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
string reason = "Hybrid A* 搜索内部错误：" + exception.GetType().Name +
|
||||
(string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
|
||||
return CreateResult(PlanningStatus.InternalError, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null, reason);
|
||||
}
|
||||
}
|
||||
|
||||
private static HybridAStarSearchResult CreateResult(
|
||||
PlanningStatus status,
|
||||
IEnumerable<HybridAStarNode> nodes,
|
||||
int expandedNodeCount,
|
||||
int generatedNodeCount,
|
||||
int reopenedNodeCount,
|
||||
int staleOpenListEntryCount,
|
||||
int peakOpenListCount,
|
||||
int? successNodeIndex,
|
||||
string terminationReason = null)
|
||||
{
|
||||
string reason = status == PlanningStatus.Success
|
||||
? string.Empty
|
||||
: terminationReason ?? GetDefaultTerminationReason(status);
|
||||
return new HybridAStarSearchResult(status, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, successNodeIndex, reason);
|
||||
}
|
||||
|
||||
private static string GetDefaultTerminationReason(PlanningStatus status)
|
||||
{
|
||||
switch (status)
|
||||
{
|
||||
case PlanningStatus.Cancelled:
|
||||
return "Hybrid A* 搜索已取消。";
|
||||
case PlanningStatus.InvalidRequest:
|
||||
return "Hybrid A* 搜索请求缺少必要对象或包含非法数值。";
|
||||
case PlanningStatus.InvalidMap:
|
||||
return "Hybrid A* 搜索地图结构无效。";
|
||||
case PlanningStatus.MapNotReady:
|
||||
return "Hybrid A* 搜索地图尚未准备好。";
|
||||
case PlanningStatus.InvalidVehicleParameters:
|
||||
return "Hybrid A* 搜索车辆参数无效。";
|
||||
case PlanningStatus.InvalidCurvatureConfiguration:
|
||||
return "Hybrid A* 搜索曲率、离散、代价或资源配置无效。";
|
||||
case PlanningStatus.StartOutsideMap:
|
||||
return "Hybrid A* 搜索起始扩大车体不完全位于地图内。";
|
||||
case PlanningStatus.StartInCollision:
|
||||
return "Hybrid A* 搜索起始扩大车体与障碍物相交或擦边。";
|
||||
case PlanningStatus.GoalOutsideMap:
|
||||
return "Hybrid A* 搜索目标扩大车体不完全位于地图内。";
|
||||
case PlanningStatus.GoalInCollision:
|
||||
return "Hybrid A* 搜索目标扩大车体与障碍物相交或擦边。";
|
||||
case PlanningStatus.SearchTimeout:
|
||||
return "Hybrid A* 搜索使用的总规划预算已耗尽。";
|
||||
case PlanningStatus.SearchNodeLimitExceeded:
|
||||
return "Hybrid A* 搜索达到扩展节点上限。";
|
||||
case PlanningStatus.NoFeasiblePath:
|
||||
return "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。";
|
||||
case PlanningStatus.BacktrackingFailed:
|
||||
return "Hybrid A* 成功节点无法回溯为完整父链。";
|
||||
case PlanningStatus.FinalValidationFailed:
|
||||
return "Hybrid A* 路径未通过最终复核。";
|
||||
case PlanningStatus.InternalError:
|
||||
return "Hybrid A* 搜索发生未预期内部错误。";
|
||||
default:
|
||||
return "Hybrid A* 搜索以未识别状态终止：" + status + "。";
|
||||
}
|
||||
}
|
||||
|
||||
private static PlanningStatus ValidateRequest(PlanningRequest request)
|
||||
{
|
||||
if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
|
||||
!IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
|
||||
!IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
|
||||
return PlanningStatus.InvalidRequest;
|
||||
|
||||
PlanningGridMap map = request.Map;
|
||||
if (map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters) || map.Bounds == null)
|
||||
return PlanningStatus.InvalidMap;
|
||||
if (!map.PlanningReady) return PlanningStatus.MapNotReady;
|
||||
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
|
||||
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return PlanningStatus.InvalidVehicleParameters;
|
||||
|
||||
HybridAStarConfiguration configuration = request.Configuration;
|
||||
if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
|
||||
(request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
|
||||
return PlanningStatus.InvalidCurvatureConfiguration;
|
||||
|
||||
return PlanningStatus.Success;
|
||||
}
|
||||
|
||||
private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
|
||||
{
|
||||
return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
|
||||
configuration.HeadingResolutionRadians <= 2d * Math.PI &&
|
||||
configuration.CurvatureLevelCount >= 3 && configuration.CurvatureLevelCount % 2 == 1 &&
|
||||
NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) && configuration.GoalPositionToleranceMeters >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) && configuration.GoalHeadingToleranceRadians >= 0d &&
|
||||
configuration.MaximumExpandedNodes >= 0 && configuration.SearchTimeout >= TimeSpan.Zero &&
|
||||
NumericGuard.IsFinite(configuration.HeuristicWeight) && configuration.HeuristicWeight >= 0d &&
|
||||
NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
|
||||
NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) && configuration.CurvatureChangePenaltyMetersPerLevel >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.ClearanceCostWeight) && configuration.ClearanceCostWeight >= 0d &&
|
||||
NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
|
||||
}
|
||||
|
||||
private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
|
||||
{
|
||||
if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
|
||||
|
||||
double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double longitudinalX = Math.Cos(pose.Heading);
|
||||
double longitudinalY = Math.Sin(pose.Heading);
|
||||
double lateralX = -longitudinalY;
|
||||
double lateralY = longitudinalX;
|
||||
for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
|
||||
for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
|
||||
{
|
||||
double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
|
||||
double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
|
||||
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IEnumerable<TravelDirection> GetStartDirections(PlanningRequest request, HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (request.StartDirection.HasValue)
|
||||
{
|
||||
yield return request.StartDirection.Value;
|
||||
yield break;
|
||||
}
|
||||
|
||||
yield return TravelDirection.Forward;
|
||||
if (configuration.AllowReverse) yield return TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static IEnumerable<TravelDirection> GetSuccessorDirections(HybridAStarConfiguration configuration)
|
||||
{
|
||||
yield return TravelDirection.Forward;
|
||||
if (configuration.AllowReverse) yield return TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static int GetHeadingBinCount(double headingResolutionRadians)
|
||||
{
|
||||
double rawBinCount = Math.Ceiling(2d * Math.PI / headingResolutionRadians);
|
||||
if (!NumericGuard.IsFinite(rawBinCount) || rawBinCount < 1d || rawBinCount > int.MaxValue)
|
||||
throw new ArgumentOutOfRangeException(nameof(headingResolutionRadians));
|
||||
return (int)rawBinCount;
|
||||
}
|
||||
|
||||
private static int GetNearestCurvatureLevelIndex(IReadOnlyList<double> curvatureLevels, double curvaturePerMeter)
|
||||
{
|
||||
int bestIndex = -1;
|
||||
double smallestDifference = double.PositiveInfinity;
|
||||
for (int index = 0; index < curvatureLevels.Count; index++)
|
||||
{
|
||||
double difference = Math.Abs(curvatureLevels[index] - curvaturePerMeter);
|
||||
if (difference >= smallestDifference) continue;
|
||||
smallestDifference = difference;
|
||||
bestIndex = index;
|
||||
}
|
||||
return bestIndex;
|
||||
}
|
||||
|
||||
private static double GetMinimumBodyClearance(MotionPrimitive primitive)
|
||||
{
|
||||
double minimum = double.PositiveInfinity;
|
||||
foreach (double clearanceMeters in primitive.BodyClearancesMeters)
|
||||
{
|
||||
if (double.IsNaN(clearanceMeters) || clearanceMeters < 0d) return 0d;
|
||||
minimum = Math.Min(minimum, clearanceMeters);
|
||||
}
|
||||
return minimum;
|
||||
}
|
||||
|
||||
private static double GetHeuristicCost(GridDijkstraHeuristic heuristic, int row, int column, HybridAStarConfiguration configuration)
|
||||
{
|
||||
double gridCostMeters = heuristic.GetCost(row, column);
|
||||
if (double.IsPositiveInfinity(gridCostMeters)) return gridCostMeters;
|
||||
double weightedCostMeters = gridCostMeters * configuration.HeuristicWeight;
|
||||
return NumericGuard.IsFinite(weightedCostMeters) && weightedCostMeters >= 0d
|
||||
? weightedCostMeters
|
||||
: double.PositiveInfinity;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断后继是否应进入 Open List。
|
||||
/// 参数：successor 是已完成连续碰撞检查的后继；bestKnownGCostMeters 是相同离散键普通状态的当前最优 G 值。
|
||||
/// 返回：普通状态仅在严格改善最优 G 值时入队；终点候选始终入队，以保留其未量化的连续终点位姿。
|
||||
/// </summary>
|
||||
private static bool ShouldEnqueueSuccessor(HybridAStarNode successor, double bestKnownGCostMeters)
|
||||
{
|
||||
if (successor == null || !NumericGuard.IsFinite(bestKnownGCostMeters)) return false;
|
||||
return successor.IsGoalCandidate || successor.GCostMeters < bestKnownGCostMeters - CostImprovementToleranceMeters;
|
||||
}
|
||||
|
||||
private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
|
||||
if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
|
||||
throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
|
||||
private sealed class NodeLabel
|
||||
{
|
||||
public NodeLabel(int nodeIndex, double gCostMeters, bool isClosed)
|
||||
{
|
||||
NodeIndex = nodeIndex;
|
||||
GCostMeters = gCostMeters;
|
||||
IsClosed = isClosed;
|
||||
}
|
||||
|
||||
public int NodeIndex { get; }
|
||||
public double GCostMeters { get; }
|
||||
public bool IsClosed { get; set; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
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; }
|
||||
}
|
||||
@@ -0,0 +1,173 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 生成经解析积分和连续碰撞检查的前进或倒车恒曲率原语。
|
||||
/// 每个积分点均依次经过有限数值检查、与前一点之间的扫掠碰撞检查和终点容差检查。
|
||||
/// </summary>
|
||||
public sealed class MotionPrimitiveGenerator
|
||||
{
|
||||
private const double StraightCurvatureThreshold = 1e-12d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认车辆连续碰撞检查器的原语生成器。</summary>
|
||||
public MotionPrimitiveGenerator()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车辆碰撞检查器的原语生成器。</summary>
|
||||
public MotionPrimitiveGenerator(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用完整规划请求生成一条原语。
|
||||
/// 参数：start 为当前连续位姿；curvaturePerMeter 为候选恒定曲率；direction 为前进或倒车；request 提供地图、车辆、配置和目标。
|
||||
/// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；否则返回最大长度不超过配置上限的原语。
|
||||
/// </summary>
|
||||
public MotionPrimitive Generate(Pose2D start, double curvaturePerMeter, TravelDirection direction, PlanningRequest request)
|
||||
{
|
||||
if (request == null) return null;
|
||||
return Generate(start, curvaturePerMeter, direction, request.Map, request.Vehicle, request.Configuration, request.Goal, request.GoalDirection);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用显式地图、车辆、配置和目标生成一条原语。
|
||||
/// 参数：所有位置使用 m/rad，curvaturePerMeter 使用 1/m；goalDirection 限制末段允许的进入方向。
|
||||
/// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；首次命中目标时返回 <see cref="MotionPrimitive.IsGoalTruncation"/> 为 true 的截断原语。
|
||||
/// </summary>
|
||||
public MotionPrimitive Generate(
|
||||
Pose2D start,
|
||||
double curvaturePerMeter,
|
||||
TravelDirection direction,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
HybridAStarConfiguration configuration,
|
||||
Pose2D goal,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsValidInput(start, curvaturePerMeter, direction, map, vehicle, configuration, goal, goalDirection)) return null;
|
||||
|
||||
if (GoalToleranceChecker.IsSatisfied(start, goal, configuration, direction, goalDirection))
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, 0d, Array.Empty<Pose2D>(), Array.Empty<double>(), true);
|
||||
|
||||
double pointStepMeters = Math.Min(configuration.IntegrationStepMeters,
|
||||
Math.Min(configuration.MaximumCollisionCheckStepMeters, map.ResolutionMeters / 2d));
|
||||
if (!NumericGuard.IsPositiveFinite(pointStepMeters)) return null;
|
||||
|
||||
var points = new List<Pose2D>();
|
||||
var bodyClearancesMeters = new List<double>();
|
||||
Pose2D previous = start;
|
||||
double actualLengthMeters = 0d;
|
||||
while (actualLengthMeters < configuration.PrimitiveLengthMeters)
|
||||
{
|
||||
double remainingLengthMeters = configuration.PrimitiveLengthMeters - actualLengthMeters;
|
||||
double stepMeters = Math.Min(pointStepMeters, remainingLengthMeters);
|
||||
if (!NumericGuard.IsPositiveFinite(stepMeters)) return null;
|
||||
|
||||
Pose2D next = Integrate(previous, curvaturePerMeter, direction, stepMeters);
|
||||
if (!IsFinitePose(next)) return null;
|
||||
if (!_collisionChecker.IsSweptMotionCollisionFree(previous, next, map, vehicle, stepMeters, out double bodyClearanceMeters)) return null;
|
||||
|
||||
actualLengthMeters += stepMeters;
|
||||
points.Add(next);
|
||||
bodyClearancesMeters.Add(bodyClearanceMeters);
|
||||
if (GoalToleranceChecker.IsSatisfied(next, goal, configuration, direction, goalDirection))
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, true);
|
||||
|
||||
previous = next;
|
||||
}
|
||||
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, false);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取从最大负曲率到最大正曲率均匀分布的候选曲率等级。
|
||||
/// 参数：vehicle 提供保守最大曲率；configuration 的曲率等级数必须为不小于 3 的奇数。
|
||||
/// 返回：输入无效时为空只读列表；有效时长度等于配置等级数且中间等级恒为零曲率。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> GetCurvatureLevels(VehicleParameters vehicle, HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (configuration == null || configuration.CurvatureLevelCount < 3 || configuration.CurvatureLevelCount % 2 == 0 ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return Array.Empty<double>();
|
||||
|
||||
var levels = new double[configuration.CurvatureLevelCount];
|
||||
double increment = 2d * maximumCurvaturePerMeter / (levels.Length - 1d);
|
||||
for (int index = 0; index < levels.Length; index++) levels[index] = -maximumCurvaturePerMeter + increment * index;
|
||||
levels[levels.Length / 2] = 0d;
|
||||
return levels;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两个曲率等级是否允许在相邻原语间直接切换。
|
||||
/// 参数：previousLevelIndex 与 nextLevelIndex 为从零开始的等级索引。
|
||||
/// 返回：两个索引均非负且最多相差一个等级时为 true。
|
||||
/// </summary>
|
||||
public static bool AreCurvatureLevelsAdjacent(int previousLevelIndex, int nextLevelIndex)
|
||||
{
|
||||
return previousLevelIndex >= 0 && nextLevelIndex >= 0 && Math.Abs(previousLevelIndex - nextLevelIndex) <= 1;
|
||||
}
|
||||
|
||||
private static bool IsValidInput(
|
||||
Pose2D start,
|
||||
double curvaturePerMeter,
|
||||
TravelDirection direction,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
HybridAStarConfiguration configuration,
|
||||
Pose2D goal,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsFinitePose(start) || !IsFinitePose(goal) || map == null || vehicle == null || configuration == null ||
|
||||
!NumericGuard.IsFinite(curvaturePerMeter) || !IsTravelDirection(direction) || !IsGoalDirection(goalDirection) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(map.ResolutionMeters) ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return false;
|
||||
|
||||
return Math.Abs(curvaturePerMeter) <= maximumCurvaturePerMeter;
|
||||
}
|
||||
|
||||
private static Pose2D Integrate(Pose2D previous, double curvaturePerMeter, TravelDirection direction, double stepMeters)
|
||||
{
|
||||
double signedDistanceMeters = direction == TravelDirection.Forward ? stepMeters : -stepMeters;
|
||||
double nextHeadingRadians = AngleMath.NormalizeRadians(previous.Heading + curvaturePerMeter * signedDistanceMeters);
|
||||
if (Math.Abs(curvaturePerMeter) < StraightCurvatureThreshold)
|
||||
{
|
||||
return new Pose2D(
|
||||
previous.X + signedDistanceMeters * Math.Cos(previous.Heading),
|
||||
previous.Y + signedDistanceMeters * Math.Sin(previous.Heading),
|
||||
nextHeadingRadians);
|
||||
}
|
||||
|
||||
return new Pose2D(
|
||||
previous.X + (Math.Sin(nextHeadingRadians) - Math.Sin(previous.Heading)) / curvaturePerMeter,
|
||||
previous.Y - (Math.Cos(nextHeadingRadians) - Math.Cos(previous.Heading)) / curvaturePerMeter,
|
||||
nextHeadingRadians);
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 将运动原语的长度、方向、曲率和净空转换为统一的等效米搜索代价。
|
||||
/// 此类型不修改搜索状态；换向标记必须由调用方依据相邻原语的方向关系提供。
|
||||
/// </summary>
|
||||
public sealed class SearchCostCalculator
|
||||
{
|
||||
/// <summary>创建使用调用时配置参数的等效米代价计算器。</summary>
|
||||
public SearchCostCalculator()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算一条运动原语的等效米增量代价。
|
||||
/// 参数：lengthMeters 为非负弧长 m；direction 为原语方向；isGearSwitch 表示该原语前是否换向；
|
||||
/// curvaturePerMeter 与 maximumCurvaturePerMeter 的单位为 1/m；curvatureLevelDelta 为相邻曲率等级差；
|
||||
/// bodyClearanceMeters 为非负车体保守净空 m，可为正无穷；configuration 提供非负权重和惩罚。
|
||||
/// 返回：有限且非负的等效米代价。
|
||||
/// 失败：任一数值、方向或权重无效时抛出 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
public double Calculate(
|
||||
double lengthMeters,
|
||||
TravelDirection direction,
|
||||
bool isGearSwitch,
|
||||
double curvaturePerMeter,
|
||||
double maximumCurvaturePerMeter,
|
||||
int curvatureLevelDelta,
|
||||
double bodyClearanceMeters,
|
||||
HybridAStarConfiguration configuration)
|
||||
{
|
||||
ValidateInput(lengthMeters, direction, curvaturePerMeter, maximumCurvaturePerMeter, bodyClearanceMeters, configuration);
|
||||
|
||||
double directionMultiplier = direction == TravelDirection.Reverse ? configuration.ReverseCostMultiplier : 1d;
|
||||
double normalizedCurvature = Math.Abs(curvaturePerMeter / maximumCurvaturePerMeter);
|
||||
double clearanceDeficit = GetClearanceDeficit(bodyClearanceMeters, configuration.ClearanceCostDistanceMeters);
|
||||
double levelDelta = Math.Abs((double)curvatureLevelDelta);
|
||||
double motionCost = lengthMeters * directionMultiplier *
|
||||
(1d + configuration.CurvatureMagnitudeWeight * normalizedCurvature +
|
||||
configuration.ClearanceCostWeight * clearanceDeficit);
|
||||
double gearSwitchCost = isGearSwitch ? configuration.GearSwitchPenaltyMeters : 0d;
|
||||
double curvatureChangeCost = configuration.CurvatureChangePenaltyMetersPerLevel * levelDelta;
|
||||
double totalCost = motionCost + gearSwitchCost + curvatureChangeCost;
|
||||
if (!NumericGuard.IsFinite(totalCost) || totalCost < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(lengthMeters), "The calculated cost must remain finite and non-negative.");
|
||||
|
||||
return totalCost;
|
||||
}
|
||||
|
||||
private static void ValidateInput(
|
||||
double lengthMeters,
|
||||
TravelDirection direction,
|
||||
double curvaturePerMeter,
|
||||
double maximumCurvaturePerMeter,
|
||||
double bodyClearanceMeters,
|
||||
HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(lengthMeters) || lengthMeters < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(lengthMeters));
|
||||
if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse)
|
||||
throw new ArgumentOutOfRangeException(nameof(direction));
|
||||
if (!NumericGuard.IsFinite(curvaturePerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
|
||||
if (!NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(maximumCurvaturePerMeter));
|
||||
if (Math.Abs(curvaturePerMeter) > maximumCurvaturePerMeter)
|
||||
throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
|
||||
if (double.IsNaN(bodyClearanceMeters) || bodyClearanceMeters < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(bodyClearanceMeters));
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
|
||||
ValidateNonNegativeFinite(configuration.HeuristicWeight, nameof(configuration.HeuristicWeight));
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier))
|
||||
throw new ArgumentOutOfRangeException(nameof(configuration.ReverseCostMultiplier));
|
||||
ValidateNonNegativeFinite(configuration.GearSwitchPenaltyMeters, nameof(configuration.GearSwitchPenaltyMeters));
|
||||
ValidateNonNegativeFinite(configuration.CurvatureMagnitudeWeight, nameof(configuration.CurvatureMagnitudeWeight));
|
||||
ValidateNonNegativeFinite(configuration.CurvatureChangePenaltyMetersPerLevel, nameof(configuration.CurvatureChangePenaltyMetersPerLevel));
|
||||
ValidateNonNegativeFinite(configuration.ClearanceCostWeight, nameof(configuration.ClearanceCostWeight));
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(configuration.ClearanceCostDistanceMeters));
|
||||
}
|
||||
|
||||
private static void ValidateNonNegativeFinite(double value, string parameterName)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
}
|
||||
|
||||
private static double GetClearanceDeficit(double bodyClearanceMeters, double clearanceCostDistanceMeters)
|
||||
{
|
||||
if (double.IsPositiveInfinity(bodyClearanceMeters)) return 0d;
|
||||
double deficit = 1d - bodyClearanceMeters / clearanceCostDistanceMeters;
|
||||
return deficit > 0d ? deficit : 0d;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,507 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
|
||||
/// <summary>Clumsy 粗路径手动测试可选择的固定场景。</summary>
|
||||
public enum CoarsePathTestScenario
|
||||
{
|
||||
/// <summary>明确允许的空地图直达场景。</summary>
|
||||
ExplicitEmpty,
|
||||
|
||||
/// <summary>由中央矩形阻断直线的绕行场景。</summary>
|
||||
RectangleDetour,
|
||||
|
||||
/// <summary>同时包含手工圆形、矩形与 TwoLeg 快照的多来源场景。</summary>
|
||||
ManualAndTwoLeg,
|
||||
|
||||
/// <summary>与矩形绕行输入完全一致，用于在同一服务中验证输入缓存命中。</summary>
|
||||
CacheHit,
|
||||
|
||||
/// <summary>起步前进、终点倒车进入的换向场景。</summary>
|
||||
ReverseGearSwitch,
|
||||
|
||||
/// <summary>由贯穿边界的障碍带分隔起终点的无解场景。</summary>
|
||||
NoFeasiblePath,
|
||||
}
|
||||
|
||||
/// <summary>手动障碍物输入支持的世界几何类型。</summary>
|
||||
public enum ManualCoarsePathObstacleKind
|
||||
{
|
||||
/// <summary>由世界中心和半径定义的圆形障碍物。</summary>
|
||||
Circle,
|
||||
|
||||
/// <summary>由世界中心、X 方向长度和 Y 方向宽度定义的轴对齐矩形障碍物。</summary>
|
||||
AxisAlignedRectangle,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 手动粗路径测试的不可变障碍物输入。
|
||||
/// 所有中心和尺寸均使用世界 mm；矩形始终与世界坐标轴平行，不包含旋转角。
|
||||
/// </summary>
|
||||
public sealed class ManualCoarsePathObstacle
|
||||
{
|
||||
private ManualCoarsePathObstacle(ManualCoarsePathObstacleKind kind, double centerXMillimeters,
|
||||
double centerYMillimeters, double sizeXMillimeters, double sizeYMillimeters)
|
||||
{
|
||||
Kind = kind;
|
||||
CenterXMillimeters = centerXMillimeters;
|
||||
CenterYMillimeters = centerYMillimeters;
|
||||
SizeXMillimeters = sizeXMillimeters;
|
||||
SizeYMillimeters = sizeYMillimeters;
|
||||
}
|
||||
|
||||
/// <summary>障碍物的支持几何类型。</summary>
|
||||
public ManualCoarsePathObstacleKind Kind { get; }
|
||||
|
||||
/// <summary>几何中心世界 X 坐标，单位 mm。</summary>
|
||||
public double CenterXMillimeters { get; }
|
||||
|
||||
/// <summary>几何中心世界 Y 坐标，单位 mm。</summary>
|
||||
public double CenterYMillimeters { get; }
|
||||
|
||||
/// <summary>圆形时为半径，矩形时为 X 方向长度；单位 mm。</summary>
|
||||
public double SizeXMillimeters { get; }
|
||||
|
||||
/// <summary>圆形时为半径，矩形时为 Y 方向宽度；单位 mm。</summary>
|
||||
public double SizeYMillimeters { get; }
|
||||
|
||||
/// <summary>创建圆形障碍物。参数：圆心和半径均使用世界 mm，半径必须为有限正数。</summary>
|
||||
public static ManualCoarsePathObstacle Circle(double centerXMillimeters, double centerYMillimeters,
|
||||
double radiusMillimeters)
|
||||
{
|
||||
EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
|
||||
EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
|
||||
EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
|
||||
return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.Circle, centerXMillimeters,
|
||||
centerYMillimeters, radiusMillimeters, radiusMillimeters);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建轴对齐矩形障碍物。
|
||||
/// 参数：中心、X 方向长度和 Y 方向宽度均使用世界 mm；两个尺寸必须为有限正数。
|
||||
/// </summary>
|
||||
public static ManualCoarsePathObstacle AxisAlignedRectangle(double centerXMillimeters,
|
||||
double centerYMillimeters, double lengthXMillimeters, double widthYMillimeters)
|
||||
{
|
||||
EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
|
||||
EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
|
||||
EnsurePositiveFinite(lengthXMillimeters, nameof(lengthXMillimeters));
|
||||
EnsurePositiveFinite(widthYMillimeters, nameof(widthYMillimeters));
|
||||
return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.AxisAlignedRectangle,
|
||||
centerXMillimeters, centerYMillimeters, lengthXMillimeters, widthYMillimeters);
|
||||
}
|
||||
|
||||
private static void EnsureFinite(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
|
||||
}
|
||||
|
||||
private static void EnsurePositiveFinite(double value, string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
if (value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clumsy 粗路径测试的纯输入工厂。
|
||||
/// 固定场景不读取 UI、传感器、定位或时钟；传入 AMR 位姿的手动入口仅在此处完成世界 mm/deg 到核心 m/rad 的转换。
|
||||
/// </summary>
|
||||
public static class CoarsePathScenarioFactory
|
||||
{
|
||||
private const float MapXMinMillimeters = 0f;
|
||||
private const float MapXMaxMillimeters = 6000f;
|
||||
private const float MapYMinMillimeters = 0f;
|
||||
private const float MapYMaxMillimeters = 4000f;
|
||||
private const float ResolutionMillimeters = 50f;
|
||||
private const double MillimetersPerMeter = 1000d;
|
||||
private const double DegreesToRadians = Math.PI / 180d;
|
||||
private const double ManualMapPaddingMillimeters = 8000d;
|
||||
private const int MaximumManualObstacleCount = 20;
|
||||
|
||||
/// <summary>
|
||||
/// 创建一个新的固定测试业务请求。
|
||||
/// 返回：每次调用都返回独立的可变请求对象，供调用方安全地传入同一个长期存活的规划服务。
|
||||
/// </summary>
|
||||
public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario)
|
||||
{
|
||||
return CreateCore(scenario, null);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建以当前 AMR 世界位姿为起点的固定测试业务请求。
|
||||
/// 参数：X/Y 使用世界 mm，航向使用 deg；地图、目标和障碍物仅随 AMR 坐标平移，TwoLeg 朝向保持不变。
|
||||
/// </summary>
|
||||
public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario,
|
||||
double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)
|
||||
{
|
||||
return CreateCore(scenario,
|
||||
new FixedScenarioAnchor(amrXMillimeters, amrYMillimeters, amrHeadingDegrees));
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateCore(CoarsePathTestScenario scenario, FixedScenarioAnchor anchor)
|
||||
{
|
||||
switch (scenario)
|
||||
{
|
||||
case CoarsePathTestScenario.ExplicitEmpty:
|
||||
return CreateExplicitEmpty(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
||||
case CoarsePathTestScenario.RectangleDetour:
|
||||
case CoarsePathTestScenario.CacheHit:
|
||||
return CreateRectangleDetour(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
||||
case CoarsePathTestScenario.ManualAndTwoLeg:
|
||||
return CreateManualAndTwoLeg(FixedScenarioTransform.From(1000d, 1000d, 0d, anchor));
|
||||
case CoarsePathTestScenario.ReverseGearSwitch:
|
||||
return CreateReverseGearSwitch(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
||||
case CoarsePathTestScenario.NoFeasiblePath:
|
||||
return CreateNoFeasiblePath(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建传入 AMR 世界位姿和手动世界终点的空图演示请求。
|
||||
/// 参数：X/Y 使用世界 mm，航向使用 deg；返回请求中的 <see cref="Pose2D"/> 使用世界 m/rad。
|
||||
/// 注意：这是坐标、路径和取消流程的演示空图，不能表示现场不存在障碍物。
|
||||
/// </summary>
|
||||
public static CoarsePathPlanningJob CreateManualGoalDemo(
|
||||
double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
|
||||
double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees)
|
||||
{
|
||||
return CreateManualObstacleDemo(startXMillimeters, startYMillimeters, startHeadingDegrees,
|
||||
goalXMillimeters, goalYMillimeters, goalHeadingDegrees,
|
||||
Array.Empty<ManualCoarsePathObstacle>(), 0L);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建传入 AMR 世界位姿、手动世界终点和手动障碍物快照的测试请求。
|
||||
/// 参数：位姿 X/Y、障碍物中心和尺寸使用世界 mm，航向使用 deg；返回的 <see cref="Pose2D"/> 使用 m/rad。
|
||||
/// 障碍物非空时 obstacleSnapshotVersion 必须为正数，以避免长期服务错误复用旧地图；零障碍物才创建显式空图演示。
|
||||
/// </summary>
|
||||
public static CoarsePathPlanningJob CreateManualObstacleDemo(
|
||||
double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
|
||||
double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees,
|
||||
IReadOnlyList<ManualCoarsePathObstacle> obstacles, long obstacleSnapshotVersion)
|
||||
{
|
||||
EnsureFinite(startXMillimeters, nameof(startXMillimeters));
|
||||
EnsureFinite(startYMillimeters, nameof(startYMillimeters));
|
||||
EnsureFinite(startHeadingDegrees, nameof(startHeadingDegrees));
|
||||
EnsureFinite(goalXMillimeters, nameof(goalXMillimeters));
|
||||
EnsureFinite(goalYMillimeters, nameof(goalYMillimeters));
|
||||
EnsureFinite(goalHeadingDegrees, nameof(goalHeadingDegrees));
|
||||
|
||||
if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
|
||||
if (obstacles.Count > MaximumManualObstacleCount)
|
||||
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle count exceeds the supported limit.");
|
||||
if (obstacles.Count != 0 && obstacleSnapshotVersion <= 0L)
|
||||
throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");
|
||||
|
||||
return CreateJob(
|
||||
CreateManualDemoMap(startXMillimeters, startYMillimeters, goalXMillimeters, goalYMillimeters,
|
||||
obstacles, obstacleSnapshotVersion),
|
||||
ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
|
||||
ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees),
|
||||
null,
|
||||
GoalDirectionConstraint.Any);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateExplicitEmpty(FixedScenarioTransform transform)
|
||||
{
|
||||
return CreateJob(
|
||||
CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
|
||||
transform.Pose(1d, 2d, 0d),
|
||||
transform.Pose(5d, 2d, 0d),
|
||||
null,
|
||||
GoalDirectionConstraint.Forward);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateRectangleDetour(FixedScenarioTransform transform)
|
||||
{
|
||||
IMapObstacleSource[] sources =
|
||||
{
|
||||
new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(transform.X(2700f), transform.X(3300f),
|
||||
transform.Y(1200f), transform.Y(2800f)),
|
||||
}),
|
||||
};
|
||||
CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
|
||||
transform.Pose(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
|
||||
// 固定绕行场景保留最优启发式；30 秒覆盖较慢测试环境，UI 仍可随时取消。
|
||||
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
|
||||
return job;
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateManualAndTwoLeg(FixedScenarioTransform transform)
|
||||
{
|
||||
IMapObstacleSource[] sources =
|
||||
{
|
||||
new ManualObstacleSource("manual", 2L, true, new IMapObstacle[]
|
||||
{
|
||||
new CircleObstacle(transform.X(2400f), transform.Y(1300f), 220f),
|
||||
new AxisAlignedRectangleObstacle(transform.X(3000f), transform.X(3600f),
|
||||
transform.Y(2000f), transform.Y(2600f)),
|
||||
}),
|
||||
new TwoLegObstacleSource("two-leg", 1L, true, new TwoLegProjectionInput(true,
|
||||
transform.X(3900f), transform.Y(2500f), 0d,
|
||||
-180f, -180f, -180f, 180f, 140f, "P1 fixed TwoLeg snapshot.")),
|
||||
};
|
||||
CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 1d, 0d),
|
||||
transform.Pose(5d, 3d, 0d), null, GoalDirectionConstraint.Forward);
|
||||
// 多来源场景的最优绕行会受机器负载影响；放宽演示总预算但保留全部碰撞与目标判定。
|
||||
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(15d);
|
||||
return job;
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateReverseGearSwitch(FixedScenarioTransform transform)
|
||||
{
|
||||
return CreateJob(
|
||||
CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
|
||||
transform.Pose(1d, 2d, 0d),
|
||||
transform.Pose(4d, 2d, 0d),
|
||||
TravelDirection.Forward,
|
||||
GoalDirectionConstraint.Reverse);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateNoFeasiblePath(FixedScenarioTransform transform)
|
||||
{
|
||||
IMapObstacleSource[] sources =
|
||||
{
|
||||
new ManualObstacleSource("manual", 3L, true, new IMapObstacle[]
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(transform.X(2900f), transform.X(3100f),
|
||||
transform.Y(0f), transform.Y(4000f)),
|
||||
}),
|
||||
};
|
||||
return CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
|
||||
transform.Pose(5d, 2d, 0d),
|
||||
null, GoalDirectionConstraint.Forward);
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateJob(PlanningMapRequest mapRequest, Pose2D start, Pose2D goal,
|
||||
TravelDirection? startDirection, GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
return new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = mapRequest,
|
||||
Start = start,
|
||||
Goal = goal,
|
||||
Vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
},
|
||||
Configuration = new HybridAStarConfiguration(),
|
||||
StartDirection = startDirection,
|
||||
GoalDirection = goalDirection,
|
||||
};
|
||||
}
|
||||
|
||||
private static PlanningMapRequest CreateMap(bool allowExplicitEmptyMap, IReadOnlyList<IMapObstacleSource> sources,
|
||||
FixedScenarioTransform transform)
|
||||
{
|
||||
return new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(transform.X(MapXMinMillimeters), transform.X(MapXMaxMillimeters),
|
||||
transform.Y(MapYMinMillimeters), transform.Y(MapYMaxMillimeters)),
|
||||
ResolutionMm = ResolutionMillimeters,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = allowExplicitEmptyMap,
|
||||
};
|
||||
}
|
||||
|
||||
private sealed class FixedScenarioAnchor
|
||||
{
|
||||
public FixedScenarioAnchor(double xMillimeters, double yMillimeters, double headingDegrees)
|
||||
{
|
||||
EnsureFinite(xMillimeters, nameof(xMillimeters));
|
||||
EnsureFinite(yMillimeters, nameof(yMillimeters));
|
||||
EnsureFinite(headingDegrees, nameof(headingDegrees));
|
||||
XMillimeters = xMillimeters;
|
||||
YMillimeters = yMillimeters;
|
||||
HeadingRadians = NormalizeRadians((headingDegrees % 360d) * DegreesToRadians);
|
||||
}
|
||||
|
||||
public double XMillimeters { get; }
|
||||
public double YMillimeters { get; }
|
||||
public double HeadingRadians { get; }
|
||||
}
|
||||
|
||||
private sealed class FixedScenarioTransform
|
||||
{
|
||||
private FixedScenarioTransform(double deltaXMillimeters, double deltaYMillimeters, double headingDeltaRadians)
|
||||
{
|
||||
DeltaXMillimeters = deltaXMillimeters;
|
||||
DeltaYMillimeters = deltaYMillimeters;
|
||||
HeadingDeltaRadians = headingDeltaRadians;
|
||||
}
|
||||
|
||||
public double DeltaXMillimeters { get; }
|
||||
public double DeltaYMillimeters { get; }
|
||||
public double HeadingDeltaRadians { get; }
|
||||
|
||||
public static FixedScenarioTransform From(double baselineStartXMillimeters,
|
||||
double baselineStartYMillimeters, double baselineStartHeadingRadians, FixedScenarioAnchor anchor)
|
||||
{
|
||||
if (anchor == null) return new FixedScenarioTransform(0d, 0d, 0d);
|
||||
return new FixedScenarioTransform(anchor.XMillimeters - baselineStartXMillimeters,
|
||||
anchor.YMillimeters - baselineStartYMillimeters,
|
||||
NormalizeRadians(anchor.HeadingRadians - baselineStartHeadingRadians));
|
||||
}
|
||||
|
||||
public float X(float value)
|
||||
{
|
||||
return ToFiniteFloat(value + DeltaXMillimeters, nameof(value));
|
||||
}
|
||||
|
||||
public float Y(float value)
|
||||
{
|
||||
return ToFiniteFloat(value + DeltaYMillimeters, nameof(value));
|
||||
}
|
||||
|
||||
public Pose2D Pose(double xMeters, double yMeters, double headingRadians)
|
||||
{
|
||||
return new Pose2D((xMeters * MillimetersPerMeter + DeltaXMillimeters) / MillimetersPerMeter,
|
||||
(yMeters * MillimetersPerMeter + DeltaYMillimeters) / MillimetersPerMeter,
|
||||
NormalizeRadians(headingRadians + HeadingDeltaRadians));
|
||||
}
|
||||
}
|
||||
|
||||
private static PlanningMapRequest CreateManualDemoMap(double startXMillimeters, double startYMillimeters,
|
||||
double goalXMillimeters, double goalYMillimeters, IReadOnlyList<ManualCoarsePathObstacle> obstacles,
|
||||
long obstacleSnapshotVersion)
|
||||
{
|
||||
double minimumX = Math.Min(startXMillimeters, goalXMillimeters);
|
||||
double maximumX = Math.Max(startXMillimeters, goalXMillimeters);
|
||||
double minimumY = Math.Min(startYMillimeters, goalYMillimeters);
|
||||
double maximumY = Math.Max(startYMillimeters, goalYMillimeters);
|
||||
for (int index = 0; index < obstacles.Count; index++)
|
||||
{
|
||||
ManualCoarsePathObstacle obstacle = obstacles[index] ??
|
||||
throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
|
||||
double halfX;
|
||||
double halfY;
|
||||
switch (obstacle.Kind)
|
||||
{
|
||||
case ManualCoarsePathObstacleKind.Circle:
|
||||
EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
|
||||
halfX = obstacle.SizeXMillimeters;
|
||||
halfY = obstacle.SizeYMillimeters;
|
||||
break;
|
||||
case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
|
||||
EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
|
||||
EnsurePositiveFinite(obstacle.SizeYMillimeters, nameof(obstacles));
|
||||
halfX = obstacle.SizeXMillimeters / 2d;
|
||||
halfY = obstacle.SizeYMillimeters / 2d;
|
||||
break;
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
|
||||
}
|
||||
|
||||
minimumX = Math.Min(minimumX, obstacle.CenterXMillimeters - halfX);
|
||||
maximumX = Math.Max(maximumX, obstacle.CenterXMillimeters + halfX);
|
||||
minimumY = Math.Min(minimumY, obstacle.CenterYMillimeters - halfY);
|
||||
maximumY = Math.Max(maximumY, obstacle.CenterYMillimeters + halfY);
|
||||
}
|
||||
|
||||
float xMin = ToGridLowerBound(minimumX - ManualMapPaddingMillimeters);
|
||||
float xMax = ToGridUpperBound(maximumX + ManualMapPaddingMillimeters);
|
||||
float yMin = ToGridLowerBound(minimumY - ManualMapPaddingMillimeters);
|
||||
float yMax = ToGridUpperBound(maximumY + ManualMapPaddingMillimeters);
|
||||
bool isExplicitEmptyMap = obstacles.Count == 0;
|
||||
return new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(xMin, xMax, yMin, yMax),
|
||||
ResolutionMm = ResolutionMillimeters,
|
||||
ObstacleSources = isExplicitEmptyMap ? Array.Empty<IMapObstacleSource>() :
|
||||
new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("manual-user-input", obstacleSnapshotVersion, true,
|
||||
ConvertManualObstacles(obstacles)),
|
||||
},
|
||||
AllowExplicitEmptyMap = isExplicitEmptyMap,
|
||||
};
|
||||
}
|
||||
|
||||
private static IMapObstacle[] ConvertManualObstacles(IReadOnlyList<ManualCoarsePathObstacle> obstacles)
|
||||
{
|
||||
var result = new IMapObstacle[obstacles.Count];
|
||||
for (int index = 0; index < obstacles.Count; index++)
|
||||
{
|
||||
ManualCoarsePathObstacle obstacle = obstacles[index] ??
|
||||
throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
|
||||
switch (obstacle.Kind)
|
||||
{
|
||||
case ManualCoarsePathObstacleKind.Circle:
|
||||
result[index] = new CircleObstacle(ToFiniteFloat(obstacle.CenterXMillimeters, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.CenterYMillimeters, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.SizeXMillimeters, nameof(obstacles)));
|
||||
break;
|
||||
case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
|
||||
double halfX = obstacle.SizeXMillimeters / 2d;
|
||||
double halfY = obstacle.SizeYMillimeters / 2d;
|
||||
result[index] = new AxisAlignedRectangleObstacle(
|
||||
ToFiniteFloat(obstacle.CenterXMillimeters - halfX, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.CenterXMillimeters + halfX, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.CenterYMillimeters - halfY, nameof(obstacles)),
|
||||
ToFiniteFloat(obstacle.CenterYMillimeters + halfY, nameof(obstacles)));
|
||||
break;
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
private static Pose2D ToPose(double xMillimeters, double yMillimeters, double headingDegrees)
|
||||
{
|
||||
return new Pose2D(xMillimeters / MillimetersPerMeter, yMillimeters / MillimetersPerMeter,
|
||||
headingDegrees * DegreesToRadians);
|
||||
}
|
||||
|
||||
private static double NormalizeRadians(double angle)
|
||||
{
|
||||
double normalized = angle % (2d * Math.PI);
|
||||
if (normalized <= -Math.PI) return normalized + 2d * Math.PI;
|
||||
return normalized > Math.PI ? normalized - 2d * Math.PI : normalized;
|
||||
}
|
||||
|
||||
private static float ToGridLowerBound(double millimeters)
|
||||
{
|
||||
double rounded = Math.Floor(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
|
||||
return ToFiniteFloat(rounded, nameof(millimeters));
|
||||
}
|
||||
|
||||
private static float ToGridUpperBound(double millimeters)
|
||||
{
|
||||
double rounded = Math.Ceiling(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
|
||||
return ToFiniteFloat(rounded, nameof(millimeters));
|
||||
}
|
||||
|
||||
private static float ToFiniteFloat(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
|
||||
return (float)value;
|
||||
}
|
||||
|
||||
private static void EnsureFinite(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
|
||||
}
|
||||
|
||||
private static void EnsurePositiveFinite(double value, string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
if (value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,628 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Globalization;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 显式空图的粗路径规划测试入口。
|
||||
/// 只负责创建纯规划请求；规划、取消与可视化均由共享执行器处理，不会向底盘发送任何命令。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-显式空图")]
|
||||
public sealed class CoarsePathExplicitEmptyTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ExplicitEmpty, "显式空图");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 单矩形绕行的粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-单矩形绕行")]
|
||||
// TODO：#在该测试下发生了红色矩形栅格碰撞但依然规划成功，需要进一步核实与确认
|
||||
public sealed class CoarsePathRectangleDetourTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.RectangleDetour, "单矩形绕行");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 手工圆形、矩形和 TwoLeg 快照组合的粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-多来源障碍")]
|
||||
public sealed class CoarsePathManualAndTwoLegTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ManualAndTwoLeg, "多来源障碍");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 重复输入地图缓存命中的粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-缓存命中")]
|
||||
public sealed class CoarsePathCacheHitTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.CacheHit, "缓存命中");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 前进起步、倒车到达并显示换向点的粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-倒车换向")]
|
||||
public sealed class CoarsePathReverseGearSwitchTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ReverseGearSwitch, "倒车换向");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 障碍带完全隔开起终点的无解粗路径规划测试入口。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划-无解")]
|
||||
public sealed class CoarsePathNoFeasiblePathTest : MovementTest
|
||||
{
|
||||
/// <inheritdoc />
|
||||
public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.NoFeasiblePath, "无解障碍带");
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用当前 AMR 车身几何中心位姿和人工终点的粗路径规划演示入口。
|
||||
/// 输入的 X/Y 使用世界 mm、航向使用 deg；进入规划核心前由场景工厂一次性转换为 m/rad。
|
||||
/// </summary>
|
||||
[MovementTest(name = "粗路径规划")]
|
||||
public sealed class CoarsePathPlanningTest : MovementTest
|
||||
{
|
||||
private const int MaximumManualObstacleCount = 20;
|
||||
private static long _nextManualObstacleSnapshotVersion;
|
||||
|
||||
/// <summary>
|
||||
/// 读取一次 AMR 当前世界位姿、手动终点和障碍物快照后启动规划。
|
||||
/// 注意：getCartLocation 在无定位时可能阻塞；全部输入会在启动后台任务前冻结，不会被规划线程重复读取。
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
try
|
||||
{
|
||||
var amrPose = DetourInterface.getCartLocation();
|
||||
double goalXmm = ReadFiniteInput("粗路径终点 X（世界 mm）");
|
||||
double goalYmm = ReadFiniteInput("粗路径终点 Y（世界 mm）");
|
||||
double goalHeadingDeg = ReadFiniteInput("粗路径终点航向（世界 deg）");
|
||||
TimeSpan searchTimeout = ReadPositiveTimeoutInput("粗路径规划总超时（秒，必须大于 0）");
|
||||
IReadOnlyList<ManualCoarsePathObstacle> obstacles = ReadManualObstacles();
|
||||
long snapshotVersion = obstacles.Count == 0 ? 0L :
|
||||
Interlocked.Increment(ref _nextManualObstacleSnapshotVersion);
|
||||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.CreateManualObstacleDemo(
|
||||
amrPose.x, amrPose.y, amrPose.th, goalXmm, goalYmm, goalHeadingDeg,
|
||||
obstacles, snapshotVersion);
|
||||
job.Configuration.SearchTimeout = searchTimeout;
|
||||
CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
CoarsePathPlanningTestRunner.ShowInputFailure(exception);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
|
||||
|
||||
private static IReadOnlyList<ManualCoarsePathObstacle> ReadManualObstacles()
|
||||
{
|
||||
int count = ReadBoundedIntegerInput("手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
|
||||
var obstacles = new List<ManualCoarsePathObstacle>(count);
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
string label = "障碍物 " + (index + 1);
|
||||
int kind = ReadBoundedIntegerInput(label + " 类型（1圆形，2矩形）", 1, 2);
|
||||
double centerXmm = ReadFiniteInput(label + " 中心 X（世界 mm）");
|
||||
double centerYmm = ReadFiniteInput(label + " 中心 Y（世界 mm）");
|
||||
if (kind == 1)
|
||||
{
|
||||
double radiusMm = ReadPositiveFiniteInput(label + " 半径 r（mm）");
|
||||
obstacles.Add(ManualCoarsePathObstacle.Circle(centerXmm, centerYmm, radiusMm));
|
||||
}
|
||||
else
|
||||
{
|
||||
double lengthXmm = ReadPositiveFiniteInput(label + " X方向长度（mm）");
|
||||
double widthYmm = ReadPositiveFiniteInput(label + " Y方向宽度（mm）");
|
||||
obstacles.Add(ManualCoarsePathObstacle.AxisAlignedRectangle(centerXmm, centerYmm,
|
||||
lengthXmm, widthYmm));
|
||||
}
|
||||
}
|
||||
return obstacles;
|
||||
}
|
||||
|
||||
private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
|
||||
{
|
||||
object raw = UI.GetInput(prompt);
|
||||
string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
|
||||
int value;
|
||||
if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
|
||||
!int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
|
||||
throw new ArgumentException("输入必须是整数：" + prompt);
|
||||
if (value < minimum || value > maximum)
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static double ReadPositiveFiniteInput(string prompt)
|
||||
{
|
||||
double value = ReadFiniteInput(prompt);
|
||||
if (value <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
|
||||
return value;
|
||||
}
|
||||
|
||||
private static TimeSpan ReadPositiveTimeoutInput(string prompt)
|
||||
{
|
||||
double timeoutSeconds = ReadFiniteInput(prompt);
|
||||
if (timeoutSeconds <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
|
||||
try
|
||||
{
|
||||
return TimeSpan.FromSeconds(timeoutSeconds);
|
||||
}
|
||||
catch (OverflowException)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
|
||||
}
|
||||
}
|
||||
|
||||
private static double ReadFiniteInput(string prompt)
|
||||
{
|
||||
object raw = UI.GetInput(prompt);
|
||||
string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
|
||||
double value;
|
||||
if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
|
||||
!double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
|
||||
throw new ArgumentException("输入必须是有限数字：" + prompt);
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentException("输入必须是有限数字：" + prompt);
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 粗路径 MovementTest 的共享后台会话、停止和绘制实现。
|
||||
/// 同一时刻只允许一个会话绘制；启动新会话或停止时会取消旧会话，但不会等待旧任务退出。
|
||||
/// </summary>
|
||||
internal static class CoarsePathPlanningTestRunner
|
||||
{
|
||||
private const string PainterLayerName = "CoarsePathPlanningV1";
|
||||
private const float MillimetersPerMeter = 1000f;
|
||||
private const int MaximumVisibleGridLines = 100;
|
||||
|
||||
private static readonly object SessionSync = new object();
|
||||
private static readonly CoarsePathPlanningService PlanningService = new CoarsePathPlanningService();
|
||||
private static readonly Painter Painter = UI.GetPainter(PainterLayerName, true);
|
||||
|
||||
private static CancellationTokenSource _activeCancellation;
|
||||
private static Task<CoarsePathPlanningJobResult> _activeTask;
|
||||
private static long _nextSessionId;
|
||||
private static long _activeSessionId;
|
||||
|
||||
/// <summary>
|
||||
/// 固定场景启动时冻结的 AMR 位姿。规划后台不会重新读取定位，确保输入一致。
|
||||
/// </summary>
|
||||
private sealed class AmrPoseSnapshot
|
||||
{
|
||||
public AmrPoseSnapshot(double xMillimeters, double yMillimeters, double headingDegrees)
|
||||
{
|
||||
EnsureFiniteAmrValue(xMillimeters, "X");
|
||||
EnsureFiniteAmrValue(yMillimeters, "Y");
|
||||
EnsureFiniteAmrValue(headingDegrees, "航向");
|
||||
XMillimeters = xMillimeters;
|
||||
YMillimeters = yMillimeters;
|
||||
HeadingDegrees = headingDegrees;
|
||||
}
|
||||
|
||||
public double XMillimeters { get; }
|
||||
public double YMillimeters { get; }
|
||||
public double HeadingDegrees { get; }
|
||||
|
||||
public string DisplayText
|
||||
{
|
||||
get
|
||||
{
|
||||
return "AMR 起点：X=" + XMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
|
||||
" mm，Y=" + YMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
|
||||
" mm，航向=" + HeadingDegrees.ToString("F1", CultureInfo.InvariantCulture) + " deg";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建指定固定场景并将其提交给共享后台服务。
|
||||
/// </summary>
|
||||
internal static void RunScenario(CoarsePathTestScenario scenario, string scenarioName)
|
||||
{
|
||||
try
|
||||
{
|
||||
var pose = DetourInterface.getCartLocation();
|
||||
if (ReferenceEquals(pose, null)) throw new ArgumentException("AMR 位姿为空。");
|
||||
var snapshot = new AmrPoseSnapshot(pose.x, pose.y, pose.th);
|
||||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenario,
|
||||
snapshot.XMillimeters, snapshot.YMillimeters, snapshot.HeadingDegrees);
|
||||
Run(scenarioName, job, snapshot);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
ShowInputFailure(new ArgumentException("AMR 位姿不可用：" + exception.Message, exception));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提交已经冻结输入的一次规划请求。调用立即返回，结果只会由对应会话的完成回调绘制。
|
||||
/// </summary>
|
||||
internal static void Run(string scenarioName, CoarsePathPlanningJob job)
|
||||
{
|
||||
Run(scenarioName, job, null);
|
||||
}
|
||||
|
||||
private static void Run(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
|
||||
{
|
||||
if (job == null) throw new ArgumentNullException(nameof(job));
|
||||
|
||||
var cancellation = new CancellationTokenSource();
|
||||
CancellationTokenSource previousCancellation;
|
||||
long sessionId;
|
||||
lock (SessionSync)
|
||||
{
|
||||
previousCancellation = _activeCancellation;
|
||||
_activeCancellation = cancellation;
|
||||
_activeTask = null;
|
||||
sessionId = ++_nextSessionId;
|
||||
_activeSessionId = sessionId;
|
||||
}
|
||||
|
||||
// 先发布新会话编号，再取消旧任务，避免旧完成回调覆盖新画面。
|
||||
if (previousCancellation != null) previousCancellation.Cancel();
|
||||
Painter.Clear();
|
||||
DrawPending(scenarioName, job, amrPose);
|
||||
|
||||
Task<CoarsePathPlanningJobResult> task = Task.Run(() => PlanningService.Plan(job, cancellation.Token));
|
||||
lock (SessionSync)
|
||||
{
|
||||
if (_activeSessionId == sessionId) _activeTask = task;
|
||||
}
|
||||
|
||||
_ = task.ContinueWith(completed => Finish(sessionId, scenarioName, job, amrPose, cancellation, completed),
|
||||
TaskScheduler.Default);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 取消当前会话并清空专用图层；不等待后台任务结束。
|
||||
/// 已取消任务的完成回调只释放资源，不再记录或绘制结果。
|
||||
/// </summary>
|
||||
internal static void Stop()
|
||||
{
|
||||
CancellationTokenSource cancellation;
|
||||
lock (SessionSync)
|
||||
{
|
||||
cancellation = _activeCancellation;
|
||||
_activeCancellation = null;
|
||||
_activeTask = null;
|
||||
_activeSessionId = 0;
|
||||
}
|
||||
|
||||
if (cancellation != null) cancellation.Cancel();
|
||||
Painter.Clear();
|
||||
Hedingben.ToastText("粗路径规划已请求停止。", PainterLayerName);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 显示输入读取或校验失败，且不改变现有规划任务。
|
||||
/// </summary>
|
||||
internal static void ShowInputFailure(Exception exception)
|
||||
{
|
||||
string message = exception == null ? "未知输入错误。" : exception.Message;
|
||||
Painter.DrawText(Color.LightYellow, "粗路径规划未启动：" + message, 0f, 0f);
|
||||
Hedingben.ToastText("粗路径规划未启动：" + message, PainterLayerName);
|
||||
}
|
||||
|
||||
private static void Finish(long sessionId, string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
|
||||
CancellationTokenSource cancellation, Task<CoarsePathPlanningJobResult> completed)
|
||||
{
|
||||
try
|
||||
{
|
||||
CoarsePathPlanningJobResult result = completed.GetAwaiter().GetResult();
|
||||
bool isCurrent;
|
||||
lock (SessionSync)
|
||||
{
|
||||
isCurrent = _activeSessionId == sessionId;
|
||||
if (isCurrent)
|
||||
{
|
||||
_activeTask = null;
|
||||
_activeCancellation = null;
|
||||
}
|
||||
}
|
||||
|
||||
if (!isCurrent) return;
|
||||
DrawResult(scenarioName, job, amrPose, result);
|
||||
Hedingben.ToastText(BuildToastMessage(scenarioName, result), PainterLayerName);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
bool isCurrent;
|
||||
lock (SessionSync)
|
||||
{
|
||||
isCurrent = _activeSessionId == sessionId;
|
||||
if (isCurrent)
|
||||
{
|
||||
_activeTask = null;
|
||||
_activeCancellation = null;
|
||||
}
|
||||
}
|
||||
|
||||
if (isCurrent)
|
||||
Hedingben.ToastText("粗路径规划任务异常：" + exception.GetType().Name + "。" + exception.Message,
|
||||
PainterLayerName);
|
||||
}
|
||||
finally
|
||||
{
|
||||
cancellation.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawPending(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
|
||||
{
|
||||
DrawPose(job.Start, Color.LimeGreen, "起点");
|
||||
DrawPose(job.Goal, Color.Orange, "终点");
|
||||
Painter.DrawText(Color.LightGray, "场景：" + scenarioName + "（规划中）", 0f, 0f);
|
||||
if (amrPose != null) Painter.DrawText(Color.LightGray, amrPose.DisplayText, 0f, -120f);
|
||||
}
|
||||
|
||||
private static void DrawResult(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
|
||||
CoarsePathPlanningJobResult result)
|
||||
{
|
||||
Painter.Clear();
|
||||
PlanningGridMap map = result.MapResult.Map;
|
||||
if (map != null) DrawMap(map);
|
||||
|
||||
DrawPose(job.Start, Color.LimeGreen, "起点");
|
||||
DrawGoal(job.Goal, job.Configuration, Color.Orange);
|
||||
if (result.PlanningResult.Status == PlanningStatus.Success)
|
||||
DrawPath(result.PlanningResult, job.Vehicle);
|
||||
|
||||
DrawLegend(map);
|
||||
DrawStatus(scenarioName, job, result, map, amrPose);
|
||||
}
|
||||
|
||||
private static void DrawMap(PlanningGridMap map)
|
||||
{
|
||||
float xMin = map.Bounds.XMin;
|
||||
float xMax = map.Bounds.XMax;
|
||||
float yMin = map.Bounds.YMin;
|
||||
float yMax = map.Bounds.YMax;
|
||||
float resolution = map.ResolutionMm;
|
||||
int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) / (double)MaximumVisibleGridLines));
|
||||
|
||||
// 真实 ResolutionMm 决定网格位置，gridStride 只影响显示抽稀。
|
||||
for (int col = 0; col <= map.Cols; col += gridStride)
|
||||
{
|
||||
float x = Math.Min(xMax, xMin + col * resolution);
|
||||
Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
|
||||
}
|
||||
for (int row = 0; row <= map.Rows; row += gridStride)
|
||||
{
|
||||
float y = Math.Min(yMax, yMin + row * resolution);
|
||||
Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
|
||||
}
|
||||
|
||||
DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
|
||||
if (xMin <= 0f && 0f < xMax) Painter.DrawLine(Color.DimGray, 0f, yMin, 0f, yMax, width: 2);
|
||||
if (yMin <= 0f && 0f < yMax) Painter.DrawLine(Color.DimGray, xMin, 0f, xMax, 0f, width: 2);
|
||||
|
||||
for (int row = 0; row < map.Rows; row++)
|
||||
{
|
||||
for (int col = 0; col < map.Cols; col++)
|
||||
{
|
||||
if (!map.IsOccupied(row, col)) continue;
|
||||
float x = xMin + col * resolution + resolution / 2f;
|
||||
float y = yMin + row * resolution;
|
||||
Painter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + resolution,
|
||||
width: Math.Max(1, (int)Math.Round(resolution)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawPose(Pose2D pose, Color color, string label)
|
||||
{
|
||||
if (pose == null) return;
|
||||
float x = ToMillimeters(pose.X);
|
||||
float y = ToMillimeters(pose.Y);
|
||||
Painter.DrawCircle(color, x, y, 80f);
|
||||
DrawHeadingArrow(x, y, pose.Heading, color, 260f);
|
||||
Painter.DrawText(color, label, x + 100f, y + 100f);
|
||||
}
|
||||
|
||||
private static void DrawGoal(Pose2D goal, HybridAStarConfiguration configuration, Color color)
|
||||
{
|
||||
DrawPose(goal, color, "终点");
|
||||
if (goal == null || configuration == null) return;
|
||||
Painter.DrawCircle(Color.FromArgb(150, color), ToMillimeters(goal.X), ToMillimeters(goal.Y),
|
||||
ToMillimeters(configuration.GoalPositionToleranceMeters));
|
||||
}
|
||||
|
||||
private static void DrawPath(PlanningResult planningResult, VehicleParameters vehicle)
|
||||
{
|
||||
if (planningResult.Path == null || planningResult.Path.Count == 0) return;
|
||||
|
||||
int frameStride = Math.Max(1, planningResult.Path.Count / 10);
|
||||
for (int index = 1; index < planningResult.Path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint previous = planningResult.Path[index - 1];
|
||||
CoarsePathPoint current = planningResult.Path[index];
|
||||
Color color = current.Direction == TravelDirection.Forward ? Color.LimeGreen : Color.DeepSkyBlue;
|
||||
Painter.DrawLine(color, ToMillimeters(previous.X), ToMillimeters(previous.Y),
|
||||
ToMillimeters(current.X), ToMillimeters(current.Y), width: 4);
|
||||
|
||||
if (index % frameStride == 0 || current.IsGearSwitchPoint || index == planningResult.Path.Count - 1)
|
||||
DrawVehicleFrame(current, vehicle);
|
||||
if (index % Math.Max(1, frameStride / 2) == 0)
|
||||
DrawHeadingArrow(ToMillimeters(current.X), ToMillimeters(current.Y),
|
||||
current.Heading + (current.Direction == TravelDirection.Reverse ? Math.PI : 0d), color, 140f);
|
||||
if (!current.IsGearSwitchPoint) continue;
|
||||
|
||||
float x = ToMillimeters(current.X);
|
||||
float y = ToMillimeters(current.Y);
|
||||
Painter.DrawCircle(Color.MediumPurple, x, y, 100f);
|
||||
Painter.DrawText(Color.MediumPurple, "换向", x + 110f, y - 110f);
|
||||
}
|
||||
|
||||
DrawVehicleFrame(planningResult.Path[0], vehicle);
|
||||
}
|
||||
|
||||
private static void DrawVehicleFrame(CoarsePathPoint point, VehicleParameters vehicle)
|
||||
{
|
||||
if (point == null || vehicle == null) return;
|
||||
float halfLength = ToMillimeters(vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters);
|
||||
float halfWidth = ToMillimeters(vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters);
|
||||
float centerX = ToMillimeters(point.X);
|
||||
float centerY = ToMillimeters(point.Y);
|
||||
double cos = Math.Cos(point.Heading);
|
||||
double sin = Math.Sin(point.Heading);
|
||||
|
||||
TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, halfWidth, out float frontLeftX, out float frontLeftY);
|
||||
TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, -halfWidth, out float frontRightX, out float frontRightY);
|
||||
TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, -halfWidth, out float rearRightX, out float rearRightY);
|
||||
TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, halfWidth, out float rearLeftX, out float rearLeftY);
|
||||
Painter.DrawLine(Color.Gold, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 2);
|
||||
Painter.DrawLine(Color.Gold, frontRightX, frontRightY, rearRightX, rearRightY, width: 2);
|
||||
Painter.DrawLine(Color.Gold, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 2);
|
||||
Painter.DrawLine(Color.Gold, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 2);
|
||||
}
|
||||
|
||||
private static void TransformVehicleCorner(float centerX, float centerY, double cos, double sin,
|
||||
float longitudinal, float lateral, out float x, out float y)
|
||||
{
|
||||
x = centerX + (float)(cos * longitudinal - sin * lateral);
|
||||
y = centerY + (float)(sin * longitudinal + cos * lateral);
|
||||
}
|
||||
|
||||
private static void DrawHeadingArrow(float x, float y, double headingRadians, Color color, float length)
|
||||
{
|
||||
float endX = x + (float)Math.Cos(headingRadians) * length;
|
||||
float endY = y + (float)Math.Sin(headingRadians) * length;
|
||||
Painter.DrawLine(color, x, y, endX, endY, endArrow: true, width: 3);
|
||||
}
|
||||
|
||||
private static void DrawLegend(PlanningGridMap map)
|
||||
{
|
||||
float x = map == null ? 0f : map.Bounds.XMin + 150f;
|
||||
float y = map == null ? 250f : map.Bounds.YMax - 180f;
|
||||
Painter.DrawText(Color.White, "图例", x, y);
|
||||
DrawLegendItem(x, y - 130f, Color.Gainsboro, "边界 / 栅格");
|
||||
DrawLegendItem(x, y - 260f, Color.Firebrick, "占据格");
|
||||
DrawLegendItem(x, y - 390f, Color.LimeGreen, "起点 / 前进");
|
||||
DrawLegendItem(x, y - 520f, Color.Orange, "终点 / 容差");
|
||||
DrawLegendItem(x, y - 650f, Color.DeepSkyBlue, "倒车");
|
||||
DrawLegendItem(x, y - 780f, Color.MediumPurple, "换向");
|
||||
DrawLegendItem(x, y - 910f, Color.Gold, "扩大车体检查框");
|
||||
}
|
||||
|
||||
private static void DrawLegendItem(float x, float y, Color color, string text)
|
||||
{
|
||||
Painter.DrawLine(color, x, y, x + 90f, y, width: 5);
|
||||
Painter.DrawText(color, text, x + 120f, y - 30f);
|
||||
}
|
||||
|
||||
private static void DrawStatus(string scenarioName, CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult result, PlanningGridMap map, AmrPoseSnapshot amrPose)
|
||||
{
|
||||
float x = map == null ? 0f : map.Bounds.XMin + 150f;
|
||||
float y = map == null ? -250f : map.Bounds.YMin + 150f;
|
||||
string snapshot = map == null ? "无" : map.SnapshotId.ToString(CultureInfo.InvariantCulture);
|
||||
string resolution = map == null ? "无" : map.ResolutionMm.ToString("F0", CultureInfo.InvariantCulture) + " mm";
|
||||
PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
|
||||
string reason = diagnostics.TerminationReason ?? string.Empty;
|
||||
string turningRadius = "无";
|
||||
if (job != null && VehicleKinematics.TryGetMaximumCurvaturePerMeter(job.Vehicle, out double maximumCurvaturePerMeter))
|
||||
turningRadius = (1d / maximumCurvaturePerMeter).ToString("F2", CultureInfo.InvariantCulture) + " m";
|
||||
Painter.DrawText(Color.White, "场景：" + scenarioName, x, y);
|
||||
float detailOffset = 0f;
|
||||
if (amrPose != null)
|
||||
{
|
||||
Painter.DrawText(Color.White, amrPose.DisplayText, x, y + 120f);
|
||||
detailOffset = 120f;
|
||||
}
|
||||
Painter.DrawText(Color.White, "地图：" + result.MapResult.Status + "，缓存：" + result.MapResult.CacheHit + "，快照：" + snapshot,
|
||||
x, y + 120f + detailOffset);
|
||||
Painter.DrawText(Color.White, "栅格：" + resolution + "，规划：" + result.PlanningResult.Status + "，总耗时：" +
|
||||
diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms，路径搜索：" +
|
||||
diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms", x, y + 240f + detailOffset);
|
||||
Painter.DrawText(Color.White, "节点：扩展=" + diagnostics.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) +
|
||||
"，生成=" + diagnostics.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，Open List峰值=" +
|
||||
diagnostics.PeakOpenListCount.ToString(CultureInfo.InvariantCulture), x, y + 360f + detailOffset);
|
||||
if (job != null && job.Vehicle != null)
|
||||
{
|
||||
Painter.DrawText(Color.White, "演示车辆：长=" + job.Vehicle.LengthMeters.ToString("F2", CultureInfo.InvariantCulture) +
|
||||
" m，宽=" + job.Vehicle.WidthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，余量=" +
|
||||
job.Vehicle.SafetyMarginMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，最小转弯半径=" +
|
||||
turningRadius, x, y + 480f + detailOffset);
|
||||
}
|
||||
if (!string.IsNullOrEmpty(reason))
|
||||
Painter.DrawText(Color.LightYellow, "原因：" + reason, x, y + 600f + detailOffset);
|
||||
}
|
||||
|
||||
private static string BuildToastMessage(string scenarioName, CoarsePathPlanningJobResult result)
|
||||
{
|
||||
PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
|
||||
string message = "粗路径[" + scenarioName + "]：地图=" + result.MapResult.Status + "，规划=" +
|
||||
result.PlanningResult.Status + "，总耗时=" +
|
||||
diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms，路径搜索=" +
|
||||
diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms";
|
||||
if (result.PlanningResult.Status != PlanningStatus.Success && !string.IsNullOrEmpty(diagnostics.TerminationReason))
|
||||
message += "，原因=" + diagnostics.TerminationReason;
|
||||
return message + "。";
|
||||
}
|
||||
|
||||
private static void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
|
||||
{
|
||||
Painter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
|
||||
Painter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
|
||||
Painter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
|
||||
Painter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
|
||||
}
|
||||
|
||||
private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
|
||||
|
||||
private static void EnsureFiniteAmrValue(double value, string name)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
throw new ArgumentException(name + " 必须是有限数。");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,160 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
|
||||
/// <summary>
|
||||
/// 对以车辆几何中心表示的扩大矩形执行连续碰撞检查。
|
||||
/// 地图查询使用 m；任何地图外车辆部分、占据格相交或擦边均按碰撞处理。
|
||||
/// </summary>
|
||||
public sealed class FootprintCollisionChecker
|
||||
{
|
||||
/// <summary>创建连续车辆碰撞检查器。</summary>
|
||||
public FootprintCollisionChecker()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断单个车辆位姿是否无碰撞。
|
||||
/// 参数：pose 为车辆几何中心的世界 m/rad 位姿；map 为不可变规划地图；vehicle 为车辆尺寸；
|
||||
/// additionalMarginMeters 为临时额外安全余量，单位 m；bodyClearanceMeters 输出不含该临时余量的保守车体净空下界，单位 m。
|
||||
/// 返回：扩大车辆矩形完整位于地图内且不与任何占据格相交或擦边时为 true；无效输入保守地返回 false。
|
||||
/// </summary>
|
||||
public bool IsPoseCollisionFree(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
|
||||
double additionalMarginMeters, out double bodyClearanceMeters)
|
||||
{
|
||||
bodyClearanceMeters = 0d;
|
||||
if (map == null || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d ||
|
||||
!VehicleFootprint.TryCreate(pose, vehicle, 0d, out VehicleFootprint bodyFootprint) ||
|
||||
!VehicleFootprint.TryCreate(pose, vehicle, additionalMarginMeters, out VehicleFootprint checkedFootprint))
|
||||
return false;
|
||||
|
||||
if (!AreCornersInsideMap(checkedFootprint, map)) return false;
|
||||
|
||||
double centerDistanceMeters = map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y);
|
||||
bodyClearanceMeters = GetBodyClearance(centerDistanceMeters, bodyFootprint.CircumscribedRadiusMeters);
|
||||
if (centerDistanceMeters > checkedFootprint.CircumscribedRadiusMeters) return true;
|
||||
|
||||
return !IntersectsOccupiedCell(checkedFootprint, map);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两个位姿之间的平移和转向扫掠是否无碰撞。
|
||||
/// 参数：from、to 为世界 m/rad 位姿；maximumCenterStepMeters 为允许的最大中心采样间距，单位 m；
|
||||
/// minimumBodyClearanceMeters 输出沿途不含临时扫掠余量的保守车体净空下界，单位 m。
|
||||
/// 返回：端点和每个分段扫掠均无碰撞时为 true；无效输入、地图外或任一中间碰撞时返回 false。
|
||||
/// </summary>
|
||||
public bool IsSweptMotionCollisionFree(Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
|
||||
double maximumCenterStepMeters, out double minimumBodyClearanceMeters)
|
||||
{
|
||||
minimumBodyClearanceMeters = 0d;
|
||||
if (map == null || from == null || to == null || !NumericGuard.IsFinite(maximumCenterStepMeters) || maximumCenterStepMeters <= 0d ||
|
||||
!NumericGuard.IsFinite(from.X) || !NumericGuard.IsFinite(from.Y) || !NumericGuard.IsFinite(from.Heading) ||
|
||||
!NumericGuard.IsFinite(to.X) || !NumericGuard.IsFinite(to.Y) || !NumericGuard.IsFinite(to.Heading) ||
|
||||
!VehicleFootprint.TryCreate(from, vehicle, 0d, out VehicleFootprint bodyFootprint))
|
||||
return false;
|
||||
|
||||
double allowedStepMeters = Math.Min(maximumCenterStepMeters, map.ResolutionMeters / 2d);
|
||||
if (!NumericGuard.IsPositiveFinite(allowedStepMeters)) return false;
|
||||
|
||||
if (!IsPoseCollisionFree(from, map, vehicle, 0d, out double fromClearanceMeters)) return false;
|
||||
minimumBodyClearanceMeters = fromClearanceMeters;
|
||||
|
||||
double deltaX = to.X - from.X;
|
||||
double deltaY = to.Y - from.Y;
|
||||
double centerDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsFinite(centerDistanceMeters)) return false;
|
||||
double headingDeltaRadians = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
|
||||
if (!NumericGuard.IsFinite(headingDeltaRadians)) return false;
|
||||
double rawSegmentCount = Math.Ceiling(centerDistanceMeters / allowedStepMeters);
|
||||
if (!NumericGuard.IsFinite(rawSegmentCount) || rawSegmentCount > int.MaxValue) return false;
|
||||
int segmentCount = Math.Max(1, (int)rawSegmentCount);
|
||||
|
||||
Pose2D previousPose = from;
|
||||
for (int segment = 1; segment <= segmentCount; segment++)
|
||||
{
|
||||
double endFraction = (double)segment / segmentCount;
|
||||
double middleFraction = ((double)segment - 0.5d) / segmentCount;
|
||||
var currentPose = new Pose2D(
|
||||
from.X + deltaX * endFraction,
|
||||
from.Y + deltaY * endFraction,
|
||||
from.Heading + headingDeltaRadians * endFraction);
|
||||
var middlePose = new Pose2D(
|
||||
from.X + deltaX * middleFraction,
|
||||
from.Y + deltaY * middleFraction,
|
||||
from.Heading + headingDeltaRadians * middleFraction);
|
||||
double segmentDeltaX = currentPose.X - previousPose.X;
|
||||
double segmentDeltaY = currentPose.Y - previousPose.Y;
|
||||
double segmentCenterDisplacementMeters = Math.Sqrt(segmentDeltaX * segmentDeltaX + segmentDeltaY * segmentDeltaY);
|
||||
double segmentHeadingDeltaRadians = currentPose.Heading - previousPose.Heading;
|
||||
double temporaryMarginMeters = 0.5d * (segmentCenterDisplacementMeters +
|
||||
bodyFootprint.CircumscribedRadiusMeters * Math.Abs(segmentHeadingDeltaRadians));
|
||||
if (!NumericGuard.IsFinite(temporaryMarginMeters) ||
|
||||
!IsPoseCollisionFree(middlePose, map, vehicle, temporaryMarginMeters, out double middleClearanceMeters))
|
||||
return false;
|
||||
minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, middleClearanceMeters);
|
||||
previousPose = currentPose;
|
||||
}
|
||||
|
||||
if (!IsPoseCollisionFree(to, map, vehicle, 0d, out double toClearanceMeters)) return false;
|
||||
minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, toClearanceMeters);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool AreCornersInsideMap(VehicleFootprint footprint, PlanningGridMap map)
|
||||
{
|
||||
for (int index = 0; index < 4; index++)
|
||||
{
|
||||
footprint.GetCorner(index, out double cornerX, out double cornerY);
|
||||
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double GetBodyClearance(double centerDistanceMeters, double bodyRadiusMeters)
|
||||
{
|
||||
if (double.IsPositiveInfinity(centerDistanceMeters)) return double.PositiveInfinity;
|
||||
if (!NumericGuard.IsFinite(centerDistanceMeters) || !NumericGuard.IsFinite(bodyRadiusMeters)) return 0d;
|
||||
return Math.Max(0d, centerDistanceMeters - bodyRadiusMeters);
|
||||
}
|
||||
|
||||
private static bool IntersectsOccupiedCell(VehicleFootprint footprint, PlanningGridMap map)
|
||||
{
|
||||
GetCellRange(map, footprint.MinX, footprint.MaxX, footprint.MinY, footprint.MaxY,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
||||
for (int row = firstRow; row <= lastRow; row++)
|
||||
for (int col = firstCol; col <= lastCol; col++)
|
||||
{
|
||||
if (!map.IsOccupied(row, col)) continue;
|
||||
GetCellBoundsMeters(map, row, col, out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY);
|
||||
if (OrientedRectangleCellIntersection.Intersects(footprint, cellMinX, cellMaxX, cellMinY, cellMaxY)) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static void GetCellRange(PlanningGridMap map, double minX, double maxX, double minY, double maxY,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol)
|
||||
{
|
||||
double minimumMapX = map.Bounds.XMin / 1000d;
|
||||
double minimumMapY = map.Bounds.YMin / 1000d;
|
||||
firstCol = Clamp((int)Math.Floor((minX - minimumMapX) / map.ResolutionMeters) - 1, 0, map.Cols - 1);
|
||||
lastCol = Clamp((int)Math.Floor((maxX - minimumMapX) / map.ResolutionMeters), 0, map.Cols - 1);
|
||||
firstRow = Clamp((int)Math.Floor((minY - minimumMapY) / map.ResolutionMeters) - 1, 0, map.Rows - 1);
|
||||
lastRow = Clamp((int)Math.Floor((maxY - minimumMapY) / map.ResolutionMeters), 0, map.Rows - 1);
|
||||
}
|
||||
|
||||
private static void GetCellBoundsMeters(PlanningGridMap map, int row, int col,
|
||||
out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY)
|
||||
{
|
||||
cellMinX = map.Bounds.XMin / 1000d + col * map.ResolutionMeters;
|
||||
cellMinY = map.Bounds.YMin / 1000d + row * map.ResolutionMeters;
|
||||
cellMaxX = Math.Min(map.Bounds.XMax / 1000d, cellMinX + map.ResolutionMeters);
|
||||
cellMaxY = Math.Min(map.Bounds.YMax / 1000d, cellMinY + map.ResolutionMeters);
|
||||
}
|
||||
|
||||
private static int Clamp(int value, int minimum, int maximum)
|
||||
{
|
||||
return value < minimum ? minimum : value > maximum ? maximum : value;
|
||||
}
|
||||
}
|
||||
+33
@@ -0,0 +1,33 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
|
||||
/// <summary>旋转矩形与轴对齐栅格的分离轴相交判定。</summary>
|
||||
internal static class OrientedRectangleCellIntersection
|
||||
{
|
||||
/// <summary>任一投影轴没有严格分离时返回 true；擦边按相交处理。</summary>
|
||||
public static bool Intersects(VehicleFootprint rectangle, double cellMinX, double cellMaxX, double cellMinY, double cellMaxY)
|
||||
{
|
||||
if (rectangle == null || cellMaxX < cellMinX || cellMaxY < cellMinY) return false;
|
||||
double cellCenterX = (cellMinX + cellMaxX) / 2d;
|
||||
double cellCenterY = (cellMinY + cellMaxY) / 2d;
|
||||
double cellHalfX = (cellMaxX - cellMinX) / 2d;
|
||||
double cellHalfY = (cellMaxY - cellMinY) / 2d;
|
||||
|
||||
return !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLongitudinalX, rectangle.AxisLongitudinalY) &&
|
||||
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLateralX, rectangle.AxisLateralY) &&
|
||||
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 1d, 0d) &&
|
||||
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 0d, 1d);
|
||||
}
|
||||
|
||||
private static bool HasStrictSeparation(VehicleFootprint rectangle, double cellCenterX, double cellCenterY, double cellHalfX, double cellHalfY,
|
||||
double axisX, double axisY)
|
||||
{
|
||||
double rectangleCenter = rectangle.CenterX * axisX + rectangle.CenterY * axisY;
|
||||
double cellCenter = cellCenterX * axisX + cellCenterY * axisY;
|
||||
double rectangleRadius = rectangle.HalfLengthMeters * Math.Abs(rectangle.AxisLongitudinalX * axisX + rectangle.AxisLongitudinalY * axisY) +
|
||||
rectangle.HalfWidthMeters * Math.Abs(rectangle.AxisLateralX * axisX + rectangle.AxisLateralY * axisY);
|
||||
double cellRadius = cellHalfX * Math.Abs(axisX) + cellHalfY * Math.Abs(axisY);
|
||||
return Math.Abs(rectangleCenter - cellCenter) > rectangleRadius + cellRadius;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
|
||||
/// <summary>以车辆几何中心为原点的扩大旋转矩形。</summary>
|
||||
internal sealed class VehicleFootprint
|
||||
{
|
||||
private VehicleFootprint(Pose2D pose, double halfLengthMeters, double halfWidthMeters)
|
||||
{
|
||||
CenterX = pose.X;
|
||||
CenterY = pose.Y;
|
||||
HalfLengthMeters = halfLengthMeters;
|
||||
HalfWidthMeters = halfWidthMeters;
|
||||
AxisLongitudinalX = Math.Cos(pose.Heading);
|
||||
AxisLongitudinalY = Math.Sin(pose.Heading);
|
||||
AxisLateralX = -AxisLongitudinalY;
|
||||
AxisLateralY = AxisLongitudinalX;
|
||||
CircumscribedRadiusMeters = Math.Sqrt(halfLengthMeters * halfLengthMeters + halfWidthMeters * halfWidthMeters);
|
||||
|
||||
double minX = double.PositiveInfinity;
|
||||
double maxX = double.NegativeInfinity;
|
||||
double minY = double.PositiveInfinity;
|
||||
double maxY = double.NegativeInfinity;
|
||||
for (int index = 0; index < 4; index++)
|
||||
{
|
||||
GetCorner(index, out double x, out double y);
|
||||
minX = Math.Min(minX, x);
|
||||
maxX = Math.Max(maxX, x);
|
||||
minY = Math.Min(minY, y);
|
||||
maxY = Math.Max(maxY, y);
|
||||
}
|
||||
MinX = minX;
|
||||
MaxX = maxX;
|
||||
MinY = minY;
|
||||
MaxY = maxY;
|
||||
}
|
||||
|
||||
public double CenterX { get; }
|
||||
public double CenterY { get; }
|
||||
public double HalfLengthMeters { get; }
|
||||
public double HalfWidthMeters { get; }
|
||||
public double AxisLongitudinalX { get; }
|
||||
public double AxisLongitudinalY { get; }
|
||||
public double AxisLateralX { get; }
|
||||
public double AxisLateralY { get; }
|
||||
public double CircumscribedRadiusMeters { get; }
|
||||
public double MinX { get; }
|
||||
public double MaxX { get; }
|
||||
public double MinY { get; }
|
||||
public double MaxY { get; }
|
||||
|
||||
/// <summary>创建包含车辆安全余量和临时扫掠余量的矩形。</summary>
|
||||
public static bool TryCreate(Pose2D pose, VehicleParameters vehicle, double additionalMarginMeters, out VehicleFootprint footprint)
|
||||
{
|
||||
footprint = null;
|
||||
if (pose == null || vehicle == null || !NumericGuard.IsFinite(pose.X) || !NumericGuard.IsFinite(pose.Y) ||
|
||||
!NumericGuard.IsFinite(pose.Heading) || !NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(vehicle.WidthMeters) || !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) ||
|
||||
vehicle.SafetyMarginMeters < 0d || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d)
|
||||
return false;
|
||||
|
||||
double totalMarginMeters = vehicle.SafetyMarginMeters + additionalMarginMeters;
|
||||
if (!NumericGuard.IsFinite(totalMarginMeters)) return false;
|
||||
double halfLengthMeters = vehicle.LengthMeters / 2d + totalMarginMeters;
|
||||
double halfWidthMeters = vehicle.WidthMeters / 2d + totalMarginMeters;
|
||||
if (!NumericGuard.IsPositiveFinite(halfLengthMeters) || !NumericGuard.IsPositiveFinite(halfWidthMeters)) return false;
|
||||
|
||||
footprint = new VehicleFootprint(pose, halfLengthMeters, halfWidthMeters);
|
||||
return NumericGuard.IsFinite(footprint.CircumscribedRadiusMeters) && NumericGuard.IsFinite(footprint.MinX) &&
|
||||
NumericGuard.IsFinite(footprint.MaxX) && NumericGuard.IsFinite(footprint.MinY) && NumericGuard.IsFinite(footprint.MaxY);
|
||||
}
|
||||
|
||||
/// <summary>获取指定角点。索引按逆时针顺序为 0 到 3。</summary>
|
||||
public void GetCorner(int index, out double x, out double y)
|
||||
{
|
||||
double longitudinalSign;
|
||||
double lateralSign;
|
||||
switch (index)
|
||||
{
|
||||
case 0: longitudinalSign = 1d; lateralSign = 1d; break;
|
||||
case 1: longitudinalSign = -1d; lateralSign = 1d; break;
|
||||
case 2: longitudinalSign = -1d; lateralSign = -1d; break;
|
||||
case 3: longitudinalSign = 1d; lateralSign = -1d; break;
|
||||
default: throw new ArgumentOutOfRangeException(nameof(index));
|
||||
}
|
||||
x = CenterX + longitudinalSign * HalfLengthMeters * AxisLongitudinalX + lateralSign * HalfWidthMeters * AxisLateralX;
|
||||
y = CenterY + longitudinalSign * HalfLengthMeters * AxisLongitudinalY + lateralSign * HalfWidthMeters * AxisLateralY;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
|
||||
/// <summary>
|
||||
/// 从车辆参数提取运动学限制的辅助方法。
|
||||
/// 曲率单位为 1/m；当最大曲率与最小转弯半径同时给出时，始终选择更保守的较小曲率。
|
||||
/// </summary>
|
||||
public static class VehicleKinematics
|
||||
{
|
||||
/// <summary>
|
||||
/// 尝试获取车辆允许的最大绝对曲率。
|
||||
/// 参数：vehicle 为车辆参数；maximumCurvaturePerMeter 为输出的正有限曲率，单位 1/m。
|
||||
/// 返回：至少提供一种正有限曲率限制时为 true；任一已提供限制无效或两种限制均未提供时为 false。
|
||||
/// </summary>
|
||||
public static bool TryGetMaximumCurvaturePerMeter(VehicleParameters vehicle, out double maximumCurvaturePerMeter)
|
||||
{
|
||||
maximumCurvaturePerMeter = 0d;
|
||||
if (vehicle == null) return false;
|
||||
|
||||
bool hasMaximumCurvature = vehicle.MaximumCurvaturePerMeter.HasValue;
|
||||
bool hasMinimumRadius = vehicle.MinimumTurningRadiusMeters.HasValue;
|
||||
if (hasMaximumCurvature && !NumericGuard.IsPositiveFinite(vehicle.MaximumCurvaturePerMeter.Value)) return false;
|
||||
if (hasMinimumRadius && !NumericGuard.IsPositiveFinite(vehicle.MinimumTurningRadiusMeters.Value)) return false;
|
||||
if (!hasMaximumCurvature && !hasMinimumRadius) return false;
|
||||
|
||||
if (hasMaximumCurvature && hasMinimumRadius)
|
||||
{
|
||||
maximumCurvaturePerMeter = System.Math.Min(
|
||||
vehicle.MaximumCurvaturePerMeter.Value,
|
||||
1d / vehicle.MinimumTurningRadiusMeters.Value);
|
||||
return true;
|
||||
}
|
||||
|
||||
maximumCurvaturePerMeter = hasMaximumCurvature
|
||||
? vehicle.MaximumCurvaturePerMeter.Value
|
||||
: 1d / vehicle.MinimumTurningRadiusMeters.Value;
|
||||
return NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user