ParkingRobot/ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningService.cs

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;
    }
}