109 lines
5.1 KiB
C#
109 lines
5.1 KiB
C#
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;
|
||
}
|
||
}
|