265 lines
15 KiB
C#
265 lines
15 KiB
C#
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));
|
||
}
|
||
}
|