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,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);
|
||||
}
|
||||
}
|
||||
+812
@@ -0,0 +1,812 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8"/>
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1"/>
|
||||
<title>AMR 非结构化道路 Hybrid A* 粗路径规划总体技术方案</title>
|
||||
<style>
|
||||
:root{
|
||||
--bg:#0b1020;
|
||||
--panel:#121a2f;
|
||||
--panel2:#18223c;
|
||||
--text:#eef4ff;
|
||||
--muted:#aebbd2;
|
||||
--line:#2b3a5f;
|
||||
--accent:#68a7ff;
|
||||
--accent2:#7ce7c4;
|
||||
--warn:#ffd479;
|
||||
--danger:#ff8b8b;
|
||||
--ok:#78e08f;
|
||||
--code:#09101f;
|
||||
--shadow:0 14px 40px rgba(0,0,0,.28);
|
||||
}
|
||||
*{box-sizing:border-box}
|
||||
html{scroll-behavior:smooth}
|
||||
body{
|
||||
margin:0;
|
||||
font-family:"Segoe UI","Microsoft YaHei",system-ui,-apple-system,sans-serif;
|
||||
background:linear-gradient(180deg,#08101f 0%,#0d1427 45%,#0b1020 100%);
|
||||
color:var(--text);
|
||||
line-height:1.78;
|
||||
}
|
||||
a{color:var(--accent);text-decoration:none}
|
||||
a:hover{text-decoration:underline}
|
||||
.hero{
|
||||
padding:64px 7vw 40px;
|
||||
border-bottom:1px solid var(--line);
|
||||
background:
|
||||
radial-gradient(circle at 85% 10%,rgba(104,167,255,.20),transparent 34%),
|
||||
radial-gradient(circle at 10% 0%,rgba(124,231,196,.12),transparent 30%);
|
||||
}
|
||||
.hero h1{font-size:clamp(30px,5vw,56px);line-height:1.18;margin:0 0 18px}
|
||||
.hero p{max-width:1000px;color:var(--muted);font-size:18px}
|
||||
.badges{display:flex;gap:10px;flex-wrap:wrap;margin-top:24px}
|
||||
.badge{padding:6px 12px;border:1px solid var(--line);border-radius:999px;background:rgba(255,255,255,.03);font-size:13px}
|
||||
.badge.ok{border-color:rgba(120,224,143,.5);color:var(--ok)}
|
||||
.badge.todo{border-color:rgba(255,212,121,.5);color:var(--warn)}
|
||||
.layout{display:grid;grid-template-columns:290px minmax(0,1fr);gap:28px;max-width:1500px;margin:0 auto;padding:32px 28px 80px}
|
||||
aside{
|
||||
position:sticky;top:18px;align-self:start;max-height:calc(100vh - 36px);overflow:auto;
|
||||
background:rgba(18,26,47,.86);border:1px solid var(--line);border-radius:18px;padding:18px;box-shadow:var(--shadow);
|
||||
}
|
||||
aside h3{margin:0 0 10px;font-size:16px}
|
||||
aside a{display:block;padding:7px 9px;border-radius:8px;color:var(--muted);font-size:14px}
|
||||
aside a:hover{background:var(--panel2);color:var(--text);text-decoration:none}
|
||||
main{min-width:0}
|
||||
section{
|
||||
background:rgba(18,26,47,.90);border:1px solid var(--line);border-radius:20px;
|
||||
padding:30px;margin-bottom:24px;box-shadow:var(--shadow);
|
||||
}
|
||||
section h2{margin-top:0;font-size:28px;border-bottom:1px solid var(--line);padding-bottom:12px}
|
||||
section h3{margin-top:26px;font-size:20px}
|
||||
section h4{margin-top:20px;font-size:17px;color:var(--accent2)}
|
||||
p,li{color:#dce6f8}
|
||||
.note,.warn,.success,.decision{
|
||||
border-left:4px solid var(--accent);padding:14px 16px;background:rgba(104,167,255,.08);border-radius:8px;margin:18px 0;
|
||||
}
|
||||
.warn{border-color:var(--warn);background:rgba(255,212,121,.08)}
|
||||
.success{border-color:var(--ok);background:rgba(120,224,143,.08)}
|
||||
.decision{border-color:var(--accent2);background:rgba(124,231,196,.08)}
|
||||
code{background:var(--code);padding:2px 6px;border-radius:6px;color:#d8e6ff}
|
||||
pre{
|
||||
background:var(--code);border:1px solid #223253;border-radius:12px;padding:16px;overflow:auto;
|
||||
color:#d9e7ff;line-height:1.55;
|
||||
}
|
||||
table{width:100%;border-collapse:collapse;margin:16px 0;font-size:14px}
|
||||
th,td{border:1px solid var(--line);padding:10px 12px;vertical-align:top}
|
||||
th{background:var(--panel2);text-align:left}
|
||||
.flow{
|
||||
display:grid;gap:10px;margin:18px 0
|
||||
}
|
||||
.flow .node{
|
||||
border:1px solid var(--line);background:linear-gradient(135deg,#15213b,#10182b);
|
||||
border-radius:12px;padding:13px 16px;position:relative
|
||||
}
|
||||
.flow .node:not(:last-child)::after{
|
||||
content:"↓";display:block;text-align:center;color:var(--accent);font-size:22px;position:relative;bottom:-18px;height:20px
|
||||
}
|
||||
.grid2{display:grid;grid-template-columns:repeat(2,minmax(0,1fr));gap:16px}
|
||||
.grid3{display:grid;grid-template-columns:repeat(3,minmax(0,1fr));gap:16px}
|
||||
.card{border:1px solid var(--line);background:var(--panel2);border-radius:14px;padding:16px}
|
||||
.card h4{margin:0 0 8px}
|
||||
.kpi{font-size:28px;font-weight:700;color:var(--accent2)}
|
||||
.small{font-size:13px;color:var(--muted)}
|
||||
.step-id{display:inline-flex;align-items:center;justify-content:center;width:34px;height:34px;border-radius:50%;background:var(--accent);color:#06101f;font-weight:800;margin-right:8px}
|
||||
.status{float:right;font-size:13px;border-radius:999px;padding:4px 10px;border:1px solid var(--line)}
|
||||
.status.done{color:var(--ok);border-color:rgba(120,224,143,.4)}
|
||||
.status.todo{color:var(--warn);border-color:rgba(255,212,121,.4)}
|
||||
.diagram{
|
||||
font-family:Consolas,monospace;white-space:pre;overflow:auto;background:var(--code);border:1px solid var(--line);
|
||||
padding:18px;border-radius:12px;color:#cfe1ff
|
||||
}
|
||||
footer{max-width:1500px;margin:0 auto;padding:0 28px 50px;color:var(--muted);font-size:13px}
|
||||
@media(max-width:980px){
|
||||
.layout{grid-template-columns:1fr}
|
||||
aside{position:relative;top:0;max-height:none}
|
||||
.grid2,.grid3{grid-template-columns:1fr}
|
||||
}
|
||||
@media print{
|
||||
body{background:white;color:#111}
|
||||
.hero,section,aside{background:white;color:#111;box-shadow:none}
|
||||
.layout{display:block}
|
||||
aside{display:none}
|
||||
p,li{color:#222}
|
||||
code,pre,.diagram{background:#f5f5f5;color:#111}
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<header class="hero">
|
||||
<h1>AMR 非结构化道路 Hybrid A* 粗路径规划总体技术方案</h1>
|
||||
<p>
|
||||
面向四舵轮AMR的第一阶段工程实现：暂不考虑蟹行与纯横移，仅采用车式运动模型，
|
||||
支持前进、倒车及换向，输出供后续SQP使用的无时间空间粗路径。
|
||||
本文统一整理已经确认的第1至第14步，作为软件设计、编码、调试与验收依据。
|
||||
</p>
|
||||
<div class="badges">
|
||||
<span class="badge ok">步骤1–12：已确认</span>
|
||||
<span class="badge todo">步骤13：性能优化TODO</span>
|
||||
<span class="badge ok">步骤14：接口与验收已确认</span>
|
||||
<span class="badge">地图分辨率初值：0.05 m</span>
|
||||
<span class="badge">原语长度：0.50 m</span>
|
||||
<span class="badge">积分步长：0.05 m</span>
|
||||
</div>
|
||||
</header>
|
||||
|
||||
<div class="layout">
|
||||
<aside>
|
||||
<h3>目录</h3>
|
||||
<a href="#overview">0. 总体概览</a>
|
||||
<a href="#step1">1. 任务边界</a>
|
||||
<a href="#step2">2. 地图表达</a>
|
||||
<a href="#step3">3. 运动模型与曲率</a>
|
||||
<a href="#step4">4. 搜索节点状态</a>
|
||||
<a href="#step5">5. 运动原语</a>
|
||||
<a href="#step6">6. 原语终止与积分</a>
|
||||
<a href="#step7">7. 碰撞检测与模板</a>
|
||||
<a href="#step8">8. 代价与启发</a>
|
||||
<a href="#step9">9. 终点与Reeds–Shepp</a>
|
||||
<a href="#step10">10. 回溯与稠密点</a>
|
||||
<a href="#step11">11. 重采样</a>
|
||||
<a href="#step12">12. 平滑与校验</a>
|
||||
<a href="#step13">13. 性能优化TODO</a>
|
||||
<a href="#step14">14. 接口与验收</a>
|
||||
<a href="#params">附录A. 推荐参数</a>
|
||||
<a href="#pseudocode">附录B. 总体伪代码</a>
|
||||
<a href="#milestones">附录C. 开发里程碑</a>
|
||||
</aside>
|
||||
|
||||
<main>
|
||||
<section id="overview">
|
||||
<h2>0. 总体概览</h2>
|
||||
<div class="grid3">
|
||||
<div class="card"><div class="kpi">车式运动</div><div class="small">车头/车尾方向运动，不考虑蟹行</div></div>
|
||||
<div class="card"><div class="kpi">前进 + 倒车</div><div class="small">允许在原语边界切换方向</div></div>
|
||||
<div class="card"><div class="kpi">空间粗路径</div><div class="small">不包含速度、加速度、时间戳</div></div>
|
||||
</div>
|
||||
|
||||
<h3>0.1 总体职责边界</h3>
|
||||
<table>
|
||||
<tr><th>模块</th><th>主要职责</th><th>不负责</th></tr>
|
||||
<tr><td>Hybrid A*</td><td>拓扑可达、无碰撞、基本运动学可行、前进/倒车结构、目标位置与航向接近</td><td>最终速度、加速度、时间、四轮舵角与轮速</td></tr>
|
||||
<tr><td>路径后处理</td><td>回溯、稠密点恢复、关键点保留、弧长重采样、快速曲线平滑、完整复核</td><td>动态约束和最终轨迹时间参数化</td></tr>
|
||||
<tr><td>后续SQP</td><td>终点精确收敛、速度/加速度/角速度/时间戳、换向点停车、动态可跟踪性</td><td>重新决定绕障侧或凭空增加倒车拓扑</td></tr>
|
||||
<tr><td>底盘逆解与控制</td><td>四轮舵角、轮速分配和轨迹跟踪</td><td>全局绕障搜索</td></tr>
|
||||
</table>
|
||||
|
||||
<h3>0.2 总体数据流</h3>
|
||||
<div class="flow">
|
||||
<div class="node">感知障碍物 / 点云 → 过滤、投影到二维</div>
|
||||
<div class="node">二维占据栅格 + 障碍物距离场</div>
|
||||
<div class="node">起点、终点、车辆参数、规划配置</div>
|
||||
<div class="node">二维Dijkstra启发图 + Hybrid A*搜索</div>
|
||||
<div class="node">机会式/强制 Reeds–Shepp 终点连接</div>
|
||||
<div class="node">父节点回溯 + 0.05 m内部积分点恢复</div>
|
||||
<div class="node">关键点保留 + 分方向弧长重采样</div>
|
||||
<div class="node">三次B样条优先，局部Bézier/五次多项式，局部QP兜底</div>
|
||||
<div class="node">最终碰撞、净空、曲率、换向结构校验</div>
|
||||
<div class="node">输出空间粗路径 → 后续SQP</div>
|
||||
</div>
|
||||
|
||||
<div class="decision">
|
||||
<strong>第一版核心原则：</strong>先保证算法完整跑通、路径正确、可解释、可复现；性能优化整体放入第十三步TODO，待前12步稳定后再开展。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step1">
|
||||
<h2><span class="step-id">1</span>任务边界与输出定义 <span class="status done">已确认</span></h2>
|
||||
<h3>1.1 车辆运动模式</h3>
|
||||
<p>目标平台为四舵轮AMR，但第一阶段只使用车式运动能力：</p>
|
||||
<ul>
|
||||
<li>车辆沿车头或车尾方向运动；</li>
|
||||
<li>允许前进、倒车以及换向；</li>
|
||||
<li>暂不考虑蟹行、纯横移和任意方向全向运动；</li>
|
||||
<li>车体航向角始终表示车头朝向，即使车辆正在倒车。</li>
|
||||
</ul>
|
||||
|
||||
<h3>1.2 Hybrid A*最小输出</h3>
|
||||
<p>输出空间路径点序列，至少包含：</p>
|
||||
<pre>位置：x, y
|
||||
车体航向：heading
|
||||
运动方向：Forward / Reverse
|
||||
累计弧长：s
|
||||
参考/几何曲率：kappa
|
||||
换向标记：isGearSwitchPoint
|
||||
安全净空：clearance</pre>
|
||||
<p>不输出速度、加速度、角速度、时间戳、四个舵轮角度与轮速。</p>
|
||||
|
||||
<h3>1.3 成功判据</h3>
|
||||
<ul>
|
||||
<li>路径全程无碰撞且不进入未知区域；</li>
|
||||
<li>粗路径基本满足最大曲率约束；</li>
|
||||
<li>包含正确绕障侧、前进/后退和换向结构；</li>
|
||||
<li>终点位置与航向满足第九步终止规则；</li>
|
||||
<li>可作为后续SQP的有效初值。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<section id="step2">
|
||||
<h2><span class="step-id">2</span>地图表达与环境输入 <span class="status done">已确认</span></h2>
|
||||
<h3>2.1 地图生成流程</h3>
|
||||
<div class="diagram">原始点云 / 障碍信息
|
||||
↓ 过滤噪声、地面、离群点
|
||||
投影到二维平面
|
||||
↓
|
||||
二维占据栅格 Occupancy Grid
|
||||
↓
|
||||
障碍物距离场 Distance Field / EDT</div>
|
||||
|
||||
<h3>2.2 栅格定义</h3>
|
||||
<ul>
|
||||
<li>地图分辨率初值：<code>0.05 m</code>；</li>
|
||||
<li>状态：Free、Occupied、Unknown；</li>
|
||||
<li>第一版中Unknown按障碍物处理；</li>
|
||||
<li>地图包含Origin、Resolution、Width、Height及Version；</li>
|
||||
<li>算法内部不得写死分辨率，统一读取地图参数。</li>
|
||||
</ul>
|
||||
|
||||
<h3>2.3 车体碰撞模型</h3>
|
||||
<p>AMR使用旋转矩形车体碰撞模型，不能只用中心点或单圆近似。安全余量主要加到车体矩形上：</p>
|
||||
<pre>CheckLength = VehicleLength + 2 × SafetyMargin
|
||||
CheckWidth = VehicleWidth + 2 × SafetyMargin</pre>
|
||||
<p>初始安全余量为<code>0.03 m</code>。避免地图膨胀与车体扩大同时重复计算同一余量。</p>
|
||||
</section>
|
||||
|
||||
<section id="step3">
|
||||
<h2><span class="step-id">3</span>车辆运动模型与最大曲率 <span class="status done">已确认</span></h2>
|
||||
<h3>3.1 参考点</h3>
|
||||
<p>车辆状态参考点采用AMR几何中心。</p>
|
||||
|
||||
<h3>3.2 基于路径弧长的运动学</h3>
|
||||
<pre>dx/ds = cos(theta)
|
||||
dy/ds = sin(theta)
|
||||
dtheta/ds = kappa</pre>
|
||||
<p>其中<code>theta</code>为车头航向角，<code>kappa</code>为车体中心路径曲率。</p>
|
||||
|
||||
<h3>3.3 最大曲率来源</h3>
|
||||
<p>必须同时支持两种来源：</p>
|
||||
<ol>
|
||||
<li>外部直接输入<code>MaxCurvature</code>，或输入<code>MinTurningRadius</code>并换算<code>kappa_max = 1 / R_min</code>；</li>
|
||||
<li>由四轮转向几何调用<code>ComputeMaxCurvature(VehicleKinematicParameters)</code>计算。</li>
|
||||
</ol>
|
||||
<p>推荐三种模式：</p>
|
||||
<pre>ExternalOnly
|
||||
GeometryOnly
|
||||
ConservativeMinimum</pre>
|
||||
<p>当两种来源同时存在时，建议取更保守的较小值。若均无有效值，则返回配置错误。</p>
|
||||
|
||||
<div class="warn">
|
||||
曲率变化率不在本步骤强制建模。第一版先保证曲率不超过上限；渐变曲率原语和实车舵轮速率约束作为后续升级。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step4">
|
||||
<h2><span class="step-id">4</span>Hybrid A*搜索节点状态 <span class="status done">已确认</span></h2>
|
||||
<h3>4.1 连续状态</h3>
|
||||
<pre>(x, y, theta, direction, curvatureIndex)</pre>
|
||||
<ul>
|
||||
<li><code>x, y, theta</code>保持连续值用于运动积分；</li>
|
||||
<li><code>direction</code>为Forward或Reverse；</li>
|
||||
<li><code>curvatureIndex</code>表示当前离散曲率等级。</li>
|
||||
</ul>
|
||||
|
||||
<h3>4.2 Closed Set键</h3>
|
||||
<pre>(ix, iy, iHeading, direction, curvatureIndex)</pre>
|
||||
<p>推荐初始离散：</p>
|
||||
<ul>
|
||||
<li>位置Closed Set分辨率：<code>0.05–0.10 m</code>；</li>
|
||||
<li>航向离散：<code>5°</code>；</li>
|
||||
<li>方向和曲率等级均进入键，避免把运动状态不同的节点错误合并。</li>
|
||||
</ul>
|
||||
|
||||
<h3>4.3 搜索元数据</h3>
|
||||
<p>节点还应保存G/H/F代价、父节点索引、生成当前节点的原语积分点、最小净空、终止原因等。这些属于搜索管理信息，不属于车辆物理状态。</p>
|
||||
</section>
|
||||
|
||||
<section id="step5">
|
||||
<h2><span class="step-id">5</span>运动原语与扩展规则 <span class="status done">已确认</span></h2>
|
||||
<h3>5.1 第一版原语集合</h3>
|
||||
<pre>{ -kappa_max, -0.5 kappa_max, 0, 0.5 kappa_max, kappa_max }</pre>
|
||||
<p>每种曲率均支持Forward与Reverse。</p>
|
||||
|
||||
<h3>5.2 曲率切换限制</h3>
|
||||
<p>相邻原语曲率等级最多变化1级。例如：</p>
|
||||
<pre>允许：0 → 0.5κmax
|
||||
允许：0.5κmax → κmax
|
||||
不允许：κmax → -κmax</pre>
|
||||
<p>该限制可减少“左打死后立即右打死”的不合理跳变。</p>
|
||||
|
||||
<h3>5.3 换向规则</h3>
|
||||
<ul>
|
||||
<li>前进/倒车只能在运动原语边界切换；</li>
|
||||
<li>起始曲率若上层可提供则使用实际值，否则默认0；</li>
|
||||
<li>第一版采用恒定曲率原语。</li>
|
||||
</ul>
|
||||
|
||||
<div class="warn">
|
||||
<strong>TODO：</strong>当恒定曲率原语造成曲率跳变过大、平滑后仍不可跟踪或实车舵轮变化速度不足时，增加线性渐变曲率原语。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step6">
|
||||
<h2><span class="step-id">6</span>原语长度、积分与终止 <span class="status done">已确认</span></h2>
|
||||
<h3>6.1 固定参数</h3>
|
||||
<pre>PrimitiveLength = 0.50 m
|
||||
IntegrationStep = 0.05 m</pre>
|
||||
<p>完整原语最多包含10个内部积分点。只有原语终点进入Open List，内部积分点用于碰撞、净空和最终路径恢复。</p>
|
||||
|
||||
<h3>6.2 终止条件</h3>
|
||||
<table>
|
||||
<tr><th>条件</th><th>处理</th></tr>
|
||||
<tr><td>累计长度达到0.50 m</td><td>生成正常候选节点</td></tr>
|
||||
<tr><td>任一积分点碰撞</td><td>原语无效，立即终止</td></tr>
|
||||
<tr><td>越界或进入未知区域</td><td>原语无效</td></tr>
|
||||
<tr><td>中途满足目标规则</td><td>提前终止并保存实际有效段</td></tr>
|
||||
<tr><td>Reeds–Shepp连接成功</td><td>结束搜索</td></tr>
|
||||
</table>
|
||||
|
||||
<div class="warn">
|
||||
<strong>TODO：</strong>后续根据障碍物距离、目标距离和窄通道自动调整原语长度；碰撞积分步长仍保持0.05 m。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step7">
|
||||
<h2><span class="step-id">7</span>碰撞检测与航向栅格模板 <span class="status done">已确认</span></h2>
|
||||
<h3>7.1 两级碰撞检测</h3>
|
||||
<ol>
|
||||
<li><strong>距离场快速放行：</strong>若车辆中心到最近障碍物距离大于扩大车体外接圆半径及离散补偿，则该点必然安全；</li>
|
||||
<li><strong>旋转矩形模板精确检测：</strong>距离不足以保证安全时，检查当前航向下扩大车体覆盖的所有栅格。</li>
|
||||
</ol>
|
||||
|
||||
<h3>7.2 外接圆仅用于“安全放行”</h3>
|
||||
<pre>R_outer = 0.5 × sqrt(CheckLength² + CheckWidth²)</pre>
|
||||
<p><code>D > R_outer</code>表示安全；<code>D ≤ R_outer</code>只表示“无法确定”，不等于碰撞，必须进入矩形模板检查。</p>
|
||||
|
||||
<h3>7.3 航向栅格模板的准备</h3>
|
||||
<p>航向分辨率5°时，在初始化阶段自动生成72组模板：</p>
|
||||
<pre>0°, 5°, 10°, ... , 355°</pre>
|
||||
<p>每个模板保存旋转后的扩大车体矩形与地图栅格相交所对应的相对栅格偏移：</p>
|
||||
<pre>GridOffset { Dx, Dy }</pre>
|
||||
<p>运行时把当前车辆中心转换为栅格坐标，选择最近航向模板，将模板中的相对偏移平移到当前位置并查询占据状态。</p>
|
||||
|
||||
<h3>7.4 模板生成规则</h3>
|
||||
<ul>
|
||||
<li>模板以车体几何中心为原点；</li>
|
||||
<li>车体尺寸包含安全余量与少量离散补偿；</li>
|
||||
<li>只要栅格方块与旋转矩形有交集，就加入模板；</li>
|
||||
<li>不能只判断栅格中心是否在矩形内部，否则可能漏掉边角碰撞；</li>
|
||||
<li>真实车体始终是矩形，模板呈阶梯状只是栅格化结果。</li>
|
||||
</ul>
|
||||
|
||||
<h3>7.5 每个积分点的处理</h3>
|
||||
<pre>读取距离场
|
||||
↓
|
||||
可安全放行？——是→继续
|
||||
↓否
|
||||
选择航向模板
|
||||
↓
|
||||
平移模板并查询Occupied / Unknown / 越界
|
||||
↓
|
||||
任一命中→碰撞；全部通过→安全</pre>
|
||||
</section>
|
||||
|
||||
<section id="step8">
|
||||
<h2><span class="step-id">8</span>搜索代价与启发函数 <span class="status done">已确认</span></h2>
|
||||
<h3>8.1 节点排序</h3>
|
||||
<pre>f(n) = g(n) + w_h × h(n)</pre>
|
||||
<p>第一版<code>HeuristicWeight = 1.3</code>。</p>
|
||||
|
||||
<h3>8.2 累计代价</h3>
|
||||
<pre>g_new =
|
||||
g_parent
|
||||
+ C_length
|
||||
+ C_reverse
|
||||
+ C_switch
|
||||
+ C_curvature
|
||||
+ C_curvatureChange
|
||||
+ C_obstacle</pre>
|
||||
<ul>
|
||||
<li>路径长度是基础代价；</li>
|
||||
<li>后退系数初值<code>ReversePenalty = 1.3</code>；</li>
|
||||
<li>换向固定惩罚初值<code>GearSwitchPenalty = 2.0</code>；</li>
|
||||
<li>曲率代价轻度抑制长期极限转向；</li>
|
||||
<li>曲率变化代价偏好平缓动作序列；</li>
|
||||
<li>障碍物代价让路径尽量远离障碍物。</li>
|
||||
</ul>
|
||||
|
||||
<h3>8.3 双启发</h3>
|
||||
<pre>h = max(h_2D, h_RS)</pre>
|
||||
<ul>
|
||||
<li><code>h_2D</code>：从目标反向运行二维Dijkstra得到的绕障距离；</li>
|
||||
<li><code>h_RS</code>：基于当前位姿、目标位姿和最小转弯半径的Reeds–Shepp距离。</li>
|
||||
</ul>
|
||||
<p>所有权重必须外部可配置。</p>
|
||||
</section>
|
||||
|
||||
<section id="step9">
|
||||
<h2><span class="step-id">9</span>终点判定、SQP精修与Reeds–Shepp <span class="status done">已确认</span></h2>
|
||||
<h3>9.1 基本终点容差</h3>
|
||||
<pre>GoalPositionTolerance = 0.15 m
|
||||
GoalHeadingTolerance = 5°</pre>
|
||||
<p>在每个0.05 m内部积分点检查位置和航向误差，避免越过目标。</p>
|
||||
|
||||
<h3>9.2 容差到达与SQP</h3>
|
||||
<p>Hybrid A*可以在容差范围内结束，由后续SQP通过终端约束精确收敛到目标。但必须满足：</p>
|
||||
<ul>
|
||||
<li>粗路径已包含正确绕障、倒车和换向拓扑；</li>
|
||||
<li>当前终点、目标位姿及末端局部区域无碰撞；</li>
|
||||
<li>终点附近存在足够局部调整空间；</li>
|
||||
<li>SQP只负责局部修正，不负责创造新的换向或改变绕障侧。</li>
|
||||
</ul>
|
||||
|
||||
<h3>9.3 目标进入方向</h3>
|
||||
<pre>GoalDirectionConstraint = Any | Forward | Reverse</pre>
|
||||
<p>默认Any。该字段约束最后一段运动方向，不改变目标车头航向定义。</p>
|
||||
|
||||
<h3>9.4 Reeds–Shepp模式</h3>
|
||||
<table>
|
||||
<tr><th>模式</th><th>连接失败后的行为</th><th>适用场景</th></tr>
|
||||
<tr><td>Disabled</td><td>不尝试，满足容差即可结束</td><td>开阔终点、调试对照</td></tr>
|
||||
<tr><td>Opportunistic</td><td>失败继续搜索，满足容差仍可交SQP</td><td>默认、普通导航</td></tr>
|
||||
<tr><td>RequiredNearGoal</td><td>失败不能按容差结束，必须继续找可连接节点</td><td>狭窄钻入、精确泊入</td></tr>
|
||||
</table>
|
||||
|
||||
<h3>9.5 自动触发</h3>
|
||||
<pre>2–5 m：每扩展10个节点尝试一次
|
||||
≤2 m：每扩展3个节点尝试一次
|
||||
采样碰撞步长：0.05 m</pre>
|
||||
<p>连接路径必须检查地图边界、Unknown、扩大车体碰撞、安全净空、最大曲率、方向约束及与当前曲率的衔接。</p>
|
||||
|
||||
<h3>9.6 默认策略</h3>
|
||||
<pre>ReedsSheppMode = Opportunistic</pre>
|
||||
<p>无需人工每次开启，由程序自动触发。任务类型可覆盖默认模式。</p>
|
||||
</section>
|
||||
|
||||
<section id="step10">
|
||||
<h2><span class="step-id">10</span>搜索回溯与原始稠密路径 <span class="status done">已确认</span></h2>
|
||||
<h3>10.1 回溯内容</h3>
|
||||
<p>搜索成功后从终止节点沿父节点索引回溯到起点，反转节点链，并恢复每条原语保存的0.05 m内部积分点。</p>
|
||||
|
||||
<h3>10.2 不能只输出0.5 m搜索节点</h3>
|
||||
<p>只输出原语终点会丢失圆弧形状、碰撞检查细节和换向局部结构。因此必须复用搜索时已经计算的内部点。</p>
|
||||
|
||||
<h3>10.3 路径点字段</h3>
|
||||
<pre>x, y
|
||||
heading, unwrappedHeading
|
||||
curvature
|
||||
direction
|
||||
arcLength
|
||||
clearance
|
||||
isGearSwitchPoint
|
||||
source</pre>
|
||||
|
||||
<h3>10.4 特殊处理</h3>
|
||||
<ul>
|
||||
<li>删除相邻原语重复边界点；</li>
|
||||
<li>中途终止原语只保留真实有效积分点；</li>
|
||||
<li>Reeds–Shepp连接点追加到普通搜索路径之后；</li>
|
||||
<li>机会式容差终点不允许在本步骤被“硬改”为目标点；</li>
|
||||
<li>航向同时保存归一化值和连续解缠值；</li>
|
||||
<li>累计弧长始终单调增加，方向单独保存；</li>
|
||||
<li>按换向点划分Forward/Reverse连续段。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<section id="step11">
|
||||
<h2><span class="step-id">11</span>关键点保留与弧长重采样 <span class="status done">已确认</span></h2>
|
||||
<h3>11.1 处理目标</h3>
|
||||
<p>原始0.05 m稠密路径不直接全部送入平滑和SQP。先保留关键结构，再按照累计弧长重采样。</p>
|
||||
|
||||
<h3>11.2 必须保留的关键点</h3>
|
||||
<ul>
|
||||
<li>起点与终点；</li>
|
||||
<li>所有换向点；</li>
|
||||
<li>曲率等级变化点；</li>
|
||||
<li>直线/弯道切换点；</li>
|
||||
<li>Reeds–Shepp分段和连接点；</li>
|
||||
<li>障碍物附近最小净空点；</li>
|
||||
<li>必要的明显航向变化点。</li>
|
||||
</ul>
|
||||
|
||||
<h3>11.3 分方向重采样</h3>
|
||||
<p>每个Forward或Reverse段独立处理，不能跨越换向点插值。</p>
|
||||
|
||||
<h3>11.4 推荐间距</h3>
|
||||
<pre>普通区域：0.10 m
|
||||
重点区域：0.05 m</pre>
|
||||
<p>以下区域采用0.05 m：</p>
|
||||
<ul>
|
||||
<li>障碍距离小于0.50 m；</li>
|
||||
<li>|κ| ≥ 0.5 κmax；</li>
|
||||
<li>目标2 m范围内；</li>
|
||||
<li>换向点前后0.30 m；</li>
|
||||
<li>曲率变化点前后0.25 m；</li>
|
||||
<li>Reeds–Shepp连接段与窄通道。</li>
|
||||
</ul>
|
||||
|
||||
<h3>11.5 重采样后复核</h3>
|
||||
<p>新插值点必须重新执行车体碰撞检查。最终输出点可为0.10 m，但碰撞验证内部步长仍不得大于0.05 m。</p>
|
||||
</section>
|
||||
|
||||
<section id="step12">
|
||||
<h2><span class="step-id">12</span>快速曲线平滑、局部QP兜底与最终校验 <span class="status done">已确认</span></h2>
|
||||
<h3>12.1 最终采用的分层方案</h3>
|
||||
<div class="flow">
|
||||
<div class="node">按Forward/Reverse方向段拆分</div>
|
||||
<div class="node">普通路径段：三次B样条快速平滑</div>
|
||||
<div class="node">局部短转角：分段三次Bézier</div>
|
||||
<div class="node">末端短连接：五次多项式（可选）</div>
|
||||
<div class="node">0.05 m完整碰撞、净空、曲率验证</div>
|
||||
<div class="node">失败局部：安全走廊约束QP</div>
|
||||
<div class="node">局部QP仍失败：回退第十一步原始可行路径</div>
|
||||
</div>
|
||||
|
||||
<h3>12.2 为什么不默认全路径QP</h3>
|
||||
<p>全路径多轮QP还需要走廊生成、曲率线性化和反复碰撞检测，可能与后续SQP功能重复并增加耗时。因此第一版采用快速曲线优先，仅对失败局部使用QP。</p>
|
||||
|
||||
<h3>12.3 平滑结构约束</h3>
|
||||
<ul>
|
||||
<li>换向点固定，不能使用一条曲线跨越换向；</li>
|
||||
<li>不改变绕障侧、前进/后退顺序和换向次数；</li>
|
||||
<li>Reeds–Shepp精确目标点固定；</li>
|
||||
<li>容差终点不在本步骤强行修改为目标；</li>
|
||||
<li>障碍物附近控制点移动范围收紧。</li>
|
||||
</ul>
|
||||
|
||||
<h3>12.4 典型偏移范围</h3>
|
||||
<pre>普通区域最大偏移：0.10–0.15 m
|
||||
障碍物附近最大偏移：0.02–0.05 m
|
||||
局部QP问题区间：失败点前后各约1.0 m</pre>
|
||||
|
||||
<h3>12.5 航向与曲率</h3>
|
||||
<ul>
|
||||
<li>平滑后必须根据新几何路径重新计算切线与航向；</li>
|
||||
<li>前进段车头航向与路径切线一致；</li>
|
||||
<li>倒车段车头航向与路径切线相差π；</li>
|
||||
<li>区分几何曲率与车辆控制曲率；</li>
|
||||
<li>建议平滑后最大车辆曲率不超过<code>0.95 κmax</code>。</li>
|
||||
</ul>
|
||||
|
||||
<h3>12.6 最终验证</h3>
|
||||
<ul>
|
||||
<li>0.05 m采样完整车体碰撞检测；</li>
|
||||
<li>Unknown与越界检查；</li>
|
||||
<li>安全余量检查；</li>
|
||||
<li>最大曲率与曲率变化检查；</li>
|
||||
<li>起终点、换向点、方向分段和路径连续性检查。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<section id="step13">
|
||||
<h2><span class="step-id">13</span>性能优化阶段 <span class="status todo">整体TODO</span></h2>
|
||||
<p>第十三步不作为第一版主流程实现要求。前12步完整跑通并建立正确性基线后，再进行性能剖析与针对性优化。</p>
|
||||
|
||||
<h3>13.1 第一版仅保留防失控措施</h3>
|
||||
<ul>
|
||||
<li>较宽松的搜索超时；</li>
|
||||
<li>最大扩展节点和最大生成节点限制；</li>
|
||||
<li>取消请求机制；</li>
|
||||
<li>各阶段基础耗时与节点数量统计。</li>
|
||||
</ul>
|
||||
|
||||
<h3>13.2 后续性能TODO</h3>
|
||||
<ol>
|
||||
<li>统计地图处理、Dijkstra、Hybrid A*、碰撞检测、Reeds–Shepp、回溯和平滑耗时；</li>
|
||||
<li>根据剖析结果识别真实瓶颈；</li>
|
||||
<li>优化Open List、Closed Set、节点内存与对象分配；</li>
|
||||
<li>预计算运动原语、航向模板、距离场和启发图；</li>
|
||||
<li>地图裁剪、路径热启动、增量更新、多分辨率搜索；</li>
|
||||
<li>最终验证平均、P95、最坏时间和100 ms达成率。</li>
|
||||
</ol>
|
||||
|
||||
<div class="decision">
|
||||
开发顺序：正确性 → 可行性 → 可复现性 → 性能剖析 → 针对性优化 → 100 ms验收。
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="step14">
|
||||
<h2><span class="step-id">14</span>输入输出接口、错误状态与验收 <span class="status done">已确认</span></h2>
|
||||
<h3>14.1 推荐模块</h3>
|
||||
<pre>HybridAStarPlanner
|
||||
├── MapProcessor
|
||||
├── VehicleModel
|
||||
├── MotionPrimitiveGenerator
|
||||
├── CollisionChecker
|
||||
├── HeuristicProvider
|
||||
├── ReedsSheppConnector
|
||||
├── GoalChecker
|
||||
├── PathBacktracker
|
||||
├── PathResampler
|
||||
├── PathSmoother
|
||||
├── PathValidator
|
||||
└── PlanningDiagnostics</pre>
|
||||
|
||||
<h3>14.2 主接口</h3>
|
||||
<pre>public interface IHybridAStarPlanner
|
||||
{
|
||||
PlanningResult Plan(PlanningRequest request);
|
||||
}</pre>
|
||||
|
||||
<h3>14.3 输入</h3>
|
||||
<ul>
|
||||
<li>二维占据栅格；</li>
|
||||
<li>起点位姿与可选起始方向、起始曲率；</li>
|
||||
<li>目标位姿与目标进入方向；</li>
|
||||
<li>车辆尺寸、安全余量、最大曲率/最小转弯半径；</li>
|
||||
<li>Hybrid A*配置、Reeds–Shepp模式和平滑配置。</li>
|
||||
</ul>
|
||||
|
||||
<h3>14.4 输出</h3>
|
||||
<p>输出不带时间的空间路径点和分段信息：</p>
|
||||
<pre>X, Y
|
||||
Heading, UnwrappedHeading
|
||||
ArcLength
|
||||
GeometricCurvature
|
||||
VehicleCurvature
|
||||
Direction
|
||||
BodyClearance
|
||||
IsGearSwitchPoint
|
||||
Source</pre>
|
||||
|
||||
<h3>14.5 终点到达类型</h3>
|
||||
<pre>ExactByReedsShepp
|
||||
ReachedWithinTolerance
|
||||
ExactAfterDirectSearch</pre>
|
||||
|
||||
<h3>14.6 主要返回状态</h3>
|
||||
<pre>Success
|
||||
SuccessWithToleranceGoal
|
||||
SuccessWithSmoothingFallback
|
||||
InvalidRequest / InvalidMap / InvalidVehicleParameters
|
||||
StartOutsideMap / StartInCollision / StartInUnknownArea
|
||||
GoalOutsideMap / GoalInCollision / GoalInUnknownArea
|
||||
InvalidCurvatureConfiguration
|
||||
SearchTimeout / SearchNodeLimitExceeded / NoFeasiblePath
|
||||
ReedsSheppRequiredButFailed
|
||||
BacktrackingFailed / ResamplingFailed / FinalValidationFailed
|
||||
Cancelled / InternalError</pre>
|
||||
|
||||
<h3>14.7 第一版验收重点</h3>
|
||||
<ul>
|
||||
<li>有效场景能输出完整路径；</li>
|
||||
<li>起点一致，终点满足第九步规则；</li>
|
||||
<li>完整车体全程无碰撞且不进入Unknown；</li>
|
||||
<li>最大曲率不超过车辆上限；</li>
|
||||
<li>前进、倒车和换向结构正确；</li>
|
||||
<li>回溯无断点和异常重复；</li>
|
||||
<li>平滑失败可以回退原路径；</li>
|
||||
<li>失败场景返回明确状态；</li>
|
||||
<li>后续SQP能够读取并收敛。</li>
|
||||
</ul>
|
||||
</section>
|
||||
|
||||
<section id="params">
|
||||
<h2>附录A：第一版推荐配置参数</h2>
|
||||
<table>
|
||||
<tr><th>参数</th><th>推荐初值</th><th>说明</th></tr>
|
||||
<tr><td>MapResolution</td><td>0.05 m</td><td>读取地图配置，不写死</td></tr>
|
||||
<tr><td>HeadingResolution</td><td>5°</td><td>72个航向模板</td></tr>
|
||||
<tr><td>SafetyMargin</td><td>0.03 m</td><td>主要加在车体矩形</td></tr>
|
||||
<tr><td>PrimitiveLength</td><td>0.50 m</td><td>第一版固定</td></tr>
|
||||
<tr><td>IntegrationStep</td><td>0.05 m</td><td>碰撞与积分步长</td></tr>
|
||||
<tr><td>CurvatureLevels</td><td>-1,-0.5,0,0.5,1 × κmax</td><td>5级</td></tr>
|
||||
<tr><td>GoalPositionTolerance</td><td>0.15 m</td><td>Hybrid A*基础容差</td></tr>
|
||||
<tr><td>GoalHeadingTolerance</td><td>5°</td><td>Hybrid A*基础容差</td></tr>
|
||||
<tr><td>HeuristicWeight</td><td>1.3</td><td>Weighted A*</td></tr>
|
||||
<tr><td>ReversePenalty</td><td>1.3</td><td>允许倒车但略微惩罚</td></tr>
|
||||
<tr><td>GearSwitchPenalty</td><td>2.0</td><td>减少频繁换向</td></tr>
|
||||
<tr><td>ReedsSheppMode</td><td>Opportunistic</td><td>默认自动机会式</td></tr>
|
||||
<tr><td>AnalyticExpansionDistance</td><td>5.0 m</td><td>进入后周期尝试</td></tr>
|
||||
<tr><td>NearGoalDistance</td><td>2.0 m</td><td>提高尝试频率</td></tr>
|
||||
<tr><td>AnalyticExpansionInterval</td><td>10节点</td><td>2–5 m</td></tr>
|
||||
<tr><td>NearGoalInterval</td><td>3节点</td><td>≤2 m</td></tr>
|
||||
<tr><td>NormalResampleSpacing</td><td>0.10 m</td><td>普通区域</td></tr>
|
||||
<tr><td>FineResampleSpacing</td><td>0.05 m</td><td>重点区域</td></tr>
|
||||
<tr><td>FineObstacleDistance</td><td>0.50 m</td><td>小于此值加密</td></tr>
|
||||
<tr><td>FineGoalDistance</td><td>2.0 m</td><td>目标附近加密</td></tr>
|
||||
<tr><td>GearSwitchDenseRange</td><td>±0.30 m</td><td>换向点附近</td></tr>
|
||||
<tr><td>CurvatureTransitionDenseRange</td><td>±0.25 m</td><td>曲率切换附近</td></tr>
|
||||
<tr><td>CurvatureLimitRatio</td><td>0.95</td><td>平滑后留控制余量</td></tr>
|
||||
<tr><td>SearchTimeLimitMs</td><td>3000–5000 ms</td><td>第一版防失控，不是性能指标</td></tr>
|
||||
</table>
|
||||
</section>
|
||||
|
||||
<section id="pseudocode">
|
||||
<h2>附录B：总体伪代码</h2>
|
||||
<pre>PlanningResult Plan(request)
|
||||
{
|
||||
ValidateRequest(request);
|
||||
ValidateMap(request.Map);
|
||||
ResolveVehicleCurvatureLimit(request.Vehicle);
|
||||
ValidateStartAndGoal();
|
||||
|
||||
PrepareDistanceFieldIfNeeded();
|
||||
PrepareHeadingFootprintTemplatesIfNeeded();
|
||||
BuildGoalDijkstraHeuristic();
|
||||
|
||||
startNode = CreateStartNode();
|
||||
PushOpen(startNode);
|
||||
|
||||
while (OpenList not empty)
|
||||
{
|
||||
CheckCancellationAndSafetyLimits();
|
||||
|
||||
current = PopBestValidNode();
|
||||
|
||||
if (ShouldTryReedsShepp(current))
|
||||
{
|
||||
rsPath = TryReedsShepp(current, goal);
|
||||
if (ValidateAnalyticPath(rsPath))
|
||||
return BuildFinalResult(current, rsPath);
|
||||
}
|
||||
|
||||
if (GoalChecker.CanTerminateByTolerance(current))
|
||||
return BuildFinalResult(current, noAnalyticPath);
|
||||
|
||||
foreach (primitive in GenerateAllowedPrimitives(current))
|
||||
{
|
||||
integrated = IntegratePrimitive(
|
||||
current,
|
||||
primitive,
|
||||
step = 0.05 m,
|
||||
maxLength = 0.50 m);
|
||||
|
||||
if (!integrated.Valid)
|
||||
continue;
|
||||
|
||||
child = CreateChildNode(integrated);
|
||||
if (!ImproveBestCost(child))
|
||||
continue;
|
||||
|
||||
PushOpen(child);
|
||||
}
|
||||
}
|
||||
|
||||
return Failure(NoFeasiblePath);
|
||||
}
|
||||
|
||||
BuildFinalResult(goalNode, analyticPath)
|
||||
{
|
||||
rawDense = BacktrackAndRestorePrimitivePoints(goalNode);
|
||||
AppendAnalyticPathIfAny(rawDense, analyticPath);
|
||||
|
||||
segmented = SplitByMotionDirection(rawDense);
|
||||
resampled = PreserveKeyPointsAndResample(segmented);
|
||||
|
||||
smoothed = TryFastSplineSmoothing(resampled);
|
||||
if (!ValidatePath(smoothed))
|
||||
smoothed = TryLocalCurveOrQPFallback(resampled);
|
||||
|
||||
if (!ValidatePath(smoothed))
|
||||
smoothed = resampled;
|
||||
|
||||
if (!ValidatePath(smoothed))
|
||||
return Failure(FinalValidationFailed);
|
||||
|
||||
return Success(smoothed, diagnostics);
|
||||
}</pre>
|
||||
</section>
|
||||
|
||||
<section id="milestones">
|
||||
<h2>附录C：建议开发里程碑</h2>
|
||||
<table>
|
||||
<tr><th>阶段</th><th>范围</th><th>完成标准</th></tr>
|
||||
<tr><td>M0 数据与工具</td><td>Pose、地图、车辆参数、角度/坐标工具</td><td>单元测试通过</td></tr>
|
||||
<tr><td>M1 运动学与碰撞</td><td>恒曲率积分、航向模板、矩形碰撞</td><td>可视化验证不同角度无漏检</td></tr>
|
||||
<tr><td>M2 最小Hybrid A*</td><td>无障碍、只前进、基础启发</td><td>稳定从起点到终点</td></tr>
|
||||
<tr><td>M3 障碍与倒车</td><td>Dijkstra、前进/倒车、换向代价</td><td>绕障与一次倒车场景通过</td></tr>
|
||||
<tr><td>M4 终点模块</td><td>容差、GoalDirection、Reeds–Shepp三模式</td><td>普通与狭窄目标场景通过</td></tr>
|
||||
<tr><td>M5 后处理</td><td>回溯、0.05 m稠密点、分段、重采样</td><td>路径结构无断点、换向明确</td></tr>
|
||||
<tr><td>M6 平滑与复核</td><td>B样条、Bézier、局部QP兜底</td><td>不改变拓扑，失败可回退</td></tr>
|
||||
<tr><td>M7 接口与集成</td><td>PlanningRequest/Result、状态、诊断</td><td>可接入SQP</td></tr>
|
||||
<tr><td>M8 性能TODO</td><td>剖析、优化、100 ms目标</td><td>在正确性基线后执行</td></tr>
|
||||
</table>
|
||||
</section>
|
||||
</main>
|
||||
</div>
|
||||
|
||||
<footer>
|
||||
文档用途：四舵轮AMR第一阶段非结构化道路粗路径规划开发依据。内部单位统一采用米、弧度、1/米。
|
||||
</footer>
|
||||
</body>
|
||||
</html>
|
||||
@@ -1,4 +1,4 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed partial class EmPlannerConfiguration
|
||||
{
|
||||
@@ -17,8 +17,8 @@ public sealed partial class EmPlannerConfiguration
|
||||
Scheduling = new SchedulingConfiguration
|
||||
{
|
||||
ReplanPeriodSeconds = 0.20d,
|
||||
TimeHorizonSeconds = 6d,
|
||||
DistanceHorizonMeters = 5d,
|
||||
TimeHorizonSeconds = 6d, //窗口的规划时间长度。单位s
|
||||
DistanceHorizonMeters = 500d, //最大探索s里程距离，单位m
|
||||
OutputTimeStepSeconds = 0.05d,
|
||||
SolverTimeoutSeconds = 0.10d,
|
||||
HandoffLookaheadSeconds = 0.30d,
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
@@ -43,11 +44,19 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
EmitDebug(request, "direction-segment selection succeeded");
|
||||
|
||||
var projector = new FrenetProjector();
|
||||
if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, segment.LengthMeters,
|
||||
double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
|
||||
? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
|
||||
: segment.LengthMeters;
|
||||
if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
|
||||
configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
|
||||
{
|
||||
return Failure(EmPlanningStatus.ProjectionFailed, request,
|
||||
"Vehicle pose could not be projected inside the selected direction segment.");
|
||||
"Vehicle pose could not be projected at an admissible start of the selected direction segment.");
|
||||
}
|
||||
if (Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
|
||||
{
|
||||
return Failure(EmPlanningStatus.ProjectionFailed, request,
|
||||
"Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
|
||||
}
|
||||
EmitDebug(request, "bounded ego projection succeeded");
|
||||
|
||||
@@ -78,9 +87,13 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
|
||||
var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
|
||||
request.Vehicle, configuration, previousSeed);
|
||||
TimeSpan totalSolveBudget = TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds);
|
||||
var solveBudgetStopwatch = Stopwatch.StartNew();
|
||||
LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
|
||||
if (!IsSuccess(lateral.Status))
|
||||
return Failure(lateral.Status, request, lateral.FailureReason);
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
|
||||
EmitDebug(request, "LS optimization and validation succeeded");
|
||||
|
||||
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
|
||||
@@ -106,8 +119,16 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
new LongitudinalPreviousTrajectorySeedBuilder().Build(
|
||||
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
|
||||
segment.SegmentIndex, segment.Direction);
|
||||
TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
|
||||
if (remainingSolveBudget <= TimeSpan.Zero)
|
||||
{
|
||||
return Failure(EmPlanningStatus.SolverTimedOut, request,
|
||||
"LS/ST optimization exhausted the shared solve budget before ST optimization.");
|
||||
}
|
||||
EmPlannerConfiguration longitudinalConfiguration = configuration.Copy();
|
||||
longitudinalConfiguration.Scheduling.SolverTimeoutSeconds = remainingSolveBudget.TotalSeconds;
|
||||
var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
|
||||
initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
|
||||
initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, longitudinalConfiguration,
|
||||
request.PlanningScope, knotSchedule,
|
||||
previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
|
||||
envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
|
||||
@@ -118,6 +139,8 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
|
||||
if (!IsSuccess(longitudinal.Status))
|
||||
return Failure(longitudinal.Status, request, longitudinal.FailureReason);
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
|
||||
EmitDebug(request, "ST optimization and validation succeeded");
|
||||
|
||||
var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
|
||||
@@ -128,6 +151,8 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
|
||||
if (assemblyStatus != EmPlanningStatus.Success)
|
||||
return Failure(assemblyStatus, request, assemblyFailure);
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
|
||||
EmitDebug(request, "trajectory assembly succeeded");
|
||||
|
||||
EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
|
||||
@@ -135,7 +160,7 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
? TerminalPose(lateral.Path)
|
||||
: null;
|
||||
EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
|
||||
request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS,
|
||||
request.Vehicle, configuration, segment.SegmentIndex, segment.LengthMeters, longitudinalInput.PathUpperBoundS,
|
||||
terminalPose, terminalBoundary);
|
||||
if (!publication.IsValid)
|
||||
{
|
||||
@@ -144,6 +169,9 @@ public sealed class EmPlanningService : IEmPlanningService
|
||||
}
|
||||
EmitDebug(request, "world-space publication validation succeeded");
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");
|
||||
|
||||
EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
|
||||
longitudinal.Status == EmPlanningStatus.SuccessWithFallback
|
||||
? EmPlanningStatus.SuccessWithFallback
|
||||
|
||||
@@ -35,7 +35,7 @@ public sealed class LateralConstraintBuilder
|
||||
_objectiveBuilder.AddTerms(input, layout, linearization, hessian, linearCost);
|
||||
|
||||
int terminalRows = input.TerminalType == EmTerminalType.RollingSafetyStop ? 0 : 2;
|
||||
var constraints = new SparseTripletBuilder(7 * layout.StationCount - 2 + terminalRows, layout.VariableCount);
|
||||
var constraints = new SparseTripletBuilder(8 * layout.StationCount - 2 + terminalRows, layout.VariableCount);
|
||||
var lower = new List<double>();
|
||||
var upper = new List<double>();
|
||||
int row = 0;
|
||||
@@ -43,12 +43,13 @@ public sealed class LateralConstraintBuilder
|
||||
if (!TryAddLateralBounds(input, layout, linearization, constraints, lower, upper, ref row, out failureReason))
|
||||
return false;
|
||||
AddDerivativeBounds(input, layout, constraints, lower, upper, ref row);
|
||||
AddCurvatureBounds(input, layout, linearization, constraints, lower, upper, ref row);
|
||||
AddStartConstraints(input, layout, constraints, lower, upper, ref row);
|
||||
AddExactDynamics(input.ReferenceStations, layout, constraints, lower, upper, ref row);
|
||||
if (input.TerminalType != EmTerminalType.RollingSafetyStop)
|
||||
AddTerminalConstraints(layout, constraints, lower, upper, ref row);
|
||||
|
||||
if (row != 7 * layout.StationCount - 2 + terminalRows)
|
||||
if (row != 8 * layout.StationCount - 2 + terminalRows)
|
||||
throw new InvalidOperationException("Lateral constraint row accounting is inconsistent.");
|
||||
problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
|
||||
return true;
|
||||
@@ -131,6 +132,21 @@ public sealed class LateralConstraintBuilder
|
||||
AddSingleVariableRow(constraints, lower, upper, ref row, layout.DDDL(interval), -third, third);
|
||||
}
|
||||
|
||||
private static void AddCurvatureBounds(LateralPlanningInput input, LateralVariableLayout layout,
|
||||
LateralCandidate linearization, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
|
||||
ref int row)
|
||||
{
|
||||
double maximumCurvature = LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle);
|
||||
IReadOnlyList<LateralCurvatureLinearization> affines = LateralCurvatureLinearization.Create(input, layout,
|
||||
linearization);
|
||||
for (int station = 0; station < affines.Count; station++)
|
||||
{
|
||||
LateralCurvatureLinearization affine = affines[station];
|
||||
AddRow(constraints, lower, upper, ref row, -maximumCurvature - affine.Constant,
|
||||
maximumCurvature - affine.Constant, affine.VariableIndices, affine.Gradient);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AddStartConstraints(LateralPlanningInput input, LateralVariableLayout layout,
|
||||
SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
|
||||
{
|
||||
|
||||
@@ -0,0 +1,101 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Shared affine vehicle-curvature model used by the LS objective and hard QP constraints.</summary>
|
||||
internal sealed class LateralCurvatureLinearization
|
||||
{
|
||||
private LateralCurvatureLinearization(int[] variableIndices, double[] gradient, double constant)
|
||||
{
|
||||
VariableIndices = variableIndices;
|
||||
Gradient = gradient;
|
||||
Constant = constant;
|
||||
}
|
||||
|
||||
internal IReadOnlyList<int> VariableIndices { get; }
|
||||
|
||||
internal IReadOnlyList<double> Gradient { get; }
|
||||
|
||||
internal double Constant { get; }
|
||||
|
||||
internal static IReadOnlyList<LateralCurvatureLinearization> Create(LateralPlanningInput input,
|
||||
LateralVariableLayout layout, LateralCandidate linearization)
|
||||
{
|
||||
if (input == null) throw new ArgumentNullException(nameof(input));
|
||||
if (layout == null) throw new ArgumentNullException(nameof(layout));
|
||||
if (linearization == null) throw new ArgumentNullException(nameof(linearization));
|
||||
if (layout.StationCount != input.ReferenceStations.Count ||
|
||||
linearization.ReferenceStations.Count != layout.StationCount)
|
||||
{
|
||||
throw new ArgumentException("Curvature linearization stations must match the lateral layout.", nameof(linearization));
|
||||
}
|
||||
|
||||
double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
var affines = new List<LateralCurvatureLinearization>(layout.StationCount);
|
||||
for (int station = 0; station < layout.StationCount; station++)
|
||||
{
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
|
||||
input.ReferenceStations[station]);
|
||||
double l = linearization.L[station];
|
||||
double dl = linearization.DL[station];
|
||||
double ddl = linearization.DDL[station];
|
||||
double referenceCurvature = reference.GeometricCurvature;
|
||||
double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
|
||||
double a = 1d - referenceCurvature * l;
|
||||
double denominatorSquared = a * a + dl * dl;
|
||||
if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
|
||||
throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));
|
||||
|
||||
double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
|
||||
double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
|
||||
double numerator = a * a * referenceCurvature + a * ddl +
|
||||
referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
|
||||
double geometricCurvature = numerator / denominatorPow3Over2;
|
||||
double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
|
||||
referenceCurvature * ddl + referenceCurvatureDerivative * dl;
|
||||
double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
|
||||
double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
|
||||
double dDenominatorSquaredDSlope = 2d * dl;
|
||||
double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
|
||||
1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
|
||||
double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
|
||||
1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
|
||||
double dGeometricDSecondDerivative = a / denominatorPow3Over2;
|
||||
double vehicleCurvature = directionSign * geometricCurvature;
|
||||
double[] gradient =
|
||||
{
|
||||
directionSign * dGeometricDLateral,
|
||||
directionSign * dGeometricDSlope,
|
||||
directionSign * dGeometricDSecondDerivative,
|
||||
};
|
||||
double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
|
||||
if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
|
||||
!IsFinite(gradient[1]) || !IsFinite(gradient[2]))
|
||||
{
|
||||
throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
|
||||
}
|
||||
affines.Add(new LateralCurvatureLinearization(
|
||||
new[] { layout.L(station), layout.DL(station), layout.DDL(station) }, gradient, constant));
|
||||
}
|
||||
return affines;
|
||||
}
|
||||
|
||||
internal static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
|
||||
{
|
||||
if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
|
||||
double maximum = vehicle.MaximumCurvaturePerMeter ??
|
||||
(vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d
|
||||
? 1d / vehicle.MinimumTurningRadiusMeters.Value
|
||||
: double.NaN);
|
||||
if (!IsFinite(maximum) || maximum <= 0d)
|
||||
throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -27,7 +27,8 @@ public sealed class LateralObjectiveBuilder
|
||||
double slopeScale = RequirePositive(lateral.MaximumLateralSlope, "slope scale");
|
||||
double secondDerivativeScale = RequirePositive(lateral.MaximumLateralSecondDerivativePerMeter, "second-derivative scale");
|
||||
double thirdDerivativeScale = RequirePositive(lateral.MaximumLateralThirdDerivativePerSquareMeter, "third-derivative scale");
|
||||
double curvatureScale = RequirePositive(GetMaximumVehicleCurvature(input.Vehicle), "curvature scale");
|
||||
double curvatureScale = RequirePositive(LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle),
|
||||
"curvature scale");
|
||||
double curvatureVariationScale = GetCurvatureVariationScale(input);
|
||||
|
||||
for (int station = 0; station < layout.StationCount; station++)
|
||||
@@ -46,10 +47,11 @@ public sealed class LateralObjectiveBuilder
|
||||
}
|
||||
|
||||
AddPreviousTrajectoryTerms(input, layout, hessian, linearCost, weights.PreviousTrajectory, lateralScale);
|
||||
CurvatureAffine[] curvature = CreateCurvatureAffines(input, layout, linearization);
|
||||
for (int station = 0; station < curvature.Length; station++)
|
||||
IReadOnlyList<LateralCurvatureLinearization> curvature = LateralCurvatureLinearization.Create(input, layout,
|
||||
linearization);
|
||||
for (int station = 0; station < curvature.Count; station++)
|
||||
{
|
||||
AddSquaredResidual(hessian, linearCost, curvature[station].Indices, curvature[station].Gradient,
|
||||
AddSquaredResidual(hessian, linearCost, curvature[station].VariableIndices, curvature[station].Gradient,
|
||||
curvature[station].Constant, weights.Curvature, curvatureScale);
|
||||
}
|
||||
AddCurvatureVariationTerms(input.ReferenceStations, curvature, hessian, linearCost, weights.CurvatureVariation,
|
||||
@@ -75,79 +77,27 @@ public sealed class LateralObjectiveBuilder
|
||||
}
|
||||
}
|
||||
|
||||
private static CurvatureAffine[] CreateCurvatureAffines(LateralPlanningInput input, LateralVariableLayout layout,
|
||||
LateralCandidate linearization)
|
||||
{
|
||||
var affines = new CurvatureAffine[layout.StationCount];
|
||||
double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
for (int station = 0; station < layout.StationCount; station++)
|
||||
{
|
||||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
|
||||
input.ReferenceStations[station]);
|
||||
double l = linearization.L[station];
|
||||
double dl = linearization.DL[station];
|
||||
double ddl = linearization.DDL[station];
|
||||
double referenceCurvature = reference.GeometricCurvature;
|
||||
double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
|
||||
double a = 1d - referenceCurvature * l;
|
||||
double denominatorSquared = a * a + dl * dl;
|
||||
if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
|
||||
throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));
|
||||
|
||||
double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
|
||||
double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
|
||||
double numerator = a * a * referenceCurvature + a * ddl +
|
||||
referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
|
||||
double geometricCurvature = numerator / denominatorPow3Over2;
|
||||
double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
|
||||
referenceCurvature * ddl + referenceCurvatureDerivative * dl;
|
||||
double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
|
||||
double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
|
||||
double dDenominatorSquaredDSlope = 2d * dl;
|
||||
double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
|
||||
1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
|
||||
double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
|
||||
1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
|
||||
double dGeometricDSecondDerivative = a / denominatorPow3Over2;
|
||||
double vehicleCurvature = directionSign * geometricCurvature;
|
||||
double[] gradient =
|
||||
{
|
||||
directionSign * dGeometricDLateral,
|
||||
directionSign * dGeometricDSlope,
|
||||
directionSign * dGeometricDSecondDerivative,
|
||||
};
|
||||
double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
|
||||
if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
|
||||
!IsFinite(gradient[1]) || !IsFinite(gradient[2]))
|
||||
{
|
||||
throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
|
||||
}
|
||||
affines[station] = new CurvatureAffine(new[] { layout.L(station), layout.DL(station), layout.DDL(station) },
|
||||
gradient, constant);
|
||||
}
|
||||
return affines;
|
||||
}
|
||||
|
||||
private static void AddCurvatureVariationTerms(IReadOnlyList<double> stations, CurvatureAffine[] curvature,
|
||||
private static void AddCurvatureVariationTerms(IReadOnlyList<double> stations,
|
||||
IReadOnlyList<LateralCurvatureLinearization> curvature,
|
||||
SparseTripletBuilder hessian, IList<double> linearCost, double weight, double scale)
|
||||
{
|
||||
for (int station = 0; station < curvature.Length; station++)
|
||||
for (int station = 0; station < curvature.Count; station++)
|
||||
{
|
||||
int lower = station == 0 ? 0 : station - 1;
|
||||
int upper = station == curvature.Length - 1 ? curvature.Length - 1 : station + 1;
|
||||
int upper = station == curvature.Count - 1 ? curvature.Count - 1 : station + 1;
|
||||
double ds = stations[upper] - stations[lower];
|
||||
if (!IsFinite(ds) || ds <= 0d)
|
||||
throw new ArgumentException("Curvature variation requires strictly increasing stations.", nameof(stations));
|
||||
CurvatureAffine left = curvature[lower];
|
||||
CurvatureAffine right = curvature[upper];
|
||||
var indices = new int[left.Indices.Length + right.Indices.Length];
|
||||
LateralCurvatureLinearization left = curvature[lower];
|
||||
LateralCurvatureLinearization right = curvature[upper];
|
||||
var indices = new int[left.VariableIndices.Count + right.VariableIndices.Count];
|
||||
var gradient = new double[indices.Length];
|
||||
for (int index = 0; index < left.Indices.Length; index++)
|
||||
for (int index = 0; index < left.VariableIndices.Count; index++)
|
||||
{
|
||||
indices[index] = left.Indices[index];
|
||||
indices[index] = left.VariableIndices[index];
|
||||
gradient[index] = -left.Gradient[index] / ds;
|
||||
indices[left.Indices.Length + index] = right.Indices[index];
|
||||
gradient[left.Indices.Length + index] = right.Gradient[index] / ds;
|
||||
indices[left.VariableIndices.Count + index] = right.VariableIndices[index];
|
||||
gradient[left.VariableIndices.Count + index] = right.Gradient[index] / ds;
|
||||
}
|
||||
AddSquaredResidual(hessian, linearCost, indices, gradient, (right.Constant - left.Constant) / ds, weight, scale);
|
||||
}
|
||||
@@ -209,17 +159,6 @@ public sealed class LateralObjectiveBuilder
|
||||
return Math.Max(1d, maximum);
|
||||
}
|
||||
|
||||
private static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
|
||||
{
|
||||
if (vehicle == null)
|
||||
throw new ArgumentNullException(nameof(vehicle));
|
||||
if (vehicle.MaximumCurvaturePerMeter.HasValue)
|
||||
return vehicle.MaximumCurvaturePerMeter.Value;
|
||||
if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d)
|
||||
return 1d / vehicle.MinimumTurningRadiusMeters.Value;
|
||||
throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
|
||||
}
|
||||
|
||||
private static double RequirePositive(double value, string name)
|
||||
{
|
||||
if (!IsFinite(value) || value <= 0d)
|
||||
@@ -232,17 +171,4 @@ public sealed class LateralObjectiveBuilder
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private sealed class CurvatureAffine
|
||||
{
|
||||
public CurvatureAffine(int[] indices, double[] gradient, double constant)
|
||||
{
|
||||
Indices = indices;
|
||||
Gradient = gradient;
|
||||
Constant = constant;
|
||||
}
|
||||
|
||||
public int[] Indices { get; }
|
||||
public double[] Gradient { get; }
|
||||
public double Constant { get; }
|
||||
}
|
||||
}
|
||||
|
||||
@@ -68,6 +68,8 @@ public sealed class SequentialConvexOptimizer
|
||||
remainingBudget, settings.EnableWarmStart, settings.EnablePolishing, settings.EnableNativeVerboseOutput),
|
||||
warmStart, cancellationToken);
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failed(EmPlanningStatus.Cancelled, "Lateral SQP was cancelled after the QP solve.");
|
||||
if (solved == null)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed, "The lateral QP solver returned no result.");
|
||||
|
||||
@@ -100,16 +102,27 @@ public sealed class SequentialConvexOptimizer
|
||||
continue;
|
||||
if (!_geometryEvaluator.TryEvaluate(input, candidate, out LateralPath evaluatedPath, out _))
|
||||
continue;
|
||||
|
||||
// A geometrically evaluable QP solution that remains inside the static
|
||||
// corridor is the next SQP linearization point, even when the independent
|
||||
// validator rejects it. Otherwise the next outer pass rebuilds the identical
|
||||
// convex subproblem and cannot correct that result. An out-of-corridor vector
|
||||
// must not become an iterate: it can make the following trust region infeasible.
|
||||
LateralCandidate previousIterate = iterate;
|
||||
if (RespectsStaticCorridor(input, candidate))
|
||||
{
|
||||
iterate = candidate;
|
||||
warmStart = CopyValues(solved.Primal);
|
||||
}
|
||||
|
||||
if (!_solutionValidator.TryValidate(input, candidate, evaluatedPath, out LateralPath validatedPath, out _))
|
||||
continue;
|
||||
|
||||
double maximumLateralChange = MaximumLateralChange(iterate, candidate);
|
||||
double maximumLateralChange = MaximumLateralChange(previousIterate, candidate);
|
||||
double relativeObjectiveImprovement = hasPreviousObjective
|
||||
? RelativeObjectiveImprovement(previousObjective, solved.Objective)
|
||||
: double.PositiveInfinity;
|
||||
lastValidatedPath = CopyPath(validatedPath);
|
||||
iterate = candidate;
|
||||
warmStart = CopyValues(solved.Primal);
|
||||
previousObjective = solved.Objective;
|
||||
hasPreviousObjective = true;
|
||||
|
||||
@@ -119,7 +132,10 @@ public sealed class SequentialConvexOptimizer
|
||||
|
||||
return lastValidatedPath == null
|
||||
? Failed(EmPlanningStatus.LateralInfeasible, "No independently validated lateral candidate was found.")
|
||||
: new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
|
||||
: new LateralPlanningResult(
|
||||
EmPlanningStatus.SuccessWithFallback,
|
||||
lastValidatedPath,
|
||||
"Lateral SQP reached its outer-iteration limit before strict convergence.");
|
||||
}
|
||||
|
||||
private static bool TryCreateSettings(LateralPlanningInput input, out QpSolverSettings settings, out TimeSpan totalBudget,
|
||||
@@ -226,6 +242,17 @@ public sealed class SequentialConvexOptimizer
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static bool RespectsStaticCorridor(LateralPlanningInput input, LateralCandidate candidate)
|
||||
{
|
||||
for (int index = 0; index < candidate.L.Count; index++)
|
||||
{
|
||||
LateralInterval interval = input.Corridor.Stations[index];
|
||||
if (candidate.L[index] < interval.MinimumL || candidate.L[index] > interval.MaximumL)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double RelativeObjectiveImprovement(double previous, double current)
|
||||
{
|
||||
return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
|
||||
@@ -233,7 +260,7 @@ public sealed class SequentialConvexOptimizer
|
||||
|
||||
private static LateralPlanningResult FallbackOrFailure(LateralPath path, EmPlanningStatus failureStatus, string failureReason)
|
||||
{
|
||||
return path == null
|
||||
return failureStatus == EmPlanningStatus.Cancelled || path == null
|
||||
? Failed(failureStatus, failureReason)
|
||||
: new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, path, failureReason);
|
||||
}
|
||||
|
||||
+9
-1
@@ -119,6 +119,8 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
settings.EnablePolishing,
|
||||
settings.EnableNativeVerboseOutput),
|
||||
warmStart, cancellationToken);
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled after the QP solve.");
|
||||
if (solved == null)
|
||||
return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
|
||||
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
||||
@@ -293,6 +295,12 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
settings.EnablePolishing, settings.EnableNativeVerboseOutput),
|
||||
ToPrimal(linearizationIterate), cancellationToken);
|
||||
projectionSolveCount++;
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
failureStatus = EmPlanningStatus.Cancelled;
|
||||
failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled after the QP solve.");
|
||||
return false;
|
||||
}
|
||||
if (solved == null)
|
||||
{
|
||||
failureStatus = EmPlanningStatus.Failed;
|
||||
@@ -1196,7 +1204,7 @@ public sealed class SequentialLongitudinalOptimizer
|
||||
private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
|
||||
EmPlanningStatus failureStatus, string failureReason)
|
||||
{
|
||||
return candidate == null
|
||||
return failureStatus == EmPlanningStatus.Cancelled || candidate == null
|
||||
? Failed(failureStatus, failureReason)
|
||||
: new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
|
||||
}
|
||||
|
||||
@@ -81,7 +81,7 @@ public sealed class EmTrajectoryAssembler
|
||||
EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None;
|
||||
double signedSpeed = directionSign * sample.ProgressSpeed;
|
||||
points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart,
|
||||
geometry.VehicleCurvature, metadata.SegmentIndex, sample.PathS, sample.PathS, metadata.Direction,
|
||||
geometry.VehicleCurvature, metadata.SegmentIndex, geometry.ReferenceS, sample.PathS, metadata.Direction,
|
||||
boundaryType, sample.Acceleration, sample.Jerk));
|
||||
}
|
||||
|
||||
|
||||
@@ -46,6 +46,7 @@ internal sealed class LateralPathInterpolator
|
||||
LateralPathPoint left = path.Points[index - 1];
|
||||
double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
|
||||
return new InterpolatedLateralPathPoint(
|
||||
Linear(left.ReferenceS, right.ReferenceS, ratio),
|
||||
Linear(left.X, right.X, ratio),
|
||||
Linear(left.Y, right.Y, ratio),
|
||||
NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
|
||||
@@ -58,7 +59,8 @@ internal sealed class LateralPathInterpolator
|
||||
|
||||
private static InterpolatedLateralPathPoint From(LateralPathPoint point)
|
||||
{
|
||||
return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
|
||||
return new InterpolatedLateralPathPoint(point.ReferenceS, point.X, point.Y, NormalizeYaw(point.VehicleYaw),
|
||||
point.VehicleCurvature);
|
||||
}
|
||||
|
||||
private static double Linear(double left, double right, double ratio)
|
||||
@@ -84,14 +86,16 @@ internal sealed class LateralPathInterpolator
|
||||
|
||||
internal sealed class InterpolatedLateralPathPoint
|
||||
{
|
||||
public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
|
||||
public InterpolatedLateralPathPoint(double referenceS, double x, double y, double yaw, double vehicleCurvature)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
X = x;
|
||||
Y = y;
|
||||
Yaw = yaw;
|
||||
VehicleCurvature = vehicleCurvature;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double Yaw { get; }
|
||||
|
||||
@@ -81,7 +81,7 @@ public sealed class EmTrajectoryValidator
|
||||
public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
|
||||
EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, EmBoundaryType terminalBoundary)
|
||||
{
|
||||
return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, null,
|
||||
return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, double.PositiveInfinity, pathUpperBoundS, null,
|
||||
terminalBoundary, false);
|
||||
}
|
||||
|
||||
@@ -89,12 +89,34 @@ public sealed class EmTrajectoryValidator
|
||||
EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
|
||||
EmBoundaryType terminalBoundary)
|
||||
{
|
||||
return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, terminalPose,
|
||||
return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, double.PositiveInfinity, pathUpperBoundS, terminalPose,
|
||||
terminalBoundary, true);
|
||||
}
|
||||
|
||||
public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
|
||||
EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS,
|
||||
EmBoundaryType terminalBoundary)
|
||||
{
|
||||
if (!IsFinite(segmentUpperBoundS) || segmentUpperBoundS < 0d)
|
||||
return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
|
||||
"Segment-local trajectory bounds are invalid.");
|
||||
return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, segmentUpperBoundS, pathUpperBoundS,
|
||||
null, terminalBoundary, false);
|
||||
}
|
||||
|
||||
public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
|
||||
EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS,
|
||||
Pose2D terminalPose, EmBoundaryType terminalBoundary)
|
||||
{
|
||||
if (!IsFinite(segmentUpperBoundS) || segmentUpperBoundS < 0d)
|
||||
return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
|
||||
"Segment-local trajectory bounds are invalid.");
|
||||
return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, segmentUpperBoundS, pathUpperBoundS,
|
||||
terminalPose, terminalBoundary, true);
|
||||
}
|
||||
|
||||
private EmTrajectoryValidationResult ValidateCore(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
|
||||
EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
|
||||
EmPlannerConfiguration configuration, int segmentIndex, double segmentUpperBoundS, double pathUpperBoundS, Pose2D terminalPose,
|
||||
EmBoundaryType terminalBoundary, bool requiresTerminalPose)
|
||||
{
|
||||
if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
|
||||
@@ -111,7 +133,7 @@ public sealed class EmTrajectoryValidator
|
||||
EmTrajectoryPoint point = trajectory.Points[index];
|
||||
if (point == null || !HasOnlyFiniteValues(point))
|
||||
return Reject(EmTrajectoryValidationFailure.NonFinite, index, "Trajectory contains a non-finite point.");
|
||||
if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > pathUpperBoundS + limits.SpatialTolerance ||
|
||||
if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > segmentUpperBoundS + limits.SpatialTolerance ||
|
||||
point.PathS > pathUpperBoundS + limits.SpatialTolerance)
|
||||
{
|
||||
return Reject(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, index,
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,92 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 建图阶段使用的可变环境占据栅格。
|
||||
///
|
||||
/// 单位：边界、世界查询和栅格边长均为 mm。
|
||||
/// 注意：只有 <see cref="MapObstacleRasterizer"/> 可以写入占据状态；规划阶段应改用不可变的 <see cref="PlanningGridMap"/>。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentGridMap
|
||||
{
|
||||
private readonly byte[] _cells;
|
||||
private int _occupiedCount;
|
||||
|
||||
/// <summary>
|
||||
/// 创建空的环境占据栅格。
|
||||
///
|
||||
/// 参数：bounds 为左闭右开的世界边界，单位 mm；resolutionMm 为格边长，单位 mm。
|
||||
/// 返回：无；边界为空或分辨率不合法时抛出异常。
|
||||
/// </summary>
|
||||
public EnvironmentGridMap(MapBoundsMm bounds, float resolutionMm)
|
||||
{
|
||||
if (bounds == null) throw new ArgumentNullException(nameof(bounds));
|
||||
bounds.GetDimensions(resolutionMm, out int rows, out int cols);
|
||||
Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
|
||||
_cells = new byte[checked(rows * cols)];
|
||||
}
|
||||
|
||||
/// <summary>地图世界边界，单位 mm，采用左闭右开规则。</summary>
|
||||
public MapBoundsMm Bounds { get; }
|
||||
/// <summary>单个栅格边长，单位 mm。</summary>
|
||||
public float ResolutionMm { get; }
|
||||
/// <summary>栅格行数，Y 方向从下限向上递增。</summary>
|
||||
public int Rows { get; }
|
||||
/// <summary>栅格列数，X 方向从下限向右递增。</summary>
|
||||
public int Cols { get; }
|
||||
/// <summary>当前已被标记为障碍的格数。</summary>
|
||||
public int OccupiedCount { get { return _occupiedCount; } }
|
||||
|
||||
/// <summary>判断行列索引是否有效。参数 row、col 分别为从零开始的行和列；有效时返回 true。</summary>
|
||||
public bool IsInBounds(int row, int col) { return row >= 0 && row < Rows && col >= 0 && col < Cols; }
|
||||
/// <summary>判断世界坐标是否位于地图内。参数 xMm、yMm 单位为 mm；上边界与右边界返回 false。</summary>
|
||||
public bool IsWorldInBounds(float xMm, float yMm) { return Bounds.Contains(xMm, yMm); }
|
||||
|
||||
/// <summary>
|
||||
/// 将世界坐标转换为栅格索引。
|
||||
///
|
||||
/// 参数：xMm、yMm 为世界坐标，单位 mm；row、col 为输出索引。
|
||||
/// 返回：坐标在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
|
||||
/// </summary>
|
||||
public bool TryWorldToGrid(float xMm, float yMm, out int row, out int col)
|
||||
{
|
||||
row = -1; col = -1;
|
||||
if (!IsWorldInBounds(xMm, yMm)) return false;
|
||||
col = (int)Math.Floor(((double)xMm - Bounds.XMin) / ResolutionMm);
|
||||
row = (int)Math.Floor(((double)yMm - Bounds.YMin) / ResolutionMm);
|
||||
return IsInBounds(row, col);
|
||||
}
|
||||
|
||||
/// <summary>查询栅格是否占据。越界索引按障碍处理，返回 true。</summary>
|
||||
public bool IsOccupied(int row, int col) { return !IsInBounds(row, col) || _cells[row * Cols + col] != 0; }
|
||||
/// <summary>按世界坐标查询占据状态。参数 xMm、yMm 单位为 mm；坐标越界时保守地返回 true。</summary>
|
||||
public bool IsOccupiedWorld(float xMm, float yMm)
|
||||
{
|
||||
return !TryWorldToGrid(xMm, yMm, out int row, out int col) || IsOccupied(row, col);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取一个栅格的世界坐标范围。
|
||||
///
|
||||
/// 参数：row、col 为有效索引；xMin、xMax、yMin、yMax 为输出边界，单位 mm。
|
||||
/// 返回：无；索引越界时抛出 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
public void GetCellBounds(int row, int col, out float xMin, out float xMax, out float yMin, out float yMax)
|
||||
{
|
||||
if (!IsInBounds(row, col)) throw new ArgumentOutOfRangeException();
|
||||
xMin = Bounds.XMin + col * ResolutionMm;
|
||||
yMin = Bounds.YMin + row * ResolutionMm;
|
||||
xMax = Math.Min(Bounds.XMax, xMin + ResolutionMm);
|
||||
yMax = Math.Min(Bounds.YMax, yMin + ResolutionMm);
|
||||
}
|
||||
|
||||
internal void MarkOccupied(int row, int col)
|
||||
{
|
||||
if (!IsInBounds(row, col)) return;
|
||||
int index = row * Cols + col;
|
||||
if (_cells[index] == 0) { _cells[index] = 1; _occupiedCount++; }
|
||||
}
|
||||
internal byte[] CopyCells() { return (byte[])_cells.Clone(); }
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 环境占据图构建结果。
|
||||
/// 返回：成功时提供可供适配的 EnvironmentGridMap；失败时提供失败原因和已处理来源状态。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentMapBuildResult
|
||||
{
|
||||
private EnvironmentMapBuildResult(bool succeeded, EnvironmentGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults, string failureReason, PlanningOperationStopReason stopReason)
|
||||
{
|
||||
Succeeded = succeeded; Map = map; SourceResults = sourceResults ?? Array.Empty<ObstacleProjectionResult>(); FailureReason = failureReason ?? string.Empty; StopReason = stopReason;
|
||||
}
|
||||
/// <summary>构建是否成功。true 时 Map 非空；false 时读取 FailureReason。</summary>
|
||||
public bool Succeeded { get; }
|
||||
/// <summary>成功生成的构建期环境栅格；失败时为 null。</summary>
|
||||
public EnvironmentGridMap Map { get; }
|
||||
/// <summary>已尝试来源的投影结果，用于记录已应用、空或失败状态。</summary>
|
||||
public IReadOnlyList<ObstacleProjectionResult> SourceResults { get; }
|
||||
/// <summary>失败原因。成功时为空字符串。</summary>
|
||||
public string FailureReason { get; }
|
||||
/// <summary>内部预算停止原因；普通构建成功或失败时为 None。</summary>
|
||||
internal PlanningOperationStopReason StopReason { get; }
|
||||
/// <summary>创建成功结果。参数 map 为已完成栅格，sourceResults 为来源投影记录。</summary>
|
||||
public static EnvironmentMapBuildResult Success(EnvironmentGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults) { return new EnvironmentMapBuildResult(true, map, sourceResults, null, PlanningOperationStopReason.None); }
|
||||
/// <summary>创建失败结果。参数 reason 为诊断文本，sourceResults 可包含失败前已处理的来源。</summary>
|
||||
public static EnvironmentMapBuildResult Failure(string reason, IReadOnlyList<ObstacleProjectionResult> sourceResults) { return new EnvironmentMapBuildResult(false, null, sourceResults, reason, PlanningOperationStopReason.None); }
|
||||
/// <summary>创建已取消或超时结果；不发布构建期可写地图。</summary>
|
||||
internal static EnvironmentMapBuildResult Stopped(PlanningOperationStopReason stopReason, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.None) throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
return new EnvironmentMapBuildResult(false, null, sourceResults,
|
||||
stopReason == PlanningOperationStopReason.Cancelled ? "地图构建已取消。" : "地图构建已超时。", stopReason);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 从排序后的纯障碍物快照事务性构建环境占据图。
|
||||
///
|
||||
/// 注意：必需来源返回不可用或无效状态时，构建整体失败；可选来源仅记录其状态并继续构建。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentMapBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// 投影所有障碍物来源并栅格化为环境地图。
|
||||
///
|
||||
/// 参数：request 包含 mm 世界边界、分辨率和来源列表；每个来源 ID 必须唯一且版本非负。
|
||||
/// 返回：成功时包含 <see cref="EnvironmentGridMap"/> 和全部来源状态；必需来源失败时返回失败结果而不产生可用地图。
|
||||
/// </summary>
|
||||
public EnvironmentMapBuildResult Build(MapBuildRequest request)
|
||||
{
|
||||
return Build(request, PlanningOperationBudget.Unlimited(CancellationToken.None));
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算投影来源并栅格化；停止时不发布可写环境地图。</summary>
|
||||
internal EnvironmentMapBuildResult Build(MapBuildRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, null);
|
||||
if (request == null || request.Bounds == null) return EnvironmentMapBuildResult.Failure("Map request and bounds are required.", null);
|
||||
if (request.ObstacleSources == null) return EnvironmentMapBuildResult.Failure("Obstacle source collection is required.", null);
|
||||
var sources = request.ObstacleSources.OrderBy(s => s == null ? string.Empty : s.SourceId, StringComparer.Ordinal).ToArray();
|
||||
var results = new List<ObstacleProjectionResult>();
|
||||
string previousId = null;
|
||||
for (int i = 0; i < sources.Length; i++)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
IMapObstacleSource source = sources[i];
|
||||
if (source == null || string.IsNullOrWhiteSpace(source.SourceId) || source.SourceVersion < 0)
|
||||
return EnvironmentMapBuildResult.Failure("Each source needs a non-empty id and non-negative version.", results);
|
||||
if (string.Equals(previousId, source.SourceId, StringComparison.Ordinal))
|
||||
return EnvironmentMapBuildResult.Failure("Obstacle source ids must be unique.", results);
|
||||
previousId = source.SourceId;
|
||||
ObstacleProjectionResult result;
|
||||
try { result = source.ProjectToWorld() ?? ObstacleProjectionResult.Invalid("Source returned no projection result."); }
|
||||
catch (Exception exception) { result = ObstacleProjectionResult.Invalid(exception.Message); }
|
||||
results.Add(result);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
if (source.IsRequired && (result.Status == ObstacleSourceStatus.Invalid || result.Status == ObstacleSourceStatus.Unavailable))
|
||||
return EnvironmentMapBuildResult.Failure("A required obstacle source failed: " + source.SourceId, results);
|
||||
}
|
||||
var map = new EnvironmentGridMap(request.Bounds, request.ResolutionMm);
|
||||
for (int i = 0; i < results.Count; i++)
|
||||
if (results[i].Status == ObstacleSourceStatus.Applied)
|
||||
for (int j = 0; j < results[i].Obstacles.Count; j++)
|
||||
{
|
||||
if (!MapObstacleRasterizer.TryRasterize(map, results[i].Obstacles[j], budget, out stopReason))
|
||||
return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
}
|
||||
return EnvironmentMapBuildResult.Success(map, results);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 有限的世界地图边界。
|
||||
/// 单位：mm；范围采用左闭右开 [XMin, XMax) × [YMin, YMax)。
|
||||
/// </summary>
|
||||
public sealed class MapBoundsMm : IEquatable<MapBoundsMm>
|
||||
{
|
||||
/// <summary>单张地图允许的最大栅格数，超过该值会拒绝创建地图。</summary>
|
||||
public const int MaximumCellCount = 4000000;
|
||||
|
||||
/// <summary>
|
||||
/// 创建地图世界边界。
|
||||
///
|
||||
/// 参数：
|
||||
/// - xMin、xMax：世界 X 轴下限和上限，单位 mm，且 xMax 必须大于 xMin。
|
||||
/// - yMin、yMax：世界 Y 轴下限和上限，单位 mm，且 yMax 必须大于 yMin。
|
||||
///
|
||||
/// 注意：边界采用左闭右开规则，上限坐标不属于地图。
|
||||
/// </summary>
|
||||
public MapBoundsMm(float xMin, float xMax, float yMin, float yMax)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(xMin) || !NumericGuard.IsFinite(xMax) ||
|
||||
!NumericGuard.IsFinite(yMin) || !NumericGuard.IsFinite(yMax) ||
|
||||
xMax <= xMin || yMax <= yMin)
|
||||
throw new ArgumentOutOfRangeException(nameof(xMax), "Map bounds must be finite and non-degenerate.");
|
||||
XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 轴下限，单位 mm，包含在地图内。</summary>
|
||||
public float XMin { get; }
|
||||
/// <summary>世界 X 轴上限，单位 mm，不包含在地图内。</summary>
|
||||
public float XMax { get; }
|
||||
/// <summary>世界 Y 轴下限，单位 mm，包含在地图内。</summary>
|
||||
public float YMin { get; }
|
||||
/// <summary>世界 Y 轴上限，单位 mm，不包含在地图内。</summary>
|
||||
public float YMax { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 判断世界坐标是否属于地图边界。
|
||||
///
|
||||
/// 参数：xMm、yMm 为世界坐标，单位 mm。
|
||||
/// 返回：坐标位于 [XMin, XMax) × [YMin, YMax) 时为 true，否则为 false。
|
||||
/// </summary>
|
||||
public bool Contains(float xMm, float yMm)
|
||||
{
|
||||
return xMm >= XMin && xMm < XMax && yMm >= YMin && yMm < YMax;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据栅格分辨率计算行列数。
|
||||
///
|
||||
/// 参数：resolutionMm 为每个方格的边长，单位 mm，取值必须在 [20, 200]；rows、cols 为输出行数和列数。
|
||||
/// 返回：无；当分辨率无效或总格数超过 <see cref="MaximumCellCount"/> 时抛出异常。
|
||||
/// </summary>
|
||||
public void GetDimensions(float resolutionMm, out int rows, out int cols)
|
||||
{
|
||||
if (!NumericGuard.IsInRange(resolutionMm, 20f, 200f))
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "ResolutionMm must be within [20, 200].");
|
||||
double columnCount = Math.Ceiling(((double)XMax - XMin) / resolutionMm);
|
||||
double rowCount = Math.Ceiling(((double)YMax - YMin) / resolutionMm);
|
||||
if (columnCount > int.MaxValue || rowCount > int.MaxValue || columnCount <= 0d || rowCount <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "Map dimensions are invalid.");
|
||||
cols = (int)columnCount; rows = (int)rowCount;
|
||||
long cellCount = checked((long)rows * cols);
|
||||
if (cellCount > MaximumCellCount)
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "Map cell count exceeds 4,000,000.");
|
||||
}
|
||||
|
||||
/// <summary>比较两个边界的四个 mm 坐标是否完全相同。</summary>
|
||||
public bool Equals(MapBoundsMm other)
|
||||
{
|
||||
return other != null && XMin.Equals(other.XMin) && XMax.Equals(other.XMax) &&
|
||||
YMin.Equals(other.YMin) && YMax.Equals(other.YMax);
|
||||
}
|
||||
/// <summary>比较当前边界与指定对象是否表示相同的世界范围。</summary>
|
||||
public override bool Equals(object obj) { return Equals(obj as MapBoundsMm); }
|
||||
/// <summary>返回由四个边界坐标组成的哈希值，用于缓存键比较。</summary>
|
||||
public override int GetHashCode()
|
||||
{
|
||||
unchecked { int hash = XMin.GetHashCode(); hash = hash * 31 + XMax.GetHashCode(); hash = hash * 31 + YMin.GetHashCode(); return hash * 31 + YMax.GetHashCode(); }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 环境占据图构建器的输入数据。
|
||||
/// 注意：通常由 PlanningMapFactory 从公开请求转换得到，调用者无需直接使用。
|
||||
/// </summary>
|
||||
public sealed class MapBuildRequest
|
||||
{
|
||||
/// <summary>环境图世界边界。单位：mm；不能为空。</summary>
|
||||
public MapBoundsMm Bounds { get; set; }
|
||||
/// <summary>环境栅格边长。单位：mm；必须满足 MapBoundsMm 的分辨率限制。</summary>
|
||||
public float ResolutionMm { get; set; }
|
||||
/// <summary>待投影的障碍物来源列表；每个来源 ID 必须唯一。</summary>
|
||||
public IReadOnlyList<IMapObstacleSource> ObstacleSources { get; set; } = Array.Empty<IMapObstacleSource>();
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>边与世界 X/Y 轴平行的矩形障碍物，坐标单位为 mm。</summary>
|
||||
public sealed class AxisAlignedRectangleObstacle : IMapObstacle
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建轴对齐矩形障碍物。
|
||||
///
|
||||
/// 参数：xMin、xMax、yMin、yMax 分别为矩形世界坐标边界，单位 mm。
|
||||
/// 注意：构造不校验边界顺序，请通过 <see cref="IsValid"/> 判断后再使用。
|
||||
/// </summary>
|
||||
public AxisAlignedRectangleObstacle(float xMin, float xMax, float yMin, float yMax)
|
||||
{
|
||||
XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax;
|
||||
}
|
||||
/// <summary>矩形世界 X 下边界，单位 mm。</summary>
|
||||
public float XMin { get; }
|
||||
/// <summary>矩形世界 X 上边界，单位 mm。</summary>
|
||||
public float XMax { get; }
|
||||
/// <summary>矩形世界 Y 下边界，单位 mm。</summary>
|
||||
public float YMin { get; }
|
||||
/// <summary>矩形世界 Y 上边界，单位 mm。</summary>
|
||||
public float YMax { get; }
|
||||
/// <summary>四个边界均为有限数且 XMax≥XMin、YMax≥YMin 时为 true；否则为 false。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return NumericGuard.IsFinite(XMin) && NumericGuard.IsFinite(XMax) && NumericGuard.IsFinite(YMin) && NumericGuard.IsFinite(YMax) && XMax >= XMin && YMax >= YMin; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>以世界 mm 坐标表示的圆形障碍物。</summary>
|
||||
public sealed class CircleObstacle : IMapObstacle
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建圆形障碍物。
|
||||
///
|
||||
/// 参数：centerX、centerY 为圆心世界坐标，单位 mm；radiusMm 为半径，单位 mm。
|
||||
/// 注意：构造不抛出几何校验异常，请通过 <see cref="IsValid"/> 判断后再使用。
|
||||
/// </summary>
|
||||
public CircleObstacle(float centerX, float centerY, float radiusMm)
|
||||
{
|
||||
CenterX = centerX; CenterY = centerY; RadiusMm = radiusMm;
|
||||
}
|
||||
/// <summary>圆心世界 X 坐标，单位 mm。</summary>
|
||||
public float CenterX { get; }
|
||||
/// <summary>圆心世界 Y 坐标，单位 mm。</summary>
|
||||
public float CenterY { get; }
|
||||
/// <summary>圆的半径，单位 mm。</summary>
|
||||
public float RadiusMm { get; }
|
||||
/// <summary>圆心和半径均为有限数且半径不小于零时为 true；否则为 false。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return NumericGuard.IsFinite(CenterX) && NumericGuard.IsFinite(CenterY) && NumericGuard.IsFinite(RadiusMm) && RadiusMm >= 0f; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 世界坐标中的不可变障碍物几何。
|
||||
///
|
||||
/// 单位：所有几何坐标与尺寸均为 mm。实现类型必须能由 <see cref="MapObstacleRasterizer"/> 栅格化。
|
||||
/// </summary>
|
||||
public interface IMapObstacle
|
||||
{
|
||||
/// <summary>几何数据是否有效。true 表示数值有限且尺寸满足该几何类型的约束；false 表示不得投影到地图。</summary>
|
||||
bool IsValid { get; }
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 将障碍物几何保守投影为栅格占据状态的唯一写入入口。
|
||||
///
|
||||
/// 注意：调用方不能直接改写 <see cref="EnvironmentGridMap"/>；相交或贴边的栅格均按占据处理。
|
||||
/// </summary>
|
||||
public static class MapObstacleRasterizer
|
||||
{
|
||||
/// <summary>
|
||||
/// 将一个有效障碍物栅格化到环境地图。
|
||||
///
|
||||
/// 参数：map 为待写入的环境栅格；obstacle 为世界 mm 坐标的圆形或轴对齐矩形障碍物。
|
||||
/// 返回：无。地图或障碍物为空、障碍物无效、几何类型不受支持时抛出异常。
|
||||
/// 注意：该方法只增加占据格，不会清除已有障碍。
|
||||
/// </summary>
|
||||
public static void Rasterize(EnvironmentGridMap map, IMapObstacle obstacle)
|
||||
{
|
||||
if (!TryRasterize(map, obstacle, PlanningOperationBudget.Unlimited(CancellationToken.None), out _))
|
||||
throw new InvalidOperationException("Unbounded rasterization unexpectedly stopped.");
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算将障碍物写入环境栅格；停止时返回 false 且不发布环境地图。</summary>
|
||||
internal static bool TryRasterize(EnvironmentGridMap map, IMapObstacle obstacle, PlanningOperationBudget budget,
|
||||
out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (obstacle == null || !obstacle.IsValid) throw new ArgumentException("Obstacle must be valid.", nameof(obstacle));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
int workItemCount = 0;
|
||||
var circle = obstacle as CircleObstacle;
|
||||
if (circle != null) return TryRasterizeCircle(map, circle, budget, ref workItemCount, out stopReason);
|
||||
var rectangle = obstacle as AxisAlignedRectangleObstacle;
|
||||
if (rectangle != null) return TryRasterizeRectangle(map, rectangle, budget, ref workItemCount, out stopReason);
|
||||
throw new NotSupportedException("Unsupported map obstacle geometry.");
|
||||
}
|
||||
|
||||
private static bool TryRasterizeCircle(EnvironmentGridMap map, CircleObstacle circle, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
GetCandidateRange(map, circle.CenterX - circle.RadiusMm, circle.CenterX + circle.RadiusMm,
|
||||
circle.CenterY - circle.RadiusMm, circle.CenterY + circle.RadiusMm,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
||||
double radiusSquared = (double)circle.RadiusMm * circle.RadiusMm;
|
||||
for (int row = firstRow; row <= lastRow; row++)
|
||||
for (int col = firstCol; col <= lastCol; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
|
||||
double nearestX = Math.Max(xMin, Math.Min(circle.CenterX, xMax));
|
||||
double nearestY = Math.Max(yMin, Math.Min(circle.CenterY, yMax));
|
||||
double dx = circle.CenterX - nearestX;
|
||||
double dy = circle.CenterY - nearestY;
|
||||
if (dx * dx + dy * dy <= radiusSquared) map.MarkOccupied(row, col);
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryRasterizeRectangle(EnvironmentGridMap map, AxisAlignedRectangleObstacle rectangle, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
GetCandidateRange(map, rectangle.XMin, rectangle.XMax, rectangle.YMin, rectangle.YMax,
|
||||
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++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
|
||||
if (rectangle.XMax >= xMin && rectangle.XMin <= xMax && rectangle.YMax >= yMin && rectangle.YMin <= yMax)
|
||||
map.MarkOccupied(row, col);
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void GetCandidateRange(EnvironmentGridMap map, float xMin, float xMax, float yMin, float yMax,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol)
|
||||
{
|
||||
if (xMax < map.Bounds.XMin || xMin > map.Bounds.XMax || yMax < map.Bounds.YMin || yMin > map.Bounds.YMax)
|
||||
{ firstRow = 1; lastRow = 0; firstCol = 1; lastCol = 0; return; }
|
||||
// Geometry is closed for conservative rasterisation. Include the cell on
|
||||
// the lower side when a boundary lies exactly on a grid line.
|
||||
firstCol = Clamp((int)Math.Floor(((double)xMin - map.Bounds.XMin) / map.ResolutionMm) - 1, 0, map.Cols - 1);
|
||||
lastCol = Clamp((int)Math.Floor(((double)xMax - map.Bounds.XMin) / map.ResolutionMm), 0, map.Cols - 1);
|
||||
firstRow = Clamp((int)Math.Floor(((double)yMin - map.Bounds.YMin) / map.ResolutionMm) - 1, 0, map.Rows - 1);
|
||||
lastRow = Clamp((int)Math.Floor(((double)yMax - map.Bounds.YMin) / map.ResolutionMm), 0, map.Rows - 1);
|
||||
}
|
||||
private static int Clamp(int value, int min, int max) { return value < min ? min : value > max ? max : value; }
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>针对行主序二值栅格计算精确欧氏距离平方的内部算法。</summary>
|
||||
internal static class EuclideanDistanceTransform
|
||||
{
|
||||
/// <summary>
|
||||
/// 计算每个栅格到最近障碍栅格的距离平方。
|
||||
///
|
||||
/// 参数：occupied 为行主序占据数组，非零表示障碍；rows、cols 为数组尺寸。
|
||||
/// 返回：行主序距离平方数组，单位为栅格边长的平方；不含任何 mm 或 m 换算。
|
||||
/// </summary>
|
||||
public static double[] ComputeSquaredDistances(byte[] occupied, int rows, int cols)
|
||||
{
|
||||
if (!TryComputeSquaredDistances(occupied, rows, cols, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out double[] squared, out _))
|
||||
throw new InvalidOperationException("Unbounded Euclidean distance transform unexpectedly stopped.");
|
||||
return squared;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算计算距离平方；停止时不返回部分数组。</summary>
|
||||
internal static bool TryComputeSquaredDistances(byte[] occupied, int rows, int cols, PlanningOperationBudget budget,
|
||||
out double[] squaredDistances, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (occupied == null) throw new ArgumentNullException(nameof(occupied));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
squaredDistances = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
double noObstacleDistanceSquared = (double)rows * rows + (double)cols * cols + 1d;
|
||||
var intermediate = new double[occupied.Length];
|
||||
var result = new double[occupied.Length];
|
||||
var input = new double[Math.Max(rows, cols)];
|
||||
var output = new double[Math.Max(rows, cols)];
|
||||
int workItemCount = 0;
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
int offset = row * cols;
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
input[col] = occupied[offset + col] == 0 ? noObstacleDistanceSquared : 0d;
|
||||
}
|
||||
if (!TryTransform1D(input, cols, output, budget, ref workItemCount, out stopReason)) return false;
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
intermediate[offset + col] = output[col];
|
||||
}
|
||||
}
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
input[row] = intermediate[row * cols + col];
|
||||
}
|
||||
if (!TryTransform1D(input, rows, output, budget, ref workItemCount, out stopReason)) return false;
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
result[row * cols + col] = output[row];
|
||||
}
|
||||
}
|
||||
squaredDistances = result;
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryTransform1D(double[] f, int length, double[] result, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
var locations = new int[length];
|
||||
var boundaries = new double[length + 1];
|
||||
int k = 0;
|
||||
locations[0] = 0;
|
||||
boundaries[0] = double.NegativeInfinity;
|
||||
boundaries[1] = double.PositiveInfinity;
|
||||
for (int q = 1; q < length; q++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
double intersection;
|
||||
do
|
||||
{
|
||||
int p = locations[k];
|
||||
intersection = ((f[q] + (double)q * q) - (f[p] + (double)p * p)) / (2d * (q - p));
|
||||
if (intersection <= boundaries[k])
|
||||
{
|
||||
k--;
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
}
|
||||
} while (k >= 0 && intersection <= boundaries[k]);
|
||||
if (k < 0)
|
||||
{
|
||||
k = 0; locations[0] = q; boundaries[0] = double.NegativeInfinity; boundaries[1] = double.PositiveInfinity;
|
||||
}
|
||||
else
|
||||
{
|
||||
k++; locations[k] = q; boundaries[k] = intersection; boundaries[k + 1] = double.PositiveInfinity;
|
||||
}
|
||||
}
|
||||
k = 0;
|
||||
for (int q = 0; q < length; q++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
while (boundaries[k + 1] < q) k++;
|
||||
double delta = q - locations[k];
|
||||
result[q] = delta * delta + f[locations[k]];
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>以 m 表示的障碍物净距离保守下界。</summary>
|
||||
internal sealed class ObstacleDistanceField
|
||||
{
|
||||
private readonly double[] _conservativeDistances;
|
||||
private ObstacleDistanceField(double[] conservativeDistances) { _conservativeDistances = conservativeDistances; }
|
||||
/// <summary>
|
||||
/// 从占据栅格创建距离场。
|
||||
///
|
||||
/// 参数：occupied 为行主序占据数组；rows、cols 为其尺寸；resolutionMeters 为格边长，单位 m。
|
||||
/// 返回：每个格到最近障碍物的保守净距离下界，单位 m；全空地图中的每项为正无穷。
|
||||
/// </summary>
|
||||
public static ObstacleDistanceField Create(byte[] occupied, int rows, int cols, double resolutionMeters)
|
||||
{
|
||||
if (!TryCreate(occupied, rows, cols, resolutionMeters, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out ObstacleDistanceField field, out _))
|
||||
throw new InvalidOperationException("Unbounded distance-field creation unexpectedly stopped.");
|
||||
return field;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建距离场；停止时不返回部分距离数据。</summary>
|
||||
internal static bool TryCreate(byte[] occupied, int rows, int cols, double resolutionMeters,
|
||||
PlanningOperationBudget budget, out ObstacleDistanceField field, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (occupied == null) throw new ArgumentNullException(nameof(occupied));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
field = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
bool hasObstacle = false;
|
||||
int workItemCount = 0;
|
||||
for (int i = 0; i < occupied.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
if (occupied[i] != 0) { hasObstacle = true; break; }
|
||||
}
|
||||
var distances = new double[occupied.Length];
|
||||
if (!hasObstacle)
|
||||
{
|
||||
for (int i = 0; i < distances.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
distances[i] = double.PositiveInfinity;
|
||||
}
|
||||
field = new ObstacleDistanceField(distances);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
if (!EuclideanDistanceTransform.TryComputeSquaredDistances(occupied, rows, cols, budget, out double[] squared, out stopReason))
|
||||
return false;
|
||||
double conservativeOffset = Math.Sqrt(2d) * resolutionMeters;
|
||||
for (int i = 0; i < distances.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
distances[i] = Math.Max(0d, Math.Sqrt(squared[i]) * resolutionMeters - conservativeOffset);
|
||||
}
|
||||
field = new ObstacleDistanceField(distances);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
internal double[] CopyDistances() { return (double[])_conservativeDistances.Clone(); }
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 供粗路径规划使用的不可变地图快照。
|
||||
///
|
||||
/// 单位：世界查询方法使用 m；<see cref="Bounds"/> 和 <see cref="ResolutionMm"/> 保留原始 mm 数据。
|
||||
/// 注意：世界坐标越界一律按占据处理，净距离为零。
|
||||
/// </summary>
|
||||
public sealed class PlanningGridMap
|
||||
{
|
||||
private readonly byte[] _occupied;
|
||||
private readonly double[] _conservativeDistances;
|
||||
|
||||
internal PlanningGridMap(MapBoundsMm bounds, float resolutionMm, int rows, int cols, byte[] occupied, double[] conservativeDistances,
|
||||
long snapshotId, bool planningReady, string planningBlockReason, string inputFingerprint, string occupancyHash)
|
||||
{
|
||||
Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
|
||||
_occupied = occupied; _conservativeDistances = conservativeDistances;
|
||||
SnapshotId = snapshotId; PlanningReady = planningReady; PlanningBlockReason = planningBlockReason ?? string.Empty;
|
||||
InputFingerprint = inputFingerprint ?? string.Empty; OccupancyHash = occupancyHash ?? string.Empty;
|
||||
}
|
||||
/// <summary>源环境图的世界边界，单位 mm，采用左闭右开规则。</summary>
|
||||
public MapBoundsMm Bounds { get; }
|
||||
/// <summary>源环境图的栅格边长，单位 mm。</summary>
|
||||
public float ResolutionMm { get; }
|
||||
/// <summary>规划世界查询对应的栅格边长，单位 m。</summary>
|
||||
public double ResolutionMeters { get { return ResolutionMm / 1000d; } }
|
||||
/// <summary>栅格行数。</summary>
|
||||
public int Rows { get; }
|
||||
/// <summary>栅格列数。</summary>
|
||||
public int Cols { get; }
|
||||
/// <summary>工厂为本次返回快照分配的单调编号，用于区分不同构建结果。</summary>
|
||||
public long SnapshotId { get; }
|
||||
/// <summary>地图是否允许进入粗路径规划。true 时可直接查询；false 时应先处理 <see cref="PlanningBlockReason"/>。</summary>
|
||||
public bool PlanningReady { get; }
|
||||
/// <summary>禁止规划的原因。<see cref="PlanningReady"/> 为 true 时为空字符串。</summary>
|
||||
public string PlanningBlockReason { get; }
|
||||
/// <summary>完整建图输入的稳定指纹，用于识别精确输入缓存命中。</summary>
|
||||
public string InputFingerprint { get; }
|
||||
/// <summary>占据栅格内容哈希，用于识别可复用的占据与距离数组。</summary>
|
||||
public string OccupancyHash { get; }
|
||||
|
||||
/// <summary>查询世界位置是否占据。参数 xMeters、yMeters 单位为 m；位置越界时保守地返回 true。</summary>
|
||||
public bool IsOccupiedWorld(double xMeters, double yMeters)
|
||||
{
|
||||
return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) || _occupied[row * Cols + col] != 0;
|
||||
}
|
||||
/// <summary>
|
||||
/// 查询到最近障碍物的保守净距离下界。
|
||||
///
|
||||
/// 参数：xMeters、yMeters 为世界坐标，单位 m。
|
||||
/// 返回：单位 m 的非负距离下界；地图内无障碍物时为正无穷，越界时为零。
|
||||
/// </summary>
|
||||
public double GetConservativeObstacleDistanceMeters(double xMeters, double yMeters)
|
||||
{
|
||||
return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) ? 0d : _conservativeDistances[row * Cols + col];
|
||||
}
|
||||
/// <summary>
|
||||
/// 将规划世界坐标转换为栅格索引。
|
||||
///
|
||||
/// 参数：xMeters、yMeters 为世界坐标，单位 m；row、col 为输出索引。
|
||||
/// 返回：位置在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
|
||||
/// </summary>
|
||||
public bool TryWorldToGrid(double xMeters, double yMeters, out int row, out int col)
|
||||
{
|
||||
row = -1; col = -1;
|
||||
double xMm = xMeters * 1000d, yMm = yMeters * 1000d;
|
||||
if (xMm < Bounds.XMin || xMm >= Bounds.XMax || yMm < Bounds.YMin || yMm >= Bounds.YMax) return false;
|
||||
col = (int)Math.Floor((xMm - Bounds.XMin) / ResolutionMm);
|
||||
row = (int)Math.Floor((yMm - Bounds.YMin) / ResolutionMm);
|
||||
return row >= 0 && row < Rows && col >= 0 && col < Cols;
|
||||
}
|
||||
/// <summary>按行列索引查询占据状态。参数从零开始；任一索引越界时返回 true。</summary>
|
||||
public bool IsOccupied(int row, int col) { return row < 0 || row >= Rows || col < 0 || col >= Cols || _occupied[row * Cols + col] != 0; }
|
||||
internal byte[] CopyOccupied() { return (byte[])_occupied.Clone(); }
|
||||
internal bool OccupancyEquals(PlanningGridMap other)
|
||||
{
|
||||
if (other == null || Rows != other.Rows || Cols != other.Cols || ResolutionMm != other.ResolutionMm || !Bounds.Equals(other.Bounds) || _occupied.Length != other._occupied.Length) return false;
|
||||
for (int i = 0; i < _occupied.Length; i++) if (_occupied[i] != other._occupied[i]) return false;
|
||||
return true;
|
||||
}
|
||||
internal PlanningGridMap WithMetadata(long snapshotId, bool planningReady, string blockReason, string inputFingerprint, string occupancyHash)
|
||||
{
|
||||
return new PlanningGridMap(Bounds, ResolutionMm, Rows, Cols, _occupied, _conservativeDistances, snapshotId, planningReady, blockReason, inputFingerprint, occupancyHash);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>将建图阶段的 mm 环境栅格适配为规划阶段的不可变 m 查询快照。</summary>
|
||||
public static class PlanningMapAdapter
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建规划地图快照及其保守距离场。
|
||||
///
|
||||
/// 参数：environmentMap 为已经完成障碍物栅格化的环境图，坐标与分辨率单位均为 mm。
|
||||
/// 返回：不可变的 <see cref="PlanningGridMap"/>；其世界查询使用 m，初始元数据由工厂随后分配。
|
||||
/// </summary>
|
||||
public static PlanningGridMap Create(EnvironmentGridMap environmentMap)
|
||||
{
|
||||
if (!TryCreate(environmentMap, PlanningOperationBudget.Unlimited(CancellationToken.None), out PlanningGridMap map, out _))
|
||||
throw new InvalidOperationException("Unbounded planning-map adaptation unexpectedly stopped.");
|
||||
return map;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建规划快照；停止时不返回部分地图。</summary>
|
||||
internal static bool TryCreate(EnvironmentGridMap environmentMap, PlanningOperationBudget budget,
|
||||
out PlanningGridMap map, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (environmentMap == null) throw new ArgumentNullException(nameof(environmentMap));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
map = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
byte[] occupied = environmentMap.CopyCells();
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
if (!ObstacleDistanceField.TryCreate(occupied, environmentMap.Rows, environmentMap.Cols,
|
||||
environmentMap.ResolutionMm / 1000d, budget, out ObstacleDistanceField field, out stopReason))
|
||||
return false;
|
||||
map = new PlanningGridMap(environmentMap.Bounds, environmentMap.ResolutionMm, environmentMap.Rows, environmentMap.Cols,
|
||||
occupied, field.CopyDistances(), 0, false, "Map metadata has not been assigned.", string.Empty, string.Empty);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason == PlanningOperationStopReason.None) return true;
|
||||
map = null;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>线程安全、容量为四的 LRU 缓存，分别复用精确输入结果和不可变占据数组。</summary>
|
||||
internal sealed class PlanningMapCache
|
||||
{
|
||||
private const int Capacity = 4;
|
||||
private readonly object _gate = new object();
|
||||
private readonly LinkedList<InputEntry> _inputs = new LinkedList<InputEntry>();
|
||||
private readonly LinkedList<OccupancyEntry> _occupancies = new LinkedList<OccupancyEntry>();
|
||||
|
||||
/// <summary>按完整输入描述查询缓存。命中时返回原始快照与来源结果，并提升其最近使用顺序。</summary>
|
||||
public bool TryGetInput(string descriptor, out PlanningGridMap map, out IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
lock (_gate)
|
||||
{
|
||||
for (var node = _inputs.First; node != null; node = node.Next)
|
||||
if (string.Equals(node.Value.Descriptor, descriptor, StringComparison.Ordinal))
|
||||
{ map = node.Value.Map; sourceResults = node.Value.SourceResults; _inputs.Remove(node); _inputs.AddFirst(node); return true; }
|
||||
map = null; sourceResults = null; return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>按占据哈希及逐格比较查询缓存。命中时返回共享数组的规范快照，供工厂创建新的元数据快照。</summary>
|
||||
public bool TryGetOccupancy(string hash, PlanningGridMap candidate, out PlanningGridMap canonical)
|
||||
{
|
||||
lock (_gate)
|
||||
{
|
||||
for (var node = _occupancies.First; node != null; node = node.Next)
|
||||
if (string.Equals(node.Value.Hash, hash, StringComparison.Ordinal) && node.Value.Map.OccupancyEquals(candidate))
|
||||
{ canonical = node.Value.Map; _occupancies.Remove(node); _occupancies.AddFirst(node); return true; }
|
||||
canonical = null; return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>写入或更新精确输入缓存。参数 descriptor 为完整输入键，map 为不可变快照，sourceResults 为对应来源结果。</summary>
|
||||
public void AddInput(string descriptor, PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
lock (_gate)
|
||||
{
|
||||
for (var node = _inputs.First; node != null; node = node.Next)
|
||||
if (string.Equals(node.Value.Descriptor, descriptor, StringComparison.Ordinal)) { node.Value.Map = map; node.Value.SourceResults = sourceResults; _inputs.Remove(node); _inputs.AddFirst(node); return; }
|
||||
_inputs.AddFirst(new InputEntry(descriptor, map, sourceResults));
|
||||
while (_inputs.Count > Capacity) _inputs.RemoveLast();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>写入占据缓存。参数 hash 为占据内容哈希，map 为包含可复用占据与距离数组的快照。</summary>
|
||||
public void AddOccupancy(string hash, PlanningGridMap map)
|
||||
{
|
||||
lock (_gate)
|
||||
{
|
||||
for (var node = _occupancies.First; node != null; node = node.Next)
|
||||
if (string.Equals(node.Value.Hash, hash, StringComparison.Ordinal) && node.Value.Map.OccupancyEquals(map))
|
||||
{ _occupancies.Remove(node); _occupancies.AddFirst(node); return; }
|
||||
_occupancies.AddFirst(new OccupancyEntry(hash, map));
|
||||
while (_occupancies.Count > Capacity) _occupancies.RemoveLast();
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class InputEntry { public InputEntry(string descriptor, PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults) { Descriptor = descriptor; Map = map; SourceResults = sourceResults; } public string Descriptor { get; } public PlanningGridMap Map { get; set; } public IReadOnlyList<ObstacleProjectionResult> SourceResults { get; set; } }
|
||||
private sealed class OccupancyEntry { public OccupancyEntry(string hash, PlanningGridMap map) { Hash = hash; Map = map; } public string Hash { get; } public PlanningGridMap Map { get; } }
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>规划地图创建的终止状态。</summary>
|
||||
public enum PlanningMapBuildStatus
|
||||
{
|
||||
/// <summary>已成功创建可用的不可变地图快照。</summary>
|
||||
Success,
|
||||
/// <summary>输入、来源或地图构建失败。</summary>
|
||||
Failed,
|
||||
/// <summary>调用方在地图创建期间取消了操作。</summary>
|
||||
Cancelled,
|
||||
/// <summary>地图创建消耗了整次规划操作的总超时预算。</summary>
|
||||
TimedOut,
|
||||
}
|
||||
|
||||
/// <summary>本次地图创建使用的缓存层级，反映快照或占据数组的复用方式。</summary>
|
||||
public enum PlanningMapCacheHit
|
||||
{
|
||||
/// <summary>未命中缓存；本次重新创建了占据图和距离场。</summary>
|
||||
None,
|
||||
/// <summary>完整输入命中；返回与上次完全相同的不可变地图对象。</summary>
|
||||
Input,
|
||||
/// <summary>占据内容命中；复用占据和距离数组，但生成新的快照元数据。</summary>
|
||||
Occupancy,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 规划地图创建的最终结果。
|
||||
///
|
||||
/// 返回：
|
||||
/// - Succeeded 为 true 时 Map 可用；false 时通过 FailureReason 获取原因。
|
||||
/// - SourceResults 始终保留已处理来源的投影状态，便于诊断。
|
||||
/// </summary>
|
||||
public sealed class PlanningMapBuildResult
|
||||
{
|
||||
private PlanningMapBuildResult(PlanningMapBuildStatus status, string failureReason, IReadOnlyList<ObstacleProjectionResult> sourceResults, PlanningMapCacheHit cacheHit, PlanningGridMap map)
|
||||
{
|
||||
Status = status; FailureReason = failureReason ?? string.Empty; SourceResults = sourceResults ?? Array.Empty<ObstacleProjectionResult>(); CacheHit = cacheHit; Map = map;
|
||||
}
|
||||
/// <summary>本次建图的显式终止状态；取消和超时不提供地图快照。</summary>
|
||||
public PlanningMapBuildStatus Status { get; }
|
||||
/// <summary>本次建图是否成功。true 表示 Status 为 Success 且 Map 不为空；false 时读取 FailureReason。</summary>
|
||||
public bool Succeeded { get { return Status == PlanningMapBuildStatus.Success; } }
|
||||
/// <summary>建图失败的可读诊断。成功时为空字符串。</summary>
|
||||
public string FailureReason { get; }
|
||||
/// <summary>每个障碍物来源的投影结果，按建图器排序后的来源顺序排列。</summary>
|
||||
public IReadOnlyList<ObstacleProjectionResult> SourceResults { get; }
|
||||
/// <summary>本次调用的缓存复用层级，用于性能诊断，不影响地图正确性。</summary>
|
||||
public PlanningMapCacheHit CacheHit { get; }
|
||||
/// <summary>成功时返回的不可变规划地图；失败时为 null。</summary>
|
||||
public PlanningGridMap Map { get; }
|
||||
internal static PlanningMapBuildResult Success(PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults, PlanningMapCacheHit cacheHit) { return new PlanningMapBuildResult(PlanningMapBuildStatus.Success, null, sourceResults, cacheHit, map); }
|
||||
internal static PlanningMapBuildResult Failure(string reason, IReadOnlyList<ObstacleProjectionResult> sourceResults) { return new PlanningMapBuildResult(PlanningMapBuildStatus.Failed, reason, sourceResults, PlanningMapCacheHit.None, null); }
|
||||
internal static PlanningMapBuildResult Stopped(PlanningOperationStopReason stopReason, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.Cancelled)
|
||||
return new PlanningMapBuildResult(PlanningMapBuildStatus.Cancelled, "地图创建已取消。", sourceResults, PlanningMapCacheHit.None, null);
|
||||
if (stopReason == PlanningOperationStopReason.TimedOut)
|
||||
return new PlanningMapBuildResult(PlanningMapBuildStatus.TimedOut, "地图创建已超时。", sourceResults, PlanningMapCacheHit.None, null);
|
||||
throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,184 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Security.Cryptography;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 规划地图的唯一公开创建入口。
|
||||
///
|
||||
/// 注意：
|
||||
/// - 应在规划服务生命周期内长期复用同一实例，才能命中输入和占据两级缓存。
|
||||
/// - 本类只消费请求中的纯障碍物快照，不读取传感器、定位、UI 或系统时钟。
|
||||
/// </summary>
|
||||
public sealed class PlanningMapFactory
|
||||
{
|
||||
private readonly EnvironmentMapBuilder _builder = new EnvironmentMapBuilder();
|
||||
private readonly PlanningMapCache _cache = new PlanningMapCache();
|
||||
private readonly object _createGate = new object();
|
||||
private long _nextSnapshotId;
|
||||
|
||||
/// <summary>
|
||||
/// 创建规划地图快照。
|
||||
///
|
||||
/// 参数：
|
||||
/// - request：完整建图请求；包含世界范围、栅格分辨率、障碍物来源和空图策略，不能为空。
|
||||
///
|
||||
/// 返回：
|
||||
/// - PlanningMapBuildResult：成功时包含不可变 PlanningGridMap、每个来源的投影结果和缓存命中类型；
|
||||
/// 失败时包含失败原因，不提供地图。
|
||||
///
|
||||
/// 注意：
|
||||
/// - 完全相同的输入返回同一个快照对象；版本变化但占据相同则复用底层栅格并生成新快照编号。
|
||||
/// </summary>
|
||||
public PlanningMapBuildResult Create(PlanningMapRequest request)
|
||||
{
|
||||
return Create(request, PlanningOperationBudget.Unlimited(CancellationToken.None));
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建地图；取消或超时时不发布快照或缓存条目。</summary>
|
||||
internal PlanningMapBuildResult Create(PlanningMapRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, null);
|
||||
|
||||
bool enteredCreateGate = false;
|
||||
try
|
||||
{
|
||||
while (!Monitor.TryEnter(_createGate, 16))
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, null);
|
||||
}
|
||||
enteredCreateGate = true;
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, null);
|
||||
if (request == null || request.Bounds == null) return PlanningMapBuildResult.Failure("Planning map request and bounds are required.", null);
|
||||
if (request.ObstacleSources == null) return PlanningMapBuildResult.Failure("Obstacle source collection is required.", null);
|
||||
string requestKey = BuildRequestKey(request);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, null);
|
||||
if (_cache.TryGetInput(requestKey, out PlanningGridMap exactMap, out IReadOnlyList<ObstacleProjectionResult> cachedResults))
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
return stopReason == PlanningOperationStopReason.None
|
||||
? PlanningMapBuildResult.Success(exactMap, cachedResults, PlanningMapCacheHit.Input)
|
||||
: PlanningMapBuildResult.Stopped(stopReason, cachedResults);
|
||||
}
|
||||
|
||||
EnvironmentMapBuildResult environment = _builder.Build(
|
||||
new MapBuildRequest { Bounds = request.Bounds, ResolutionMm = request.ResolutionMm, ObstacleSources = request.ObstacleSources }, budget);
|
||||
if (!environment.Succeeded)
|
||||
return environment.StopReason == PlanningOperationStopReason.None
|
||||
? PlanningMapBuildResult.Failure(environment.FailureReason, environment.SourceResults)
|
||||
: PlanningMapBuildResult.Stopped(environment.StopReason, environment.SourceResults);
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
string descriptor = BuildInputDescriptor(request, environment.SourceResults);
|
||||
string inputFingerprint = Sha256(descriptor);
|
||||
int applied = environment.SourceResults.Count(x => x.Status == ObstacleSourceStatus.Applied);
|
||||
bool ready = applied > 0 || request.AllowExplicitEmptyMap;
|
||||
string blockReason = ready ? string.Empty : "No source supplied obstacle geometry; set AllowExplicitEmptyMap only when an intentionally empty map is safe.";
|
||||
if (!PlanningMapAdapter.TryCreate(environment.Map, budget, out PlanningGridMap candidate, out stopReason))
|
||||
return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
if (!TryComputeOccupancyHash(candidate, budget, out string occupancyHash, out stopReason))
|
||||
return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
|
||||
PlanningGridMap map;
|
||||
PlanningMapCacheHit cacheHit;
|
||||
if (_cache.TryGetOccupancy(occupancyHash, candidate, out PlanningGridMap canonical))
|
||||
{
|
||||
map = canonical.WithMetadata(NextSnapshotId(), ready, blockReason, inputFingerprint, occupancyHash);
|
||||
cacheHit = PlanningMapCacheHit.Occupancy;
|
||||
}
|
||||
else
|
||||
{
|
||||
map = candidate.WithMetadata(NextSnapshotId(), ready, blockReason, inputFingerprint, occupancyHash);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
_cache.AddOccupancy(occupancyHash, map);
|
||||
cacheHit = PlanningMapCacheHit.None;
|
||||
}
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
_cache.AddInput(requestKey, map, environment.SourceResults);
|
||||
return PlanningMapBuildResult.Success(map, environment.SourceResults, cacheHit);
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (enteredCreateGate) Monitor.Exit(_createGate);
|
||||
}
|
||||
}
|
||||
|
||||
private long NextSnapshotId() { return Interlocked.Increment(ref _nextSnapshotId); }
|
||||
|
||||
private static string BuildInputDescriptor(PlanningMapRequest request, IReadOnlyList<ObstacleProjectionResult> results)
|
||||
{
|
||||
var builder = new StringBuilder();
|
||||
AppendFloat(builder, request.Bounds.XMin); AppendFloat(builder, request.Bounds.XMax); AppendFloat(builder, request.Bounds.YMin); AppendFloat(builder, request.Bounds.YMax); AppendFloat(builder, request.ResolutionMm); builder.Append(request.AllowExplicitEmptyMap ? '1' : '0');
|
||||
var sources = request.ObstacleSources.OrderBy(s => s == null ? string.Empty : s.SourceId, StringComparer.Ordinal).ToArray();
|
||||
for (int i = 0; i < sources.Length; i++)
|
||||
{
|
||||
IMapObstacleSource source = sources[i];
|
||||
builder.Append('|').Append(source == null ? "<null>" : source.SourceId).Append('|').Append(source == null ? -1 : source.SourceVersion).Append('|').Append(source != null && source.IsRequired ? '1' : '0');
|
||||
if (i < results.Count) AppendProjection(builder, results[i]);
|
||||
}
|
||||
return builder.ToString();
|
||||
}
|
||||
private static string BuildRequestKey(PlanningMapRequest request)
|
||||
{
|
||||
var builder = new StringBuilder();
|
||||
AppendFloat(builder, request.Bounds.XMin); AppendFloat(builder, request.Bounds.XMax); AppendFloat(builder, request.Bounds.YMin); AppendFloat(builder, request.Bounds.YMax); AppendFloat(builder, request.ResolutionMm); builder.Append(request.AllowExplicitEmptyMap ? '1' : '0');
|
||||
foreach (IMapObstacleSource source in request.ObstacleSources.OrderBy(s => s == null ? string.Empty : s.SourceId, StringComparer.Ordinal))
|
||||
builder.Append('|').Append(source == null ? "<null>" : source.SourceId).Append('|').Append(source == null ? -1 : source.SourceVersion).Append('|').Append(source != null && source.IsRequired ? '1' : '0');
|
||||
return builder.ToString();
|
||||
}
|
||||
private static void AppendProjection(StringBuilder builder, ObstacleProjectionResult result)
|
||||
{
|
||||
builder.Append('|').Append((int)result.Status);
|
||||
for (int index = 0; index < result.Obstacles.Count; index++)
|
||||
{
|
||||
var circle = result.Obstacles[index] as CircleObstacle;
|
||||
if (circle != null) { builder.Append("|C"); AppendFloat(builder, circle.CenterX); AppendFloat(builder, circle.CenterY); AppendFloat(builder, circle.RadiusMm); continue; }
|
||||
var rectangle = result.Obstacles[index] as AxisAlignedRectangleObstacle;
|
||||
if (rectangle != null) { builder.Append("|R"); AppendFloat(builder, rectangle.XMin); AppendFloat(builder, rectangle.XMax); AppendFloat(builder, rectangle.YMin); AppendFloat(builder, rectangle.YMax); }
|
||||
}
|
||||
}
|
||||
private static void AppendFloat(StringBuilder builder, float value) { builder.Append(BitConverter.ToInt32(BitConverter.GetBytes(value), 0).ToString("X8")); }
|
||||
private static bool TryComputeOccupancyHash(PlanningGridMap map, PlanningOperationBudget budget, out string occupancyHash,
|
||||
out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
byte[] occupied = map.CopyOccupied();
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) { occupancyHash = string.Empty; return false; }
|
||||
using (var sha = SHA256.Create())
|
||||
{
|
||||
const int blockLength = 4096;
|
||||
int offset = 0;
|
||||
while (occupied.Length - offset > blockLength)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) { occupancyHash = string.Empty; return false; }
|
||||
sha.TransformBlock(occupied, offset, blockLength, occupied, offset);
|
||||
offset += blockLength;
|
||||
}
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) { occupancyHash = string.Empty; return false; }
|
||||
byte[] hash = sha.TransformFinalBlock(occupied, offset, occupied.Length - offset);
|
||||
occupancyHash = BitConverter.ToString(hash).Replace("-", string.Empty);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
private static string Sha256(string text)
|
||||
{
|
||||
using (var sha = SHA256.Create()) return BitConverter.ToString(sha.ComputeHash(Encoding.UTF8.GetBytes(text))).Replace("-", string.Empty);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 规划地图的完整建图输入。
|
||||
///
|
||||
/// 注意：
|
||||
/// - 此对象只描述地图内容，不包含车辆、起终点、PNG 或终端日志等调试参数。
|
||||
/// - 障碍物来源快照变化时，调用方必须更新对应的 SourceVersion。
|
||||
/// </summary>
|
||||
public sealed class PlanningMapRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 地图世界边界。
|
||||
/// 单位：mm；边界采用左闭右开范围，由 MapBoundsMm 的 XMin、XMax、YMin、YMax 定义。
|
||||
/// </summary>
|
||||
public MapBoundsMm Bounds { get; set; }
|
||||
/// <summary>
|
||||
/// 栅格边长。
|
||||
/// 单位：mm；当前 MapBoundsMm 允许的范围为 20 至 200 mm。
|
||||
/// </summary>
|
||||
public float ResolutionMm { get; set; }
|
||||
/// <summary>
|
||||
/// 参与建图的统一障碍物来源。
|
||||
/// 参数：列表中的每个来源必须具有唯一 ID 和非负版本号；可为空列表。
|
||||
/// </summary>
|
||||
public IReadOnlyList<IMapObstacleSource> ObstacleSources { get; set; } = Array.Empty<IMapObstacleSource>();
|
||||
/// <summary>
|
||||
/// 是否明确允许没有任何障碍物几何的地图参与规划。
|
||||
/// 返回语义：false 时，隐式空图会被标记为不可规划；true 时空图可用于规划。
|
||||
/// </summary>
|
||||
public bool AllowExplicitEmptyMap { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,367 @@
|
||||
# Map 模块说明
|
||||
|
||||
`Map` 为粗路径规划提供只读、可复用的规划地图快照。它只负责把外部障碍物投影并栅格化，生成占据图和保守障碍距离场；不读取传感器、定位、UI 或系统时钟，也不写入 AMR 自身占据和安全外扩。
|
||||
|
||||
规划器应长期持有一个 `PlanningMapFactory`，通过一次 `Create` 调用取得 `PlanningGridMap`。
|
||||
|
||||
## 文件结构
|
||||
|
||||
```text
|
||||
Map/
|
||||
├── README.md # 本模块说明：结构、数据流、单位和调用方式
|
||||
├── PlanningMapRequest.cs # 公开建图输入：边界、分辨率、障碍来源、空图策略
|
||||
├── PlanningMapBuildResult.cs # 公开建图输出：状态、地图、来源结果、失败原因、缓存命中类型
|
||||
├── PlanningMapFactory.cs # 唯一公开建图门面；负责两级缓存和快照编号
|
||||
├── Core/
|
||||
│ ├── MapBoundsMm.cs # 世界地图范围及行列尺寸计算
|
||||
│ ├── MapBuildRequest.cs # EnvironmentMapBuilder 的内部建图输入
|
||||
│ ├── EnvironmentMapBuildResult.cs # 环境栅格构建结果
|
||||
│ ├── EnvironmentGridMap.cs # 构建期可写的环境占据栅格
|
||||
│ └── EnvironmentMapBuilder.cs # 汇总障碍来源并事务性创建环境图
|
||||
├── Obstacles/
|
||||
│ ├── IMapObstacle.cs # 世界坐标障碍物几何契约
|
||||
│ ├── CircleObstacle.cs # 圆形障碍物几何
|
||||
│ ├── AxisAlignedRectangleObstacle.cs # 与坐标轴平行的矩形障碍物几何
|
||||
│ └── MapObstacleRasterizer.cs # 唯一允许写入环境栅格的障碍物栅格化器
|
||||
├── Sources/
|
||||
│ ├── IMapObstacleSource.cs # 统一障碍来源接口
|
||||
│ ├── ObstacleSourceStatus.cs # 来源投影状态：已应用、空、不可用、无效
|
||||
│ ├── ObstacleProjectionResult.cs # 单个来源的世界几何和诊断结果
|
||||
│ ├── ManualObstacleSource.cs # 手工输入的圆形/矩形障碍来源
|
||||
│ ├── TwoLegProjectionInput.cs # 检测时刻的 TwoLeg 纯数据快照
|
||||
│ ├── TwoLegObstacleProjector.cs # 将 TwoLeg 局部坐标投影为世界坐标圆障碍物
|
||||
│ └── TwoLegObstacleSource.cs # 将 TwoLeg 快照包装为统一障碍来源
|
||||
├── Planning/
|
||||
│ ├── PlanningGridMap.cs # 不可变规划快照；规划查询使用米
|
||||
│ ├── PlanningMapAdapter.cs # 环境图到规划快照与距离场的适配器
|
||||
│ ├── EuclideanDistanceTransform.cs # 二值栅格的精确平方欧氏距离变换
|
||||
│ ├── ObstacleDistanceField.cs # 对规划器暴露的保守障碍净距
|
||||
│ └── PlanningMapCache.cs # 容量为 4 的输入/占据两级 LRU 缓存
|
||||
└── Test/
|
||||
├── MovementTest.MapTest.cs # Clumsy 手工建图测试入口与终端调试开关
|
||||
└── Visualization/
|
||||
├── PlanningMapImageExportRequest.cs # PNG 导出输入：规划快照和输出目录
|
||||
├── PlanningMapImageExportResult.cs # PNG 导出状态、路径、尺寸和诊断
|
||||
├── PlanningMapImageExporter.cs # 可选 PNG 导出门面和输出保护
|
||||
├── PlanningMapImageRenderer.cs # 只读快照到 RGBA 像素的渲染器
|
||||
└── ValidatedPngWriter.cs # 写入并校验 PNG 结构和 CRC
|
||||
```
|
||||
|
||||
## 建图数据流
|
||||
|
||||
```text
|
||||
PlanningMapRequest
|
||||
│
|
||||
▼
|
||||
IMapObstacleSource.ProjectToWorld()
|
||||
│ 输出世界坐标的圆形或矩形几何
|
||||
▼
|
||||
EnvironmentMapBuilder + MapObstacleRasterizer
|
||||
│ 写入构建期 EnvironmentGridMap
|
||||
▼
|
||||
PlanningMapAdapter + ObstacleDistanceField
|
||||
│ 生成占据数组和保守距离数组
|
||||
▼
|
||||
PlanningGridMap
|
||||
```
|
||||
|
||||
具体规则：
|
||||
|
||||
1. 调用者准备 `PlanningMapRequest` 和一个或多个 `IMapObstacleSource`。
|
||||
2. 每个来源通过 `ProjectToWorld()` 输出世界坐标几何；Map 核心不主动读取 TwoLeg、定位或其他传感器。
|
||||
3. `EnvironmentMapBuilder` 按来源 ID 排序，必需来源失败则整个建图失败；可选来源失败只保留诊断状态。
|
||||
4. `MapObstacleRasterizer` 是唯一写入 `EnvironmentGridMap` 占据格的组件。
|
||||
5. `PlanningMapAdapter` 生成不可变 `PlanningGridMap`，并附带保守障碍距离场。
|
||||
6. `PlanningMapFactory` 返回最终快照及来源投影结果；粗路径规划只应消费这个快照。
|
||||
|
||||
## 构建状态与停止
|
||||
|
||||
`PlanningMapBuildResult.Status` 的类型为 `PlanningMapBuildStatus`：
|
||||
|
||||
- `Success`：成功发布不可变 `PlanningGridMap`；
|
||||
- `Failed`：输入、来源或常规建图失败，读取 `FailureReason`；
|
||||
- `Cancelled`：调用方取消了带预算的建图，`Map` 为 `null`，不会写入缓存；
|
||||
- `TimedOut`：建图耗尽调用方的总超时预算，`Map` 为 `null`，不会写入缓存。
|
||||
|
||||
公开的 `PlanningMapFactory.Create(request)` 保持兼容且不设置时间限制；粗规划门面使用内部预算入口，使取消和超时能够覆盖地图创建、距离场和后续搜索。
|
||||
|
||||
## 坐标与单位
|
||||
|
||||
- 环境地图边界、障碍物几何、TwoLeg 投影输入均使用世界坐标，单位为 **mm**。
|
||||
- `MapBoundsMm` 范围采用左闭右开：`[XMin, XMax) × [YMin, YMax)`；最大边界不属于地图。
|
||||
- `EnvironmentGridMap` 查询使用 mm；`PlanningGridMap` 的世界查询使用 **m**。
|
||||
- 行 `row` 对应 Y 方向，列 `col` 对应 X 方向，存储顺序为行主序 `row * Cols + col`。
|
||||
- `PlanningGridMap` 的越界位置按占据处理，障碍净距返回 0 m。
|
||||
- Map 不做车辆自身占据或安全外扩；车辆外形与安全裕度由后续碰撞检测负责。
|
||||
|
||||
## 最小调用示例
|
||||
|
||||
```csharp
|
||||
var mapFactory = new PlanningMapFactory(); // 长期持有，不要每次规划重新创建
|
||||
|
||||
IMapObstacle[] obstacles =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
|
||||
new CircleObstacle(3600f, 1200f, 180f),
|
||||
};
|
||||
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("manual", 1L, true, obstacles),
|
||||
},
|
||||
AllowExplicitEmptyMap = false,
|
||||
};
|
||||
|
||||
PlanningMapBuildResult result = mapFactory.Create(request);
|
||||
if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
|
||||
throw new InvalidOperationException(result.FailureReason);
|
||||
|
||||
PlanningGridMap map = result.Map; // 交给粗路径规划器
|
||||
```
|
||||
|
||||
真实 TwoLeg 数据应由上层检测模块在检测时刻构造成 `TwoLegProjectionInput`，再交给 `TwoLegObstacleSource`。不要让 Map 模块主动读取检测器或定位器。
|
||||
|
||||
## 缓存与版本
|
||||
|
||||
`PlanningMapFactory` 内部有容量为 4 的两级 LRU 缓存：
|
||||
|
||||
- **输入命中（Input）**：边界、分辨率、空图策略、来源 ID、`SourceVersion` 和必需性均相同，直接返回同一个 `PlanningGridMap` 对象。
|
||||
- **占据命中（Occupancy）**：来源版本变化，但最终占据栅格相同，复用不可变的占据/距离数组，并颁发新的 `SnapshotId`。
|
||||
- **未命中（None）**：栅格内容变化，重新生成规划快照。
|
||||
|
||||
因此，障碍来源的快照内容发生变化时，调用方必须增加其 `SourceVersion`。未递增版本会错误复用旧地图；仅修改日志、PNG 开关、起终点或车辆参数不应改变地图版本。
|
||||
|
||||
## 测试与调试
|
||||
|
||||
- `Test/MovementTest.MapTest.cs` 是手工 Clumsy 测试入口。文件顶部可设置地图范围、分辨率、TwoLeg 测试快照、`EnableTerminalDebugLog` 和 `SavePng`。
|
||||
- `PlanningMapImageExporter` 仅在 `SavePng` 开启时输出 PNG；它只读取 `PlanningGridMap`，不参与建图、缓存键或规划结果。
|
||||
- 自动检查脚本位于 `ClumsyPilot/tests`：工具、工厂、适配器、PNG 和 MapTest 配置分别有独立验证脚本。
|
||||
- 旧版 `Occupancygird_Map/Map_test/TrapMapImageExporter.cs` 与 `MovementTest.Trapmaptest.cs` 仍保留作历史对照；它们不是新粗路径规划的运行时地图入口。
|
||||
|
||||
## 详细使用指南
|
||||
|
||||
本节说明调用方如何从“障碍物数据”逐步得到可交给粗路径规划器的 `PlanningGridMap`。新地图的唯一创建入口是：
|
||||
|
||||
```csharp
|
||||
PlanningMapBuildResult result = mapFactory.Create(request);
|
||||
```
|
||||
|
||||
其中 `mapFactory` 是长期持有的 `PlanningMapFactory`，`request` 是本次建图输入，`result.Map` 是成功时的只读规划地图。
|
||||
|
||||
### 第 1 步：长期创建地图工厂
|
||||
|
||||
地图工厂内部维护容量为 4 的缓存，因此不要在每次规划前重新创建它。应把它作为规划服务或 MovementTest 的字段长期保存：
|
||||
|
||||
```csharp
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
|
||||
```
|
||||
|
||||
### 第 2 步：准备手工障碍物
|
||||
|
||||
圆形和轴对齐矩形都实现 `IMapObstacle`。障碍物的坐标是**世界坐标**，单位都是 **mm**。
|
||||
|
||||
```csharp
|
||||
IMapObstacle[] manualObstacles =
|
||||
{
|
||||
// 参数依次为：X最小值、X最大值、Y最小值、Y最大值，单位均为 mm。
|
||||
new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
|
||||
|
||||
// 参数依次为：圆心X、圆心Y、半径，单位均为 mm。
|
||||
new CircleObstacle(3600f, 1200f, 180f),
|
||||
};
|
||||
```
|
||||
|
||||
目前 Map 只负责“环境障碍物”。不要在这里添加 AMR 自身外形，也不要在圆半径或矩形尺寸中叠加安全裕度；车辆外形与安全距离由粗路径的碰撞检查处理。
|
||||
|
||||
### 第 3 步：包装为统一障碍来源
|
||||
|
||||
所有障碍物必须通过 `IMapObstacleSource` 进入地图。手工障碍使用 `ManualObstacleSource`：
|
||||
|
||||
```csharp
|
||||
var manualSource = new ManualObstacleSource(
|
||||
sourceId: "manual",
|
||||
sourceVersion: 1L,
|
||||
isRequired: true,
|
||||
obstacles: manualObstacles);
|
||||
```
|
||||
|
||||
参数意义如下：
|
||||
|
||||
- `sourceId`：来源的唯一名称；同一次请求内不能重复。
|
||||
- `sourceVersion`：来源快照版本。障碍物位置、数量、半径或尺寸变化后，必须递增。
|
||||
- `isRequired`：`true` 表示来源无效时整次建图失败；`false` 表示仅记录该来源状态并继续建图。
|
||||
- `obstacles`：当前时刻的不可变障碍物集合。
|
||||
|
||||
例如障碍物内容变化后，应创建带新版本号的来源：
|
||||
|
||||
```csharp
|
||||
var changedManualSource = new ManualObstacleSource(
|
||||
"manual",
|
||||
2L, // 1L 变为 2L，通知工厂地图输入已改变
|
||||
true,
|
||||
changedObstacles);
|
||||
```
|
||||
|
||||
### 第 4 步：可选地加入 TwoLeg 障碍物
|
||||
|
||||
Map 不主动调用 TwoLeg 检测器。上层检测模块应在检测时刻取得 AMR 世界位姿和两腿局部坐标，构造成 `TwoLegProjectionInput`：
|
||||
|
||||
```csharp
|
||||
var twoLegInput = new TwoLegProjectionInput(
|
||||
hasDetection: true,
|
||||
|
||||
// 检测时 AMR 的世界位姿：位置单位 mm，航向单位 rad。
|
||||
detectionWorldX: 1000f,
|
||||
detectionWorldY: 2000f,
|
||||
detectionHeadingRadians: 0d,
|
||||
|
||||
// 两条腿相对于检测时 AMR 位姿的局部坐标，单位 mm。
|
||||
firstLocalX: 300f,
|
||||
firstLocalY: 150f,
|
||||
secondLocalX: 300f,
|
||||
secondLocalY: -150f,
|
||||
|
||||
// 每条腿最终投影为圆形障碍物的半径，单位 mm。
|
||||
radiusMm: 80f,
|
||||
diagnostic: "TwoLeg 检测快照");
|
||||
|
||||
var twoLegSource = new TwoLegObstacleSource(
|
||||
sourceId: "two-leg",
|
||||
sourceVersion: 5L,
|
||||
isRequired: false,
|
||||
input: twoLegInput);
|
||||
```
|
||||
|
||||
没有检测结果时仍可传入空快照：
|
||||
|
||||
```csharp
|
||||
var noTwoLegInput = new TwoLegProjectionInput(
|
||||
false, 0f, 0f, 0d, 0f, 0f, 0f, 0f, 0f,
|
||||
"当前没有 TwoLeg 检测结果");
|
||||
```
|
||||
|
||||
`hasDetection` 为 `false` 时，该来源会返回“空”结果，不会将零坐标当作障碍物。若 TwoLeg 只是可选信息，建议 `isRequired` 设置为 `false`。
|
||||
|
||||
### 第 5 步：创建建图请求
|
||||
|
||||
边界与分辨率仍使用 **mm**。范围采用左闭右开，例如 `XMax = 6000f` 时，`x = 6000f` 不属于地图。
|
||||
|
||||
```csharp
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
// 世界范围：[0, 6000) × [0, 4000)，单位 mm。
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
|
||||
// 每格 50 mm。
|
||||
ResolutionMm = 50f,
|
||||
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
manualSource,
|
||||
twoLegSource,
|
||||
},
|
||||
|
||||
// false：没有任何有效障碍物时，地图不能直接交给规划器。
|
||||
// true：调用方明确确认空地图安全时，才允许空图参与规划。
|
||||
AllowExplicitEmptyMap = false,
|
||||
};
|
||||
```
|
||||
|
||||
### 第 6 步：创建地图并处理失败
|
||||
|
||||
```csharp
|
||||
PlanningMapBuildResult result = _mapFactory.Create(request);
|
||||
|
||||
if (!result.Succeeded)
|
||||
{
|
||||
Console.WriteLine("建图失败：" + result.FailureReason);
|
||||
return;
|
||||
}
|
||||
|
||||
if (result.Map == null || !result.Map.PlanningReady)
|
||||
{
|
||||
Console.WriteLine("地图不能用于规划：" + result.Map?.PlanningBlockReason);
|
||||
return;
|
||||
}
|
||||
|
||||
PlanningGridMap planningMap = result.Map;
|
||||
```
|
||||
|
||||
此处必须同时检查：
|
||||
|
||||
- `Succeeded`：请求、来源和栅格构建过程是否成功；
|
||||
- `Map != null`：是否产出了规划快照；
|
||||
- `PlanningReady`：是否允许把该快照交给规划器。隐式空图会在这一项被拦截。
|
||||
|
||||
需要查看每个来源是否成功投影时，读取 `result.SourceResults`；需要观察缓存效果时，读取 `result.CacheHit`。
|
||||
|
||||
```csharp
|
||||
Console.WriteLine(
|
||||
"快照编号=" + planningMap.SnapshotId
|
||||
+ "，缓存=" + result.CacheHit
|
||||
+ "，栅格=" + planningMap.Cols + "×" + planningMap.Rows);
|
||||
```
|
||||
|
||||
### 第 7 步：交给粗路径规划器查询
|
||||
|
||||
`PlanningGridMap` 是不可变对象，可以安全地作为一次规划任务的输入。注意：它的世界查询坐标单位已经变成 **m**，而不是 mm。
|
||||
|
||||
```csharp
|
||||
// 查询 (2.5 m, 1.0 m) 是否位于占据格；越界也会返回 true。
|
||||
bool occupied = planningMap.IsOccupiedWorld(2.5d, 1.0d);
|
||||
|
||||
// 查询该位置到最近障碍物的保守净距，单位 m；越界返回 0 m。
|
||||
double clearanceMeters =
|
||||
planningMap.GetConservativeObstacleDistanceMeters(2.5d, 1.0d);
|
||||
```
|
||||
|
||||
粗路径规划器应只使用 `PlanningGridMap` 的占据与距离查询，不应直接修改或重建其中的栅格。
|
||||
|
||||
### 完整实例
|
||||
|
||||
```csharp
|
||||
private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
|
||||
|
||||
private PlanningGridMap CreateMapForCoarsePlanning()
|
||||
{
|
||||
IMapObstacle[] obstacles =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
|
||||
new CircleObstacle(3600f, 1200f, 180f),
|
||||
};
|
||||
|
||||
var manualSource = new ManualObstacleSource("manual", 1L, true, obstacles);
|
||||
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = new IMapObstacleSource[] { manualSource },
|
||||
AllowExplicitEmptyMap = false,
|
||||
};
|
||||
|
||||
PlanningMapBuildResult result = _mapFactory.Create(request);
|
||||
if (!result.Succeeded)
|
||||
throw new InvalidOperationException("建图失败：" + result.FailureReason);
|
||||
if (result.Map == null || !result.Map.PlanningReady)
|
||||
throw new InvalidOperationException("地图不可规划：" + result.Map?.PlanningBlockReason);
|
||||
|
||||
return result.Map;
|
||||
}
|
||||
```
|
||||
|
||||
### 常见错误
|
||||
|
||||
| 情况 | 原因 | 处理方式 |
|
||||
| --- | --- | --- |
|
||||
| 改了障碍物但地图仍复用旧快照 | 未递增 `SourceVersion` | 障碍内容每次变化后增加该来源版本号 |
|
||||
| 规划查询位置总是越界 | 将 mm 坐标传给了 `PlanningGridMap` | 规划查询前将 mm 除以 1000 转为 m |
|
||||
| 地图创建成功但不能规划 | 未明确允许空图且没有有效障碍物 | 补充有效来源，或确认安全后设置 `AllowExplicitEmptyMap = true` |
|
||||
| TwoLeg 出现在错误位置 | 未使用检测时刻位姿，或混用了 mm 与 m | 用检测时刻的世界位姿和局部 mm 坐标创建快照 |
|
||||
| 缓存总是未命中 | 每次都新建 `PlanningMapFactory`，或版本号无意义变化 | 长期复用工厂；仅在来源内容实际变化时递增版本 |
|
||||
@@ -0,0 +1,22 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 提供一次性障碍物快照的纯数据来源。
|
||||
///
|
||||
/// 注意：<see cref="ProjectToWorld"/> 不得读取传感器、定位、UI 或时钟；调用前应由上层采集并封装完整快照。
|
||||
/// </summary>
|
||||
public interface IMapObstacleSource
|
||||
{
|
||||
/// <summary>来源的稳定唯一标识。不能为空；同一次建图请求中不得重复。</summary>
|
||||
string SourceId { get; }
|
||||
/// <summary>来源快照版本号，必须非负。快照几何或有效性变化时必须递增，以使输入缓存失效。</summary>
|
||||
long SourceVersion { get; }
|
||||
/// <summary>该来源是否必需。true 时不可用或无效会使整张地图构建失败；false 时只记录状态。</summary>
|
||||
bool IsRequired { get; }
|
||||
/// <summary>
|
||||
/// 将已采集的快照投影为世界障碍物几何。
|
||||
///
|
||||
/// 返回：世界坐标、单位 mm 的障碍物及其状态；不得访问任何实时外部状态。
|
||||
/// </summary>
|
||||
ObstacleProjectionResult ProjectToWorld();
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>直接提供世界 mm 几何列表的手工障碍物快照来源。</summary>
|
||||
public sealed class ManualObstacleSource : IMapObstacleSource
|
||||
{
|
||||
private readonly IReadOnlyList<IMapObstacle> _obstacles;
|
||||
/// <summary>
|
||||
/// 创建手工障碍物来源。
|
||||
///
|
||||
/// 参数：sourceId 为唯一来源标识；sourceVersion 为非负快照版本；isRequired 指示失败是否阻断建图；obstacles 为世界 mm 几何列表。
|
||||
/// 注意：几何内容变化时，调用方必须同步提高 sourceVersion。
|
||||
/// </summary>
|
||||
public ManualObstacleSource(string sourceId, long sourceVersion, bool isRequired, IReadOnlyList<IMapObstacle> obstacles)
|
||||
{
|
||||
SourceId = sourceId; SourceVersion = sourceVersion; IsRequired = isRequired; _obstacles = obstacles;
|
||||
}
|
||||
/// <summary>该快照的唯一来源标识。</summary>
|
||||
public string SourceId { get; }
|
||||
/// <summary>该快照的版本号；内容变化时必须递增。</summary>
|
||||
public long SourceVersion { get; }
|
||||
/// <summary>true 表示无效来源会阻断建图；false 表示只记录来源状态。</summary>
|
||||
public bool IsRequired { get; }
|
||||
/// <summary>
|
||||
/// 返回手工障碍物的世界几何。
|
||||
///
|
||||
/// 返回：列表为空时为 Empty；列表为空引用或含无效几何时为 Invalid；其余情况为 Applied。
|
||||
/// </summary>
|
||||
public ObstacleProjectionResult ProjectToWorld()
|
||||
{
|
||||
if (_obstacles == null) return ObstacleProjectionResult.Invalid("Manual obstacle collection is null.");
|
||||
if (_obstacles.Count == 0) return ObstacleProjectionResult.Empty("Manual obstacle collection is empty.");
|
||||
for (int i = 0; i < _obstacles.Count; i++)
|
||||
if (_obstacles[i] == null || !_obstacles[i].IsValid) return ObstacleProjectionResult.Invalid("Manual obstacle geometry is invalid.");
|
||||
return ObstacleProjectionResult.Applied(_obstacles, "Manual obstacle snapshot applied.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>障碍物来源的一次投影结果，包含状态、世界 mm 几何和可选诊断信息。</summary>
|
||||
public sealed class ObstacleProjectionResult
|
||||
{
|
||||
private ObstacleProjectionResult(ObstacleSourceStatus status, IReadOnlyList<IMapObstacle> obstacles, string diagnostic)
|
||||
{
|
||||
Status = status; Obstacles = obstacles ?? Array.Empty<IMapObstacle>(); Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
/// <summary>投影状态。只有 <see cref="ObstacleSourceStatus.Applied"/> 的几何会被栅格化。</summary>
|
||||
public ObstacleSourceStatus Status { get; }
|
||||
/// <summary>世界 mm 坐标的障碍物列表。非 Applied 状态时为空列表，不为 null。</summary>
|
||||
public IReadOnlyList<IMapObstacle> Obstacles { get; }
|
||||
/// <summary>用于日志和诊断的说明文本；未提供时为空字符串。</summary>
|
||||
public string Diagnostic { get; }
|
||||
/// <summary>创建成功投影结果。参数 obstacles 为有效世界 mm 几何；diagnostic 为可选诊断文本。返回的状态为 Applied。</summary>
|
||||
public static ObstacleProjectionResult Applied(IReadOnlyList<IMapObstacle> obstacles, string diagnostic = null)
|
||||
{
|
||||
return new ObstacleProjectionResult(ObstacleSourceStatus.Applied, obstacles, diagnostic);
|
||||
}
|
||||
/// <summary>创建空投影结果。参数 diagnostic 为可选原因；返回状态为 Empty，障碍物列表为空。</summary>
|
||||
public static ObstacleProjectionResult Empty(string diagnostic = null) { return new ObstacleProjectionResult(ObstacleSourceStatus.Empty, null, diagnostic); }
|
||||
/// <summary>创建不可用投影结果。参数 diagnostic 应说明快照缺失原因；返回状态为 Unavailable。</summary>
|
||||
public static ObstacleProjectionResult Unavailable(string diagnostic) { return new ObstacleProjectionResult(ObstacleSourceStatus.Unavailable, null, diagnostic); }
|
||||
/// <summary>创建无效投影结果。参数 diagnostic 应说明数据或几何错误；返回状态为 Invalid。</summary>
|
||||
public static ObstacleProjectionResult Invalid(string diagnostic) { return new ObstacleProjectionResult(ObstacleSourceStatus.Invalid, null, diagnostic); }
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>一个障碍物来源投影后的状态，供建图结果和调试日志判断。</summary>
|
||||
public enum ObstacleSourceStatus
|
||||
{
|
||||
/// <summary>成功得到一个或多个有效的世界 mm 障碍物，并会被栅格化。</summary>
|
||||
Applied,
|
||||
/// <summary>来源有效但本次没有障碍物；不会写入任何栅格。</summary>
|
||||
Empty,
|
||||
/// <summary>来源快照不可取得；若来源必需则整图构建失败。</summary>
|
||||
Unavailable,
|
||||
/// <summary>来源数据或几何无效；若来源必需则整图构建失败。</summary>
|
||||
Invalid,
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>将检测局部坐标中的两条腿转换为世界 mm 坐标圆形障碍物。</summary>
|
||||
public static class TwoLegObstacleProjector
|
||||
{
|
||||
/// <summary>
|
||||
/// 投影一份 TwoLeg 快照。
|
||||
///
|
||||
/// 参数：input 为检测时刻采集的纯数据快照，可为 null。
|
||||
/// 返回：null 为 Unavailable；未检测到目标为 Empty；数值无效为 Invalid；成功时为含两个圆形障碍物的 Applied。
|
||||
/// 注意：本方法只读取 input，不访问传感器、定位、UI 或时钟。
|
||||
/// </summary>
|
||||
public static ObstacleProjectionResult Project(TwoLegProjectionInput input)
|
||||
{
|
||||
if (input == null) return ObstacleProjectionResult.Unavailable("TwoLeg snapshot is unavailable.");
|
||||
if (!input.HasDetection) return ObstacleProjectionResult.Empty(input.Diagnostic);
|
||||
if (!input.IsValid) return ObstacleProjectionResult.Invalid("TwoLeg snapshot contains invalid values.");
|
||||
CoordinateTransform.LocalToWorld(input.DetectionWorldX, input.DetectionWorldY, input.DetectionHeadingRadians,
|
||||
input.FirstLocalX, input.FirstLocalY, out double firstX, out double firstY);
|
||||
CoordinateTransform.LocalToWorld(input.DetectionWorldX, input.DetectionWorldY, input.DetectionHeadingRadians,
|
||||
input.SecondLocalX, input.SecondLocalY, out double secondX, out double secondY);
|
||||
var obstacles = new List<IMapObstacle>
|
||||
{
|
||||
new CircleObstacle((float)firstX, (float)firstY, input.RadiusMm),
|
||||
new CircleObstacle((float)secondX, (float)secondY, input.RadiusMm),
|
||||
};
|
||||
return ObstacleProjectionResult.Applied(obstacles, input.Diagnostic);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>将一个 <see cref="TwoLegProjectionInput"/> 快照包装为统一障碍物来源。</summary>
|
||||
public sealed class TwoLegObstacleSource : IMapObstacleSource
|
||||
{
|
||||
private readonly TwoLegProjectionInput _input;
|
||||
/// <summary>
|
||||
/// 创建 TwoLeg 障碍物来源。
|
||||
///
|
||||
/// 参数：sourceId 为唯一标识；sourceVersion 为快照版本，输入内容改变时必须递增；isRequired 指示失败是否阻断建图；input 为检测时刻快照，可为空。
|
||||
/// </summary>
|
||||
public TwoLegObstacleSource(string sourceId, long sourceVersion, bool isRequired, TwoLegProjectionInput input)
|
||||
{
|
||||
SourceId = sourceId; SourceVersion = sourceVersion; IsRequired = isRequired; _input = input;
|
||||
}
|
||||
/// <summary>该快照来源的唯一标识。</summary>
|
||||
public string SourceId { get; }
|
||||
/// <summary>快照版本号；输入内容变化时必须递增。</summary>
|
||||
public long SourceVersion { get; }
|
||||
/// <summary>true 时投影不可用或无效会阻断建图。</summary>
|
||||
public bool IsRequired { get; }
|
||||
/// <summary>投影保存的 TwoLeg 快照。返回世界 mm 圆形障碍物，且不访问实时检测、定位、UI 或时钟。</summary>
|
||||
public ObstacleProjectionResult ProjectToWorld() { return TwoLegObstacleProjector.Project(_input); }
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// TwoLeg 检测时刻的纯输入快照。
|
||||
///
|
||||
/// 单位：位置和两腿局部坐标均为 mm，航向为弧度。该对象不读取检测器或定位模块，确保投影可重复。
|
||||
/// </summary>
|
||||
public sealed class TwoLegProjectionInput
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建 TwoLeg 投影快照。
|
||||
///
|
||||
/// 参数：hasDetection 表示当前是否检测到目标；detectionWorldX/Y 为检测坐标系原点的世界 mm 坐标；detectionHeadingRadians 为其相对世界坐标系的航向；firstLocalX/Y、secondLocalX/Y 为两腿在检测局部坐标系中的 mm 坐标；radiusMm 为每条腿投影圆半径；diagnostic 为可选诊断文本。
|
||||
/// 注意：hasDetection 为 false 时其余几何参数不会参与有效性校验。
|
||||
/// </summary>
|
||||
public TwoLegProjectionInput(bool hasDetection, float detectionWorldX, float detectionWorldY,
|
||||
double detectionHeadingRadians, float firstLocalX, float firstLocalY,
|
||||
float secondLocalX, float secondLocalY, float radiusMm, string diagnostic = null)
|
||||
{
|
||||
HasDetection = hasDetection; DetectionWorldX = detectionWorldX; DetectionWorldY = detectionWorldY;
|
||||
DetectionHeadingRadians = detectionHeadingRadians; FirstLocalX = firstLocalX; FirstLocalY = firstLocalY;
|
||||
SecondLocalX = secondLocalX; SecondLocalY = secondLocalY; RadiusMm = radiusMm; Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
/// <summary>检测是否存在。false 时投影结果为 Empty。</summary>
|
||||
public bool HasDetection { get; }
|
||||
/// <summary>检测局部坐标系原点的世界 X 坐标，单位 mm。</summary>
|
||||
public float DetectionWorldX { get; }
|
||||
/// <summary>检测局部坐标系原点的世界 Y 坐标，单位 mm。</summary>
|
||||
public float DetectionWorldY { get; }
|
||||
/// <summary>检测局部坐标系相对世界坐标系的航向，单位弧度。</summary>
|
||||
public double DetectionHeadingRadians { get; }
|
||||
/// <summary>第一条腿在检测局部坐标系的 X 坐标，单位 mm。</summary>
|
||||
public float FirstLocalX { get; }
|
||||
/// <summary>第一条腿在检测局部坐标系的 Y 坐标，单位 mm。</summary>
|
||||
public float FirstLocalY { get; }
|
||||
/// <summary>第二条腿在检测局部坐标系的 X 坐标，单位 mm。</summary>
|
||||
public float SecondLocalX { get; }
|
||||
/// <summary>第二条腿在检测局部坐标系的 Y 坐标，单位 mm。</summary>
|
||||
public float SecondLocalY { get; }
|
||||
/// <summary>投影为圆形障碍物时的半径，单位 mm。</summary>
|
||||
public float RadiusMm { get; }
|
||||
/// <summary>上游检测阶段提供的可选诊断文本；未提供时为空字符串。</summary>
|
||||
public string Diagnostic { get; }
|
||||
/// <summary>未检测到目标时为 true；检测到目标时，所有数值有限且半径非负才为 true。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return !HasDetection || (NumericGuard.IsFinite(DetectionWorldX) && NumericGuard.IsFinite(DetectionWorldY) && NumericGuard.IsFinite(DetectionHeadingRadians) && NumericGuard.IsFinite(FirstLocalX) && NumericGuard.IsFinite(FirstLocalY) && NumericGuard.IsFinite(SecondLocalX) && NumericGuard.IsFinite(SecondLocalY) && NumericGuard.IsFinite(RadiusMm) && RadiusMm >= 0f); }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 用于手工调试的 Clumsy 地图测试入口，只通过公开的 <see cref="PlanningMapFactory"/> 创建快照。
|
||||
///
|
||||
/// 注意：终端日志和 PNG 开关只影响调试输出，不参与建图输入、地图内容或缓存键。
|
||||
/// </summary>
|
||||
[MovementTest(name = "规划地图快照测试V1")]
|
||||
public sealed class PlanningMapTest : MovementTest
|
||||
{
|
||||
private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
|
||||
private const bool EnableTerminalDebugLog = true;
|
||||
private const bool SavePng = true;
|
||||
private const float MapXMinMm = 0f;
|
||||
private const float MapXMaxMm = 6000f;
|
||||
private const float MapYMinMm = 0f;
|
||||
private const float MapYMaxMm = 4000f;
|
||||
private const float GridResolutionMm = 50f;
|
||||
private const bool AllowExplicitEmptyMap = false;
|
||||
private const bool EnableTwoLegSnapshot = false;
|
||||
|
||||
/// <summary>
|
||||
/// 执行一次配置好的地图创建与调试输出。
|
||||
///
|
||||
/// 返回：无。测试使用本文件顶部的地图范围、分辨率、障碍物来源、日志和 PNG 开关；不会接入实时 TwoLeg 检测。
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
IMapObstacleSource[] sources = CreateObstacleSources();
|
||||
Log("========== 规划地图创建开始 ==========");
|
||||
Log("参数：地图范围=(" + MapXMinMm + "," + MapXMaxMm + ")x(" + MapYMinMm + "," + MapYMaxMm
|
||||
+ ")mm，分辨率=" + GridResolutionMm + "mm，允许空图=" + AllowExplicitEmptyMap + "，导出图片=" + SavePng);
|
||||
Log("障碍物来源：" + FormatSources(sources));
|
||||
var result = _mapFactory.Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(MapXMinMm, MapXMaxMm, MapYMinMm, MapYMaxMm),
|
||||
ResolutionMm = GridResolutionMm,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = AllowExplicitEmptyMap,
|
||||
});
|
||||
Log("投影结果：" + FormatProjectionResults(sources, result.SourceResults));
|
||||
if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
|
||||
{
|
||||
Log("规划地图创建失败：" + result.FailureReason);
|
||||
Log("========== 规划地图创建结束 ==========");
|
||||
return;
|
||||
}
|
||||
Log("地图快照：编号=" + result.Map.SnapshotId + "，缓存=" + FormatCacheHit(result.CacheHit) + "，栅格=" + result.Map.Cols + "x" + result.Map.Rows
|
||||
+ "，可规划=" + result.Map.PlanningReady + "，指纹=" + result.Map.InputFingerprint);
|
||||
PlanningMapImageExportResult image = PlanningMapImageExporter.ExportIfEnabled(SavePng,
|
||||
new PlanningMapImageExportRequest { Map = result.Map, OutputRootDirectory = Environment.CurrentDirectory });
|
||||
Log("图片导出：" + FormatImageResult(image));
|
||||
Log("========== 规划地图创建完成 ==========");
|
||||
}
|
||||
|
||||
private static IMapObstacleSource[] CreateObstacleSources()
|
||||
{
|
||||
IMapObstacle[] manualObstacles =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
|
||||
new CircleObstacle(3600f, 1200f, 180f),
|
||||
};
|
||||
return new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("manual", 1L, true, manualObstacles),
|
||||
new TwoLegObstacleSource("two-leg", EnableTwoLegSnapshot ? 1L : 0L, false,
|
||||
new TwoLegProjectionInput(EnableTwoLegSnapshot, 0f, 0f, 0d, 0f, 0f, 0f, 0f, 0f, "No current TwoLeg snapshot.")),
|
||||
};
|
||||
}
|
||||
|
||||
private static string FormatSources(IReadOnlyList<IMapObstacleSource> sources)
|
||||
{
|
||||
return string.Join("；", sources.Select(source => source.SourceId + "（版本=" + source.SourceVersion + "，必需=" + source.IsRequired + "）"));
|
||||
}
|
||||
|
||||
private static string FormatProjectionResults(IReadOnlyList<IMapObstacleSource> sources, IReadOnlyList<ObstacleProjectionResult> results)
|
||||
{
|
||||
IMapObstacleSource[] sortedSources = sources.OrderBy(source => source.SourceId, StringComparer.Ordinal).ToArray();
|
||||
return string.Join("；", results.Select((result, index) => sortedSources[index].SourceId + "=" + FormatSourceStatus(result.Status) + "（" + result.Obstacles.Count + " 个障碍物）"));
|
||||
}
|
||||
|
||||
private static string FormatSourceStatus(ObstacleSourceStatus status)
|
||||
{
|
||||
switch (status)
|
||||
{
|
||||
case ObstacleSourceStatus.Applied: return "已应用";
|
||||
case ObstacleSourceStatus.Empty: return "空";
|
||||
case ObstacleSourceStatus.Unavailable: return "不可用";
|
||||
case ObstacleSourceStatus.Invalid: return "无效";
|
||||
default: return "未知";
|
||||
}
|
||||
}
|
||||
|
||||
private static string FormatCacheHit(PlanningMapCacheHit cacheHit)
|
||||
{
|
||||
switch (cacheHit)
|
||||
{
|
||||
case PlanningMapCacheHit.Input: return "输入命中";
|
||||
case PlanningMapCacheHit.Occupancy: return "占据图命中";
|
||||
default: return "未命中";
|
||||
}
|
||||
}
|
||||
|
||||
private static string FormatImageResult(PlanningMapImageExportResult image)
|
||||
{
|
||||
if (image.Skipped) return "未启用";
|
||||
if (image.Saved) return "已保存：" + image.FilePath;
|
||||
return "失败：" + image.Message;
|
||||
}
|
||||
|
||||
private static void Log(string message)
|
||||
{
|
||||
if (EnableTerminalDebugLog) Console.WriteLine("[PlanningMapTest] " + message);
|
||||
}
|
||||
|
||||
/// <summary>停止测试。当前测试不启动持续任务，因此无需额外清理资源。</summary>
|
||||
public override void TestStop() { }
|
||||
}
|
||||
+10
@@ -0,0 +1,10 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>可选 PNG 调试图导出请求，只读取不可变的规划地图快照。</summary>
|
||||
public sealed class PlanningMapImageExportRequest
|
||||
{
|
||||
/// <summary>待渲染的规划地图快照。不能为空；PNG 导出不会改动该地图或其缓存。</summary>
|
||||
public PlanningGridMap Map { get; set; }
|
||||
/// <summary>PNG 输出根目录。导出器会在其中创建 PlanningMapExports 子目录；不能为空。</summary>
|
||||
public string OutputRootDirectory { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>一次可选 PNG 导出的处理结果，包含状态、输出路径、像素尺寸和诊断信息。</summary>
|
||||
public sealed class PlanningMapImageExportResult
|
||||
{
|
||||
/// <summary>是否已经成功写入并完成 PNG 文件。</summary>
|
||||
public bool Saved { get; set; }
|
||||
/// <summary>是否因导出开关关闭而跳过。true 时不创建文件，也不影响地图构建或缓存。</summary>
|
||||
public bool Skipped { get; set; }
|
||||
/// <summary>成功保存时的 PNG 完整路径；未保存时通常为 null。</summary>
|
||||
public string FilePath { get; set; }
|
||||
/// <summary>保存、跳过或失败的诊断信息。</summary>
|
||||
public string Message { get; set; }
|
||||
/// <summary>成功 PNG 的文件字节数；未保存时为零。</summary>
|
||||
public long FileSizeBytes { get; set; }
|
||||
/// <summary>输出图像宽度，单位为像素。</summary>
|
||||
public int PixelWidth { get; set; }
|
||||
/// <summary>输出图像高度，单位为像素。</summary>
|
||||
public int PixelHeight { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>可选 PNG 导出器；它永远不参与地图构建、障碍物投影或缓存指纹计算。</summary>
|
||||
public static class PlanningMapImageExporter
|
||||
{
|
||||
/// <summary>每个规划栅格在输出图中占用的边长，单位为像素。</summary>
|
||||
public const int PixelsPerCell = 4;
|
||||
/// <summary>输出 PNG 单边允许的最大像素数，超过时拒绝导出。</summary>
|
||||
public const int MaximumImageEdgePixels = 4000;
|
||||
/// <summary>输出 PNG 允许的最大文件大小，单位为字节（50 MiB）。</summary>
|
||||
public const long MaximumFileSizeBytes = 50L * 1024L * 1024L;
|
||||
/// <summary>写入 PNG 的物理分辨率元数据，单位 DPI。</summary>
|
||||
public const float OutputDpi = 300f;
|
||||
private const int MaximumFilenameAttempts = 1024;
|
||||
|
||||
/// <summary>
|
||||
/// 在开关开启时导出规划地图 PNG。
|
||||
///
|
||||
/// 参数：enabled 为导出开关；request 包含不可变地图和输出根目录。
|
||||
/// 返回：开关关闭时返回 Skipped；成功时返回 Saved、路径、字节数与像素尺寸；输入或尺寸不合法时返回失败信息。
|
||||
/// 注意：该方法仅用于调试，绝不会影响地图创建和缓存结果。
|
||||
/// </summary>
|
||||
public static PlanningMapImageExportResult ExportIfEnabled(bool enabled, PlanningMapImageExportRequest request)
|
||||
{
|
||||
if (!enabled) return new PlanningMapImageExportResult { Skipped = true, Message = "Planning map image export is disabled." };
|
||||
if (request == null || request.Map == null) return Rejected("Planning map image export requires a PlanningGridMap.");
|
||||
if (string.IsNullOrWhiteSpace(request.OutputRootDirectory)) return Rejected("Planning map image export requires an output root directory.");
|
||||
long width = (long)request.Map.Cols * PixelsPerCell, height = (long)request.Map.Rows * PixelsPerCell;
|
||||
if (width <= 0 || height <= 0 || width > MaximumImageEdgePixels || height > MaximumImageEdgePixels) return Rejected("Planning map image dimensions exceed the permitted edge.", width, height);
|
||||
string temporary = null;
|
||||
try
|
||||
{
|
||||
string directory = Path.Combine(request.OutputRootDirectory, "PlanningMapExports");
|
||||
Directory.CreateDirectory(directory);
|
||||
using (FileStream stream = CreateTemporaryFile(directory, out string finalPath, out temporary))
|
||||
{
|
||||
byte[] rgba = PlanningMapImageRenderer.Render(request.Map, PixelsPerCell, out int pixelWidth, out int pixelHeight);
|
||||
ValidatedPngWriter.Write(rgba, pixelWidth, pixelHeight, stream);
|
||||
}
|
||||
long length = new FileInfo(temporary).Length;
|
||||
if (length > MaximumFileSizeBytes) return Rejected("Planning map image PNG exceeds 50 MiB.", width, height);
|
||||
string completed = temporary.Substring(0, temporary.Length - 4);
|
||||
File.Move(temporary, completed); temporary = null;
|
||||
return new PlanningMapImageExportResult { Saved = true, FilePath = completed, FileSizeBytes = length, PixelWidth = (int)width, PixelHeight = (int)height, Message = "Planning map image export saved." };
|
||||
}
|
||||
catch (Exception exception) { return Rejected("Planning map image export failed: " + exception.Message, width, height); }
|
||||
finally { if (temporary != null && File.Exists(temporary)) File.Delete(temporary); }
|
||||
}
|
||||
|
||||
private static PlanningMapImageExportResult Rejected(string message, long width = 0, long height = 0) { return new PlanningMapImageExportResult { Message = message, PixelWidth = width > int.MaxValue ? int.MaxValue : (int)width, PixelHeight = height > int.MaxValue ? int.MaxValue : (int)height }; }
|
||||
private static FileStream CreateTemporaryFile(string directory, out string finalPath, out string temporaryPath)
|
||||
{
|
||||
string timestamp = DateTime.Now.ToString("yyyyMMdd_HHmmss_fff");
|
||||
for (int index = 0; index < MaximumFilenameAttempts; index++)
|
||||
{
|
||||
string suffix = index == 0 ? string.Empty : "_" + index;
|
||||
finalPath = Path.Combine(directory, "PlanningMap_" + timestamp + suffix + ".png"); temporaryPath = finalPath + ".tmp";
|
||||
if (File.Exists(finalPath)) continue;
|
||||
try { return new FileStream(temporaryPath, FileMode.CreateNew, FileAccess.Write, FileShare.None); }
|
||||
catch (IOException) { }
|
||||
}
|
||||
finalPath = null; temporaryPath = null; throw new IOException("Could not reserve a unique PlanningMap PNG filename.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>从只读规划地图生成简单的 RGBA 占据图字节数组。</summary>
|
||||
internal static class PlanningMapImageRenderer
|
||||
{
|
||||
/// <summary>
|
||||
/// 渲染规划地图为行主序 RGBA 像素。
|
||||
///
|
||||
/// 参数：map 为只读规划快照；pixelsPerCell 为每个栅格的像素边长；width、height 返回图像像素尺寸。
|
||||
/// 返回：长度为 width × height × 4 的 RGBA 字节数组；不会修改地图。
|
||||
/// </summary>
|
||||
public static byte[] Render(PlanningGridMap map, int pixelsPerCell, out int width, out int height)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
width = checked(map.Cols * pixelsPerCell);
|
||||
height = checked(map.Rows * pixelsPerCell);
|
||||
var rgba = new byte[checked(width * height * 4)];
|
||||
for (int row = 0; row < map.Rows; row++)
|
||||
for (int col = 0; col < map.Cols; col++)
|
||||
{
|
||||
bool occupied = map.IsOccupied(row, col);
|
||||
byte red = occupied ? (byte)220 : (byte)245;
|
||||
byte green = occupied ? (byte)45 : (byte)245;
|
||||
byte blue = occupied ? (byte)45 : (byte)245;
|
||||
int displayRow = map.Rows - 1 - row;
|
||||
for (int py = 0; py < pixelsPerCell; py++)
|
||||
for (int px = 0; px < pixelsPerCell; px++)
|
||||
{
|
||||
int index = ((displayRow * pixelsPerCell + py) * width + col * pixelsPerCell + px) * 4;
|
||||
rgba[index] = red; rgba[index + 1] = green; rgba[index + 2] = blue; rgba[index + 3] = 255;
|
||||
}
|
||||
}
|
||||
return rgba;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using StbImageWriteSharp;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>编码 RGBA 像素并校验 PNG 文件头、必要块和全部 CRC 的内部写入器。</summary>
|
||||
internal static class ValidatedPngWriter
|
||||
{
|
||||
private static readonly byte[] Signature = { 137, 80, 78, 71, 13, 10, 26, 10 };
|
||||
private static readonly byte[] PhysType = { 112, 72, 89, 115 };
|
||||
|
||||
/// <summary>
|
||||
/// 将 RGBA 像素写入经过校验的 PNG 流。
|
||||
///
|
||||
/// 参数：rgba 为行主序、每像素四字节的红绿蓝透明度数组；width、height 为像素尺寸；output 为可写目标流。
|
||||
/// 返回：无。输入长度不等于 width × height × 4 或 PNG 校验失败时抛出异常。
|
||||
/// </summary>
|
||||
public static void Write(byte[] rgba, int width, int height, Stream output)
|
||||
{
|
||||
Write(rgba, width, height, output, 11811u);
|
||||
}
|
||||
|
||||
/// <summary>将 RGBA 像素写入带指定物理分辨率元数据的经过校验的 PNG 流。</summary>
|
||||
internal static void Write(byte[] rgba, int width, int height, Stream output, uint pixelsPerMeter)
|
||||
{
|
||||
if (rgba == null || output == null || width <= 0 || height <= 0 || pixelsPerMeter == 0u || rgba.Length != checked(width * height * 4))
|
||||
throw new ArgumentException("Invalid RGBA PNG input.");
|
||||
byte[] encoded;
|
||||
using (var memory = new MemoryStream())
|
||||
{
|
||||
new ImageWriter().WritePng(rgba, width, height, ColorComponents.RedGreenBlueAlpha, memory);
|
||||
encoded = memory.ToArray();
|
||||
}
|
||||
Validate(encoded);
|
||||
const int ihdrEndOffset = 8 + 4 + 4 + 13 + 4;
|
||||
using (var outputMemory = new MemoryStream())
|
||||
{
|
||||
outputMemory.Write(encoded, 0, ihdrEndOffset);
|
||||
var phys = new byte[9];
|
||||
WriteUInt32BigEndian(phys, 0, pixelsPerMeter);
|
||||
WriteUInt32BigEndian(phys, 4, pixelsPerMeter);
|
||||
phys[8] = 1;
|
||||
WriteChunk(outputMemory, PhysType, phys);
|
||||
outputMemory.Write(encoded, ihdrEndOffset, encoded.Length - ihdrEndOffset);
|
||||
encoded = outputMemory.ToArray();
|
||||
}
|
||||
Validate(encoded);
|
||||
output.Write(encoded, 0, encoded.Length);
|
||||
}
|
||||
|
||||
private static void Validate(byte[] png)
|
||||
{
|
||||
if (png == null || png.Length < 45) throw new InvalidDataException("PNG is truncated.");
|
||||
for (int i = 0; i < Signature.Length; i++) if (png[i] != Signature[i]) throw new InvalidDataException("PNG signature is invalid.");
|
||||
int offset = Signature.Length, ihdr = 0, iend = 0;
|
||||
while (offset < png.Length)
|
||||
{
|
||||
if (png.Length - offset < 12) throw new InvalidDataException("PNG chunk header is truncated.");
|
||||
uint length = ReadUInt32BigEndian(png, offset);
|
||||
long crcOffset = (long)offset + 8L + length;
|
||||
if (crcOffset + 4L > png.Length) throw new InvalidDataException("PNG chunk is truncated.");
|
||||
int typeOffset = offset + 4;
|
||||
uint expected = ReadUInt32BigEndian(png, (int)crcOffset);
|
||||
uint actual = ComputeCrc32(png, typeOffset, checked((int)length + 4));
|
||||
if (expected != actual) throw new InvalidDataException("PNG chunk CRC is invalid.");
|
||||
bool isIhdr = IsType(png, typeOffset, 73, 72, 68, 82);
|
||||
bool isIend = IsType(png, typeOffset, 73, 69, 78, 68);
|
||||
if (offset == Signature.Length && !isIhdr) throw new InvalidDataException("PNG must start with IHDR.");
|
||||
if (isIhdr) ihdr++;
|
||||
if (isIend) { iend++; if (crcOffset + 4L != png.Length) throw new InvalidDataException("PNG data follows IEND."); }
|
||||
offset = checked((int)crcOffset + 4);
|
||||
}
|
||||
if (ihdr != 1 || iend != 1) throw new InvalidDataException("PNG must contain exactly one IHDR and IEND.");
|
||||
}
|
||||
private static bool IsType(byte[] bytes, int offset, byte a, byte b, byte c, byte d) { return bytes[offset] == a && bytes[offset + 1] == b && bytes[offset + 2] == c && bytes[offset + 3] == d; }
|
||||
private static uint ReadUInt32BigEndian(byte[] bytes, int offset) { return ((uint)bytes[offset] << 24) | ((uint)bytes[offset + 1] << 16) | ((uint)bytes[offset + 2] << 8) | bytes[offset + 3]; }
|
||||
private static void WriteUInt32BigEndian(byte[] bytes, int offset, uint value) { bytes[offset] = (byte)(value >> 24); bytes[offset + 1] = (byte)(value >> 16); bytes[offset + 2] = (byte)(value >> 8); bytes[offset + 3] = (byte)value; }
|
||||
private static void WriteChunk(Stream output, byte[] type, byte[] data)
|
||||
{
|
||||
var length = new byte[4]; WriteUInt32BigEndian(length, 0, (uint)data.Length); output.Write(length, 0, 4); output.Write(type, 0, 4); output.Write(data, 0, data.Length);
|
||||
var crcBytes = new byte[4]; WriteUInt32BigEndian(crcBytes, 0, ComputeCrc32(type, 0, type.Length, data)); output.Write(crcBytes, 0, 4);
|
||||
}
|
||||
private static uint ComputeCrc32(byte[] data, int offset, int count) { return ComputeCrc32(data, offset, count, null); }
|
||||
private static uint ComputeCrc32(byte[] first, int offset, int count, byte[] second)
|
||||
{
|
||||
uint crc = 0xffffffffu;
|
||||
for (int i = 0; i < count; i++) crc = UpdateCrc(crc, first[offset + i]);
|
||||
if (second != null) for (int i = 0; i < second.Length; i++) crc = UpdateCrc(crc, second[i]);
|
||||
return crc ^ 0xffffffffu;
|
||||
}
|
||||
private static uint UpdateCrc(uint crc, byte value)
|
||||
{
|
||||
crc ^= value;
|
||||
for (int bit = 0; bit < 8; bit++) crc = (crc & 1u) == 0u ? crc >> 1 : 0xedb88320u ^ (crc >> 1);
|
||||
return crc;
|
||||
}
|
||||
}
|
||||
@@ -1,412 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>按方向段独立生成夹持三次 B 样条原始几何候选。</summary>
|
||||
internal sealed class CubicBSplineSmoother : IPathSmoother
|
||||
{
|
||||
private const int Degree = 3;
|
||||
private const int SamplesPerSpan = 64;
|
||||
private const double StraightToleranceMeters = 1e-9d;
|
||||
private const double EndpointProbeParameter = 1e-6d;
|
||||
private const double MinimumTangentHandleLengthMeters = 1e-10d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null || input.OriginalPath == null ||
|
||||
input.Options == null ||
|
||||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
||||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
||||
input.MinimumClearanceReserveMeters < 0d)
|
||||
{
|
||||
return SmoothingCandidate.Failed("B 样条输入、强度或净空预留无效。");
|
||||
}
|
||||
|
||||
var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
||||
if (!TrySmoothSegment(
|
||||
sourceSegment,
|
||||
effectiveStrength,
|
||||
input.MinimumClearanceReserveMeters,
|
||||
input.Options.CubicBSplineEndpointTangentScale,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> points,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status))
|
||||
{
|
||||
return status == SmoothingCandidateStatus.RetryableInfeasible
|
||||
? SmoothingCandidate.RetryableInfeasible(reason)
|
||||
: SmoothingCandidate.Failed(reason);
|
||||
}
|
||||
|
||||
candidateSegments.Add(new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
points,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch,
|
||||
sourceSegment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
return SmoothingCandidate.Success(candidateSegments);
|
||||
}
|
||||
|
||||
private static bool TrySmoothSegment(
|
||||
PreparedDirectionSegment sourceSegment,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
double endpointTangentScale,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> result,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
result = null;
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
|
||||
{
|
||||
reason = "B 样条方向段为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
||||
for (int index = 0; index < anchors.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!IsValidAnchor(anchors[index]))
|
||||
{
|
||||
reason = "B 样条方向段包含非法锚点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (anchors.Count <= Degree || IsStraight(anchors))
|
||||
{
|
||||
result = anchors;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, endpointTangentScale,
|
||||
cancellationToken, out Point2D[] controls, out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double[] knots = CreateClampedKnots(controls.Length);
|
||||
var sampled = new List<SmoothingPoint2D>();
|
||||
int spanCount = controls.Length - Degree;
|
||||
int uniformIntervals = spanCount * SamplesPerSpan;
|
||||
if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
|
||||
return false;
|
||||
if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
||||
sampled, cancellationToken, out reason, out status))
|
||||
return false;
|
||||
for (int index = 1; index < uniformIntervals; index++)
|
||||
{
|
||||
if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
|
||||
sampled, cancellationToken, out reason, out status))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
||||
sampled, cancellationToken, out reason, out status))
|
||||
return false;
|
||||
if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
|
||||
return false;
|
||||
|
||||
result = sampled;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateControls(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
TravelDirection direction,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
double endpointTangentScale,
|
||||
CancellationToken cancellationToken,
|
||||
out Point2D[] controls,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
controls = new Point2D[anchors.Count];
|
||||
controls[0] = Point2D.FromAnchor(anchors[0]);
|
||||
controls[controls.Length - 1] = Point2D.FromAnchor(anchors[anchors.Count - 1]);
|
||||
|
||||
double startHandleLength = Distance(anchors[0], anchors[1]) * endpointTangentScale * strength;
|
||||
double startTravelHeading = GetTravelHeading(anchors[0], direction);
|
||||
if (!TryConstrainTangentHandle(
|
||||
Point2D.FromAnchor(anchors[0]),
|
||||
Math.Cos(startTravelHeading),
|
||||
Math.Sin(startTravelHeading),
|
||||
startHandleLength,
|
||||
anchors[1],
|
||||
GetAllowedRadius(anchors[1], reserveMeters),
|
||||
out controls[1]))
|
||||
{
|
||||
reason = "B 样条起点切向手柄无法同时满足相邻锚点移动范围。";
|
||||
return false;
|
||||
}
|
||||
|
||||
int finalIndex = anchors.Count - 1;
|
||||
double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * endpointTangentScale * strength;
|
||||
double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
|
||||
if (!TryConstrainTangentHandle(
|
||||
Point2D.FromAnchor(anchors[finalIndex]),
|
||||
-Math.Cos(endTravelHeading),
|
||||
-Math.Sin(endTravelHeading),
|
||||
endHandleLength,
|
||||
anchors[finalIndex - 1],
|
||||
GetAllowedRadius(anchors[finalIndex - 1], reserveMeters),
|
||||
out controls[finalIndex - 1]))
|
||||
{
|
||||
reason = "B 样条终点切向手柄无法同时满足相邻锚点移动范围。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 2; index < finalIndex - 1; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D previous = anchors[index - 1];
|
||||
SmoothingPoint2D current = anchors[index];
|
||||
SmoothingPoint2D next = anchors[index + 1];
|
||||
Point2D target = new Point2D(
|
||||
(previous.X + current.X + next.X) / 3d,
|
||||
(previous.Y + current.Y + next.Y) / 3d);
|
||||
Point2D proposed = new Point2D(
|
||||
current.X + strength * (target.X - current.X),
|
||||
current.Y + strength * (target.Y - current.Y));
|
||||
controls[index] = ClampDisplacement(current, proposed, GetAllowedRadius(current, reserveMeters));
|
||||
}
|
||||
|
||||
for (int index = 0; index < controls.Length; index++)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(controls[index].X) || !NumericGuard.IsFinite(controls[index].Y))
|
||||
{
|
||||
reason = "B 样条控制点构造产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAddSample(
|
||||
double parameter,
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
Point2D[] controls,
|
||||
double[] knots,
|
||||
double reserveMeters,
|
||||
List<SmoothingPoint2D> output,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Point2D evaluated = Evaluate(controls, knots, parameter);
|
||||
if (!NumericGuard.IsFinite(evaluated.X) || !NumericGuard.IsFinite(evaluated.Y))
|
||||
{
|
||||
reason = "B 样条评估产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double targetArcLength = parameter * anchors[anchors.Count - 1].ArcLength;
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(anchors, targetArcLength,
|
||||
out SmoothingPoint2D reference, out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double displacement = Distance(evaluated, reference);
|
||||
if (!NumericGuard.IsFinite(displacement))
|
||||
{
|
||||
reason = "B 样条评估点位移产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
if (displacement > GetAllowedRadius(reference, reserveMeters))
|
||||
{
|
||||
reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
|
||||
status = SmoothingCandidateStatus.RetryableInfeasible;
|
||||
return false;
|
||||
}
|
||||
bool endpoint = parameter == 0d || parameter == 1d;
|
||||
output.Add(new SmoothingPoint2D(
|
||||
evaluated.X,
|
||||
evaluated.Y,
|
||||
reference.ArcLength,
|
||||
reference.Heading,
|
||||
reference.UnwrappedHeading,
|
||||
reference.BodyClearance,
|
||||
endpoint && reference.IsGearSwitchPoint,
|
||||
endpoint ? reference.Source : SmoothedPathPointSource.Interpolated));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryConstrainTangentHandle(
|
||||
Point2D endpoint,
|
||||
double rayDirectionX,
|
||||
double rayDirectionY,
|
||||
double desiredLength,
|
||||
SmoothingPoint2D adjacentAnchor,
|
||||
double allowedRadius,
|
||||
out Point2D control)
|
||||
{
|
||||
control = default;
|
||||
double offsetX = adjacentAnchor.X - endpoint.X;
|
||||
double offsetY = adjacentAnchor.Y - endpoint.Y;
|
||||
double projectedLength = offsetX * rayDirectionX + offsetY * rayDirectionY;
|
||||
double perpendicularX = offsetX - projectedLength * rayDirectionX;
|
||||
double perpendicularY = offsetY - projectedLength * rayDirectionY;
|
||||
double discriminant = allowedRadius * allowedRadius -
|
||||
(perpendicularX * perpendicularX + perpendicularY * perpendicularY);
|
||||
if (!NumericGuard.IsFinite(discriminant) || discriminant < 0d) return false;
|
||||
|
||||
double halfInterval = Math.Sqrt(discriminant);
|
||||
double minimumLength = Math.Max(MinimumTangentHandleLengthMeters, projectedLength - halfInterval);
|
||||
double maximumLength = projectedLength + halfInterval;
|
||||
if (!NumericGuard.IsFinite(maximumLength) || maximumLength < minimumLength) return false;
|
||||
|
||||
double constrainedLength = Math.Max(minimumLength, Math.Min(desiredLength, maximumLength));
|
||||
control = new Point2D(
|
||||
endpoint.X + constrainedLength * rayDirectionX,
|
||||
endpoint.Y + constrainedLength * rayDirectionY);
|
||||
return NumericGuard.IsFinite(control.X) && NumericGuard.IsFinite(control.Y);
|
||||
}
|
||||
|
||||
private static Point2D Evaluate(Point2D[] controls, double[] knots, double parameter)
|
||||
{
|
||||
if (parameter <= 0d) return controls[0];
|
||||
if (parameter >= 1d) return controls[controls.Length - 1];
|
||||
|
||||
var point = new Point2D(0d, 0d);
|
||||
for (int index = 0; index < controls.Length; index++)
|
||||
{
|
||||
double basis = EvaluateBasis(index, Degree, parameter, knots);
|
||||
point = new Point2D(point.X + basis * controls[index].X, point.Y + basis * controls[index].Y);
|
||||
}
|
||||
return point;
|
||||
}
|
||||
|
||||
private static double EvaluateBasis(int index, int degree, double parameter, double[] knots)
|
||||
{
|
||||
if (degree == 0)
|
||||
return knots[index] <= parameter && parameter < knots[index + 1] ? 1d : 0d;
|
||||
|
||||
double left = 0d;
|
||||
double leftDenominator = knots[index + degree] - knots[index];
|
||||
if (leftDenominator > 0d)
|
||||
left = (parameter - knots[index]) / leftDenominator * EvaluateBasis(index, degree - 1, parameter, knots);
|
||||
|
||||
double right = 0d;
|
||||
double rightDenominator = knots[index + degree + 1] - knots[index + 1];
|
||||
if (rightDenominator > 0d)
|
||||
right = (knots[index + degree + 1] - parameter) / rightDenominator *
|
||||
EvaluateBasis(index + 1, degree - 1, parameter, knots);
|
||||
return left + right;
|
||||
}
|
||||
|
||||
private static double[] CreateClampedKnots(int controlCount)
|
||||
{
|
||||
var knots = new double[controlCount + Degree + 1];
|
||||
for (int index = Degree + 1; index < controlCount; index++)
|
||||
knots[index] = (double)(index - Degree) / (controlCount - Degree);
|
||||
for (int index = controlCount; index < knots.Length; index++) knots[index] = 1d;
|
||||
return knots;
|
||||
}
|
||||
|
||||
private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius)
|
||||
{
|
||||
double deltaX = proposed.X - anchor.X;
|
||||
double deltaY = proposed.Y - anchor.Y;
|
||||
double distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= allowedRadius) return proposed;
|
||||
if (distance == 0d || allowedRadius == 0d) return Point2D.FromAnchor(anchor);
|
||||
double scale = allowedRadius / distance;
|
||||
return new Point2D(anchor.X + deltaX * scale, anchor.Y + deltaY * scale);
|
||||
}
|
||||
|
||||
private static bool IsStraight(IReadOnlyList<SmoothingPoint2D> anchors)
|
||||
{
|
||||
if (anchors.Count < 3) return true;
|
||||
SmoothingPoint2D first = anchors[0];
|
||||
SmoothingPoint2D last = anchors[anchors.Count - 1];
|
||||
double directionX = last.X - first.X;
|
||||
double directionY = last.Y - first.Y;
|
||||
double length = Math.Sqrt(directionX * directionX + directionY * directionY);
|
||||
if (!NumericGuard.IsFinite(length) || length <= StraightToleranceMeters) return false;
|
||||
for (int index = 1; index < anchors.Count - 1; index++)
|
||||
{
|
||||
double offsetX = anchors[index].X - first.X;
|
||||
double offsetY = anchors[index].Y - first.Y;
|
||||
double perpendicularDeviation = Math.Abs(directionX * offsetY - directionY * offsetX) / length;
|
||||
if (perpendicularDeviation >= StraightToleranceMeters) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidAnchor(SmoothingPoint2D point)
|
||||
{
|
||||
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||
NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.Heading) &&
|
||||
NumericGuard.IsFinite(point.UnwrappedHeading) && NumericGuard.IsFinite(point.BodyClearance) &&
|
||||
point.BodyClearance >= 0d;
|
||||
}
|
||||
|
||||
private static double GetAllowedRadius(SmoothingPoint2D anchor, double reserveMeters)
|
||||
{
|
||||
return Math.Max(0d, anchor.BodyClearance - reserveMeters);
|
||||
}
|
||||
|
||||
private static double GetTravelHeading(SmoothingPoint2D point, TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward ? point.Heading : point.Heading - Math.PI;
|
||||
}
|
||||
|
||||
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double deltaX = right.X - left.X;
|
||||
double deltaY = right.Y - left.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private static double Distance(Point2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double deltaX = right.X - left.X;
|
||||
double deltaY = right.Y - left.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private readonly struct Point2D
|
||||
{
|
||||
internal Point2D(double x, double y)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
}
|
||||
|
||||
internal double X { get; }
|
||||
internal double Y { get; }
|
||||
|
||||
internal static Point2D FromAnchor(SmoothingPoint2D anchor)
|
||||
{
|
||||
return new Point2D(anchor.X, anchor.Y);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,14 +0,0 @@
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>单一平滑方法生成原始几何候选的内部契约。</summary>
|
||||
internal interface IPathSmoother
|
||||
{
|
||||
SmoothingMethod Method { get; }
|
||||
|
||||
SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken);
|
||||
}
|
||||
@@ -1,427 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>在方向段内以局部三次 Bézier 连接替换明显转角。</summary>
|
||||
internal sealed class LocalCubicBezierSmoother : IPathSmoother
|
||||
{
|
||||
private const double WindowToleranceMeters = 1e-9d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.LocalCubicBezier;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null || input.OriginalPath == null || input.Options == null ||
|
||||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
||||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
||||
input.MinimumClearanceReserveMeters < 0d)
|
||||
{
|
||||
return SmoothingCandidate.Failed("Bézier 输入、强度或净空预留无效。");
|
||||
}
|
||||
|
||||
var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
||||
if (!TrySmoothSegment(
|
||||
sourceSegment,
|
||||
input.Options.BezierCornerHeadingThresholdRadians,
|
||||
input.Options.BezierMaximumWindowLengthMeters,
|
||||
input.Options.BezierHandleLengthRatio,
|
||||
effectiveStrength,
|
||||
input.MinimumClearanceReserveMeters,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> points,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status))
|
||||
{
|
||||
return status == SmoothingCandidateStatus.RetryableInfeasible
|
||||
? SmoothingCandidate.RetryableInfeasible(reason)
|
||||
: SmoothingCandidate.Failed(reason);
|
||||
}
|
||||
|
||||
candidateSegments.Add(new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
points,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch,
|
||||
sourceSegment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
return SmoothingCandidate.Success(candidateSegments);
|
||||
}
|
||||
|
||||
private static bool TrySmoothSegment(
|
||||
PreparedDirectionSegment sourceSegment,
|
||||
double cornerThresholdRadians,
|
||||
double maximumWindowLengthMeters,
|
||||
double handleLengthRatio,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> result,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
result = null;
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
|
||||
{
|
||||
reason = "Bézier 方向段为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
||||
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
|
||||
if (anchors.Count < 3)
|
||||
{
|
||||
result = anchors;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!TryCreateMergedWindows(
|
||||
anchors,
|
||||
cornerThresholdRadians,
|
||||
maximumWindowLengthMeters,
|
||||
cancellationToken,
|
||||
out List<Window> windows,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (windows.Count == 0)
|
||||
{
|
||||
result = anchors;
|
||||
return true;
|
||||
}
|
||||
|
||||
var output = new List<SmoothingPoint2D>(anchors.Count);
|
||||
int anchorIndex = 0;
|
||||
for (int windowIndex = 0; windowIndex < windows.Count; windowIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Window window = windows[windowIndex];
|
||||
while (anchorIndex <= window.StartIndex)
|
||||
{
|
||||
output.Add(anchors[anchorIndex]);
|
||||
anchorIndex++;
|
||||
}
|
||||
|
||||
if (!TryAppendWindowInterior(
|
||||
anchors,
|
||||
window,
|
||||
handleLengthRatio,
|
||||
strength,
|
||||
reserveMeters,
|
||||
output,
|
||||
cancellationToken,
|
||||
out reason,
|
||||
out status))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
output.Add(anchors[window.EndIndex]);
|
||||
anchorIndex = window.EndIndex + 1;
|
||||
}
|
||||
|
||||
while (anchorIndex < anchors.Count)
|
||||
{
|
||||
output.Add(anchors[anchorIndex]);
|
||||
anchorIndex++;
|
||||
}
|
||||
|
||||
result = output;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateAnchors(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int index = 0; index < anchors.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D point = anchors[index];
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
|
||||
!NumericGuard.IsFinite(point.BodyClearance) || point.BodyClearance < 0d ||
|
||||
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
|
||||
{
|
||||
reason = "Bézier 方向段包含非有限、非递增弧长或无效净空的锚点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateMergedWindows(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
double cornerThresholdRadians,
|
||||
double maximumWindowLengthMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out List<Window> windows,
|
||||
out string reason)
|
||||
{
|
||||
windows = new List<Window>();
|
||||
var candidates = new List<Window>();
|
||||
reason = string.Empty;
|
||||
for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!TryGetTravelTangent(anchors[cornerIndex - 1], anchors[cornerIndex], out Point2D entryTangent) ||
|
||||
!TryGetTravelTangent(anchors[cornerIndex], anchors[cornerIndex + 1], out Point2D exitTangent))
|
||||
{
|
||||
reason = "Bézier 转角包含零长度或非有限行进切向。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double cross = entryTangent.X * exitTangent.Y - entryTangent.Y * exitTangent.X;
|
||||
double dot = entryTangent.X * exitTangent.X + entryTangent.Y * exitTangent.Y;
|
||||
double turnRadians = Math.Atan2(Math.Abs(cross), dot);
|
||||
if (!NumericGuard.IsFinite(turnRadians))
|
||||
{
|
||||
reason = "Bézier 转角计算产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
if (turnRadians < cornerThresholdRadians) continue;
|
||||
|
||||
int startIndex = cornerIndex - 1;
|
||||
int endIndex = cornerIndex + 1;
|
||||
double windowLength = anchors[endIndex].ArcLength - anchors[startIndex].ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(windowLength))
|
||||
{
|
||||
reason = "Bézier 局部窗口弧长无效。";
|
||||
return false;
|
||||
}
|
||||
if (windowLength > maximumWindowLengthMeters + WindowToleranceMeters) continue;
|
||||
if (ContainsGearSwitch(anchors, startIndex, endIndex)) continue;
|
||||
|
||||
candidates.Add(new Window(startIndex, endIndex));
|
||||
}
|
||||
|
||||
MergeBoundedConnectedWindows(anchors, candidates, maximumWindowLengthMeters, windows);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void MergeBoundedConnectedWindows(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
IReadOnlyList<Window> candidates,
|
||||
double maximumWindowLengthMeters,
|
||||
List<Window> windows)
|
||||
{
|
||||
int candidateIndex = 0;
|
||||
while (candidateIndex < candidates.Count)
|
||||
{
|
||||
Window merged = candidates[candidateIndex];
|
||||
candidateIndex++;
|
||||
while (candidateIndex < candidates.Count &&
|
||||
candidates[candidateIndex].StartIndex <= merged.EndIndex + 1)
|
||||
{
|
||||
merged = new Window(merged.StartIndex,
|
||||
Math.Max(merged.EndIndex, candidates[candidateIndex].EndIndex));
|
||||
candidateIndex++;
|
||||
}
|
||||
|
||||
double mergedLength = anchors[merged.EndIndex].ArcLength - anchors[merged.StartIndex].ArcLength;
|
||||
if (mergedLength <= maximumWindowLengthMeters + WindowToleranceMeters)
|
||||
{
|
||||
windows.Add(merged);
|
||||
}
|
||||
// A connected group that exceeds the cap is declined as a whole. Splitting it into
|
||||
// adjacent local curves would introduce unrequested joins; accepting it would violate
|
||||
// the maximum-window contract. Its original anchors therefore remain unchanged.
|
||||
}
|
||||
}
|
||||
|
||||
private static bool TryAppendWindowInterior(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
Window window,
|
||||
double handleLengthRatio,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
List<SmoothingPoint2D> output,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
SmoothingPoint2D p0 = anchors[window.StartIndex];
|
||||
SmoothingPoint2D p3 = anchors[window.EndIndex];
|
||||
if (!TryGetTravelTangent(p0, anchors[window.StartIndex + 1], out Point2D entryTangent) ||
|
||||
!TryGetTravelTangent(anchors[window.EndIndex - 1], p3, out Point2D exitTangent))
|
||||
{
|
||||
reason = "Bézier 窗口端点包含无效行进切向。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double arcLength = p3.ArcLength - p0.ArcLength;
|
||||
double chordLength = Distance(p0, p3);
|
||||
double handleLength = chordLength * handleLengthRatio * strength;
|
||||
if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(chordLength) ||
|
||||
!NumericGuard.IsPositiveFinite(handleLength))
|
||||
{
|
||||
reason = "Bézier 窗口弧长、端点弦长或控制柄长度无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D control1 = new Point2D(
|
||||
p0.X + entryTangent.X * handleLength,
|
||||
p0.Y + entryTangent.Y * handleLength);
|
||||
Point2D control2 = new Point2D(
|
||||
p3.X - exitTangent.X * handleLength,
|
||||
p3.Y - exitTangent.Y * handleLength);
|
||||
if (!IsFinite(control1) || !IsFinite(control2))
|
||||
{
|
||||
reason = "Bézier 控制点产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = window.StartIndex + 1; index < window.EndIndex; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D anchor = anchors[index];
|
||||
double parameter = (anchor.ArcLength - p0.ArcLength) / arcLength;
|
||||
if (!NumericGuard.IsFinite(parameter) || parameter <= 0d || parameter >= 1d)
|
||||
{
|
||||
reason = "Bézier 窗口参数无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D evaluated = Evaluate(p0, control1, control2, p3, parameter);
|
||||
if (!IsFinite(evaluated))
|
||||
{
|
||||
reason = "Bézier 评估产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double referenceArcLength = p0.ArcLength + parameter * arcLength;
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
||||
anchors,
|
||||
referenceArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double displacement = Distance(evaluated, reference);
|
||||
if (!NumericGuard.IsFinite(displacement))
|
||||
{
|
||||
reason = "Bézier 评估点位移产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
|
||||
if (displacement > allowedDisplacement)
|
||||
{
|
||||
reason = "Bézier 评估点超过对应原始弧长参考点的允许移动范围。";
|
||||
status = SmoothingCandidateStatus.RetryableInfeasible;
|
||||
return false;
|
||||
}
|
||||
|
||||
output.Add(new SmoothingPoint2D(
|
||||
evaluated.X,
|
||||
evaluated.Y,
|
||||
reference.ArcLength,
|
||||
reference.Heading,
|
||||
reference.UnwrappedHeading,
|
||||
reference.BodyClearance,
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ContainsGearSwitch(IReadOnlyList<SmoothingPoint2D> anchors, int startIndex, int endIndex)
|
||||
{
|
||||
for (int index = startIndex; index <= endIndex; index++)
|
||||
{
|
||||
if (anchors[index].IsGearSwitchPoint) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool TryGetTravelTangent(SmoothingPoint2D start, SmoothingPoint2D end, out Point2D tangent)
|
||||
{
|
||||
tangent = default;
|
||||
double deltaX = end.X - start.X;
|
||||
double deltaY = end.Y - start.Y;
|
||||
double length = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsPositiveFinite(length)) return false;
|
||||
tangent = new Point2D(deltaX / length, deltaY / length);
|
||||
return IsFinite(tangent);
|
||||
}
|
||||
|
||||
private static Point2D Evaluate(SmoothingPoint2D p0, Point2D p1, Point2D p2, SmoothingPoint2D p3, double parameter)
|
||||
{
|
||||
double oneMinusParameter = 1d - parameter;
|
||||
double p0Weight = oneMinusParameter * oneMinusParameter * oneMinusParameter;
|
||||
double p1Weight = 3d * oneMinusParameter * oneMinusParameter * parameter;
|
||||
double p2Weight = 3d * oneMinusParameter * parameter * parameter;
|
||||
double p3Weight = parameter * parameter * parameter;
|
||||
return new Point2D(
|
||||
p0Weight * p0.X + p1Weight * p1.X + p2Weight * p2.X + p3Weight * p3.X,
|
||||
p0Weight * p0.Y + p1Weight * p1.Y + p2Weight * p2.Y + p3Weight * p3.Y);
|
||||
}
|
||||
|
||||
private static bool IsFinite(Point2D point)
|
||||
{
|
||||
return NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y);
|
||||
}
|
||||
|
||||
private static double Distance(Point2D point, SmoothingPoint2D reference)
|
||||
{
|
||||
double deltaX = point.X - reference.X;
|
||||
double deltaY = point.Y - reference.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double deltaX = left.X - right.X;
|
||||
double deltaY = left.Y - right.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private readonly struct Window
|
||||
{
|
||||
internal Window(int startIndex, int endIndex)
|
||||
{
|
||||
StartIndex = startIndex;
|
||||
EndIndex = endIndex;
|
||||
}
|
||||
|
||||
internal int StartIndex { get; }
|
||||
|
||||
internal int EndIndex { get; }
|
||||
}
|
||||
|
||||
private readonly struct Point2D
|
||||
{
|
||||
internal Point2D(double x, double y)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
}
|
||||
|
||||
internal double X { get; }
|
||||
|
||||
internal double Y { get; }
|
||||
}
|
||||
}
|
||||
@@ -1,557 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>按方向段局部弧长构造 C2 连续的分段五次 Hermite 原始几何候选。</summary>
|
||||
internal sealed class PiecewiseQuinticSmoother : IPathSmoother
|
||||
{
|
||||
private const int SamplesPerInterval = 8;
|
||||
private const double DoubleMachineEpsilon = 2.2204460492503131e-16d;
|
||||
private const double EndpointNormalizationUlps = 32d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.PiecewiseQuintic;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null || input.OriginalPath == null || input.Options == null ||
|
||||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
||||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
||||
input.MinimumClearanceReserveMeters < 0d ||
|
||||
!NumericGuard.IsPositiveFinite(input.Options.QuinticKnotSpacingMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(input.Options.QuinticMinimumKnotSpacingMeters) ||
|
||||
input.Options.QuinticKnotSpacingMeters < input.Options.QuinticMinimumKnotSpacingMeters)
|
||||
{
|
||||
return SmoothingCandidate.Failed("五次 Hermite 输入、强度、净空预留或结点间距无效。");
|
||||
}
|
||||
|
||||
var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
||||
if (!TrySmoothSegment(
|
||||
sourceSegment,
|
||||
effectiveStrength,
|
||||
input.MinimumClearanceReserveMeters,
|
||||
input.Options.QuinticKnotSpacingMeters,
|
||||
input.Options.QuinticMinimumKnotSpacingMeters,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> points,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status))
|
||||
{
|
||||
return status == SmoothingCandidateStatus.RetryableInfeasible
|
||||
? SmoothingCandidate.RetryableInfeasible(reason)
|
||||
: SmoothingCandidate.Failed(reason);
|
||||
}
|
||||
|
||||
candidateSegments.Add(new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
points,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch,
|
||||
sourceSegment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
return SmoothingCandidate.Success(candidateSegments);
|
||||
}
|
||||
|
||||
private static bool TrySmoothSegment(
|
||||
PreparedDirectionSegment sourceSegment,
|
||||
double effectiveStrength,
|
||||
double reserveMeters,
|
||||
double knotSpacingMeters,
|
||||
double minimumKnotSpacingMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> result,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
result = null;
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count < 2)
|
||||
{
|
||||
reason = "五次 Hermite 方向段至少需要两个锚点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
||||
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
|
||||
|
||||
if (!TryCreateKnots(
|
||||
anchors,
|
||||
sourceSegment.Direction,
|
||||
effectiveStrength,
|
||||
knotSpacingMeters,
|
||||
minimumKnotSpacingMeters,
|
||||
cancellationToken,
|
||||
out List<Knot> knots,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Each physical acceleration is blended once into its shared knot and then scaled by
|
||||
// the left/right local interval independently. Reusing this value is what makes the
|
||||
// curve C2 with respect to local arc length, even for nonuniform final intervals.
|
||||
if (!TryAssignSharedAccelerations(knots, out reason)) return false;
|
||||
if (!TryCreateIntervals(knots, out List<QuinticInterval> intervals, out reason)) return false;
|
||||
|
||||
var sampled = new List<SmoothingPoint2D>(1 + intervals.Count * SamplesPerInterval);
|
||||
for (int intervalIndex = 0; intervalIndex < intervals.Count; intervalIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
QuinticInterval interval = intervals[intervalIndex];
|
||||
int firstSample = intervalIndex == 0 ? 0 : 1;
|
||||
for (int sampleIndex = firstSample; sampleIndex <= SamplesPerInterval; sampleIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
double parameter = (double)sampleIndex / SamplesPerInterval;
|
||||
double referenceArcLength = interval.Start.ArcLength + parameter * interval.Length;
|
||||
if (!NumericGuard.IsFinite(referenceArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 采样参考弧长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
||||
anchors,
|
||||
referenceArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D evaluated;
|
||||
if (sampleIndex == 0)
|
||||
evaluated = interval.Start.Position;
|
||||
else if (sampleIndex == SamplesPerInterval)
|
||||
evaluated = interval.End.Position;
|
||||
else if (!interval.TryEvaluate(parameter, out evaluated, out Point2D derivative, out Point2D secondDerivative))
|
||||
{
|
||||
reason = "五次 Hermite 采样产生非有限位置或导数。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double displacement = Distance(evaluated, reference);
|
||||
if (!NumericGuard.IsFinite(displacement))
|
||||
{
|
||||
reason = "五次 Hermite 采样位移无效。";
|
||||
return false;
|
||||
}
|
||||
double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
|
||||
if (displacement > allowedDisplacement)
|
||||
{
|
||||
reason = "五次 Hermite 采样点超过对应局部弧长参考点的允许移动范围。";
|
||||
status = SmoothingCandidateStatus.RetryableInfeasible;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool firstEndpoint = intervalIndex == 0 && sampleIndex == 0;
|
||||
bool lastEndpoint = intervalIndex == intervals.Count - 1 && sampleIndex == SamplesPerInterval;
|
||||
if (firstEndpoint)
|
||||
{
|
||||
sampled.Add(anchors[0]);
|
||||
}
|
||||
else if (lastEndpoint)
|
||||
{
|
||||
sampled.Add(anchors[anchors.Count - 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sampled.Add(new SmoothingPoint2D(
|
||||
evaluated.X,
|
||||
evaluated.Y,
|
||||
reference.ArcLength,
|
||||
reference.Heading,
|
||||
reference.UnwrappedHeading,
|
||||
reference.BodyClearance,
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
result = sampled;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateAnchors(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int index = 0; index < anchors.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D point = anchors[index];
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.ArcLength) || !NumericGuard.IsFinite(point.Heading) ||
|
||||
!NumericGuard.IsFinite(point.UnwrappedHeading) || !NumericGuard.IsFinite(point.BodyClearance) ||
|
||||
point.ArcLength < 0d || point.BodyClearance < 0d ||
|
||||
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 方向段包含非有限、非递增弧长或无效净空的锚点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateKnots(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
TravelDirection direction,
|
||||
double effectiveStrength,
|
||||
double knotSpacingMeters,
|
||||
double minimumKnotSpacingMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out List<Knot> knots,
|
||||
out string reason)
|
||||
{
|
||||
knots = new List<Knot>();
|
||||
reason = string.Empty;
|
||||
double startArcLength = anchors[0].ArcLength;
|
||||
double endArcLength = anchors[anchors.Count - 1].ArcLength;
|
||||
double totalLength = endArcLength - startArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(totalLength) || totalLength < minimumKnotSpacingMeters)
|
||||
{
|
||||
reason = "五次 Hermite 方向段短于配置的最小结点间距。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryCreateKnot(anchors[0], direction, effectiveStrength, out Knot first))
|
||||
{
|
||||
reason = "五次 Hermite 起点结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(first);
|
||||
|
||||
double endpointTolerance = GetEndpointNormalizationTolerance(
|
||||
startArcLength,
|
||||
endArcLength,
|
||||
knotSpacingMeters);
|
||||
double previousArcLength = startArcLength;
|
||||
for (long knotOrdinal = 1L; ; knotOrdinal++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
double targetArcLength = startArcLength + knotOrdinal * knotSpacingMeters;
|
||||
if (!NumericGuard.IsFinite(targetArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 内部结点弧长无效。";
|
||||
return false;
|
||||
}
|
||||
if (targetArcLength >= endArcLength - endpointTolerance) break;
|
||||
if (targetArcLength <= previousArcLength)
|
||||
{
|
||||
reason = "五次 Hermite 内部结点无法在浮点弧长尺度上保持递增。";
|
||||
return false;
|
||||
}
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
||||
anchors,
|
||||
targetArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out reason) ||
|
||||
!TryCreateKnot(reference, direction, effectiveStrength, out Knot knot))
|
||||
{
|
||||
if (string.IsNullOrEmpty(reason)) reason = "五次 Hermite 内部结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(knot);
|
||||
previousArcLength = targetArcLength;
|
||||
}
|
||||
|
||||
if (!TryCreateKnot(anchors[anchors.Count - 1], direction, effectiveStrength, out Knot last))
|
||||
{
|
||||
reason = "五次 Hermite 终点结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(last);
|
||||
|
||||
for (int index = 1; index < knots.Count; index++)
|
||||
{
|
||||
double intervalLength = knots[index].ArcLength - knots[index - 1].ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(intervalLength) || intervalLength < minimumKnotSpacingMeters)
|
||||
{
|
||||
reason = "五次 Hermite 结点间隔无效或短于配置的最小间距。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double GetEndpointNormalizationTolerance(
|
||||
double startArcLength,
|
||||
double endArcLength,
|
||||
double knotSpacingMeters)
|
||||
{
|
||||
double magnitude = Math.Max(
|
||||
Math.Abs(startArcLength),
|
||||
Math.Max(Math.Abs(endArcLength), Math.Abs(knotSpacingMeters)));
|
||||
return EndpointNormalizationUlps * DoubleMachineEpsilon * magnitude;
|
||||
}
|
||||
|
||||
private static bool TryCreateKnot(
|
||||
SmoothingPoint2D reference,
|
||||
TravelDirection direction,
|
||||
double effectiveStrength,
|
||||
out Knot knot)
|
||||
{
|
||||
knot = default;
|
||||
if (reference == null || !NumericGuard.IsFinite(reference.X) || !NumericGuard.IsFinite(reference.Y) ||
|
||||
!NumericGuard.IsFinite(reference.ArcLength) || !NumericGuard.IsFinite(reference.Heading))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double travelHeading = direction == TravelDirection.Forward
|
||||
? reference.Heading
|
||||
: reference.Heading - Math.PI;
|
||||
double tangentX = Math.Cos(travelHeading);
|
||||
double tangentY = Math.Sin(travelHeading);
|
||||
if (!NumericGuard.IsFinite(tangentX) || !NumericGuard.IsFinite(tangentY)) return false;
|
||||
|
||||
var velocity = new Point2D(tangentX * effectiveStrength, tangentY * effectiveStrength);
|
||||
if (!velocity.IsFinite) return false;
|
||||
knot = new Knot(reference.ArcLength, new Point2D(reference.X, reference.Y), velocity);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAssignSharedAccelerations(List<Knot> knots, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int index = 0; index < knots.Count; index++)
|
||||
{
|
||||
Point2D acceleration;
|
||||
if (index == 0)
|
||||
{
|
||||
if (!TryAcceleration(knots[0], knots[1], out acceleration))
|
||||
{
|
||||
reason = "五次 Hermite 起点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (index == knots.Count - 1)
|
||||
{
|
||||
if (!TryAcceleration(knots[index - 1], knots[index], out acceleration))
|
||||
{
|
||||
reason = "五次 Hermite 终点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!TryAcceleration(knots[index - 1], knots[index], out Point2D left) ||
|
||||
!TryAcceleration(knots[index], knots[index + 1], out Point2D right))
|
||||
{
|
||||
reason = "五次 Hermite 共享结点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
acceleration = new Point2D((left.X + right.X) / 2d, (left.Y + right.Y) / 2d);
|
||||
if (!acceleration.IsFinite)
|
||||
{
|
||||
reason = "五次 Hermite 共享结点加速度混合产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
knots[index] = knots[index].WithAcceleration(acceleration);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAcceleration(Knot start, Knot end, out Point2D acceleration)
|
||||
{
|
||||
acceleration = default;
|
||||
double intervalLength = end.ArcLength - start.ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(intervalLength)) return false;
|
||||
acceleration = new Point2D(
|
||||
(end.Velocity.X - start.Velocity.X) / intervalLength,
|
||||
(end.Velocity.Y - start.Velocity.Y) / intervalLength);
|
||||
return acceleration.IsFinite;
|
||||
}
|
||||
|
||||
private static bool TryCreateIntervals(
|
||||
IReadOnlyList<Knot> knots,
|
||||
out List<QuinticInterval> intervals,
|
||||
out string reason)
|
||||
{
|
||||
intervals = new List<QuinticInterval>(knots.Count - 1);
|
||||
reason = string.Empty;
|
||||
for (int index = 1; index < knots.Count; index++)
|
||||
{
|
||||
if (!QuinticInterval.TryCreate(knots[index - 1], knots[index], out QuinticInterval interval))
|
||||
{
|
||||
reason = "五次 Hermite 系数、端点导数或结点区间无效。";
|
||||
return false;
|
||||
}
|
||||
intervals.Add(interval);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double Distance(Point2D point, SmoothingPoint2D reference)
|
||||
{
|
||||
double deltaX = point.X - reference.X;
|
||||
double deltaY = point.Y - reference.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private readonly struct Knot
|
||||
{
|
||||
internal Knot(double arcLength, Point2D position, Point2D velocity)
|
||||
{
|
||||
ArcLength = arcLength;
|
||||
Position = position;
|
||||
Velocity = velocity;
|
||||
Acceleration = default;
|
||||
}
|
||||
|
||||
internal double ArcLength { get; }
|
||||
|
||||
internal Point2D Position { get; }
|
||||
|
||||
internal Point2D Velocity { get; }
|
||||
|
||||
internal Point2D Acceleration { get; }
|
||||
|
||||
internal Knot WithAcceleration(Point2D acceleration)
|
||||
{
|
||||
return new Knot(ArcLength, Position, Velocity, acceleration);
|
||||
}
|
||||
|
||||
private Knot(double arcLength, Point2D position, Point2D velocity, Point2D acceleration)
|
||||
{
|
||||
ArcLength = arcLength;
|
||||
Position = position;
|
||||
Velocity = velocity;
|
||||
Acceleration = acceleration;
|
||||
}
|
||||
}
|
||||
|
||||
private readonly struct QuinticInterval
|
||||
{
|
||||
private QuinticInterval(Knot start, Knot end, Point2D c0, Point2D c1, Point2D c2, Point2D c3, Point2D c4, Point2D c5)
|
||||
{
|
||||
Start = start;
|
||||
End = end;
|
||||
Length = end.ArcLength - start.ArcLength;
|
||||
_c0 = c0;
|
||||
_c1 = c1;
|
||||
_c2 = c2;
|
||||
_c3 = c3;
|
||||
_c4 = c4;
|
||||
_c5 = c5;
|
||||
}
|
||||
|
||||
private readonly Point2D _c0;
|
||||
private readonly Point2D _c1;
|
||||
private readonly Point2D _c2;
|
||||
private readonly Point2D _c3;
|
||||
private readonly Point2D _c4;
|
||||
private readonly Point2D _c5;
|
||||
|
||||
internal Knot Start { get; }
|
||||
|
||||
internal Knot End { get; }
|
||||
|
||||
internal double Length { get; }
|
||||
|
||||
internal static bool TryCreate(Knot start, Knot end, out QuinticInterval interval)
|
||||
{
|
||||
interval = default;
|
||||
double length = end.ArcLength - start.ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(length) || !start.Position.IsFinite || !end.Position.IsFinite ||
|
||||
!start.Velocity.IsFinite || !end.Velocity.IsFinite ||
|
||||
!start.Acceleration.IsFinite || !end.Acceleration.IsFinite)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D c0 = start.Position;
|
||||
Point2D c1 = Scale(start.Velocity, length);
|
||||
Point2D c2 = Scale(start.Acceleration, length * length / 2d);
|
||||
Point2D difference = Subtract(end.Position, start.Position);
|
||||
Point2D endVelocity = Scale(end.Velocity, length);
|
||||
Point2D startAcceleration = Scale(start.Acceleration, length * length);
|
||||
Point2D endAcceleration = Scale(end.Acceleration, length * length);
|
||||
Point2D c3 = Add(
|
||||
Add(Scale(difference, 10d), Scale(c1, -6d)),
|
||||
Add(Scale(endVelocity, -4d), Add(Scale(startAcceleration, -1.5d), Scale(endAcceleration, 0.5d))));
|
||||
Point2D c4 = Add(
|
||||
Add(Scale(difference, -15d), Scale(c1, 8d)),
|
||||
Add(Scale(endVelocity, 7d), Add(Scale(startAcceleration, 1.5d), Scale(endAcceleration, -1d))));
|
||||
Point2D c5 = Add(
|
||||
Add(Scale(difference, 6d), Add(Scale(c1, -3d), Scale(endVelocity, -3d))),
|
||||
Add(Scale(startAcceleration, -0.5d), Scale(endAcceleration, 0.5d)));
|
||||
if (!c0.IsFinite || !c1.IsFinite || !c2.IsFinite || !c3.IsFinite || !c4.IsFinite || !c5.IsFinite)
|
||||
return false;
|
||||
|
||||
interval = new QuinticInterval(start, end, c0, c1, c2, c3, c4, c5);
|
||||
return interval.TryEvaluate(0d, out _, out _, out _) && interval.TryEvaluate(1d, out _, out _, out _);
|
||||
}
|
||||
|
||||
internal bool TryEvaluate(double parameter, out Point2D position, out Point2D derivative, out Point2D secondDerivative)
|
||||
{
|
||||
position = default;
|
||||
derivative = default;
|
||||
secondDerivative = default;
|
||||
if (!NumericGuard.IsFinite(parameter) || parameter < 0d || parameter > 1d) return false;
|
||||
|
||||
double t2 = parameter * parameter;
|
||||
double t3 = t2 * parameter;
|
||||
double t4 = t3 * parameter;
|
||||
double t5 = t4 * parameter;
|
||||
position = Add(Add(Add(_c0, Scale(_c1, parameter)), Add(Scale(_c2, t2), Scale(_c3, t3))),
|
||||
Add(Scale(_c4, t4), Scale(_c5, t5)));
|
||||
derivative = Add(Add(_c1, Scale(_c2, 2d * parameter)),
|
||||
Add(Scale(_c3, 3d * t2), Add(Scale(_c4, 4d * t3), Scale(_c5, 5d * t4))));
|
||||
secondDerivative = Add(Scale(_c2, 2d),
|
||||
Add(Scale(_c3, 6d * parameter), Add(Scale(_c4, 12d * t2), Scale(_c5, 20d * t3))));
|
||||
return position.IsFinite && derivative.IsFinite && secondDerivative.IsFinite;
|
||||
}
|
||||
}
|
||||
|
||||
private readonly struct Point2D
|
||||
{
|
||||
internal Point2D(double x, double y)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
}
|
||||
|
||||
internal double X { get; }
|
||||
|
||||
internal double Y { get; }
|
||||
|
||||
internal bool IsFinite => NumericGuard.IsFinite(X) && NumericGuard.IsFinite(Y);
|
||||
}
|
||||
|
||||
private static Point2D Add(Point2D left, Point2D right)
|
||||
{
|
||||
return new Point2D(left.X + right.X, left.Y + right.Y);
|
||||
}
|
||||
|
||||
private static Point2D Subtract(Point2D left, Point2D right)
|
||||
{
|
||||
return new Point2D(left.X - right.X, left.Y - right.Y);
|
||||
}
|
||||
|
||||
private static Point2D Scale(Point2D point, double scale)
|
||||
{
|
||||
return new Point2D(point.X * scale, point.Y * scale);
|
||||
}
|
||||
}
|
||||
@@ -1,44 +0,0 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>单次算法运行共享的已预处理路径和独立复核上下文。</summary>
|
||||
internal sealed class SmoothingAlgorithmInput
|
||||
{
|
||||
internal SmoothingAlgorithmInput(
|
||||
PreparedPath originalPath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
double maximumCollisionCheckStepMeters,
|
||||
double minimumClearanceReserveMeters,
|
||||
SmoothingOptionsSnapshot options)
|
||||
{
|
||||
OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath));
|
||||
Map = map ?? throw new ArgumentNullException(nameof(map));
|
||||
Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle));
|
||||
MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters;
|
||||
MinimumClearanceReserveMeters = minimumClearanceReserveMeters;
|
||||
Options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
}
|
||||
|
||||
/// <summary>已校验并按方向分段的原始路径。</summary>
|
||||
internal PreparedPath OriginalPath { get; }
|
||||
|
||||
/// <summary>用于完整车体复核的不可变规划地图。</summary>
|
||||
internal PlanningGridMap Map { get; }
|
||||
|
||||
/// <summary>用于曲率和足迹复核的车辆参数快照。</summary>
|
||||
internal VehicleParameters Vehicle { get; }
|
||||
|
||||
/// <summary>连续车体碰撞检查的最大步长，单位 m。</summary>
|
||||
internal double MaximumCollisionCheckStepMeters { get; }
|
||||
|
||||
/// <summary>候选几何必须从原始保守净空中预留的最小安全余量，单位 m。</summary>
|
||||
internal double MinimumClearanceReserveMeters { get; }
|
||||
|
||||
/// <summary>本次算法运行使用的已验证方法选项快照。</summary>
|
||||
internal SmoothingOptionsSnapshot Options { get; }
|
||||
}
|
||||
@@ -1,443 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>在有限强度计划内运行单一算法，并以共享分析和安全复核决定是否接受候选。</summary>
|
||||
internal sealed class SmoothingAlgorithmRunner
|
||||
{
|
||||
private static readonly double[] RetryStrengthScales = { 1d, 0.75d, 0.50d, 0.25d };
|
||||
private readonly PathGeometryAnalyzer _analyzer;
|
||||
private readonly SmoothedPathValidator _validator;
|
||||
|
||||
internal SmoothingAlgorithmRunner()
|
||||
: this(new PathGeometryAnalyzer(), new SmoothedPathValidator())
|
||||
{
|
||||
}
|
||||
|
||||
internal SmoothingAlgorithmRunner(PathGeometryAnalyzer analyzer, SmoothedPathValidator validator)
|
||||
{
|
||||
_analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
|
||||
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 依次尝试配置的有限强度比例。取消直接向上传播，由门面转换为最终状态；
|
||||
/// 只有算法明确标记为可重试的不可行性才会使用较低强度；终止失败和统一复核失败均不重试。
|
||||
/// </summary>
|
||||
internal AlgorithmRunResult Run(
|
||||
IPathSmoother smoother,
|
||||
SmoothingAlgorithmInput input,
|
||||
PathSmoothingConfiguration configuration,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var attemptedStrengths = new List<double>();
|
||||
var failureReasons = new List<string>();
|
||||
if (smoother == null || input == null || input.Options == null || configuration == null)
|
||||
return AlgorithmRunResult.Failed("平滑算法、输入或配置无效。", attemptedStrengths, failureReasons);
|
||||
|
||||
foreach (double scale in RetryStrengthScales)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
double effectiveStrength = configuration.SmoothingStrength * scale;
|
||||
if (!IsPositiveFinite(effectiveStrength))
|
||||
return AlgorithmRunResult.Failed("平滑强度或重试比例无效。", attemptedStrengths, failureReasons);
|
||||
|
||||
attemptedStrengths.Add(effectiveStrength);
|
||||
SmoothingCandidate candidate = smoother.Smooth(input, effectiveStrength, cancellationToken);
|
||||
if (candidate == null)
|
||||
return AlgorithmRunResult.Failed("平滑算法未返回候选。", attemptedStrengths, failureReasons);
|
||||
if (candidate.Status == SmoothingCandidateStatus.RetryableInfeasible)
|
||||
{
|
||||
failureReasons.Add(candidate.Reason);
|
||||
continue;
|
||||
}
|
||||
if (candidate.Status == SmoothingCandidateStatus.Failed)
|
||||
{
|
||||
failureReasons.Add(candidate.Reason);
|
||||
return AlgorithmRunResult.Failed(candidate.Reason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
if (candidate.Status != SmoothingCandidateStatus.Success)
|
||||
{
|
||||
const string unknownStatusReason = "平滑算法返回未知候选状态。";
|
||||
failureReasons.Add(unknownStatusReason);
|
||||
return AlgorithmRunResult.Failed(unknownStatusReason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
if (!_analyzer.TryAnalyze(candidate.Segments, configuration.OutputSpacingMeters,
|
||||
out PathGeometryAnalysis analysis, out string reason))
|
||||
{
|
||||
failureReasons.Add(reason);
|
||||
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
if (_validator.TryValidate(analysis.Path, analysis.Segments, input.OriginalPath, input.Map, input.Vehicle,
|
||||
input.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearanceMeters, out reason))
|
||||
{
|
||||
return AlgorithmRunResult.Success(safePath, analysis.Segments,
|
||||
CreateMetrics(analysis, minimumClearanceMeters), effectiveStrength,
|
||||
attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
failureReasons.Add(reason);
|
||||
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
return AlgorithmRunResult.Infeasible(null, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
|
||||
{
|
||||
return new PathQualityMetrics(
|
||||
true,
|
||||
analysis.PathLengthMeters,
|
||||
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
analysis.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
analysis.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
analysis.CurvatureVariationEnergy,
|
||||
minimumClearanceMeters,
|
||||
0d,
|
||||
0d,
|
||||
0d,
|
||||
0d);
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reflection-only deterministic coverage seam. It is nested in an internal runner and intentionally
|
||||
/// does not construct or register a production smoothing method.
|
||||
/// </summary>
|
||||
public static class TestHooks
|
||||
{
|
||||
/// <summary>执行一个固定的内部假平滑器场景并返回可反射读取的快照。</summary>
|
||||
public static RunnerTestSnapshot Execute(string scenario)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
|
||||
|
||||
var cancellationSource = new CancellationTokenSource();
|
||||
var smoother = new DeterministicTestSmoother(ParseScenario(scenario), cancellationSource);
|
||||
var runner = new SmoothingAlgorithmRunner();
|
||||
SmoothingAlgorithmInput input = CreateTestInput();
|
||||
var configuration = new PathSmoothingConfiguration();
|
||||
try
|
||||
{
|
||||
AlgorithmRunResult result = runner.Run(smoother, input, configuration, cancellationSource.Token);
|
||||
return RunnerTestSnapshot.FromResult(result, false);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return new RunnerTestSnapshot(
|
||||
"OperationCanceledException",
|
||||
smoother.AttemptedStrengths,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
true);
|
||||
}
|
||||
finally
|
||||
{
|
||||
cancellationSource.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>供 PowerShell 断言使用的不可变执行摘要。</summary>
|
||||
public sealed class RunnerTestSnapshot
|
||||
{
|
||||
internal RunnerTestSnapshot(
|
||||
string status,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
int acceptedPathPointCount,
|
||||
int rejectedComparisonCandidatePointCount,
|
||||
int failureCount,
|
||||
bool cancellationPropagated)
|
||||
{
|
||||
Status = status ?? string.Empty;
|
||||
AttemptedStrengths = CopyReadOnly(attemptedStrengths);
|
||||
AcceptedPathPointCount = acceptedPathPointCount;
|
||||
RejectedComparisonCandidatePointCount = rejectedComparisonCandidatePointCount;
|
||||
FailureCount = failureCount;
|
||||
CancellationPropagated = cancellationPropagated;
|
||||
}
|
||||
|
||||
public string Status { get; }
|
||||
public IReadOnlyList<double> AttemptedStrengths { get; }
|
||||
public int AcceptedPathPointCount { get; }
|
||||
public int RejectedComparisonCandidatePointCount { get; }
|
||||
public int FailureCount { get; }
|
||||
public bool CancellationPropagated { get; }
|
||||
|
||||
internal static RunnerTestSnapshot FromResult(AlgorithmRunResult result, bool cancellationPropagated)
|
||||
{
|
||||
int rejectedPointCount = result.RejectedComparisonCandidate == null
|
||||
? 0
|
||||
: CountPoints(result.RejectedComparisonCandidate.Segments);
|
||||
return new RunnerTestSnapshot(
|
||||
result.Status.ToString(),
|
||||
result.AttemptedStrengths,
|
||||
result.Path.Count,
|
||||
rejectedPointCount,
|
||||
result.FailureReasons.Count,
|
||||
cancellationPropagated);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
|
||||
private enum TestScenario
|
||||
{
|
||||
RetryableInfeasible,
|
||||
AcceptFirst,
|
||||
TerminalFailed,
|
||||
CancelBeforeNextAttempt,
|
||||
}
|
||||
|
||||
private sealed class DeterministicTestSmoother : IPathSmoother
|
||||
{
|
||||
private readonly TestScenario _scenario;
|
||||
private readonly CancellationTokenSource _cancellationSource;
|
||||
|
||||
internal DeterministicTestSmoother(TestScenario scenario, CancellationTokenSource cancellationSource)
|
||||
{
|
||||
_scenario = scenario;
|
||||
_cancellationSource = cancellationSource;
|
||||
AttemptedStrengths = new List<double>();
|
||||
}
|
||||
|
||||
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
||||
|
||||
internal List<double> AttemptedStrengths { get; }
|
||||
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
AttemptedStrengths.Add(effectiveStrength);
|
||||
if (_scenario == TestScenario.TerminalFailed)
|
||||
return SmoothingCandidate.Failed("确定性数值退化。");
|
||||
if (_scenario == TestScenario.AcceptFirst)
|
||||
return CreateAcceptedCandidate();
|
||||
|
||||
if (_scenario == TestScenario.CancelBeforeNextAttempt)
|
||||
_cancellationSource.Cancel();
|
||||
return SmoothingCandidate.RetryableInfeasible("确定性可重试不可行。" );
|
||||
}
|
||||
}
|
||||
|
||||
private static TestScenario ParseScenario(string scenario)
|
||||
{
|
||||
if (string.Equals(scenario, nameof(TestScenario.RetryableInfeasible), StringComparison.Ordinal)) return TestScenario.RetryableInfeasible;
|
||||
if (string.Equals(scenario, nameof(TestScenario.AcceptFirst), StringComparison.Ordinal)) return TestScenario.AcceptFirst;
|
||||
if (string.Equals(scenario, nameof(TestScenario.TerminalFailed), StringComparison.Ordinal)) return TestScenario.TerminalFailed;
|
||||
if (string.Equals(scenario, nameof(TestScenario.CancelBeforeNextAttempt), StringComparison.Ordinal)) return TestScenario.CancelBeforeNextAttempt;
|
||||
throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
|
||||
private static SmoothingAlgorithmInput CreateTestInput()
|
||||
{
|
||||
var mapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 5000f, 0f, 5000f),
|
||||
ResolutionMm = 50f,
|
||||
AllowExplicitEmptyMap = true,
|
||||
};
|
||||
PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(mapRequest);
|
||||
if (!mapResult.Succeeded || mapResult.Map == null)
|
||||
throw new InvalidOperationException("The runner test hook could not create its empty map.");
|
||||
|
||||
var originalSegments = new List<PreparedDirectionSegment>
|
||||
{
|
||||
new PreparedDirectionSegment(
|
||||
0,
|
||||
TravelDirection.Forward,
|
||||
new List<SmoothingPoint2D>
|
||||
{
|
||||
CreatePoint(0.5d, 0.5d, 0d),
|
||||
CreatePoint(1.5d, 0.5d, 1d),
|
||||
},
|
||||
false,
|
||||
false),
|
||||
};
|
||||
var vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.20d,
|
||||
WidthMeters = 0.20d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 100d,
|
||||
MinimumTurningRadiusMeters = 0.01d,
|
||||
};
|
||||
return new SmoothingAlgorithmInput(
|
||||
new PreparedPath(originalSegments),
|
||||
mapResult.Map,
|
||||
vehicle,
|
||||
0.05d,
|
||||
0.02d,
|
||||
new SmoothingOptionsSnapshot(new PathSmoothingConfiguration()));
|
||||
}
|
||||
|
||||
private static SmoothingCandidate CreateAcceptedCandidate()
|
||||
{
|
||||
return SmoothingCandidate.Success(new List<PreparedDirectionSegment>
|
||||
{
|
||||
new PreparedDirectionSegment(
|
||||
0,
|
||||
TravelDirection.Forward,
|
||||
new List<SmoothingPoint2D>
|
||||
{
|
||||
CreatePoint(0.5d, 0.5d, 0d),
|
||||
CreatePoint(1.5d, 0.5d, 1d),
|
||||
},
|
||||
false,
|
||||
false),
|
||||
});
|
||||
}
|
||||
|
||||
private static SmoothingPoint2D CreatePoint(double x, double y, double arcLength)
|
||||
{
|
||||
return new SmoothingPoint2D(
|
||||
x,
|
||||
y,
|
||||
arcLength,
|
||||
0d,
|
||||
0d,
|
||||
1d,
|
||||
false,
|
||||
SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private static int CountPoints(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
int count = 0;
|
||||
for (int index = 0; index < segments.Count; index++) count += segments[index].Points.Count;
|
||||
return count;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>内部运行结果；只有成功路径可被正式门面发布，拒绝候选仅供比较诊断读取。</summary>
|
||||
internal sealed class AlgorithmRunResult
|
||||
{
|
||||
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
||||
new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
||||
private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
|
||||
new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
|
||||
|
||||
private AlgorithmRunResult(
|
||||
PathSmoothingStatus status,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathQualityMetrics metrics,
|
||||
double acceptedStrength,
|
||||
SmoothingCandidate rejectedComparisonCandidate,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
IReadOnlyList<string> failureReasons,
|
||||
string reason)
|
||||
{
|
||||
Status = status;
|
||||
Path = path ?? EmptyPath;
|
||||
Segments = segments ?? EmptySegments;
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
AcceptedStrength = acceptedStrength;
|
||||
RejectedComparisonCandidate = rejectedComparisonCandidate;
|
||||
AttemptedStrengths = CopyReadOnly(attemptedStrengths);
|
||||
FailureReasons = CopyReadOnly(failureReasons);
|
||||
Reason = reason ?? string.Empty;
|
||||
}
|
||||
|
||||
internal PathSmoothingStatus Status { get; }
|
||||
internal IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
internal PathQualityMetrics Metrics { get; }
|
||||
internal double AcceptedStrength { get; }
|
||||
internal SmoothingCandidate RejectedComparisonCandidate { get; }
|
||||
internal IReadOnlyList<double> AttemptedStrengths { get; }
|
||||
internal IReadOnlyList<string> FailureReasons { get; }
|
||||
internal string Reason { get; }
|
||||
|
||||
internal static AlgorithmRunResult Success(
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathQualityMetrics metrics,
|
||||
double acceptedStrength,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
IReadOnlyList<string> failureReasons)
|
||||
{
|
||||
return new AlgorithmRunResult(
|
||||
PathSmoothingStatus.Success,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
metrics,
|
||||
acceptedStrength,
|
||||
null,
|
||||
attemptedStrengths,
|
||||
failureReasons,
|
||||
string.Empty);
|
||||
}
|
||||
|
||||
internal static AlgorithmRunResult Infeasible(
|
||||
SmoothingCandidate rejectedComparisonCandidate,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
IReadOnlyList<string> failureReasons)
|
||||
{
|
||||
string reason = failureReasons == null || failureReasons.Count == 0
|
||||
? "所有有限平滑尝试均未通过复核。"
|
||||
: failureReasons[failureReasons.Count - 1];
|
||||
return new AlgorithmRunResult(
|
||||
PathSmoothingStatus.Infeasible,
|
||||
null,
|
||||
null,
|
||||
null,
|
||||
0d,
|
||||
rejectedComparisonCandidate,
|
||||
attemptedStrengths,
|
||||
failureReasons,
|
||||
reason);
|
||||
}
|
||||
|
||||
internal static AlgorithmRunResult Failed(
|
||||
string reason,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
IReadOnlyList<string> failureReasons)
|
||||
{
|
||||
return new AlgorithmRunResult(
|
||||
PathSmoothingStatus.Failed,
|
||||
null,
|
||||
null,
|
||||
null,
|
||||
0d,
|
||||
null,
|
||||
attemptedStrengths,
|
||||
failureReasons,
|
||||
reason);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,83 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
internal enum SmoothingCandidateStatus
|
||||
{
|
||||
Success,
|
||||
RetryableInfeasible,
|
||||
Failed,
|
||||
}
|
||||
|
||||
/// <summary>平滑方法产生的原始方向段候选，尚未经过统一几何或安全复核。</summary>
|
||||
internal sealed class SmoothingCandidate
|
||||
{
|
||||
private SmoothingCandidate(
|
||||
SmoothingCandidateStatus status,
|
||||
IReadOnlyList<PreparedDirectionSegment> segments,
|
||||
string reason)
|
||||
{
|
||||
Status = status;
|
||||
if (status == SmoothingCandidateStatus.Success)
|
||||
{
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("A successful smoothing candidate requires direction segments.", nameof(segments));
|
||||
for (int index = 0; index < segments.Count; index++)
|
||||
{
|
||||
if (segments[index] == null || segments[index].Points == null || segments[index].Points.Count == 0)
|
||||
throw new ArgumentException("A successful smoothing candidate requires complete direction segments.", nameof(segments));
|
||||
}
|
||||
Segments = CopyReadOnly(segments);
|
||||
Reason = string.Empty;
|
||||
return;
|
||||
}
|
||||
|
||||
if (string.IsNullOrWhiteSpace(reason))
|
||||
throw new ArgumentException("An unsuccessful smoothing candidate requires a reason.", nameof(reason));
|
||||
Segments = CopyReadOnly<PreparedDirectionSegment>(null);
|
||||
Reason = reason;
|
||||
}
|
||||
|
||||
/// <summary>候选是否成功产生有限的原始几何。</summary>
|
||||
internal bool Succeeded => Status == SmoothingCandidateStatus.Success;
|
||||
|
||||
/// <summary>候选的可重试性和终止性状态。</summary>
|
||||
internal SmoothingCandidateStatus Status { get; }
|
||||
|
||||
/// <summary>候选方向段；失败候选始终为空。</summary>
|
||||
internal IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
||||
|
||||
/// <summary>失败或退化时的稳定说明；成功时为空。</summary>
|
||||
internal string Reason { get; }
|
||||
|
||||
/// <summary>创建待统一分析和验证的成功候选。</summary>
|
||||
internal static SmoothingCandidate Success(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.Success, segments, string.Empty);
|
||||
}
|
||||
|
||||
/// <summary>创建可由较低平滑强度重新尝试的不可行候选。</summary>
|
||||
internal static SmoothingCandidate RetryableInfeasible(string reason)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.RetryableInfeasible, null, reason);
|
||||
}
|
||||
|
||||
/// <summary>创建不应重试的数值或构造失败候选。</summary>
|
||||
internal static SmoothingCandidate Failed(string reason)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.Failed, null, reason);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -1,50 +0,0 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>供单次平滑算法运行使用的、已校验的不可变方法选项快照。</summary>
|
||||
internal sealed class SmoothingOptionsSnapshot
|
||||
{
|
||||
internal SmoothingOptionsSnapshot(PathSmoothingConfiguration configuration)
|
||||
{
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
|
||||
CubicBSplineEndpointTangentScale = configuration.CubicBSpline.EndpointTangentScale;
|
||||
BezierCornerHeadingThresholdRadians = configuration.LocalCubicBezier.CornerHeadingThresholdRadians;
|
||||
BezierMaximumWindowLengthMeters = configuration.LocalCubicBezier.MaximumWindowLengthMeters;
|
||||
BezierHandleLengthRatio = configuration.LocalCubicBezier.HandleLengthRatio;
|
||||
QuinticKnotSpacingMeters = configuration.PiecewiseQuintic.KnotSpacingMeters;
|
||||
QuinticMinimumKnotSpacingMeters = configuration.PiecewiseQuintic.MinimumKnotSpacingMeters;
|
||||
|
||||
ValidatePositiveFinite(CubicBSplineEndpointTangentScale, nameof(CubicBSplineEndpointTangentScale));
|
||||
if (!NumericGuard.IsFinite(BezierCornerHeadingThresholdRadians) ||
|
||||
BezierCornerHeadingThresholdRadians <= 0d || BezierCornerHeadingThresholdRadians > Math.PI)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(BezierCornerHeadingThresholdRadians));
|
||||
}
|
||||
ValidatePositiveFinite(BezierMaximumWindowLengthMeters, nameof(BezierMaximumWindowLengthMeters));
|
||||
ValidatePositiveFinite(BezierHandleLengthRatio, nameof(BezierHandleLengthRatio));
|
||||
ValidatePositiveFinite(QuinticKnotSpacingMeters, nameof(QuinticKnotSpacingMeters));
|
||||
ValidatePositiveFinite(QuinticMinimumKnotSpacingMeters, nameof(QuinticMinimumKnotSpacingMeters));
|
||||
if (QuinticKnotSpacingMeters < QuinticMinimumKnotSpacingMeters)
|
||||
throw new ArgumentOutOfRangeException(nameof(QuinticKnotSpacingMeters));
|
||||
}
|
||||
|
||||
internal double CubicBSplineEndpointTangentScale { get; }
|
||||
|
||||
internal double BezierCornerHeadingThresholdRadians { get; }
|
||||
|
||||
internal double BezierMaximumWindowLengthMeters { get; }
|
||||
|
||||
internal double BezierHandleLengthRatio { get; }
|
||||
|
||||
internal double QuinticKnotSpacingMeters { get; }
|
||||
|
||||
internal double QuinticMinimumKnotSpacingMeters { get; }
|
||||
|
||||
private static void ValidatePositiveFinite(double value, string name)
|
||||
{
|
||||
if (!NumericGuard.IsPositiveFinite(value)) throw new ArgumentOutOfRangeException(name);
|
||||
}
|
||||
}
|
||||
-60
@@ -1,60 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>同一粗路径的离线平滑比较请求。</summary>
|
||||
public sealed class PathSmoothingComparisonRequest
|
||||
{
|
||||
private static readonly SmoothingMethod[] DefaultMethods =
|
||||
{
|
||||
SmoothingMethod.CubicBSpline,
|
||||
SmoothingMethod.LocalCubicBezier,
|
||||
SmoothingMethod.PiecewiseQuintic,
|
||||
};
|
||||
|
||||
/// <summary>创建比较请求，并固定原始输入与方法顺序。</summary>
|
||||
public PathSmoothingComparisonRequest(
|
||||
PathSmoothingRequest smoothingRequest,
|
||||
IReadOnlyList<SmoothingMethod> methods = null)
|
||||
{
|
||||
SmoothingRequest = CopyRequest(smoothingRequest);
|
||||
Methods = CopyMethods(methods ?? DefaultMethods);
|
||||
}
|
||||
|
||||
/// <summary>所有方法共享的不可变粗路径、地图、车辆和配置快照。</summary>
|
||||
public PathSmoothingRequest SmoothingRequest { get; }
|
||||
|
||||
/// <summary>按调用方指定稳定顺序运行的方法集合。</summary>
|
||||
public IReadOnlyList<SmoothingMethod> Methods { get; }
|
||||
|
||||
private static PathSmoothingRequest CopyRequest(PathSmoothingRequest source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
source.Configuration);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingMethod> CopyMethods(IReadOnlyList<SmoothingMethod> source)
|
||||
{
|
||||
var copy = new List<SmoothingMethod>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
SmoothingMethod method = source[index];
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
|
||||
throw new ArgumentOutOfRangeException(nameof(source), "比较方法无效。");
|
||||
if (copy.Contains(method))
|
||||
throw new ArgumentException("比较方法不能重复。", nameof(source));
|
||||
copy.Add(method);
|
||||
}
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingMethod>(copy);
|
||||
}
|
||||
}
|
||||
@@ -1,60 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>按公开字典序选择唯一的推荐平滑方法。</summary>
|
||||
public static class SmoothingMethodRanker
|
||||
{
|
||||
/// <summary>从当前场景的可行且确定性候选中选择最佳方法；没有合格候选时返回空。</summary>
|
||||
public static SmoothingMethod? Rank(IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
||||
{
|
||||
PathSmoothingComparisonEntry best = null;
|
||||
if (entries == null) return null;
|
||||
|
||||
for (int index = 0; index < entries.Count; index++)
|
||||
{
|
||||
PathSmoothingComparisonEntry candidate = entries[index];
|
||||
if (candidate == null || !candidate.IsEligibleForRecommendation) continue;
|
||||
if (best == null || Compare(candidate, best) < 0) best = candidate;
|
||||
}
|
||||
return best == null ? (SmoothingMethod?)null : best.Method;
|
||||
}
|
||||
|
||||
private static int Compare(PathSmoothingComparisonEntry left, PathSmoothingComparisonEntry right)
|
||||
{
|
||||
int comparison = CompareAscending(left.Metrics.CurvatureVariationEnergy, right.Metrics.CurvatureVariationEnergy);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(
|
||||
left.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
right.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareDescending(left.Metrics.MinimumBodyClearanceMeters, right.Metrics.MinimumBodyClearanceMeters);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(left.Metrics.LengthChangePercent, right.Metrics.LengthChangePercent);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(left.Timing.MedianElapsedMilliseconds, right.Timing.MedianElapsedMilliseconds);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
return ((int)left.Method.Value).CompareTo((int)right.Method.Value);
|
||||
}
|
||||
|
||||
private static int CompareAscending(double left, double right)
|
||||
{
|
||||
return Normalize(left).CompareTo(Normalize(right));
|
||||
}
|
||||
|
||||
private static int CompareDescending(double left, double right)
|
||||
{
|
||||
return Normalize(right).CompareTo(Normalize(left));
|
||||
}
|
||||
|
||||
private static double Normalize(double value)
|
||||
{
|
||||
return double.IsNaN(value) || double.IsInfinity(value) ? double.PositiveInfinity : value;
|
||||
}
|
||||
}
|
||||
@@ -1,8 +0,0 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>三次 B 样条平滑参数。</summary>
|
||||
public sealed class CubicBSplineOptions
|
||||
{
|
||||
/// <summary>端点切向控制柄相对于相邻弦长的比例。</summary>
|
||||
public double EndpointTangentScale { get; set; } = 1d / 3d;
|
||||
}
|
||||
@@ -1,16 +0,0 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>局部三次 Bézier 平滑参数。</summary>
|
||||
public sealed class LocalCubicBezierOptions
|
||||
{
|
||||
/// <summary>判定为明显转角的最小航向变化，单位 rad。</summary>
|
||||
public double CornerHeadingThresholdRadians { get; set; } = Math.PI / 18d;
|
||||
|
||||
/// <summary>单个局部平滑窗口的最大弧长，单位 m。</summary>
|
||||
public double MaximumWindowLengthMeters { get; set; } = 0.60d;
|
||||
|
||||
/// <summary>控制柄相对于窗口局部弦长的比例。</summary>
|
||||
public double HandleLengthRatio { get; set; } = 1d / 3d;
|
||||
}
|
||||
+11
-35
@@ -1,53 +1,29 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的公共配置；所有距离使用 m。</summary>
|
||||
/// <summary>Local G2 路径平滑管线共享配置；所有距离单位为 m，曲率相关限制使用 1/m。</summary>
|
||||
public sealed class PathSmoothingConfiguration
|
||||
{
|
||||
/// <summary>创建带有安全默认值的平滑配置。</summary>
|
||||
/// <summary>创建采用首版安全采样、碰撞步长、净空和曲率容差默认值的可编辑配置。</summary>
|
||||
public PathSmoothingConfiguration()
|
||||
{
|
||||
OutputSpacingMeters = 0.05d;
|
||||
OutputSpacingMeters = 0.025d;
|
||||
MaximumCollisionCheckStepMeters = 0.025d;
|
||||
MinimumClearanceReserveMeters = 0.02d;
|
||||
SmoothingStrength = 1d;
|
||||
AllowFallbackToCoarsePath = true;
|
||||
RetryStrengthScales = new ReadOnlyCollection<double>(
|
||||
new List<double> { 1d, 0.75d, 0.50d, 0.25d });
|
||||
MinimumClearanceReserveMeters = 0d;
|
||||
CurvatureLimitRadiusToleranceMeters = 0.002d;
|
||||
}
|
||||
|
||||
/// <summary>正式单算法入口使用的方法。</summary>
|
||||
public SmoothingMethod Method { get; set; }
|
||||
|
||||
/// <summary>输出路径的目标弧长采样间距，单位 m。</summary>
|
||||
/// <summary>发布路径沿弧长的目标采样间距，单位 m；必须为有限正数。</summary>
|
||||
public double OutputSpacingMeters { get; set; }
|
||||
|
||||
/// <summary>扫掠碰撞检查的最大步长，单位 m。</summary>
|
||||
/// <summary>连续车体扫掠复核允许的最大中心步长，单位 m；较大值会降低碰撞检查分辨率。</summary>
|
||||
public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
|
||||
/// <summary>平滑候选必须在最小净空之外保留的额外余量，单位 m。</summary>
|
||||
/// <summary>除无碰撞外还要求保留的最小额外净空，单位 m。</summary>
|
||||
public double MinimumClearanceReserveMeters { get; set; }
|
||||
|
||||
/// <summary>算法初始平滑强度。</summary>
|
||||
public double SmoothingStrength { get; set; }
|
||||
/// <summary>曲率上限复核时允许相对名义最小转弯半径的缩减容差，单位 m。</summary>
|
||||
public double CurvatureLimitRadiusToleranceMeters { get; set; }
|
||||
|
||||
/// <summary>所有平滑尝试失败时是否允许发布经过复核的原粗路径。</summary>
|
||||
public bool AllowFallbackToCoarsePath { get; set; }
|
||||
|
||||
/// <summary>有限且严格递减的平滑强度重试比例。</summary>
|
||||
public IReadOnlyList<double> RetryStrengthScales { get; }
|
||||
|
||||
/// <summary>三次 B 样条专用参数。</summary>
|
||||
public CubicBSplineOptions CubicBSpline { get; } = new CubicBSplineOptions();
|
||||
|
||||
/// <summary>局部三次 Bézier 专用参数。</summary>
|
||||
public LocalCubicBezierOptions LocalCubicBezier { get; } = new LocalCubicBezierOptions();
|
||||
|
||||
/// <summary>分段五次多项式专用参数。</summary>
|
||||
public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions();
|
||||
|
||||
/// <summary>局部 G2 五次过渡专用参数。</summary>
|
||||
/// <summary>局部 G2 五次过渡窗口与候选筛选选项；返回同一配置对象的可编辑子配置。</summary>
|
||||
public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions();
|
||||
}
|
||||
|
||||
+10
-54
@@ -2,79 +2,35 @@ using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>一次平滑尝试的不可变诊断信息。</summary>
|
||||
/// <summary>一次 Local G2 平滑尝试的不可变诊断快照，适用于成功、取消和失败结果。</summary>
|
||||
public sealed class PathSmoothingDiagnostics
|
||||
{
|
||||
/// <summary>创建不含路径指标的默认诊断信息。</summary>
|
||||
/// <summary>创建不可行、零耗时且没有终止原因的默认诊断快照。</summary>
|
||||
public PathSmoothingDiagnostics()
|
||||
: this(new PathQualityMetrics(), TimeSpan.Zero, 0, 0d, string.Empty)
|
||||
: this(new PathQualityMetrics(), TimeSpan.Zero, string.Empty)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建完整的平滑诊断快照。</summary>
|
||||
/// <summary>创建平滑诊断快照。</summary>
|
||||
/// <param name="metrics">路径可行性、曲率、净空和相对变化的质量指标。</param>
|
||||
/// <param name="elapsed">从服务入口到终止的累计耗时。</param>
|
||||
/// <param name="terminationReason">可供调用方记录的成功、取消或失败原因；为 <see langword="null"/> 时为空字符串。</param>
|
||||
public PathSmoothingDiagnostics(
|
||||
PathQualityMetrics metrics,
|
||||
TimeSpan elapsed,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string terminationReason = null)
|
||||
{
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
Elapsed = elapsed;
|
||||
RetryCount = retryCount;
|
||||
AcceptedStrength = acceptedStrength;
|
||||
TerminationReason = terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>使用所有测量量创建平滑诊断快照。</summary>
|
||||
public PathSmoothingDiagnostics(
|
||||
bool isFeasible,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters,
|
||||
double lengthChangePercent,
|
||||
double peakCurvatureChangePercent,
|
||||
double curvatureVariationChangePercent,
|
||||
double minimumClearanceChangeMeters,
|
||||
TimeSpan elapsed,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string terminationReason = null)
|
||||
: this(
|
||||
new PathQualityMetrics(
|
||||
isFeasible,
|
||||
pathLengthMeters,
|
||||
maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
rootMeanSquareVehicleCurvaturePerMeter,
|
||||
totalAbsoluteCurvatureVariationPerMeter,
|
||||
curvatureVariationEnergy,
|
||||
minimumBodyClearanceMeters,
|
||||
lengthChangePercent,
|
||||
peakCurvatureChangePercent,
|
||||
curvatureVariationChangePercent,
|
||||
minimumClearanceChangeMeters),
|
||||
elapsed,
|
||||
retryCount,
|
||||
acceptedStrength,
|
||||
terminationReason)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>路径质量指标；始终非空。</summary>
|
||||
/// <summary>本次尝试的质量指标；未提供时为全零且不可行的默认指标。</summary>
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
|
||||
/// <summary>从算法入口到返回诊断的耗时。</summary>
|
||||
/// <summary>本次尝试的累计耗时，类型为 <see cref="TimeSpan"/>。</summary>
|
||||
public TimeSpan Elapsed { get; }
|
||||
|
||||
/// <summary>已执行的安全强度重试次数。</summary>
|
||||
public int RetryCount { get; }
|
||||
|
||||
/// <summary>通过复核的平滑强度；未接受候选时为零。</summary>
|
||||
public double AcceptedStrength { get; }
|
||||
|
||||
/// <summary>面向调用方的稳定终止说明。</summary>
|
||||
/// <summary>终止原因文本；不承载可消费的部分路径。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
|
||||
@@ -5,13 +5,23 @@ using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑所需的原始粗路径、复核上下文和配置。</summary>
|
||||
/// <summary>
|
||||
/// 一次路径平滑所需的不可变输入快照。
|
||||
/// 粗路径、方向段、车辆和配置均在构造时复制；地图保留为调用方已冻结的 <see cref="PlanningGridMap"/> 快照。
|
||||
/// </summary>
|
||||
public sealed class PathSmoothingRequest
|
||||
{
|
||||
private readonly VehicleParameters _vehicle;
|
||||
private readonly PathSmoothingConfiguration _configuration;
|
||||
|
||||
/// <summary>创建路径平滑请求，并复制粗路径和方向分段集合。</summary>
|
||||
/// <summary>
|
||||
/// 创建路径平滑请求，并防御性复制可变的路径、分段、车辆和配置输入。
|
||||
/// </summary>
|
||||
/// <param name="coarsePath">按起点到终点顺序排列的粗路径点集合；位置与弧长单位为 m，航向单位为 rad。</param>
|
||||
/// <param name="segments">与 <paramref name="coarsePath"/> 对应的前进/倒车包含式方向分段集合。</param>
|
||||
/// <param name="map">用于完整车体碰撞与净空复核的已准备不可变规划地图快照。</param>
|
||||
/// <param name="vehicle">车辆长宽、安全余量和曲率约束；长度单位为 m，曲率单位为 1/m。</param>
|
||||
/// <param name="configuration">本次平滑的采样、净空、曲率和 Local G2 配置；构造后不会与调用方共享可变实例。</param>
|
||||
public PathSmoothingRequest(
|
||||
IReadOnlyList<CoarsePathPoint> coarsePath,
|
||||
IReadOnlyList<PathSegment> segments,
|
||||
@@ -26,13 +36,13 @@ public sealed class PathSmoothingRequest
|
||||
_configuration = CopyConfiguration(configuration);
|
||||
}
|
||||
|
||||
/// <summary>原始粗路径的不可变快照。</summary>
|
||||
/// <summary>原始粗路径的只读快照，按起点到终点顺序排列；位置与弧长单位为 m，航向单位为 rad。</summary>
|
||||
public IReadOnlyList<CoarsePathPoint> CoarsePath { get; }
|
||||
|
||||
/// <summary>原始粗路径方向分段的不可变快照。</summary>
|
||||
/// <summary>原始粗路径方向分段的只读快照；每段标识连续前进或倒车区间。</summary>
|
||||
public IReadOnlyList<PathSegment> Segments { get; }
|
||||
|
||||
/// <summary>用于平滑后完整车体复核的规划栅格地图。</summary>
|
||||
/// <summary>用于平滑后完整车体碰撞和净空复核的已冻结规划栅格地图快照。</summary>
|
||||
public PlanningGridMap Map { get; }
|
||||
|
||||
/// <summary>车辆几何与最大曲率约束的不可变快照副本。</summary>
|
||||
@@ -70,19 +80,11 @@ public sealed class PathSmoothingRequest
|
||||
if (source == null) return null;
|
||||
var copy = new PathSmoothingConfiguration
|
||||
{
|
||||
Method = source.Method,
|
||||
OutputSpacingMeters = source.OutputSpacingMeters,
|
||||
MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
|
||||
MinimumClearanceReserveMeters = source.MinimumClearanceReserveMeters,
|
||||
SmoothingStrength = source.SmoothingStrength,
|
||||
AllowFallbackToCoarsePath = source.AllowFallbackToCoarsePath,
|
||||
CurvatureLimitRadiusToleranceMeters = source.CurvatureLimitRadiusToleranceMeters,
|
||||
};
|
||||
copy.CubicBSpline.EndpointTangentScale = source.CubicBSpline.EndpointTangentScale;
|
||||
copy.LocalCubicBezier.CornerHeadingThresholdRadians = source.LocalCubicBezier.CornerHeadingThresholdRadians;
|
||||
copy.LocalCubicBezier.MaximumWindowLengthMeters = source.LocalCubicBezier.MaximumWindowLengthMeters;
|
||||
copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio;
|
||||
copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters;
|
||||
copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters;
|
||||
copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters;
|
||||
copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters;
|
||||
copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters;
|
||||
|
||||
@@ -4,7 +4,10 @@ using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的最终不可变结果。</summary>
|
||||
/// <summary>
|
||||
/// Local G2 路径平滑管线产生的不可变结果。
|
||||
/// 只有可发布状态携带完整、已验证的路径和方向段；失败、取消和无效输入结果始终提供空集合，不能被当作部分路径消费。
|
||||
/// </summary>
|
||||
public sealed class PathSmoothingResult
|
||||
{
|
||||
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
||||
@@ -30,64 +33,34 @@ public sealed class PathSmoothingResult
|
||||
Diagnostics = diagnostics ?? new PathSmoothingDiagnostics(
|
||||
new PathQualityMetrics(),
|
||||
TimeSpan.Zero,
|
||||
0,
|
||||
0d,
|
||||
"No smoothing diagnostics were supplied.");
|
||||
}
|
||||
|
||||
/// <summary>最终发布状态。</summary>
|
||||
/// <summary>本次平滑的终止状态；决定 <see cref="Path"/> 和 <see cref="Segments"/> 是否可消费。</summary>
|
||||
public PathSmoothingStatus Status { get; }
|
||||
|
||||
/// <summary>成功或回退时的实际（或尝试）平滑方法；失败时为空。</summary>
|
||||
/// <summary>成功发布路径时实际采用的平滑方法；失败结果为 <see langword="null"/>。</summary>
|
||||
public SmoothingMethod? Method { get; }
|
||||
|
||||
/// <summary>成功或经过复核的回退路径；其他状态始终为空且不可变。</summary>
|
||||
/// <summary>成功时按起点到终点排列的不可变平滑路径；位置和弧长单位为 m，航向单位为 rad；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>覆盖 <see cref="Path"/> 的方向分段；其他状态始终为空且不可变。</summary>
|
||||
/// <summary>成功时覆盖 <see cref="Path"/> 的不可变方向段集合；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>局部 G2 各检测区域的不可变报告；传统算法结果为空。</summary>
|
||||
/// <summary>各 Local G2 区域的不可变处理报告；失败结果不包含区域发布记录。</summary>
|
||||
public IReadOnlyList<PathSmoothingRegionReport> RegionReports { get; }
|
||||
|
||||
/// <summary>本次平滑的质量和终止诊断；始终非空。</summary>
|
||||
/// <summary>质量指标、耗时和终止原因；始终存在，供调用方诊断成功或失败。</summary>
|
||||
public PathSmoothingDiagnostics Diagnostics { get; }
|
||||
|
||||
/// <summary>创建已通过所有复核的平滑结果。</summary>
|
||||
public static PathSmoothingResult Success(
|
||||
SmoothingMethod method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
ValidatePublishedResult(method, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
PathSmoothingStatus.Success,
|
||||
method,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
EmptyRegionReports);
|
||||
}
|
||||
|
||||
/// <summary>创建经过完整复核的原始粗路径回退结果。</summary>
|
||||
public static PathSmoothingResult Fallback(
|
||||
SmoothingMethod attemptedMethod,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
ValidatePublishedResult(attemptedMethod, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
PathSmoothingStatus.FallbackToCoarsePath,
|
||||
attemptedMethod,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
EmptyRegionReports);
|
||||
}
|
||||
|
||||
/// <summary>发布经过完整复核的局部 G2 预平滑结果。</summary>
|
||||
/// <summary>发布已经过独立几何、曲率、净空和连续碰撞复核的 Local G2 结果。</summary>
|
||||
/// <param name="status">可发布状态，只能是完整、部分改进、不需要平滑或保持原样之一。</param>
|
||||
/// <param name="path">按起点到终点顺序排列的完整平滑路径；位置与弧长单位为 m，航向单位为 rad。</param>
|
||||
/// <param name="segments">覆盖完整路径的前进/倒车方向段集合。</param>
|
||||
/// <param name="diagnostics">包含可行质量指标和终止说明的诊断快照。</param>
|
||||
/// <param name="regionReports">每个局部区域的不可变处理报告集合，不能为 <see langword="null"/>。</param>
|
||||
/// <returns>携带防御性复制路径、分段和区域报告的不可变可消费结果。</returns>
|
||||
public static PathSmoothingResult PublishLocalG2(
|
||||
PathSmoothingStatus status,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
@@ -99,11 +72,13 @@ public sealed class PathSmoothingResult
|
||||
status != PathSmoothingStatus.PartialImprovement &&
|
||||
status != PathSmoothingStatus.NotNeeded &&
|
||||
status != PathSmoothingStatus.Unchanged)
|
||||
{
|
||||
throw new ArgumentException("Use a Local G2 publication status.", nameof(status));
|
||||
}
|
||||
if (regionReports == null)
|
||||
throw new ArgumentNullException(nameof(regionReports));
|
||||
|
||||
ValidatePublishedResult(SmoothingMethod.LocalG2Quintic, path, segments, diagnostics);
|
||||
ValidatePublishedResult(path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
status,
|
||||
SmoothingMethod.LocalG2Quintic,
|
||||
@@ -113,29 +88,29 @@ public sealed class PathSmoothingResult
|
||||
CopyReadOnly(regionReports));
|
||||
}
|
||||
|
||||
/// <summary>创建不发布路径的失败、不可行、取消或输入无效结果。</summary>
|
||||
/// <summary>创建明确不发布路径的失败、取消或无效输入结果。</summary>
|
||||
/// <param name="status">非可发布的终止状态。</param>
|
||||
/// <param name="diagnostics">失败原因、质量指标和耗时；为 <see langword="null"/> 时生成默认诊断。</param>
|
||||
/// <returns>路径、方向段和区域报告均为空的不可变结果，不能作为部分路径使用。</returns>
|
||||
public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
if (status == PathSmoothingStatus.Success ||
|
||||
status == PathSmoothingStatus.FallbackToCoarsePath ||
|
||||
status == PathSmoothingStatus.Complete ||
|
||||
if (status == PathSmoothingStatus.Complete ||
|
||||
status == PathSmoothingStatus.PartialImprovement ||
|
||||
status == PathSmoothingStatus.NotNeeded ||
|
||||
status == PathSmoothingStatus.Unchanged)
|
||||
throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status));
|
||||
{
|
||||
throw new ArgumentException("Use PublishLocalG2 to publish a path.", nameof(status));
|
||||
}
|
||||
if (!Enum.IsDefined(typeof(PathSmoothingStatus), status))
|
||||
throw new ArgumentOutOfRangeException(nameof(status));
|
||||
return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics, EmptyRegionReports);
|
||||
}
|
||||
|
||||
private static void ValidatePublishedResult(
|
||||
SmoothingMethod method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
|
||||
throw new ArgumentOutOfRangeException(nameof(method));
|
||||
if (path == null || path.Count == 0)
|
||||
throw new ArgumentException("Published smoothing results require a non-empty path.", nameof(path));
|
||||
if (segments == null || segments.Count == 0)
|
||||
@@ -147,8 +122,7 @@ public sealed class PathSmoothingResult
|
||||
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]);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,10 +1,8 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的最终发布状态。</summary>
|
||||
/// <summary>路径平滑的最终发布状态；只有 Complete、PartialImprovement、NotNeeded 和 Unchanged 可携带完整路径。</summary>
|
||||
public enum PathSmoothingStatus
|
||||
{
|
||||
Success,
|
||||
FallbackToCoarsePath,
|
||||
InvalidInput,
|
||||
Infeasible,
|
||||
Failed,
|
||||
|
||||
@@ -1,11 +0,0 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>分段五次 Hermite 平滑参数。</summary>
|
||||
public sealed class PiecewiseQuinticOptions
|
||||
{
|
||||
/// <summary>相邻内部结点的目标距离，单位 m。</summary>
|
||||
public double KnotSpacingMeters { get; set; } = 0.50d;
|
||||
|
||||
/// <summary>允许创建内部结点的最小间距，单位 m。</summary>
|
||||
public double MinimumKnotSpacingMeters { get; set; } = 0.10d;
|
||||
}
|
||||
@@ -1,11 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>平滑路径点的来源。</summary>
|
||||
/// <summary>平滑路径点的来源；用于区分保留锚点、常规重采样、换向锚点和 Local G2 过渡几何。</summary>
|
||||
public enum SmoothedPathPointSource
|
||||
{
|
||||
/// <summary>直接保留的原始路径锚点。</summary>
|
||||
Anchor,
|
||||
/// <summary>在同一方向段内按弧长插值得到的常规采样点。</summary>
|
||||
Interpolated,
|
||||
/// <summary>精确保留的换向边界点；不能与相邻方向段按坐标合并。</summary>
|
||||
GearSwitch,
|
||||
CoarsePathFallback,
|
||||
LocalG2Transition,
|
||||
/// <summary>由 Local G2 五次过渡曲线生成的候选或发布点。</summary>
|
||||
LocalG2Transition = 4,
|
||||
}
|
||||
|
||||
@@ -1,10 +1,7 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>支持的粗路径平滑方法。</summary>
|
||||
/// <summary>本模块可以发布的路径平滑方法枚举；当前仅支持 Local G2 五次过渡。</summary>
|
||||
public enum SmoothingMethod
|
||||
{
|
||||
CubicBSpline,
|
||||
LocalCubicBezier,
|
||||
PiecewiseQuintic,
|
||||
LocalG2Quintic,
|
||||
}
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user