46 lines
2.3 KiB
C#
46 lines
2.3 KiB
C#
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;
|
|
}
|
|
}
|