chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -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;
+    }
+}