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 System;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.Mapping;
+
+/// <summary>将建图阶段的 mm 环境栅格适配为规划阶段的不可变 m 查询快照。</summary>
+public static class PlanningMapAdapter
+{
+    /// <summary>
+    /// 创建规划地图快照及其保守距离场。
+    ///
+    /// 参数：environmentMap 为已经完成障碍物栅格化的环境图，坐标与分辨率单位均为 mm。
+    /// 返回：不可变的 <see cref="PlanningGridMap"/>；其世界查询使用 m，初始元数据由工厂随后分配。
+    /// </summary>
+    public static PlanningGridMap Create(EnvironmentGridMap environmentMap)
+    {
+        if (!TryCreate(environmentMap, PlanningOperationBudget.Unlimited(CancellationToken.None), out PlanningGridMap map, out _))
+            throw new InvalidOperationException("Unbounded planning-map adaptation unexpectedly stopped.");
+        return map;
+    }
+
+    /// <summary>使用共享预算创建规划快照；停止时不返回部分地图。</summary>
+    internal static bool TryCreate(EnvironmentGridMap environmentMap, PlanningOperationBudget budget,
+        out PlanningGridMap map, out PlanningOperationStopReason stopReason)
+    {
+        if (environmentMap == null) throw new ArgumentNullException(nameof(environmentMap));
+        if (budget == null) throw new ArgumentNullException(nameof(budget));
+        map = null;
+        stopReason = budget.GetStopReason();
+        if (stopReason != PlanningOperationStopReason.None) return false;
+        byte[] occupied = environmentMap.CopyCells();
+        stopReason = budget.GetStopReason();
+        if (stopReason != PlanningOperationStopReason.None) return false;
+        if (!ObstacleDistanceField.TryCreate(occupied, environmentMap.Rows, environmentMap.Cols,
+            environmentMap.ResolutionMm / 1000d, budget, out ObstacleDistanceField field, out stopReason))
+            return false;
+        map = new PlanningGridMap(environmentMap.Bounds, environmentMap.ResolutionMm, environmentMap.Rows, environmentMap.Cols,
+            occupied, field.CopyDistances(), 0, false, "Map metadata has not been assigned.", string.Empty, string.Empty);
+        stopReason = budget.GetStopReason();
+        if (stopReason == PlanningOperationStopReason.None) return true;
+        map = null;
+        return false;
+    }
+}