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,45 @@
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次从地图输入到 Hybrid A* 粗路径输出的完整业务请求。
+/// 地图边界、分辨率和障碍物几何位于 <see cref="MapRequest"/> 中并使用 mm；位姿和车辆几何使用 m，航向使用 rad，曲率使用 1/m。
+/// </summary>
+public sealed class CoarsePathPlanningJob
+{
+    /// <summary>创建默认目标方向、起步曲率和关闭调试旁路的业务请求。</summary>
+    public CoarsePathPlanningJob()
+    {
+        StartVehicleCurvature = 0d;
+        GoalDirection = GoalDirectionConstraint.Any;
+        DebugOptions = new PlanningDebugOptions();
+    }
+
+    /// <summary>本次唯一建图输入；边界、分辨率和障碍物几何均遵循 Map 模块的 mm 契约。</summary>
+    public PlanningMapRequest MapRequest { get; set; }
+
+    /// <summary>车辆几何中心的起始位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Start { get; set; }
+
+    /// <summary>车辆几何中心的目标位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Goal { get; set; }
+
+    /// <summary>车辆尺寸、安全余量和曲率限制；尺寸单位 m，曲率单位 1/m。</summary>
+    public VehicleParameters Vehicle { get; set; }
+
+    /// <summary>原语、离散、容差、代价和资源上限配置。</summary>
+    public HybridAStarConfiguration Configuration { get; set; }
+
+    /// <summary>车辆起步曲率，单位 1/m；默认值为 0。</summary>
+    public double StartVehicleCurvature { get; set; }
+
+    /// <summary>起步方向约束；null 表示可从前进或倒车开始。</summary>
+    public TravelDirection? StartDirection { get; set; }
+
+    /// <summary>目标进入方向约束；默认值为 <see cref="GoalDirectionConstraint.Any"/>。</summary>
+    public GoalDirectionConstraint GoalDirection { get; set; }
+
+    /// <summary>可选调试旁路配置；默认关闭且使用空接收器，不参与地图或路径计算。</summary>
+    public PlanningDebugOptions DebugOptions { get; set; }
+}
@@ -0,0 +1,45 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次粗路径业务编排的不可变结果。
+/// 始终同时保留地图构建结果和规划结果；调试旁路消息只用于诊断，不会修改地图或规划内容。
+/// </summary>
+public sealed class CoarsePathPlanningJobResult
+{
+    /// <summary>
+    /// 创建业务编排结果。
+    /// 参数：mapResult 和 planningResult 均不能为空；debugDiagnostics 可为 null，返回时会转换为只读字符串集合。
+    /// </summary>
+    public CoarsePathPlanningJobResult(PlanningMapBuildResult mapResult, PlanningResult planningResult,
+        IReadOnlyList<string> debugDiagnostics = null)
+    {
+        MapResult = mapResult ?? throw new ArgumentNullException(nameof(mapResult));
+        PlanningResult = planningResult ?? throw new ArgumentNullException(nameof(planningResult));
+        DebugDiagnostics = CopyDiagnostics(debugDiagnostics);
+    }
+
+    /// <summary>本次调用的地图创建结果；失败时读取 <see cref="PlanningMapBuildResult.FailureReason"/>。</summary>
+    public PlanningMapBuildResult MapResult { get; }
+
+    /// <summary>本次调用的粗路径结果；地图创建失败时为无路径的失败状态。</summary>
+    public PlanningResult PlanningResult { get; }
+
+    /// <summary>调试旁路的非致命诊断信息；为空时表示未启用、未发生异常或无额外调试消息。</summary>
+    public IReadOnlyList<string> DebugDiagnostics { get; }
+
+    private static IReadOnlyList<string> CopyDiagnostics(IReadOnlyList<string> source)
+    {
+        if (source == null || source.Count == 0)
+            return new ReadOnlyCollection<string>(new List<string>());
+
+        var copy = new List<string>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index] ?? string.Empty);
+        return new ReadOnlyCollection<string>(copy);
+    }
+}
@@ -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;
+    }
+}
@@ -0,0 +1,29 @@
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 接收一次地图构建和粗路径规划完成后的旁路调试数据。
+/// 实现不得修改输入对象；服务会隔离实现抛出的异常，调试失败不会改变规划结果。
+/// </summary>
+public interface IPlanningDebugSink
+{
+    /// <summary>
+    /// 发布本次编排得到的地图和规划结果。
+    /// 参数：mapResult 为地图创建结果；planningResult 为对应的完整或失败规划结果；二者均不可由接收方修改。
+    /// </summary>
+    void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult);
+}
+
+internal sealed class NullPlanningDebugSink : IPlanningDebugSink
+{
+    internal static readonly NullPlanningDebugSink Instance = new NullPlanningDebugSink();
+
+    private NullPlanningDebugSink()
+    {
+    }
+
+    public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
+    {
+    }
+}
@@ -0,0 +1,23 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次粗路径编排的可选调试旁路配置。
+/// 调试开关和接收器不参与地图输入指纹、缓存键、搜索状态或路径结果。
+/// </summary>
+public sealed class PlanningDebugOptions
+{
+    /// <summary>创建默认关闭并使用空接收器的调试配置。</summary>
+    public PlanningDebugOptions()
+    {
+        Enabled = false;
+        Sink = NullPlanningDebugSink.Instance;
+    }
+
+    /// <summary>是否在本次编排结束后向 <see cref="Sink"/> 发布旁路数据；默认值为 false。</summary>
+    public bool Enabled { get; set; }
+
+    /// <summary>
+    /// 接收地图和规划结果的旁路对象；默认为空实现。赋值为 null 时服务仍会使用空实现，且不会影响主流程。
+    /// </summary>
+    public IPlanningDebugSink Sink { get; set; }
+}