543 lines
30 KiB
C#
543 lines
30 KiB
C#
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; }
|
|
}
|
|
}
|