chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,92 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 建图阶段使用的可变环境占据栅格。
|
||||
///
|
||||
/// 单位：边界、世界查询和栅格边长均为 mm。
|
||||
/// 注意：只有 <see cref="MapObstacleRasterizer"/> 可以写入占据状态；规划阶段应改用不可变的 <see cref="PlanningGridMap"/>。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentGridMap
|
||||
{
|
||||
private readonly byte[] _cells;
|
||||
private int _occupiedCount;
|
||||
|
||||
/// <summary>
|
||||
/// 创建空的环境占据栅格。
|
||||
///
|
||||
/// 参数：bounds 为左闭右开的世界边界，单位 mm；resolutionMm 为格边长，单位 mm。
|
||||
/// 返回：无；边界为空或分辨率不合法时抛出异常。
|
||||
/// </summary>
|
||||
public EnvironmentGridMap(MapBoundsMm bounds, float resolutionMm)
|
||||
{
|
||||
if (bounds == null) throw new ArgumentNullException(nameof(bounds));
|
||||
bounds.GetDimensions(resolutionMm, out int rows, out int cols);
|
||||
Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
|
||||
_cells = new byte[checked(rows * cols)];
|
||||
}
|
||||
|
||||
/// <summary>地图世界边界，单位 mm，采用左闭右开规则。</summary>
|
||||
public MapBoundsMm Bounds { get; }
|
||||
/// <summary>单个栅格边长，单位 mm。</summary>
|
||||
public float ResolutionMm { get; }
|
||||
/// <summary>栅格行数，Y 方向从下限向上递增。</summary>
|
||||
public int Rows { get; }
|
||||
/// <summary>栅格列数，X 方向从下限向右递增。</summary>
|
||||
public int Cols { get; }
|
||||
/// <summary>当前已被标记为障碍的格数。</summary>
|
||||
public int OccupiedCount { get { return _occupiedCount; } }
|
||||
|
||||
/// <summary>判断行列索引是否有效。参数 row、col 分别为从零开始的行和列；有效时返回 true。</summary>
|
||||
public bool IsInBounds(int row, int col) { return row >= 0 && row < Rows && col >= 0 && col < Cols; }
|
||||
/// <summary>判断世界坐标是否位于地图内。参数 xMm、yMm 单位为 mm；上边界与右边界返回 false。</summary>
|
||||
public bool IsWorldInBounds(float xMm, float yMm) { return Bounds.Contains(xMm, yMm); }
|
||||
|
||||
/// <summary>
|
||||
/// 将世界坐标转换为栅格索引。
|
||||
///
|
||||
/// 参数：xMm、yMm 为世界坐标，单位 mm；row、col 为输出索引。
|
||||
/// 返回：坐标在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
|
||||
/// </summary>
|
||||
public bool TryWorldToGrid(float xMm, float yMm, out int row, out int col)
|
||||
{
|
||||
row = -1; col = -1;
|
||||
if (!IsWorldInBounds(xMm, yMm)) return false;
|
||||
col = (int)Math.Floor(((double)xMm - Bounds.XMin) / ResolutionMm);
|
||||
row = (int)Math.Floor(((double)yMm - Bounds.YMin) / ResolutionMm);
|
||||
return IsInBounds(row, col);
|
||||
}
|
||||
|
||||
/// <summary>查询栅格是否占据。越界索引按障碍处理，返回 true。</summary>
|
||||
public bool IsOccupied(int row, int col) { return !IsInBounds(row, col) || _cells[row * Cols + col] != 0; }
|
||||
/// <summary>按世界坐标查询占据状态。参数 xMm、yMm 单位为 mm；坐标越界时保守地返回 true。</summary>
|
||||
public bool IsOccupiedWorld(float xMm, float yMm)
|
||||
{
|
||||
return !TryWorldToGrid(xMm, yMm, out int row, out int col) || IsOccupied(row, col);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取一个栅格的世界坐标范围。
|
||||
///
|
||||
/// 参数：row、col 为有效索引；xMin、xMax、yMin、yMax 为输出边界，单位 mm。
|
||||
/// 返回：无；索引越界时抛出 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
public void GetCellBounds(int row, int col, out float xMin, out float xMax, out float yMin, out float yMax)
|
||||
{
|
||||
if (!IsInBounds(row, col)) throw new ArgumentOutOfRangeException();
|
||||
xMin = Bounds.XMin + col * ResolutionMm;
|
||||
yMin = Bounds.YMin + row * ResolutionMm;
|
||||
xMax = Math.Min(Bounds.XMax, xMin + ResolutionMm);
|
||||
yMax = Math.Min(Bounds.YMax, yMin + ResolutionMm);
|
||||
}
|
||||
|
||||
internal void MarkOccupied(int row, int col)
|
||||
{
|
||||
if (!IsInBounds(row, col)) return;
|
||||
int index = row * Cols + col;
|
||||
if (_cells[index] == 0) { _cells[index] = 1; _occupiedCount++; }
|
||||
}
|
||||
internal byte[] CopyCells() { return (byte[])_cells.Clone(); }
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 环境占据图构建结果。
|
||||
/// 返回：成功时提供可供适配的 EnvironmentGridMap；失败时提供失败原因和已处理来源状态。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentMapBuildResult
|
||||
{
|
||||
private EnvironmentMapBuildResult(bool succeeded, EnvironmentGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults, string failureReason, PlanningOperationStopReason stopReason)
|
||||
{
|
||||
Succeeded = succeeded; Map = map; SourceResults = sourceResults ?? Array.Empty<ObstacleProjectionResult>(); FailureReason = failureReason ?? string.Empty; StopReason = stopReason;
|
||||
}
|
||||
/// <summary>构建是否成功。true 时 Map 非空；false 时读取 FailureReason。</summary>
|
||||
public bool Succeeded { get; }
|
||||
/// <summary>成功生成的构建期环境栅格；失败时为 null。</summary>
|
||||
public EnvironmentGridMap Map { get; }
|
||||
/// <summary>已尝试来源的投影结果，用于记录已应用、空或失败状态。</summary>
|
||||
public IReadOnlyList<ObstacleProjectionResult> SourceResults { get; }
|
||||
/// <summary>失败原因。成功时为空字符串。</summary>
|
||||
public string FailureReason { get; }
|
||||
/// <summary>内部预算停止原因；普通构建成功或失败时为 None。</summary>
|
||||
internal PlanningOperationStopReason StopReason { get; }
|
||||
/// <summary>创建成功结果。参数 map 为已完成栅格，sourceResults 为来源投影记录。</summary>
|
||||
public static EnvironmentMapBuildResult Success(EnvironmentGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults) { return new EnvironmentMapBuildResult(true, map, sourceResults, null, PlanningOperationStopReason.None); }
|
||||
/// <summary>创建失败结果。参数 reason 为诊断文本，sourceResults 可包含失败前已处理的来源。</summary>
|
||||
public static EnvironmentMapBuildResult Failure(string reason, IReadOnlyList<ObstacleProjectionResult> sourceResults) { return new EnvironmentMapBuildResult(false, null, sourceResults, reason, PlanningOperationStopReason.None); }
|
||||
/// <summary>创建已取消或超时结果；不发布构建期可写地图。</summary>
|
||||
internal static EnvironmentMapBuildResult Stopped(PlanningOperationStopReason stopReason, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.None) throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
return new EnvironmentMapBuildResult(false, null, sourceResults,
|
||||
stopReason == PlanningOperationStopReason.Cancelled ? "地图构建已取消。" : "地图构建已超时。", stopReason);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 从排序后的纯障碍物快照事务性构建环境占据图。
|
||||
///
|
||||
/// 注意：必需来源返回不可用或无效状态时，构建整体失败；可选来源仅记录其状态并继续构建。
|
||||
/// </summary>
|
||||
public sealed class EnvironmentMapBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// 投影所有障碍物来源并栅格化为环境地图。
|
||||
///
|
||||
/// 参数：request 包含 mm 世界边界、分辨率和来源列表；每个来源 ID 必须唯一且版本非负。
|
||||
/// 返回：成功时包含 <see cref="EnvironmentGridMap"/> 和全部来源状态；必需来源失败时返回失败结果而不产生可用地图。
|
||||
/// </summary>
|
||||
public EnvironmentMapBuildResult Build(MapBuildRequest request)
|
||||
{
|
||||
return Build(request, PlanningOperationBudget.Unlimited(CancellationToken.None));
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算投影来源并栅格化；停止时不发布可写环境地图。</summary>
|
||||
internal EnvironmentMapBuildResult Build(MapBuildRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, null);
|
||||
if (request == null || request.Bounds == null) return EnvironmentMapBuildResult.Failure("Map request and bounds are required.", null);
|
||||
if (request.ObstacleSources == null) return EnvironmentMapBuildResult.Failure("Obstacle source collection is required.", null);
|
||||
var sources = request.ObstacleSources.OrderBy(s => s == null ? string.Empty : s.SourceId, StringComparer.Ordinal).ToArray();
|
||||
var results = new List<ObstacleProjectionResult>();
|
||||
string previousId = null;
|
||||
for (int i = 0; i < sources.Length; i++)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
IMapObstacleSource source = sources[i];
|
||||
if (source == null || string.IsNullOrWhiteSpace(source.SourceId) || source.SourceVersion < 0)
|
||||
return EnvironmentMapBuildResult.Failure("Each source needs a non-empty id and non-negative version.", results);
|
||||
if (string.Equals(previousId, source.SourceId, StringComparison.Ordinal))
|
||||
return EnvironmentMapBuildResult.Failure("Obstacle source ids must be unique.", results);
|
||||
previousId = source.SourceId;
|
||||
ObstacleProjectionResult result;
|
||||
try { result = source.ProjectToWorld() ?? ObstacleProjectionResult.Invalid("Source returned no projection result."); }
|
||||
catch (Exception exception) { result = ObstacleProjectionResult.Invalid(exception.Message); }
|
||||
results.Add(result);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
if (source.IsRequired && (result.Status == ObstacleSourceStatus.Invalid || result.Status == ObstacleSourceStatus.Unavailable))
|
||||
return EnvironmentMapBuildResult.Failure("A required obstacle source failed: " + source.SourceId, results);
|
||||
}
|
||||
var map = new EnvironmentGridMap(request.Bounds, request.ResolutionMm);
|
||||
for (int i = 0; i < results.Count; i++)
|
||||
if (results[i].Status == ObstacleSourceStatus.Applied)
|
||||
for (int j = 0; j < results[i].Obstacles.Count; j++)
|
||||
{
|
||||
if (!MapObstacleRasterizer.TryRasterize(map, results[i].Obstacles[j], budget, out stopReason))
|
||||
return EnvironmentMapBuildResult.Stopped(stopReason, results);
|
||||
}
|
||||
return EnvironmentMapBuildResult.Success(map, results);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 有限的世界地图边界。
|
||||
/// 单位：mm；范围采用左闭右开 [XMin, XMax) × [YMin, YMax)。
|
||||
/// </summary>
|
||||
public sealed class MapBoundsMm : IEquatable<MapBoundsMm>
|
||||
{
|
||||
/// <summary>单张地图允许的最大栅格数，超过该值会拒绝创建地图。</summary>
|
||||
public const int MaximumCellCount = 4000000;
|
||||
|
||||
/// <summary>
|
||||
/// 创建地图世界边界。
|
||||
///
|
||||
/// 参数：
|
||||
/// - xMin、xMax：世界 X 轴下限和上限，单位 mm，且 xMax 必须大于 xMin。
|
||||
/// - yMin、yMax：世界 Y 轴下限和上限，单位 mm，且 yMax 必须大于 yMin。
|
||||
///
|
||||
/// 注意：边界采用左闭右开规则，上限坐标不属于地图。
|
||||
/// </summary>
|
||||
public MapBoundsMm(float xMin, float xMax, float yMin, float yMax)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(xMin) || !NumericGuard.IsFinite(xMax) ||
|
||||
!NumericGuard.IsFinite(yMin) || !NumericGuard.IsFinite(yMax) ||
|
||||
xMax <= xMin || yMax <= yMin)
|
||||
throw new ArgumentOutOfRangeException(nameof(xMax), "Map bounds must be finite and non-degenerate.");
|
||||
XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 轴下限，单位 mm，包含在地图内。</summary>
|
||||
public float XMin { get; }
|
||||
/// <summary>世界 X 轴上限，单位 mm，不包含在地图内。</summary>
|
||||
public float XMax { get; }
|
||||
/// <summary>世界 Y 轴下限，单位 mm，包含在地图内。</summary>
|
||||
public float YMin { get; }
|
||||
/// <summary>世界 Y 轴上限，单位 mm，不包含在地图内。</summary>
|
||||
public float YMax { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 判断世界坐标是否属于地图边界。
|
||||
///
|
||||
/// 参数：xMm、yMm 为世界坐标，单位 mm。
|
||||
/// 返回：坐标位于 [XMin, XMax) × [YMin, YMax) 时为 true，否则为 false。
|
||||
/// </summary>
|
||||
public bool Contains(float xMm, float yMm)
|
||||
{
|
||||
return xMm >= XMin && xMm < XMax && yMm >= YMin && yMm < YMax;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据栅格分辨率计算行列数。
|
||||
///
|
||||
/// 参数：resolutionMm 为每个方格的边长，单位 mm，取值必须在 [20, 200]；rows、cols 为输出行数和列数。
|
||||
/// 返回：无；当分辨率无效或总格数超过 <see cref="MaximumCellCount"/> 时抛出异常。
|
||||
/// </summary>
|
||||
public void GetDimensions(float resolutionMm, out int rows, out int cols)
|
||||
{
|
||||
if (!NumericGuard.IsInRange(resolutionMm, 20f, 200f))
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "ResolutionMm must be within [20, 200].");
|
||||
double columnCount = Math.Ceiling(((double)XMax - XMin) / resolutionMm);
|
||||
double rowCount = Math.Ceiling(((double)YMax - YMin) / resolutionMm);
|
||||
if (columnCount > int.MaxValue || rowCount > int.MaxValue || columnCount <= 0d || rowCount <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "Map dimensions are invalid.");
|
||||
cols = (int)columnCount; rows = (int)rowCount;
|
||||
long cellCount = checked((long)rows * cols);
|
||||
if (cellCount > MaximumCellCount)
|
||||
throw new ArgumentOutOfRangeException(nameof(resolutionMm), "Map cell count exceeds 4,000,000.");
|
||||
}
|
||||
|
||||
/// <summary>比较两个边界的四个 mm 坐标是否完全相同。</summary>
|
||||
public bool Equals(MapBoundsMm other)
|
||||
{
|
||||
return other != null && XMin.Equals(other.XMin) && XMax.Equals(other.XMax) &&
|
||||
YMin.Equals(other.YMin) && YMax.Equals(other.YMax);
|
||||
}
|
||||
/// <summary>比较当前边界与指定对象是否表示相同的世界范围。</summary>
|
||||
public override bool Equals(object obj) { return Equals(obj as MapBoundsMm); }
|
||||
/// <summary>返回由四个边界坐标组成的哈希值，用于缓存键比较。</summary>
|
||||
public override int GetHashCode()
|
||||
{
|
||||
unchecked { int hash = XMin.GetHashCode(); hash = hash * 31 + XMax.GetHashCode(); hash = hash * 31 + YMin.GetHashCode(); return hash * 31 + YMax.GetHashCode(); }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 环境占据图构建器的输入数据。
|
||||
/// 注意：通常由 PlanningMapFactory 从公开请求转换得到，调用者无需直接使用。
|
||||
/// </summary>
|
||||
public sealed class MapBuildRequest
|
||||
{
|
||||
/// <summary>环境图世界边界。单位：mm；不能为空。</summary>
|
||||
public MapBoundsMm Bounds { get; set; }
|
||||
/// <summary>环境栅格边长。单位：mm；必须满足 MapBoundsMm 的分辨率限制。</summary>
|
||||
public float ResolutionMm { get; set; }
|
||||
/// <summary>待投影的障碍物来源列表；每个来源 ID 必须唯一。</summary>
|
||||
public IReadOnlyList<IMapObstacleSource> ObstacleSources { get; set; } = Array.Empty<IMapObstacleSource>();
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>边与世界 X/Y 轴平行的矩形障碍物，坐标单位为 mm。</summary>
|
||||
public sealed class AxisAlignedRectangleObstacle : IMapObstacle
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建轴对齐矩形障碍物。
|
||||
///
|
||||
/// 参数：xMin、xMax、yMin、yMax 分别为矩形世界坐标边界，单位 mm。
|
||||
/// 注意：构造不校验边界顺序，请通过 <see cref="IsValid"/> 判断后再使用。
|
||||
/// </summary>
|
||||
public AxisAlignedRectangleObstacle(float xMin, float xMax, float yMin, float yMax)
|
||||
{
|
||||
XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax;
|
||||
}
|
||||
/// <summary>矩形世界 X 下边界，单位 mm。</summary>
|
||||
public float XMin { get; }
|
||||
/// <summary>矩形世界 X 上边界，单位 mm。</summary>
|
||||
public float XMax { get; }
|
||||
/// <summary>矩形世界 Y 下边界，单位 mm。</summary>
|
||||
public float YMin { get; }
|
||||
/// <summary>矩形世界 Y 上边界，单位 mm。</summary>
|
||||
public float YMax { get; }
|
||||
/// <summary>四个边界均为有限数且 XMax≥XMin、YMax≥YMin 时为 true；否则为 false。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return NumericGuard.IsFinite(XMin) && NumericGuard.IsFinite(XMax) && NumericGuard.IsFinite(YMin) && NumericGuard.IsFinite(YMax) && XMax >= XMin && YMax >= YMin; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>以世界 mm 坐标表示的圆形障碍物。</summary>
|
||||
public sealed class CircleObstacle : IMapObstacle
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建圆形障碍物。
|
||||
///
|
||||
/// 参数：centerX、centerY 为圆心世界坐标，单位 mm；radiusMm 为半径，单位 mm。
|
||||
/// 注意：构造不抛出几何校验异常，请通过 <see cref="IsValid"/> 判断后再使用。
|
||||
/// </summary>
|
||||
public CircleObstacle(float centerX, float centerY, float radiusMm)
|
||||
{
|
||||
CenterX = centerX; CenterY = centerY; RadiusMm = radiusMm;
|
||||
}
|
||||
/// <summary>圆心世界 X 坐标，单位 mm。</summary>
|
||||
public float CenterX { get; }
|
||||
/// <summary>圆心世界 Y 坐标，单位 mm。</summary>
|
||||
public float CenterY { get; }
|
||||
/// <summary>圆的半径，单位 mm。</summary>
|
||||
public float RadiusMm { get; }
|
||||
/// <summary>圆心和半径均为有限数且半径不小于零时为 true；否则为 false。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return NumericGuard.IsFinite(CenterX) && NumericGuard.IsFinite(CenterY) && NumericGuard.IsFinite(RadiusMm) && RadiusMm >= 0f; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 世界坐标中的不可变障碍物几何。
|
||||
///
|
||||
/// 单位：所有几何坐标与尺寸均为 mm。实现类型必须能由 <see cref="MapObstacleRasterizer"/> 栅格化。
|
||||
/// </summary>
|
||||
public interface IMapObstacle
|
||||
{
|
||||
/// <summary>几何数据是否有效。true 表示数值有限且尺寸满足该几何类型的约束；false 表示不得投影到地图。</summary>
|
||||
bool IsValid { get; }
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 将障碍物几何保守投影为栅格占据状态的唯一写入入口。
|
||||
///
|
||||
/// 注意：调用方不能直接改写 <see cref="EnvironmentGridMap"/>；相交或贴边的栅格均按占据处理。
|
||||
/// </summary>
|
||||
public static class MapObstacleRasterizer
|
||||
{
|
||||
/// <summary>
|
||||
/// 将一个有效障碍物栅格化到环境地图。
|
||||
///
|
||||
/// 参数：map 为待写入的环境栅格；obstacle 为世界 mm 坐标的圆形或轴对齐矩形障碍物。
|
||||
/// 返回：无。地图或障碍物为空、障碍物无效、几何类型不受支持时抛出异常。
|
||||
/// 注意：该方法只增加占据格，不会清除已有障碍。
|
||||
/// </summary>
|
||||
public static void Rasterize(EnvironmentGridMap map, IMapObstacle obstacle)
|
||||
{
|
||||
if (!TryRasterize(map, obstacle, PlanningOperationBudget.Unlimited(CancellationToken.None), out _))
|
||||
throw new InvalidOperationException("Unbounded rasterization unexpectedly stopped.");
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算将障碍物写入环境栅格；停止时返回 false 且不发布环境地图。</summary>
|
||||
internal static bool TryRasterize(EnvironmentGridMap map, IMapObstacle obstacle, PlanningOperationBudget budget,
|
||||
out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (obstacle == null || !obstacle.IsValid) throw new ArgumentException("Obstacle must be valid.", nameof(obstacle));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
int workItemCount = 0;
|
||||
var circle = obstacle as CircleObstacle;
|
||||
if (circle != null) return TryRasterizeCircle(map, circle, budget, ref workItemCount, out stopReason);
|
||||
var rectangle = obstacle as AxisAlignedRectangleObstacle;
|
||||
if (rectangle != null) return TryRasterizeRectangle(map, rectangle, budget, ref workItemCount, out stopReason);
|
||||
throw new NotSupportedException("Unsupported map obstacle geometry.");
|
||||
}
|
||||
|
||||
private static bool TryRasterizeCircle(EnvironmentGridMap map, CircleObstacle circle, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
GetCandidateRange(map, circle.CenterX - circle.RadiusMm, circle.CenterX + circle.RadiusMm,
|
||||
circle.CenterY - circle.RadiusMm, circle.CenterY + circle.RadiusMm,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
||||
double radiusSquared = (double)circle.RadiusMm * circle.RadiusMm;
|
||||
for (int row = firstRow; row <= lastRow; row++)
|
||||
for (int col = firstCol; col <= lastCol; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
|
||||
double nearestX = Math.Max(xMin, Math.Min(circle.CenterX, xMax));
|
||||
double nearestY = Math.Max(yMin, Math.Min(circle.CenterY, yMax));
|
||||
double dx = circle.CenterX - nearestX;
|
||||
double dy = circle.CenterY - nearestY;
|
||||
if (dx * dx + dy * dy <= radiusSquared) map.MarkOccupied(row, col);
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryRasterizeRectangle(EnvironmentGridMap map, AxisAlignedRectangleObstacle rectangle, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
GetCandidateRange(map, rectangle.XMin, rectangle.XMax, rectangle.YMin, rectangle.YMax,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
|
||||
for (int row = firstRow; row <= lastRow; row++)
|
||||
for (int col = firstCol; col <= lastCol; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
map.GetCellBounds(row, col, out float xMin, out float xMax, out float yMin, out float yMax);
|
||||
if (rectangle.XMax >= xMin && rectangle.XMin <= xMax && rectangle.YMax >= yMin && rectangle.YMin <= yMax)
|
||||
map.MarkOccupied(row, col);
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void GetCandidateRange(EnvironmentGridMap map, float xMin, float xMax, float yMin, float yMax,
|
||||
out int firstRow, out int lastRow, out int firstCol, out int lastCol)
|
||||
{
|
||||
if (xMax < map.Bounds.XMin || xMin > map.Bounds.XMax || yMax < map.Bounds.YMin || yMin > map.Bounds.YMax)
|
||||
{ firstRow = 1; lastRow = 0; firstCol = 1; lastCol = 0; return; }
|
||||
// Geometry is closed for conservative rasterisation. Include the cell on
|
||||
// the lower side when a boundary lies exactly on a grid line.
|
||||
firstCol = Clamp((int)Math.Floor(((double)xMin - map.Bounds.XMin) / map.ResolutionMm) - 1, 0, map.Cols - 1);
|
||||
lastCol = Clamp((int)Math.Floor(((double)xMax - map.Bounds.XMin) / map.ResolutionMm), 0, map.Cols - 1);
|
||||
firstRow = Clamp((int)Math.Floor(((double)yMin - map.Bounds.YMin) / map.ResolutionMm) - 1, 0, map.Rows - 1);
|
||||
lastRow = Clamp((int)Math.Floor(((double)yMax - map.Bounds.YMin) / map.ResolutionMm), 0, map.Rows - 1);
|
||||
}
|
||||
private static int Clamp(int value, int min, int max) { return value < min ? min : value > max ? max : value; }
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>针对行主序二值栅格计算精确欧氏距离平方的内部算法。</summary>
|
||||
internal static class EuclideanDistanceTransform
|
||||
{
|
||||
/// <summary>
|
||||
/// 计算每个栅格到最近障碍栅格的距离平方。
|
||||
///
|
||||
/// 参数：occupied 为行主序占据数组，非零表示障碍；rows、cols 为数组尺寸。
|
||||
/// 返回：行主序距离平方数组，单位为栅格边长的平方；不含任何 mm 或 m 换算。
|
||||
/// </summary>
|
||||
public static double[] ComputeSquaredDistances(byte[] occupied, int rows, int cols)
|
||||
{
|
||||
if (!TryComputeSquaredDistances(occupied, rows, cols, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out double[] squared, out _))
|
||||
throw new InvalidOperationException("Unbounded Euclidean distance transform unexpectedly stopped.");
|
||||
return squared;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算计算距离平方；停止时不返回部分数组。</summary>
|
||||
internal static bool TryComputeSquaredDistances(byte[] occupied, int rows, int cols, PlanningOperationBudget budget,
|
||||
out double[] squaredDistances, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (occupied == null) throw new ArgumentNullException(nameof(occupied));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
squaredDistances = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
double noObstacleDistanceSquared = (double)rows * rows + (double)cols * cols + 1d;
|
||||
var intermediate = new double[occupied.Length];
|
||||
var result = new double[occupied.Length];
|
||||
var input = new double[Math.Max(rows, cols)];
|
||||
var output = new double[Math.Max(rows, cols)];
|
||||
int workItemCount = 0;
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
int offset = row * cols;
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
input[col] = occupied[offset + col] == 0 ? noObstacleDistanceSquared : 0d;
|
||||
}
|
||||
if (!TryTransform1D(input, cols, output, budget, ref workItemCount, out stopReason)) return false;
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
intermediate[offset + col] = output[col];
|
||||
}
|
||||
}
|
||||
for (int col = 0; col < cols; col++)
|
||||
{
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
input[row] = intermediate[row * cols + col];
|
||||
}
|
||||
if (!TryTransform1D(input, rows, output, budget, ref workItemCount, out stopReason)) return false;
|
||||
for (int row = 0; row < rows; row++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
result[row * cols + col] = output[row];
|
||||
}
|
||||
}
|
||||
squaredDistances = result;
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryTransform1D(double[] f, int length, double[] result, PlanningOperationBudget budget,
|
||||
ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
var locations = new int[length];
|
||||
var boundaries = new double[length + 1];
|
||||
int k = 0;
|
||||
locations[0] = 0;
|
||||
boundaries[0] = double.NegativeInfinity;
|
||||
boundaries[1] = double.PositiveInfinity;
|
||||
for (int q = 1; q < length; q++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
double intersection;
|
||||
do
|
||||
{
|
||||
int p = locations[k];
|
||||
intersection = ((f[q] + (double)q * q) - (f[p] + (double)p * p)) / (2d * (q - p));
|
||||
if (intersection <= boundaries[k])
|
||||
{
|
||||
k--;
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
}
|
||||
} while (k >= 0 && intersection <= boundaries[k]);
|
||||
if (k < 0)
|
||||
{
|
||||
k = 0; locations[0] = q; boundaries[0] = double.NegativeInfinity; boundaries[1] = double.PositiveInfinity;
|
||||
}
|
||||
else
|
||||
{
|
||||
k++; locations[k] = q; boundaries[k] = intersection; boundaries[k + 1] = double.PositiveInfinity;
|
||||
}
|
||||
}
|
||||
k = 0;
|
||||
for (int q = 0; q < length; q++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
while (boundaries[k + 1] < q) k++;
|
||||
double delta = q - locations[k];
|
||||
result[q] = delta * delta + f[locations[k]];
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>以 m 表示的障碍物净距离保守下界。</summary>
|
||||
internal sealed class ObstacleDistanceField
|
||||
{
|
||||
private readonly double[] _conservativeDistances;
|
||||
private ObstacleDistanceField(double[] conservativeDistances) { _conservativeDistances = conservativeDistances; }
|
||||
/// <summary>
|
||||
/// 从占据栅格创建距离场。
|
||||
///
|
||||
/// 参数：occupied 为行主序占据数组；rows、cols 为其尺寸；resolutionMeters 为格边长，单位 m。
|
||||
/// 返回：每个格到最近障碍物的保守净距离下界，单位 m；全空地图中的每项为正无穷。
|
||||
/// </summary>
|
||||
public static ObstacleDistanceField Create(byte[] occupied, int rows, int cols, double resolutionMeters)
|
||||
{
|
||||
if (!TryCreate(occupied, rows, cols, resolutionMeters, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out ObstacleDistanceField field, out _))
|
||||
throw new InvalidOperationException("Unbounded distance-field creation unexpectedly stopped.");
|
||||
return field;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建距离场；停止时不返回部分距离数据。</summary>
|
||||
internal static bool TryCreate(byte[] occupied, int rows, int cols, double resolutionMeters,
|
||||
PlanningOperationBudget budget, out ObstacleDistanceField field, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (occupied == null) throw new ArgumentNullException(nameof(occupied));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
field = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
bool hasObstacle = false;
|
||||
int workItemCount = 0;
|
||||
for (int i = 0; i < occupied.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
if (occupied[i] != 0) { hasObstacle = true; break; }
|
||||
}
|
||||
var distances = new double[occupied.Length];
|
||||
if (!hasObstacle)
|
||||
{
|
||||
for (int i = 0; i < distances.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
distances[i] = double.PositiveInfinity;
|
||||
}
|
||||
field = new ObstacleDistanceField(distances);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
if (!EuclideanDistanceTransform.TryComputeSquaredDistances(occupied, rows, cols, budget, out double[] squared, out stopReason))
|
||||
return false;
|
||||
double conservativeOffset = Math.Sqrt(2d) * resolutionMeters;
|
||||
for (int i = 0; i < distances.Length; i++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
distances[i] = Math.Max(0d, Math.Sqrt(squared[i]) * resolutionMeters - conservativeOffset);
|
||||
}
|
||||
field = new ObstacleDistanceField(distances);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
internal double[] CopyDistances() { return (double[])_conservativeDistances.Clone(); }
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 供粗路径规划使用的不可变地图快照。
|
||||
///
|
||||
/// 单位：世界查询方法使用 m；<see cref="Bounds"/> 和 <see cref="ResolutionMm"/> 保留原始 mm 数据。
|
||||
/// 注意：世界坐标越界一律按占据处理，净距离为零。
|
||||
/// </summary>
|
||||
public sealed class PlanningGridMap
|
||||
{
|
||||
private readonly byte[] _occupied;
|
||||
private readonly double[] _conservativeDistances;
|
||||
|
||||
internal PlanningGridMap(MapBoundsMm bounds, float resolutionMm, int rows, int cols, byte[] occupied, double[] conservativeDistances,
|
||||
long snapshotId, bool planningReady, string planningBlockReason, string inputFingerprint, string occupancyHash)
|
||||
{
|
||||
Bounds = bounds; ResolutionMm = resolutionMm; Rows = rows; Cols = cols;
|
||||
_occupied = occupied; _conservativeDistances = conservativeDistances;
|
||||
SnapshotId = snapshotId; PlanningReady = planningReady; PlanningBlockReason = planningBlockReason ?? string.Empty;
|
||||
InputFingerprint = inputFingerprint ?? string.Empty; OccupancyHash = occupancyHash ?? string.Empty;
|
||||
}
|
||||
/// <summary>源环境图的世界边界，单位 mm，采用左闭右开规则。</summary>
|
||||
public MapBoundsMm Bounds { get; }
|
||||
/// <summary>源环境图的栅格边长，单位 mm。</summary>
|
||||
public float ResolutionMm { get; }
|
||||
/// <summary>规划世界查询对应的栅格边长，单位 m。</summary>
|
||||
public double ResolutionMeters { get { return ResolutionMm / 1000d; } }
|
||||
/// <summary>栅格行数。</summary>
|
||||
public int Rows { get; }
|
||||
/// <summary>栅格列数。</summary>
|
||||
public int Cols { get; }
|
||||
/// <summary>工厂为本次返回快照分配的单调编号，用于区分不同构建结果。</summary>
|
||||
public long SnapshotId { get; }
|
||||
/// <summary>地图是否允许进入粗路径规划。true 时可直接查询；false 时应先处理 <see cref="PlanningBlockReason"/>。</summary>
|
||||
public bool PlanningReady { get; }
|
||||
/// <summary>禁止规划的原因。<see cref="PlanningReady"/> 为 true 时为空字符串。</summary>
|
||||
public string PlanningBlockReason { get; }
|
||||
/// <summary>完整建图输入的稳定指纹，用于识别精确输入缓存命中。</summary>
|
||||
public string InputFingerprint { get; }
|
||||
/// <summary>占据栅格内容哈希，用于识别可复用的占据与距离数组。</summary>
|
||||
public string OccupancyHash { get; }
|
||||
|
||||
/// <summary>查询世界位置是否占据。参数 xMeters、yMeters 单位为 m；位置越界时保守地返回 true。</summary>
|
||||
public bool IsOccupiedWorld(double xMeters, double yMeters)
|
||||
{
|
||||
return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) || _occupied[row * Cols + col] != 0;
|
||||
}
|
||||
/// <summary>
|
||||
/// 查询到最近障碍物的保守净距离下界。
|
||||
///
|
||||
/// 参数：xMeters、yMeters 为世界坐标，单位 m。
|
||||
/// 返回：单位 m 的非负距离下界；地图内无障碍物时为正无穷，越界时为零。
|
||||
/// </summary>
|
||||
public double GetConservativeObstacleDistanceMeters(double xMeters, double yMeters)
|
||||
{
|
||||
return !TryWorldToGrid(xMeters, yMeters, out int row, out int col) ? 0d : _conservativeDistances[row * Cols + col];
|
||||
}
|
||||
/// <summary>
|
||||
/// 将规划世界坐标转换为栅格索引。
|
||||
///
|
||||
/// 参数：xMeters、yMeters 为世界坐标，单位 m；row、col 为输出索引。
|
||||
/// 返回：位置在地图内时为 true 并写入索引；否则返回 false，两个输出均为 -1。
|
||||
/// </summary>
|
||||
public bool TryWorldToGrid(double xMeters, double yMeters, out int row, out int col)
|
||||
{
|
||||
row = -1; col = -1;
|
||||
double xMm = xMeters * 1000d, yMm = yMeters * 1000d;
|
||||
if (xMm < Bounds.XMin || xMm >= Bounds.XMax || yMm < Bounds.YMin || yMm >= Bounds.YMax) return false;
|
||||
col = (int)Math.Floor((xMm - Bounds.XMin) / ResolutionMm);
|
||||
row = (int)Math.Floor((yMm - Bounds.YMin) / ResolutionMm);
|
||||
return row >= 0 && row < Rows && col >= 0 && col < Cols;
|
||||
}
|
||||
/// <summary>按行列索引查询占据状态。参数从零开始；任一索引越界时返回 true。</summary>
|
||||
public bool IsOccupied(int row, int col) { return row < 0 || row >= Rows || col < 0 || col >= Cols || _occupied[row * Cols + col] != 0; }
|
||||
internal byte[] CopyOccupied() { return (byte[])_occupied.Clone(); }
|
||||
internal bool OccupancyEquals(PlanningGridMap other)
|
||||
{
|
||||
if (other == null || Rows != other.Rows || Cols != other.Cols || ResolutionMm != other.ResolutionMm || !Bounds.Equals(other.Bounds) || _occupied.Length != other._occupied.Length) return false;
|
||||
for (int i = 0; i < _occupied.Length; i++) if (_occupied[i] != other._occupied[i]) return false;
|
||||
return true;
|
||||
}
|
||||
internal PlanningGridMap WithMetadata(long snapshotId, bool planningReady, string blockReason, string inputFingerprint, string occupancyHash)
|
||||
{
|
||||
return new PlanningGridMap(Bounds, ResolutionMm, Rows, Cols, _occupied, _conservativeDistances, snapshotId, planningReady, blockReason, inputFingerprint, occupancyHash);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>线程安全、容量为四的 LRU 缓存，分别复用精确输入结果和不可变占据数组。</summary>
|
||||
internal sealed class PlanningMapCache
|
||||
{
|
||||
private const int Capacity = 4;
|
||||
private readonly object _gate = new object();
|
||||
private readonly LinkedList<InputEntry> _inputs = new LinkedList<InputEntry>();
|
||||
private readonly LinkedList<OccupancyEntry> _occupancies = new LinkedList<OccupancyEntry>();
|
||||
|
||||
/// <summary>按完整输入描述查询缓存。命中时返回原始快照与来源结果，并提升其最近使用顺序。</summary>
|
||||
public bool TryGetInput(string descriptor, out PlanningGridMap map, out IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
lock (_gate)
|
||||
{
|
||||
for (var node = _inputs.First; node != null; node = node.Next)
|
||||
if (string.Equals(node.Value.Descriptor, descriptor, StringComparison.Ordinal))
|
||||
{ map = node.Value.Map; sourceResults = node.Value.SourceResults; _inputs.Remove(node); _inputs.AddFirst(node); return true; }
|
||||
map = null; sourceResults = null; return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>按占据哈希及逐格比较查询缓存。命中时返回共享数组的规范快照，供工厂创建新的元数据快照。</summary>
|
||||
public bool TryGetOccupancy(string hash, PlanningGridMap candidate, out PlanningGridMap canonical)
|
||||
{
|
||||
lock (_gate)
|
||||
{
|
||||
for (var node = _occupancies.First; node != null; node = node.Next)
|
||||
if (string.Equals(node.Value.Hash, hash, StringComparison.Ordinal) && node.Value.Map.OccupancyEquals(candidate))
|
||||
{ canonical = node.Value.Map; _occupancies.Remove(node); _occupancies.AddFirst(node); return true; }
|
||||
canonical = null; return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>写入或更新精确输入缓存。参数 descriptor 为完整输入键，map 为不可变快照，sourceResults 为对应来源结果。</summary>
|
||||
public void AddInput(string descriptor, PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
lock (_gate)
|
||||
{
|
||||
for (var node = _inputs.First; node != null; node = node.Next)
|
||||
if (string.Equals(node.Value.Descriptor, descriptor, StringComparison.Ordinal)) { node.Value.Map = map; node.Value.SourceResults = sourceResults; _inputs.Remove(node); _inputs.AddFirst(node); return; }
|
||||
_inputs.AddFirst(new InputEntry(descriptor, map, sourceResults));
|
||||
while (_inputs.Count > Capacity) _inputs.RemoveLast();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>写入占据缓存。参数 hash 为占据内容哈希，map 为包含可复用占据与距离数组的快照。</summary>
|
||||
public void AddOccupancy(string hash, PlanningGridMap map)
|
||||
{
|
||||
lock (_gate)
|
||||
{
|
||||
for (var node = _occupancies.First; node != null; node = node.Next)
|
||||
if (string.Equals(node.Value.Hash, hash, StringComparison.Ordinal) && node.Value.Map.OccupancyEquals(map))
|
||||
{ _occupancies.Remove(node); _occupancies.AddFirst(node); return; }
|
||||
_occupancies.AddFirst(new OccupancyEntry(hash, map));
|
||||
while (_occupancies.Count > Capacity) _occupancies.RemoveLast();
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class InputEntry { public InputEntry(string descriptor, PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults) { Descriptor = descriptor; Map = map; SourceResults = sourceResults; } public string Descriptor { get; } public PlanningGridMap Map { get; set; } public IReadOnlyList<ObstacleProjectionResult> SourceResults { get; set; } }
|
||||
private sealed class OccupancyEntry { public OccupancyEntry(string hash, PlanningGridMap map) { Hash = hash; Map = map; } public string Hash { get; } public PlanningGridMap Map { get; } }
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>规划地图创建的终止状态。</summary>
|
||||
public enum PlanningMapBuildStatus
|
||||
{
|
||||
/// <summary>已成功创建可用的不可变地图快照。</summary>
|
||||
Success,
|
||||
/// <summary>输入、来源或地图构建失败。</summary>
|
||||
Failed,
|
||||
/// <summary>调用方在地图创建期间取消了操作。</summary>
|
||||
Cancelled,
|
||||
/// <summary>地图创建消耗了整次规划操作的总超时预算。</summary>
|
||||
TimedOut,
|
||||
}
|
||||
|
||||
/// <summary>本次地图创建使用的缓存层级，反映快照或占据数组的复用方式。</summary>
|
||||
public enum PlanningMapCacheHit
|
||||
{
|
||||
/// <summary>未命中缓存；本次重新创建了占据图和距离场。</summary>
|
||||
None,
|
||||
/// <summary>完整输入命中；返回与上次完全相同的不可变地图对象。</summary>
|
||||
Input,
|
||||
/// <summary>占据内容命中；复用占据和距离数组，但生成新的快照元数据。</summary>
|
||||
Occupancy,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 规划地图创建的最终结果。
|
||||
///
|
||||
/// 返回：
|
||||
/// - Succeeded 为 true 时 Map 可用；false 时通过 FailureReason 获取原因。
|
||||
/// - SourceResults 始终保留已处理来源的投影状态，便于诊断。
|
||||
/// </summary>
|
||||
public sealed class PlanningMapBuildResult
|
||||
{
|
||||
private PlanningMapBuildResult(PlanningMapBuildStatus status, string failureReason, IReadOnlyList<ObstacleProjectionResult> sourceResults, PlanningMapCacheHit cacheHit, PlanningGridMap map)
|
||||
{
|
||||
Status = status; FailureReason = failureReason ?? string.Empty; SourceResults = sourceResults ?? Array.Empty<ObstacleProjectionResult>(); CacheHit = cacheHit; Map = map;
|
||||
}
|
||||
/// <summary>本次建图的显式终止状态；取消和超时不提供地图快照。</summary>
|
||||
public PlanningMapBuildStatus Status { get; }
|
||||
/// <summary>本次建图是否成功。true 表示 Status 为 Success 且 Map 不为空；false 时读取 FailureReason。</summary>
|
||||
public bool Succeeded { get { return Status == PlanningMapBuildStatus.Success; } }
|
||||
/// <summary>建图失败的可读诊断。成功时为空字符串。</summary>
|
||||
public string FailureReason { get; }
|
||||
/// <summary>每个障碍物来源的投影结果，按建图器排序后的来源顺序排列。</summary>
|
||||
public IReadOnlyList<ObstacleProjectionResult> SourceResults { get; }
|
||||
/// <summary>本次调用的缓存复用层级，用于性能诊断，不影响地图正确性。</summary>
|
||||
public PlanningMapCacheHit CacheHit { get; }
|
||||
/// <summary>成功时返回的不可变规划地图；失败时为 null。</summary>
|
||||
public PlanningGridMap Map { get; }
|
||||
internal static PlanningMapBuildResult Success(PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults, PlanningMapCacheHit cacheHit) { return new PlanningMapBuildResult(PlanningMapBuildStatus.Success, null, sourceResults, cacheHit, map); }
|
||||
internal static PlanningMapBuildResult Failure(string reason, IReadOnlyList<ObstacleProjectionResult> sourceResults) { return new PlanningMapBuildResult(PlanningMapBuildStatus.Failed, reason, sourceResults, PlanningMapCacheHit.None, null); }
|
||||
internal static PlanningMapBuildResult Stopped(PlanningOperationStopReason stopReason, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.Cancelled)
|
||||
return new PlanningMapBuildResult(PlanningMapBuildStatus.Cancelled, "地图创建已取消。", sourceResults, PlanningMapCacheHit.None, null);
|
||||
if (stopReason == PlanningOperationStopReason.TimedOut)
|
||||
return new PlanningMapBuildResult(PlanningMapBuildStatus.TimedOut, "地图创建已超时。", sourceResults, PlanningMapCacheHit.None, null);
|
||||
throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,184 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Security.Cryptography;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 规划地图的唯一公开创建入口。
|
||||
///
|
||||
/// 注意：
|
||||
/// - 应在规划服务生命周期内长期复用同一实例，才能命中输入和占据两级缓存。
|
||||
/// - 本类只消费请求中的纯障碍物快照，不读取传感器、定位、UI 或系统时钟。
|
||||
/// </summary>
|
||||
public sealed class PlanningMapFactory
|
||||
{
|
||||
private readonly EnvironmentMapBuilder _builder = new EnvironmentMapBuilder();
|
||||
private readonly PlanningMapCache _cache = new PlanningMapCache();
|
||||
private readonly object _createGate = new object();
|
||||
private long _nextSnapshotId;
|
||||
|
||||
/// <summary>
|
||||
/// 创建规划地图快照。
|
||||
///
|
||||
/// 参数：
|
||||
/// - request：完整建图请求；包含世界范围、栅格分辨率、障碍物来源和空图策略，不能为空。
|
||||
///
|
||||
/// 返回：
|
||||
/// - PlanningMapBuildResult：成功时包含不可变 PlanningGridMap、每个来源的投影结果和缓存命中类型；
|
||||
/// 失败时包含失败原因，不提供地图。
|
||||
///
|
||||
/// 注意：
|
||||
/// - 完全相同的输入返回同一个快照对象；版本变化但占据相同则复用底层栅格并生成新快照编号。
|
||||
/// </summary>
|
||||
public PlanningMapBuildResult Create(PlanningMapRequest request)
|
||||
{
|
||||
return Create(request, PlanningOperationBudget.Unlimited(CancellationToken.None));
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建地图；取消或超时时不发布快照或缓存条目。</summary>
|
||||
internal PlanningMapBuildResult Create(PlanningMapRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, null);
|
||||
|
||||
bool enteredCreateGate = false;
|
||||
try
|
||||
{
|
||||
while (!Monitor.TryEnter(_createGate, 16))
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, null);
|
||||
}
|
||||
enteredCreateGate = true;
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, null);
|
||||
if (request == null || request.Bounds == null) return PlanningMapBuildResult.Failure("Planning map request and bounds are required.", null);
|
||||
if (request.ObstacleSources == null) return PlanningMapBuildResult.Failure("Obstacle source collection is required.", null);
|
||||
string requestKey = BuildRequestKey(request);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, null);
|
||||
if (_cache.TryGetInput(requestKey, out PlanningGridMap exactMap, out IReadOnlyList<ObstacleProjectionResult> cachedResults))
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
return stopReason == PlanningOperationStopReason.None
|
||||
? PlanningMapBuildResult.Success(exactMap, cachedResults, PlanningMapCacheHit.Input)
|
||||
: PlanningMapBuildResult.Stopped(stopReason, cachedResults);
|
||||
}
|
||||
|
||||
EnvironmentMapBuildResult environment = _builder.Build(
|
||||
new MapBuildRequest { Bounds = request.Bounds, ResolutionMm = request.ResolutionMm, ObstacleSources = request.ObstacleSources }, budget);
|
||||
if (!environment.Succeeded)
|
||||
return environment.StopReason == PlanningOperationStopReason.None
|
||||
? PlanningMapBuildResult.Failure(environment.FailureReason, environment.SourceResults)
|
||||
: PlanningMapBuildResult.Stopped(environment.StopReason, environment.SourceResults);
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
string descriptor = BuildInputDescriptor(request, environment.SourceResults);
|
||||
string inputFingerprint = Sha256(descriptor);
|
||||
int applied = environment.SourceResults.Count(x => x.Status == ObstacleSourceStatus.Applied);
|
||||
bool ready = applied > 0 || request.AllowExplicitEmptyMap;
|
||||
string blockReason = ready ? string.Empty : "No source supplied obstacle geometry; set AllowExplicitEmptyMap only when an intentionally empty map is safe.";
|
||||
if (!PlanningMapAdapter.TryCreate(environment.Map, budget, out PlanningGridMap candidate, out stopReason))
|
||||
return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
if (!TryComputeOccupancyHash(candidate, budget, out string occupancyHash, out stopReason))
|
||||
return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
|
||||
PlanningGridMap map;
|
||||
PlanningMapCacheHit cacheHit;
|
||||
if (_cache.TryGetOccupancy(occupancyHash, candidate, out PlanningGridMap canonical))
|
||||
{
|
||||
map = canonical.WithMetadata(NextSnapshotId(), ready, blockReason, inputFingerprint, occupancyHash);
|
||||
cacheHit = PlanningMapCacheHit.Occupancy;
|
||||
}
|
||||
else
|
||||
{
|
||||
map = candidate.WithMetadata(NextSnapshotId(), ready, blockReason, inputFingerprint, occupancyHash);
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
_cache.AddOccupancy(occupancyHash, map);
|
||||
cacheHit = PlanningMapCacheHit.None;
|
||||
}
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return PlanningMapBuildResult.Stopped(stopReason, environment.SourceResults);
|
||||
_cache.AddInput(requestKey, map, environment.SourceResults);
|
||||
return PlanningMapBuildResult.Success(map, environment.SourceResults, cacheHit);
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (enteredCreateGate) Monitor.Exit(_createGate);
|
||||
}
|
||||
}
|
||||
|
||||
private long NextSnapshotId() { return Interlocked.Increment(ref _nextSnapshotId); }
|
||||
|
||||
private static string BuildInputDescriptor(PlanningMapRequest request, IReadOnlyList<ObstacleProjectionResult> results)
|
||||
{
|
||||
var builder = new StringBuilder();
|
||||
AppendFloat(builder, request.Bounds.XMin); AppendFloat(builder, request.Bounds.XMax); AppendFloat(builder, request.Bounds.YMin); AppendFloat(builder, request.Bounds.YMax); AppendFloat(builder, request.ResolutionMm); builder.Append(request.AllowExplicitEmptyMap ? '1' : '0');
|
||||
var sources = request.ObstacleSources.OrderBy(s => s == null ? string.Empty : s.SourceId, StringComparer.Ordinal).ToArray();
|
||||
for (int i = 0; i < sources.Length; i++)
|
||||
{
|
||||
IMapObstacleSource source = sources[i];
|
||||
builder.Append('|').Append(source == null ? "<null>" : source.SourceId).Append('|').Append(source == null ? -1 : source.SourceVersion).Append('|').Append(source != null && source.IsRequired ? '1' : '0');
|
||||
if (i < results.Count) AppendProjection(builder, results[i]);
|
||||
}
|
||||
return builder.ToString();
|
||||
}
|
||||
private static string BuildRequestKey(PlanningMapRequest request)
|
||||
{
|
||||
var builder = new StringBuilder();
|
||||
AppendFloat(builder, request.Bounds.XMin); AppendFloat(builder, request.Bounds.XMax); AppendFloat(builder, request.Bounds.YMin); AppendFloat(builder, request.Bounds.YMax); AppendFloat(builder, request.ResolutionMm); builder.Append(request.AllowExplicitEmptyMap ? '1' : '0');
|
||||
foreach (IMapObstacleSource source in request.ObstacleSources.OrderBy(s => s == null ? string.Empty : s.SourceId, StringComparer.Ordinal))
|
||||
builder.Append('|').Append(source == null ? "<null>" : source.SourceId).Append('|').Append(source == null ? -1 : source.SourceVersion).Append('|').Append(source != null && source.IsRequired ? '1' : '0');
|
||||
return builder.ToString();
|
||||
}
|
||||
private static void AppendProjection(StringBuilder builder, ObstacleProjectionResult result)
|
||||
{
|
||||
builder.Append('|').Append((int)result.Status);
|
||||
for (int index = 0; index < result.Obstacles.Count; index++)
|
||||
{
|
||||
var circle = result.Obstacles[index] as CircleObstacle;
|
||||
if (circle != null) { builder.Append("|C"); AppendFloat(builder, circle.CenterX); AppendFloat(builder, circle.CenterY); AppendFloat(builder, circle.RadiusMm); continue; }
|
||||
var rectangle = result.Obstacles[index] as AxisAlignedRectangleObstacle;
|
||||
if (rectangle != null) { builder.Append("|R"); AppendFloat(builder, rectangle.XMin); AppendFloat(builder, rectangle.XMax); AppendFloat(builder, rectangle.YMin); AppendFloat(builder, rectangle.YMax); }
|
||||
}
|
||||
}
|
||||
private static void AppendFloat(StringBuilder builder, float value) { builder.Append(BitConverter.ToInt32(BitConverter.GetBytes(value), 0).ToString("X8")); }
|
||||
private static bool TryComputeOccupancyHash(PlanningGridMap map, PlanningOperationBudget budget, out string occupancyHash,
|
||||
out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
byte[] occupied = map.CopyOccupied();
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) { occupancyHash = string.Empty; return false; }
|
||||
using (var sha = SHA256.Create())
|
||||
{
|
||||
const int blockLength = 4096;
|
||||
int offset = 0;
|
||||
while (occupied.Length - offset > blockLength)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) { occupancyHash = string.Empty; return false; }
|
||||
sha.TransformBlock(occupied, offset, blockLength, occupied, offset);
|
||||
offset += blockLength;
|
||||
}
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) { occupancyHash = string.Empty; return false; }
|
||||
byte[] hash = sha.TransformFinalBlock(occupied, offset, occupied.Length - offset);
|
||||
occupancyHash = BitConverter.ToString(hash).Replace("-", string.Empty);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
private static string Sha256(string text)
|
||||
{
|
||||
using (var sha = SHA256.Create()) return BitConverter.ToString(sha.ComputeHash(Encoding.UTF8.GetBytes(text))).Replace("-", string.Empty);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 规划地图的完整建图输入。
|
||||
///
|
||||
/// 注意：
|
||||
/// - 此对象只描述地图内容，不包含车辆、起终点、PNG 或终端日志等调试参数。
|
||||
/// - 障碍物来源快照变化时，调用方必须更新对应的 SourceVersion。
|
||||
/// </summary>
|
||||
public sealed class PlanningMapRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 地图世界边界。
|
||||
/// 单位：mm；边界采用左闭右开范围，由 MapBoundsMm 的 XMin、XMax、YMin、YMax 定义。
|
||||
/// </summary>
|
||||
public MapBoundsMm Bounds { get; set; }
|
||||
/// <summary>
|
||||
/// 栅格边长。
|
||||
/// 单位：mm；当前 MapBoundsMm 允许的范围为 20 至 200 mm。
|
||||
/// </summary>
|
||||
public float ResolutionMm { get; set; }
|
||||
/// <summary>
|
||||
/// 参与建图的统一障碍物来源。
|
||||
/// 参数：列表中的每个来源必须具有唯一 ID 和非负版本号；可为空列表。
|
||||
/// </summary>
|
||||
public IReadOnlyList<IMapObstacleSource> ObstacleSources { get; set; } = Array.Empty<IMapObstacleSource>();
|
||||
/// <summary>
|
||||
/// 是否明确允许没有任何障碍物几何的地图参与规划。
|
||||
/// 返回语义：false 时，隐式空图会被标记为不可规划；true 时空图可用于规划。
|
||||
/// </summary>
|
||||
public bool AllowExplicitEmptyMap { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,367 @@
|
||||
# Map 模块说明
|
||||
|
||||
`Map` 为粗路径规划提供只读、可复用的规划地图快照。它只负责把外部障碍物投影并栅格化，生成占据图和保守障碍距离场；不读取传感器、定位、UI 或系统时钟，也不写入 AMR 自身占据和安全外扩。
|
||||
|
||||
规划器应长期持有一个 `PlanningMapFactory`，通过一次 `Create` 调用取得 `PlanningGridMap`。
|
||||
|
||||
## 文件结构
|
||||
|
||||
```text
|
||||
Map/
|
||||
├── README.md # 本模块说明：结构、数据流、单位和调用方式
|
||||
├── PlanningMapRequest.cs # 公开建图输入：边界、分辨率、障碍来源、空图策略
|
||||
├── PlanningMapBuildResult.cs # 公开建图输出：状态、地图、来源结果、失败原因、缓存命中类型
|
||||
├── PlanningMapFactory.cs # 唯一公开建图门面；负责两级缓存和快照编号
|
||||
├── Core/
|
||||
│ ├── MapBoundsMm.cs # 世界地图范围及行列尺寸计算
|
||||
│ ├── MapBuildRequest.cs # EnvironmentMapBuilder 的内部建图输入
|
||||
│ ├── EnvironmentMapBuildResult.cs # 环境栅格构建结果
|
||||
│ ├── EnvironmentGridMap.cs # 构建期可写的环境占据栅格
|
||||
│ └── EnvironmentMapBuilder.cs # 汇总障碍来源并事务性创建环境图
|
||||
├── Obstacles/
|
||||
│ ├── IMapObstacle.cs # 世界坐标障碍物几何契约
|
||||
│ ├── CircleObstacle.cs # 圆形障碍物几何
|
||||
│ ├── AxisAlignedRectangleObstacle.cs # 与坐标轴平行的矩形障碍物几何
|
||||
│ └── MapObstacleRasterizer.cs # 唯一允许写入环境栅格的障碍物栅格化器
|
||||
├── Sources/
|
||||
│ ├── IMapObstacleSource.cs # 统一障碍来源接口
|
||||
│ ├── ObstacleSourceStatus.cs # 来源投影状态：已应用、空、不可用、无效
|
||||
│ ├── ObstacleProjectionResult.cs # 单个来源的世界几何和诊断结果
|
||||
│ ├── ManualObstacleSource.cs # 手工输入的圆形/矩形障碍来源
|
||||
│ ├── TwoLegProjectionInput.cs # 检测时刻的 TwoLeg 纯数据快照
|
||||
│ ├── TwoLegObstacleProjector.cs # 将 TwoLeg 局部坐标投影为世界坐标圆障碍物
|
||||
│ └── TwoLegObstacleSource.cs # 将 TwoLeg 快照包装为统一障碍来源
|
||||
├── Planning/
|
||||
│ ├── PlanningGridMap.cs # 不可变规划快照；规划查询使用米
|
||||
│ ├── PlanningMapAdapter.cs # 环境图到规划快照与距离场的适配器
|
||||
│ ├── EuclideanDistanceTransform.cs # 二值栅格的精确平方欧氏距离变换
|
||||
│ ├── ObstacleDistanceField.cs # 对规划器暴露的保守障碍净距
|
||||
│ └── PlanningMapCache.cs # 容量为 4 的输入/占据两级 LRU 缓存
|
||||
└── Test/
|
||||
├── MovementTest.MapTest.cs # Clumsy 手工建图测试入口与终端调试开关
|
||||
└── Visualization/
|
||||
├── PlanningMapImageExportRequest.cs # PNG 导出输入：规划快照和输出目录
|
||||
├── PlanningMapImageExportResult.cs # PNG 导出状态、路径、尺寸和诊断
|
||||
├── PlanningMapImageExporter.cs # 可选 PNG 导出门面和输出保护
|
||||
├── PlanningMapImageRenderer.cs # 只读快照到 RGBA 像素的渲染器
|
||||
└── ValidatedPngWriter.cs # 写入并校验 PNG 结构和 CRC
|
||||
```
|
||||
|
||||
## 建图数据流
|
||||
|
||||
```text
|
||||
PlanningMapRequest
|
||||
│
|
||||
▼
|
||||
IMapObstacleSource.ProjectToWorld()
|
||||
│ 输出世界坐标的圆形或矩形几何
|
||||
▼
|
||||
EnvironmentMapBuilder + MapObstacleRasterizer
|
||||
│ 写入构建期 EnvironmentGridMap
|
||||
▼
|
||||
PlanningMapAdapter + ObstacleDistanceField
|
||||
│ 生成占据数组和保守距离数组
|
||||
▼
|
||||
PlanningGridMap
|
||||
```
|
||||
|
||||
具体规则：
|
||||
|
||||
1. 调用者准备 `PlanningMapRequest` 和一个或多个 `IMapObstacleSource`。
|
||||
2. 每个来源通过 `ProjectToWorld()` 输出世界坐标几何；Map 核心不主动读取 TwoLeg、定位或其他传感器。
|
||||
3. `EnvironmentMapBuilder` 按来源 ID 排序，必需来源失败则整个建图失败；可选来源失败只保留诊断状态。
|
||||
4. `MapObstacleRasterizer` 是唯一写入 `EnvironmentGridMap` 占据格的组件。
|
||||
5. `PlanningMapAdapter` 生成不可变 `PlanningGridMap`，并附带保守障碍距离场。
|
||||
6. `PlanningMapFactory` 返回最终快照及来源投影结果；粗路径规划只应消费这个快照。
|
||||
|
||||
## 构建状态与停止
|
||||
|
||||
`PlanningMapBuildResult.Status` 的类型为 `PlanningMapBuildStatus`：
|
||||
|
||||
- `Success`：成功发布不可变 `PlanningGridMap`；
|
||||
- `Failed`：输入、来源或常规建图失败，读取 `FailureReason`；
|
||||
- `Cancelled`：调用方取消了带预算的建图，`Map` 为 `null`，不会写入缓存；
|
||||
- `TimedOut`：建图耗尽调用方的总超时预算，`Map` 为 `null`，不会写入缓存。
|
||||
|
||||
公开的 `PlanningMapFactory.Create(request)` 保持兼容且不设置时间限制；粗规划门面使用内部预算入口，使取消和超时能够覆盖地图创建、距离场和后续搜索。
|
||||
|
||||
## 坐标与单位
|
||||
|
||||
- 环境地图边界、障碍物几何、TwoLeg 投影输入均使用世界坐标，单位为 **mm**。
|
||||
- `MapBoundsMm` 范围采用左闭右开：`[XMin, XMax) × [YMin, YMax)`；最大边界不属于地图。
|
||||
- `EnvironmentGridMap` 查询使用 mm；`PlanningGridMap` 的世界查询使用 **m**。
|
||||
- 行 `row` 对应 Y 方向，列 `col` 对应 X 方向，存储顺序为行主序 `row * Cols + col`。
|
||||
- `PlanningGridMap` 的越界位置按占据处理，障碍净距返回 0 m。
|
||||
- Map 不做车辆自身占据或安全外扩；车辆外形与安全裕度由后续碰撞检测负责。
|
||||
|
||||
## 最小调用示例
|
||||
|
||||
```csharp
|
||||
var mapFactory = new PlanningMapFactory(); // 长期持有，不要每次规划重新创建
|
||||
|
||||
IMapObstacle[] obstacles =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
|
||||
new CircleObstacle(3600f, 1200f, 180f),
|
||||
};
|
||||
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("manual", 1L, true, obstacles),
|
||||
},
|
||||
AllowExplicitEmptyMap = false,
|
||||
};
|
||||
|
||||
PlanningMapBuildResult result = mapFactory.Create(request);
|
||||
if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
|
||||
throw new InvalidOperationException(result.FailureReason);
|
||||
|
||||
PlanningGridMap map = result.Map; // 交给粗路径规划器
|
||||
```
|
||||
|
||||
真实 TwoLeg 数据应由上层检测模块在检测时刻构造成 `TwoLegProjectionInput`，再交给 `TwoLegObstacleSource`。不要让 Map 模块主动读取检测器或定位器。
|
||||
|
||||
## 缓存与版本
|
||||
|
||||
`PlanningMapFactory` 内部有容量为 4 的两级 LRU 缓存：
|
||||
|
||||
- **输入命中（Input）**：边界、分辨率、空图策略、来源 ID、`SourceVersion` 和必需性均相同，直接返回同一个 `PlanningGridMap` 对象。
|
||||
- **占据命中（Occupancy）**：来源版本变化，但最终占据栅格相同，复用不可变的占据/距离数组，并颁发新的 `SnapshotId`。
|
||||
- **未命中（None）**：栅格内容变化，重新生成规划快照。
|
||||
|
||||
因此，障碍来源的快照内容发生变化时，调用方必须增加其 `SourceVersion`。未递增版本会错误复用旧地图；仅修改日志、PNG 开关、起终点或车辆参数不应改变地图版本。
|
||||
|
||||
## 测试与调试
|
||||
|
||||
- `Test/MovementTest.MapTest.cs` 是手工 Clumsy 测试入口。文件顶部可设置地图范围、分辨率、TwoLeg 测试快照、`EnableTerminalDebugLog` 和 `SavePng`。
|
||||
- `PlanningMapImageExporter` 仅在 `SavePng` 开启时输出 PNG；它只读取 `PlanningGridMap`，不参与建图、缓存键或规划结果。
|
||||
- 自动检查脚本位于 `ClumsyPilot/tests`：工具、工厂、适配器、PNG 和 MapTest 配置分别有独立验证脚本。
|
||||
- 旧版 `Occupancygird_Map/Map_test/TrapMapImageExporter.cs` 与 `MovementTest.Trapmaptest.cs` 仍保留作历史对照；它们不是新粗路径规划的运行时地图入口。
|
||||
|
||||
## 详细使用指南
|
||||
|
||||
本节说明调用方如何从“障碍物数据”逐步得到可交给粗路径规划器的 `PlanningGridMap`。新地图的唯一创建入口是：
|
||||
|
||||
```csharp
|
||||
PlanningMapBuildResult result = mapFactory.Create(request);
|
||||
```
|
||||
|
||||
其中 `mapFactory` 是长期持有的 `PlanningMapFactory`，`request` 是本次建图输入，`result.Map` 是成功时的只读规划地图。
|
||||
|
||||
### 第 1 步：长期创建地图工厂
|
||||
|
||||
地图工厂内部维护容量为 4 的缓存，因此不要在每次规划前重新创建它。应把它作为规划服务或 MovementTest 的字段长期保存：
|
||||
|
||||
```csharp
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
|
||||
```
|
||||
|
||||
### 第 2 步：准备手工障碍物
|
||||
|
||||
圆形和轴对齐矩形都实现 `IMapObstacle`。障碍物的坐标是**世界坐标**，单位都是 **mm**。
|
||||
|
||||
```csharp
|
||||
IMapObstacle[] manualObstacles =
|
||||
{
|
||||
// 参数依次为：X最小值、X最大值、Y最小值、Y最大值，单位均为 mm。
|
||||
new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
|
||||
|
||||
// 参数依次为：圆心X、圆心Y、半径，单位均为 mm。
|
||||
new CircleObstacle(3600f, 1200f, 180f),
|
||||
};
|
||||
```
|
||||
|
||||
目前 Map 只负责“环境障碍物”。不要在这里添加 AMR 自身外形，也不要在圆半径或矩形尺寸中叠加安全裕度；车辆外形与安全距离由粗路径的碰撞检查处理。
|
||||
|
||||
### 第 3 步：包装为统一障碍来源
|
||||
|
||||
所有障碍物必须通过 `IMapObstacleSource` 进入地图。手工障碍使用 `ManualObstacleSource`：
|
||||
|
||||
```csharp
|
||||
var manualSource = new ManualObstacleSource(
|
||||
sourceId: "manual",
|
||||
sourceVersion: 1L,
|
||||
isRequired: true,
|
||||
obstacles: manualObstacles);
|
||||
```
|
||||
|
||||
参数意义如下：
|
||||
|
||||
- `sourceId`：来源的唯一名称；同一次请求内不能重复。
|
||||
- `sourceVersion`：来源快照版本。障碍物位置、数量、半径或尺寸变化后，必须递增。
|
||||
- `isRequired`：`true` 表示来源无效时整次建图失败；`false` 表示仅记录该来源状态并继续建图。
|
||||
- `obstacles`：当前时刻的不可变障碍物集合。
|
||||
|
||||
例如障碍物内容变化后，应创建带新版本号的来源：
|
||||
|
||||
```csharp
|
||||
var changedManualSource = new ManualObstacleSource(
|
||||
"manual",
|
||||
2L, // 1L 变为 2L，通知工厂地图输入已改变
|
||||
true,
|
||||
changedObstacles);
|
||||
```
|
||||
|
||||
### 第 4 步：可选地加入 TwoLeg 障碍物
|
||||
|
||||
Map 不主动调用 TwoLeg 检测器。上层检测模块应在检测时刻取得 AMR 世界位姿和两腿局部坐标，构造成 `TwoLegProjectionInput`：
|
||||
|
||||
```csharp
|
||||
var twoLegInput = new TwoLegProjectionInput(
|
||||
hasDetection: true,
|
||||
|
||||
// 检测时 AMR 的世界位姿：位置单位 mm，航向单位 rad。
|
||||
detectionWorldX: 1000f,
|
||||
detectionWorldY: 2000f,
|
||||
detectionHeadingRadians: 0d,
|
||||
|
||||
// 两条腿相对于检测时 AMR 位姿的局部坐标，单位 mm。
|
||||
firstLocalX: 300f,
|
||||
firstLocalY: 150f,
|
||||
secondLocalX: 300f,
|
||||
secondLocalY: -150f,
|
||||
|
||||
// 每条腿最终投影为圆形障碍物的半径，单位 mm。
|
||||
radiusMm: 80f,
|
||||
diagnostic: "TwoLeg 检测快照");
|
||||
|
||||
var twoLegSource = new TwoLegObstacleSource(
|
||||
sourceId: "two-leg",
|
||||
sourceVersion: 5L,
|
||||
isRequired: false,
|
||||
input: twoLegInput);
|
||||
```
|
||||
|
||||
没有检测结果时仍可传入空快照：
|
||||
|
||||
```csharp
|
||||
var noTwoLegInput = new TwoLegProjectionInput(
|
||||
false, 0f, 0f, 0d, 0f, 0f, 0f, 0f, 0f,
|
||||
"当前没有 TwoLeg 检测结果");
|
||||
```
|
||||
|
||||
`hasDetection` 为 `false` 时，该来源会返回“空”结果，不会将零坐标当作障碍物。若 TwoLeg 只是可选信息，建议 `isRequired` 设置为 `false`。
|
||||
|
||||
### 第 5 步：创建建图请求
|
||||
|
||||
边界与分辨率仍使用 **mm**。范围采用左闭右开，例如 `XMax = 6000f` 时，`x = 6000f` 不属于地图。
|
||||
|
||||
```csharp
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
// 世界范围：[0, 6000) × [0, 4000)，单位 mm。
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
|
||||
// 每格 50 mm。
|
||||
ResolutionMm = 50f,
|
||||
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
manualSource,
|
||||
twoLegSource,
|
||||
},
|
||||
|
||||
// false：没有任何有效障碍物时，地图不能直接交给规划器。
|
||||
// true：调用方明确确认空地图安全时，才允许空图参与规划。
|
||||
AllowExplicitEmptyMap = false,
|
||||
};
|
||||
```
|
||||
|
||||
### 第 6 步：创建地图并处理失败
|
||||
|
||||
```csharp
|
||||
PlanningMapBuildResult result = _mapFactory.Create(request);
|
||||
|
||||
if (!result.Succeeded)
|
||||
{
|
||||
Console.WriteLine("建图失败：" + result.FailureReason);
|
||||
return;
|
||||
}
|
||||
|
||||
if (result.Map == null || !result.Map.PlanningReady)
|
||||
{
|
||||
Console.WriteLine("地图不能用于规划：" + result.Map?.PlanningBlockReason);
|
||||
return;
|
||||
}
|
||||
|
||||
PlanningGridMap planningMap = result.Map;
|
||||
```
|
||||
|
||||
此处必须同时检查：
|
||||
|
||||
- `Succeeded`：请求、来源和栅格构建过程是否成功；
|
||||
- `Map != null`：是否产出了规划快照；
|
||||
- `PlanningReady`：是否允许把该快照交给规划器。隐式空图会在这一项被拦截。
|
||||
|
||||
需要查看每个来源是否成功投影时，读取 `result.SourceResults`；需要观察缓存效果时，读取 `result.CacheHit`。
|
||||
|
||||
```csharp
|
||||
Console.WriteLine(
|
||||
"快照编号=" + planningMap.SnapshotId
|
||||
+ "，缓存=" + result.CacheHit
|
||||
+ "，栅格=" + planningMap.Cols + "×" + planningMap.Rows);
|
||||
```
|
||||
|
||||
### 第 7 步：交给粗路径规划器查询
|
||||
|
||||
`PlanningGridMap` 是不可变对象，可以安全地作为一次规划任务的输入。注意：它的世界查询坐标单位已经变成 **m**，而不是 mm。
|
||||
|
||||
```csharp
|
||||
// 查询 (2.5 m, 1.0 m) 是否位于占据格；越界也会返回 true。
|
||||
bool occupied = planningMap.IsOccupiedWorld(2.5d, 1.0d);
|
||||
|
||||
// 查询该位置到最近障碍物的保守净距，单位 m；越界返回 0 m。
|
||||
double clearanceMeters =
|
||||
planningMap.GetConservativeObstacleDistanceMeters(2.5d, 1.0d);
|
||||
```
|
||||
|
||||
粗路径规划器应只使用 `PlanningGridMap` 的占据与距离查询，不应直接修改或重建其中的栅格。
|
||||
|
||||
### 完整实例
|
||||
|
||||
```csharp
|
||||
private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
|
||||
|
||||
private PlanningGridMap CreateMapForCoarsePlanning()
|
||||
{
|
||||
IMapObstacle[] obstacles =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
|
||||
new CircleObstacle(3600f, 1200f, 180f),
|
||||
};
|
||||
|
||||
var manualSource = new ManualObstacleSource("manual", 1L, true, obstacles);
|
||||
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
ObstacleSources = new IMapObstacleSource[] { manualSource },
|
||||
AllowExplicitEmptyMap = false,
|
||||
};
|
||||
|
||||
PlanningMapBuildResult result = _mapFactory.Create(request);
|
||||
if (!result.Succeeded)
|
||||
throw new InvalidOperationException("建图失败：" + result.FailureReason);
|
||||
if (result.Map == null || !result.Map.PlanningReady)
|
||||
throw new InvalidOperationException("地图不可规划：" + result.Map?.PlanningBlockReason);
|
||||
|
||||
return result.Map;
|
||||
}
|
||||
```
|
||||
|
||||
### 常见错误
|
||||
|
||||
| 情况 | 原因 | 处理方式 |
|
||||
| --- | --- | --- |
|
||||
| 改了障碍物但地图仍复用旧快照 | 未递增 `SourceVersion` | 障碍内容每次变化后增加该来源版本号 |
|
||||
| 规划查询位置总是越界 | 将 mm 坐标传给了 `PlanningGridMap` | 规划查询前将 mm 除以 1000 转为 m |
|
||||
| 地图创建成功但不能规划 | 未明确允许空图且没有有效障碍物 | 补充有效来源，或确认安全后设置 `AllowExplicitEmptyMap = true` |
|
||||
| TwoLeg 出现在错误位置 | 未使用检测时刻位姿，或混用了 mm 与 m | 用检测时刻的世界位姿和局部 mm 坐标创建快照 |
|
||||
| 缓存总是未命中 | 每次都新建 `PlanningMapFactory`，或版本号无意义变化 | 长期复用工厂；仅在来源内容实际变化时递增版本 |
|
||||
@@ -0,0 +1,22 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 提供一次性障碍物快照的纯数据来源。
|
||||
///
|
||||
/// 注意：<see cref="ProjectToWorld"/> 不得读取传感器、定位、UI 或时钟；调用前应由上层采集并封装完整快照。
|
||||
/// </summary>
|
||||
public interface IMapObstacleSource
|
||||
{
|
||||
/// <summary>来源的稳定唯一标识。不能为空；同一次建图请求中不得重复。</summary>
|
||||
string SourceId { get; }
|
||||
/// <summary>来源快照版本号，必须非负。快照几何或有效性变化时必须递增，以使输入缓存失效。</summary>
|
||||
long SourceVersion { get; }
|
||||
/// <summary>该来源是否必需。true 时不可用或无效会使整张地图构建失败；false 时只记录状态。</summary>
|
||||
bool IsRequired { get; }
|
||||
/// <summary>
|
||||
/// 将已采集的快照投影为世界障碍物几何。
|
||||
///
|
||||
/// 返回：世界坐标、单位 mm 的障碍物及其状态；不得访问任何实时外部状态。
|
||||
/// </summary>
|
||||
ObstacleProjectionResult ProjectToWorld();
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>直接提供世界 mm 几何列表的手工障碍物快照来源。</summary>
|
||||
public sealed class ManualObstacleSource : IMapObstacleSource
|
||||
{
|
||||
private readonly IReadOnlyList<IMapObstacle> _obstacles;
|
||||
/// <summary>
|
||||
/// 创建手工障碍物来源。
|
||||
///
|
||||
/// 参数：sourceId 为唯一来源标识；sourceVersion 为非负快照版本；isRequired 指示失败是否阻断建图；obstacles 为世界 mm 几何列表。
|
||||
/// 注意：几何内容变化时，调用方必须同步提高 sourceVersion。
|
||||
/// </summary>
|
||||
public ManualObstacleSource(string sourceId, long sourceVersion, bool isRequired, IReadOnlyList<IMapObstacle> obstacles)
|
||||
{
|
||||
SourceId = sourceId; SourceVersion = sourceVersion; IsRequired = isRequired; _obstacles = obstacles;
|
||||
}
|
||||
/// <summary>该快照的唯一来源标识。</summary>
|
||||
public string SourceId { get; }
|
||||
/// <summary>该快照的版本号；内容变化时必须递增。</summary>
|
||||
public long SourceVersion { get; }
|
||||
/// <summary>true 表示无效来源会阻断建图；false 表示只记录来源状态。</summary>
|
||||
public bool IsRequired { get; }
|
||||
/// <summary>
|
||||
/// 返回手工障碍物的世界几何。
|
||||
///
|
||||
/// 返回：列表为空时为 Empty；列表为空引用或含无效几何时为 Invalid；其余情况为 Applied。
|
||||
/// </summary>
|
||||
public ObstacleProjectionResult ProjectToWorld()
|
||||
{
|
||||
if (_obstacles == null) return ObstacleProjectionResult.Invalid("Manual obstacle collection is null.");
|
||||
if (_obstacles.Count == 0) return ObstacleProjectionResult.Empty("Manual obstacle collection is empty.");
|
||||
for (int i = 0; i < _obstacles.Count; i++)
|
||||
if (_obstacles[i] == null || !_obstacles[i].IsValid) return ObstacleProjectionResult.Invalid("Manual obstacle geometry is invalid.");
|
||||
return ObstacleProjectionResult.Applied(_obstacles, "Manual obstacle snapshot applied.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>障碍物来源的一次投影结果，包含状态、世界 mm 几何和可选诊断信息。</summary>
|
||||
public sealed class ObstacleProjectionResult
|
||||
{
|
||||
private ObstacleProjectionResult(ObstacleSourceStatus status, IReadOnlyList<IMapObstacle> obstacles, string diagnostic)
|
||||
{
|
||||
Status = status; Obstacles = obstacles ?? Array.Empty<IMapObstacle>(); Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
/// <summary>投影状态。只有 <see cref="ObstacleSourceStatus.Applied"/> 的几何会被栅格化。</summary>
|
||||
public ObstacleSourceStatus Status { get; }
|
||||
/// <summary>世界 mm 坐标的障碍物列表。非 Applied 状态时为空列表，不为 null。</summary>
|
||||
public IReadOnlyList<IMapObstacle> Obstacles { get; }
|
||||
/// <summary>用于日志和诊断的说明文本；未提供时为空字符串。</summary>
|
||||
public string Diagnostic { get; }
|
||||
/// <summary>创建成功投影结果。参数 obstacles 为有效世界 mm 几何；diagnostic 为可选诊断文本。返回的状态为 Applied。</summary>
|
||||
public static ObstacleProjectionResult Applied(IReadOnlyList<IMapObstacle> obstacles, string diagnostic = null)
|
||||
{
|
||||
return new ObstacleProjectionResult(ObstacleSourceStatus.Applied, obstacles, diagnostic);
|
||||
}
|
||||
/// <summary>创建空投影结果。参数 diagnostic 为可选原因；返回状态为 Empty，障碍物列表为空。</summary>
|
||||
public static ObstacleProjectionResult Empty(string diagnostic = null) { return new ObstacleProjectionResult(ObstacleSourceStatus.Empty, null, diagnostic); }
|
||||
/// <summary>创建不可用投影结果。参数 diagnostic 应说明快照缺失原因；返回状态为 Unavailable。</summary>
|
||||
public static ObstacleProjectionResult Unavailable(string diagnostic) { return new ObstacleProjectionResult(ObstacleSourceStatus.Unavailable, null, diagnostic); }
|
||||
/// <summary>创建无效投影结果。参数 diagnostic 应说明数据或几何错误；返回状态为 Invalid。</summary>
|
||||
public static ObstacleProjectionResult Invalid(string diagnostic) { return new ObstacleProjectionResult(ObstacleSourceStatus.Invalid, null, diagnostic); }
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>一个障碍物来源投影后的状态，供建图结果和调试日志判断。</summary>
|
||||
public enum ObstacleSourceStatus
|
||||
{
|
||||
/// <summary>成功得到一个或多个有效的世界 mm 障碍物，并会被栅格化。</summary>
|
||||
Applied,
|
||||
/// <summary>来源有效但本次没有障碍物；不会写入任何栅格。</summary>
|
||||
Empty,
|
||||
/// <summary>来源快照不可取得；若来源必需则整图构建失败。</summary>
|
||||
Unavailable,
|
||||
/// <summary>来源数据或几何无效；若来源必需则整图构建失败。</summary>
|
||||
Invalid,
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>将检测局部坐标中的两条腿转换为世界 mm 坐标圆形障碍物。</summary>
|
||||
public static class TwoLegObstacleProjector
|
||||
{
|
||||
/// <summary>
|
||||
/// 投影一份 TwoLeg 快照。
|
||||
///
|
||||
/// 参数：input 为检测时刻采集的纯数据快照，可为 null。
|
||||
/// 返回：null 为 Unavailable；未检测到目标为 Empty；数值无效为 Invalid；成功时为含两个圆形障碍物的 Applied。
|
||||
/// 注意：本方法只读取 input，不访问传感器、定位、UI 或时钟。
|
||||
/// </summary>
|
||||
public static ObstacleProjectionResult Project(TwoLegProjectionInput input)
|
||||
{
|
||||
if (input == null) return ObstacleProjectionResult.Unavailable("TwoLeg snapshot is unavailable.");
|
||||
if (!input.HasDetection) return ObstacleProjectionResult.Empty(input.Diagnostic);
|
||||
if (!input.IsValid) return ObstacleProjectionResult.Invalid("TwoLeg snapshot contains invalid values.");
|
||||
CoordinateTransform.LocalToWorld(input.DetectionWorldX, input.DetectionWorldY, input.DetectionHeadingRadians,
|
||||
input.FirstLocalX, input.FirstLocalY, out double firstX, out double firstY);
|
||||
CoordinateTransform.LocalToWorld(input.DetectionWorldX, input.DetectionWorldY, input.DetectionHeadingRadians,
|
||||
input.SecondLocalX, input.SecondLocalY, out double secondX, out double secondY);
|
||||
var obstacles = new List<IMapObstacle>
|
||||
{
|
||||
new CircleObstacle((float)firstX, (float)firstY, input.RadiusMm),
|
||||
new CircleObstacle((float)secondX, (float)secondY, input.RadiusMm),
|
||||
};
|
||||
return ObstacleProjectionResult.Applied(obstacles, input.Diagnostic);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>将一个 <see cref="TwoLegProjectionInput"/> 快照包装为统一障碍物来源。</summary>
|
||||
public sealed class TwoLegObstacleSource : IMapObstacleSource
|
||||
{
|
||||
private readonly TwoLegProjectionInput _input;
|
||||
/// <summary>
|
||||
/// 创建 TwoLeg 障碍物来源。
|
||||
///
|
||||
/// 参数：sourceId 为唯一标识；sourceVersion 为快照版本，输入内容改变时必须递增；isRequired 指示失败是否阻断建图；input 为检测时刻快照，可为空。
|
||||
/// </summary>
|
||||
public TwoLegObstacleSource(string sourceId, long sourceVersion, bool isRequired, TwoLegProjectionInput input)
|
||||
{
|
||||
SourceId = sourceId; SourceVersion = sourceVersion; IsRequired = isRequired; _input = input;
|
||||
}
|
||||
/// <summary>该快照来源的唯一标识。</summary>
|
||||
public string SourceId { get; }
|
||||
/// <summary>快照版本号；输入内容变化时必须递增。</summary>
|
||||
public long SourceVersion { get; }
|
||||
/// <summary>true 时投影不可用或无效会阻断建图。</summary>
|
||||
public bool IsRequired { get; }
|
||||
/// <summary>投影保存的 TwoLeg 快照。返回世界 mm 圆形障碍物，且不访问实时检测、定位、UI 或时钟。</summary>
|
||||
public ObstacleProjectionResult ProjectToWorld() { return TwoLegObstacleProjector.Project(_input); }
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// TwoLeg 检测时刻的纯输入快照。
|
||||
///
|
||||
/// 单位：位置和两腿局部坐标均为 mm，航向为弧度。该对象不读取检测器或定位模块，确保投影可重复。
|
||||
/// </summary>
|
||||
public sealed class TwoLegProjectionInput
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建 TwoLeg 投影快照。
|
||||
///
|
||||
/// 参数：hasDetection 表示当前是否检测到目标；detectionWorldX/Y 为检测坐标系原点的世界 mm 坐标；detectionHeadingRadians 为其相对世界坐标系的航向；firstLocalX/Y、secondLocalX/Y 为两腿在检测局部坐标系中的 mm 坐标；radiusMm 为每条腿投影圆半径；diagnostic 为可选诊断文本。
|
||||
/// 注意：hasDetection 为 false 时其余几何参数不会参与有效性校验。
|
||||
/// </summary>
|
||||
public TwoLegProjectionInput(bool hasDetection, float detectionWorldX, float detectionWorldY,
|
||||
double detectionHeadingRadians, float firstLocalX, float firstLocalY,
|
||||
float secondLocalX, float secondLocalY, float radiusMm, string diagnostic = null)
|
||||
{
|
||||
HasDetection = hasDetection; DetectionWorldX = detectionWorldX; DetectionWorldY = detectionWorldY;
|
||||
DetectionHeadingRadians = detectionHeadingRadians; FirstLocalX = firstLocalX; FirstLocalY = firstLocalY;
|
||||
SecondLocalX = secondLocalX; SecondLocalY = secondLocalY; RadiusMm = radiusMm; Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
/// <summary>检测是否存在。false 时投影结果为 Empty。</summary>
|
||||
public bool HasDetection { get; }
|
||||
/// <summary>检测局部坐标系原点的世界 X 坐标，单位 mm。</summary>
|
||||
public float DetectionWorldX { get; }
|
||||
/// <summary>检测局部坐标系原点的世界 Y 坐标，单位 mm。</summary>
|
||||
public float DetectionWorldY { get; }
|
||||
/// <summary>检测局部坐标系相对世界坐标系的航向，单位弧度。</summary>
|
||||
public double DetectionHeadingRadians { get; }
|
||||
/// <summary>第一条腿在检测局部坐标系的 X 坐标，单位 mm。</summary>
|
||||
public float FirstLocalX { get; }
|
||||
/// <summary>第一条腿在检测局部坐标系的 Y 坐标，单位 mm。</summary>
|
||||
public float FirstLocalY { get; }
|
||||
/// <summary>第二条腿在检测局部坐标系的 X 坐标，单位 mm。</summary>
|
||||
public float SecondLocalX { get; }
|
||||
/// <summary>第二条腿在检测局部坐标系的 Y 坐标，单位 mm。</summary>
|
||||
public float SecondLocalY { get; }
|
||||
/// <summary>投影为圆形障碍物时的半径，单位 mm。</summary>
|
||||
public float RadiusMm { get; }
|
||||
/// <summary>上游检测阶段提供的可选诊断文本；未提供时为空字符串。</summary>
|
||||
public string Diagnostic { get; }
|
||||
/// <summary>未检测到目标时为 true；检测到目标时，所有数值有限且半径非负才为 true。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return !HasDetection || (NumericGuard.IsFinite(DetectionWorldX) && NumericGuard.IsFinite(DetectionWorldY) && NumericGuard.IsFinite(DetectionHeadingRadians) && NumericGuard.IsFinite(FirstLocalX) && NumericGuard.IsFinite(FirstLocalY) && NumericGuard.IsFinite(SecondLocalX) && NumericGuard.IsFinite(SecondLocalY) && NumericGuard.IsFinite(RadiusMm) && RadiusMm >= 0f); }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 用于手工调试的 Clumsy 地图测试入口，只通过公开的 <see cref="PlanningMapFactory"/> 创建快照。
|
||||
///
|
||||
/// 注意：终端日志和 PNG 开关只影响调试输出，不参与建图输入、地图内容或缓存键。
|
||||
/// </summary>
|
||||
[MovementTest(name = "规划地图快照测试V1")]
|
||||
public sealed class PlanningMapTest : MovementTest
|
||||
{
|
||||
private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
|
||||
private const bool EnableTerminalDebugLog = true;
|
||||
private const bool SavePng = true;
|
||||
private const float MapXMinMm = 0f;
|
||||
private const float MapXMaxMm = 6000f;
|
||||
private const float MapYMinMm = 0f;
|
||||
private const float MapYMaxMm = 4000f;
|
||||
private const float GridResolutionMm = 50f;
|
||||
private const bool AllowExplicitEmptyMap = false;
|
||||
private const bool EnableTwoLegSnapshot = false;
|
||||
|
||||
/// <summary>
|
||||
/// 执行一次配置好的地图创建与调试输出。
|
||||
///
|
||||
/// 返回：无。测试使用本文件顶部的地图范围、分辨率、障碍物来源、日志和 PNG 开关；不会接入实时 TwoLeg 检测。
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
IMapObstacleSource[] sources = CreateObstacleSources();
|
||||
Log("========== 规划地图创建开始 ==========");
|
||||
Log("参数：地图范围=(" + MapXMinMm + "," + MapXMaxMm + ")x(" + MapYMinMm + "," + MapYMaxMm
|
||||
+ ")mm，分辨率=" + GridResolutionMm + "mm，允许空图=" + AllowExplicitEmptyMap + "，导出图片=" + SavePng);
|
||||
Log("障碍物来源：" + FormatSources(sources));
|
||||
var result = _mapFactory.Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(MapXMinMm, MapXMaxMm, MapYMinMm, MapYMaxMm),
|
||||
ResolutionMm = GridResolutionMm,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = AllowExplicitEmptyMap,
|
||||
});
|
||||
Log("投影结果：" + FormatProjectionResults(sources, result.SourceResults));
|
||||
if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
|
||||
{
|
||||
Log("规划地图创建失败：" + result.FailureReason);
|
||||
Log("========== 规划地图创建结束 ==========");
|
||||
return;
|
||||
}
|
||||
Log("地图快照：编号=" + result.Map.SnapshotId + "，缓存=" + FormatCacheHit(result.CacheHit) + "，栅格=" + result.Map.Cols + "x" + result.Map.Rows
|
||||
+ "，可规划=" + result.Map.PlanningReady + "，指纹=" + result.Map.InputFingerprint);
|
||||
PlanningMapImageExportResult image = PlanningMapImageExporter.ExportIfEnabled(SavePng,
|
||||
new PlanningMapImageExportRequest { Map = result.Map, OutputRootDirectory = Environment.CurrentDirectory });
|
||||
Log("图片导出：" + FormatImageResult(image));
|
||||
Log("========== 规划地图创建完成 ==========");
|
||||
}
|
||||
|
||||
private static IMapObstacleSource[] CreateObstacleSources()
|
||||
{
|
||||
IMapObstacle[] manualObstacles =
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
|
||||
new CircleObstacle(3600f, 1200f, 180f),
|
||||
};
|
||||
return new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("manual", 1L, true, manualObstacles),
|
||||
new TwoLegObstacleSource("two-leg", EnableTwoLegSnapshot ? 1L : 0L, false,
|
||||
new TwoLegProjectionInput(EnableTwoLegSnapshot, 0f, 0f, 0d, 0f, 0f, 0f, 0f, 0f, "No current TwoLeg snapshot.")),
|
||||
};
|
||||
}
|
||||
|
||||
private static string FormatSources(IReadOnlyList<IMapObstacleSource> sources)
|
||||
{
|
||||
return string.Join("；", sources.Select(source => source.SourceId + "（版本=" + source.SourceVersion + "，必需=" + source.IsRequired + "）"));
|
||||
}
|
||||
|
||||
private static string FormatProjectionResults(IReadOnlyList<IMapObstacleSource> sources, IReadOnlyList<ObstacleProjectionResult> results)
|
||||
{
|
||||
IMapObstacleSource[] sortedSources = sources.OrderBy(source => source.SourceId, StringComparer.Ordinal).ToArray();
|
||||
return string.Join("；", results.Select((result, index) => sortedSources[index].SourceId + "=" + FormatSourceStatus(result.Status) + "（" + result.Obstacles.Count + " 个障碍物）"));
|
||||
}
|
||||
|
||||
private static string FormatSourceStatus(ObstacleSourceStatus status)
|
||||
{
|
||||
switch (status)
|
||||
{
|
||||
case ObstacleSourceStatus.Applied: return "已应用";
|
||||
case ObstacleSourceStatus.Empty: return "空";
|
||||
case ObstacleSourceStatus.Unavailable: return "不可用";
|
||||
case ObstacleSourceStatus.Invalid: return "无效";
|
||||
default: return "未知";
|
||||
}
|
||||
}
|
||||
|
||||
private static string FormatCacheHit(PlanningMapCacheHit cacheHit)
|
||||
{
|
||||
switch (cacheHit)
|
||||
{
|
||||
case PlanningMapCacheHit.Input: return "输入命中";
|
||||
case PlanningMapCacheHit.Occupancy: return "占据图命中";
|
||||
default: return "未命中";
|
||||
}
|
||||
}
|
||||
|
||||
private static string FormatImageResult(PlanningMapImageExportResult image)
|
||||
{
|
||||
if (image.Skipped) return "未启用";
|
||||
if (image.Saved) return "已保存：" + image.FilePath;
|
||||
return "失败：" + image.Message;
|
||||
}
|
||||
|
||||
private static void Log(string message)
|
||||
{
|
||||
if (EnableTerminalDebugLog) Console.WriteLine("[PlanningMapTest] " + message);
|
||||
}
|
||||
|
||||
/// <summary>停止测试。当前测试不启动持续任务，因此无需额外清理资源。</summary>
|
||||
public override void TestStop() { }
|
||||
}
|
||||
+10
@@ -0,0 +1,10 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>可选 PNG 调试图导出请求，只读取不可变的规划地图快照。</summary>
|
||||
public sealed class PlanningMapImageExportRequest
|
||||
{
|
||||
/// <summary>待渲染的规划地图快照。不能为空；PNG 导出不会改动该地图或其缓存。</summary>
|
||||
public PlanningGridMap Map { get; set; }
|
||||
/// <summary>PNG 输出根目录。导出器会在其中创建 PlanningMapExports 子目录；不能为空。</summary>
|
||||
public string OutputRootDirectory { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>一次可选 PNG 导出的处理结果，包含状态、输出路径、像素尺寸和诊断信息。</summary>
|
||||
public sealed class PlanningMapImageExportResult
|
||||
{
|
||||
/// <summary>是否已经成功写入并完成 PNG 文件。</summary>
|
||||
public bool Saved { get; set; }
|
||||
/// <summary>是否因导出开关关闭而跳过。true 时不创建文件，也不影响地图构建或缓存。</summary>
|
||||
public bool Skipped { get; set; }
|
||||
/// <summary>成功保存时的 PNG 完整路径；未保存时通常为 null。</summary>
|
||||
public string FilePath { get; set; }
|
||||
/// <summary>保存、跳过或失败的诊断信息。</summary>
|
||||
public string Message { get; set; }
|
||||
/// <summary>成功 PNG 的文件字节数；未保存时为零。</summary>
|
||||
public long FileSizeBytes { get; set; }
|
||||
/// <summary>输出图像宽度，单位为像素。</summary>
|
||||
public int PixelWidth { get; set; }
|
||||
/// <summary>输出图像高度，单位为像素。</summary>
|
||||
public int PixelHeight { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>可选 PNG 导出器；它永远不参与地图构建、障碍物投影或缓存指纹计算。</summary>
|
||||
public static class PlanningMapImageExporter
|
||||
{
|
||||
/// <summary>每个规划栅格在输出图中占用的边长，单位为像素。</summary>
|
||||
public const int PixelsPerCell = 4;
|
||||
/// <summary>输出 PNG 单边允许的最大像素数，超过时拒绝导出。</summary>
|
||||
public const int MaximumImageEdgePixels = 4000;
|
||||
/// <summary>输出 PNG 允许的最大文件大小，单位为字节（50 MiB）。</summary>
|
||||
public const long MaximumFileSizeBytes = 50L * 1024L * 1024L;
|
||||
/// <summary>写入 PNG 的物理分辨率元数据，单位 DPI。</summary>
|
||||
public const float OutputDpi = 300f;
|
||||
private const int MaximumFilenameAttempts = 1024;
|
||||
|
||||
/// <summary>
|
||||
/// 在开关开启时导出规划地图 PNG。
|
||||
///
|
||||
/// 参数：enabled 为导出开关；request 包含不可变地图和输出根目录。
|
||||
/// 返回：开关关闭时返回 Skipped；成功时返回 Saved、路径、字节数与像素尺寸；输入或尺寸不合法时返回失败信息。
|
||||
/// 注意：该方法仅用于调试，绝不会影响地图创建和缓存结果。
|
||||
/// </summary>
|
||||
public static PlanningMapImageExportResult ExportIfEnabled(bool enabled, PlanningMapImageExportRequest request)
|
||||
{
|
||||
if (!enabled) return new PlanningMapImageExportResult { Skipped = true, Message = "Planning map image export is disabled." };
|
||||
if (request == null || request.Map == null) return Rejected("Planning map image export requires a PlanningGridMap.");
|
||||
if (string.IsNullOrWhiteSpace(request.OutputRootDirectory)) return Rejected("Planning map image export requires an output root directory.");
|
||||
long width = (long)request.Map.Cols * PixelsPerCell, height = (long)request.Map.Rows * PixelsPerCell;
|
||||
if (width <= 0 || height <= 0 || width > MaximumImageEdgePixels || height > MaximumImageEdgePixels) return Rejected("Planning map image dimensions exceed the permitted edge.", width, height);
|
||||
string temporary = null;
|
||||
try
|
||||
{
|
||||
string directory = Path.Combine(request.OutputRootDirectory, "PlanningMapExports");
|
||||
Directory.CreateDirectory(directory);
|
||||
using (FileStream stream = CreateTemporaryFile(directory, out string finalPath, out temporary))
|
||||
{
|
||||
byte[] rgba = PlanningMapImageRenderer.Render(request.Map, PixelsPerCell, out int pixelWidth, out int pixelHeight);
|
||||
ValidatedPngWriter.Write(rgba, pixelWidth, pixelHeight, stream);
|
||||
}
|
||||
long length = new FileInfo(temporary).Length;
|
||||
if (length > MaximumFileSizeBytes) return Rejected("Planning map image PNG exceeds 50 MiB.", width, height);
|
||||
string completed = temporary.Substring(0, temporary.Length - 4);
|
||||
File.Move(temporary, completed); temporary = null;
|
||||
return new PlanningMapImageExportResult { Saved = true, FilePath = completed, FileSizeBytes = length, PixelWidth = (int)width, PixelHeight = (int)height, Message = "Planning map image export saved." };
|
||||
}
|
||||
catch (Exception exception) { return Rejected("Planning map image export failed: " + exception.Message, width, height); }
|
||||
finally { if (temporary != null && File.Exists(temporary)) File.Delete(temporary); }
|
||||
}
|
||||
|
||||
private static PlanningMapImageExportResult Rejected(string message, long width = 0, long height = 0) { return new PlanningMapImageExportResult { Message = message, PixelWidth = width > int.MaxValue ? int.MaxValue : (int)width, PixelHeight = height > int.MaxValue ? int.MaxValue : (int)height }; }
|
||||
private static FileStream CreateTemporaryFile(string directory, out string finalPath, out string temporaryPath)
|
||||
{
|
||||
string timestamp = DateTime.Now.ToString("yyyyMMdd_HHmmss_fff");
|
||||
for (int index = 0; index < MaximumFilenameAttempts; index++)
|
||||
{
|
||||
string suffix = index == 0 ? string.Empty : "_" + index;
|
||||
finalPath = Path.Combine(directory, "PlanningMap_" + timestamp + suffix + ".png"); temporaryPath = finalPath + ".tmp";
|
||||
if (File.Exists(finalPath)) continue;
|
||||
try { return new FileStream(temporaryPath, FileMode.CreateNew, FileAccess.Write, FileShare.None); }
|
||||
catch (IOException) { }
|
||||
}
|
||||
finalPath = null; temporaryPath = null; throw new IOException("Could not reserve a unique PlanningMap PNG filename.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>从只读规划地图生成简单的 RGBA 占据图字节数组。</summary>
|
||||
internal static class PlanningMapImageRenderer
|
||||
{
|
||||
/// <summary>
|
||||
/// 渲染规划地图为行主序 RGBA 像素。
|
||||
///
|
||||
/// 参数：map 为只读规划快照；pixelsPerCell 为每个栅格的像素边长；width、height 返回图像像素尺寸。
|
||||
/// 返回：长度为 width × height × 4 的 RGBA 字节数组；不会修改地图。
|
||||
/// </summary>
|
||||
public static byte[] Render(PlanningGridMap map, int pixelsPerCell, out int width, out int height)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
width = checked(map.Cols * pixelsPerCell);
|
||||
height = checked(map.Rows * pixelsPerCell);
|
||||
var rgba = new byte[checked(width * height * 4)];
|
||||
for (int row = 0; row < map.Rows; row++)
|
||||
for (int col = 0; col < map.Cols; col++)
|
||||
{
|
||||
bool occupied = map.IsOccupied(row, col);
|
||||
byte red = occupied ? (byte)220 : (byte)245;
|
||||
byte green = occupied ? (byte)45 : (byte)245;
|
||||
byte blue = occupied ? (byte)45 : (byte)245;
|
||||
int displayRow = map.Rows - 1 - row;
|
||||
for (int py = 0; py < pixelsPerCell; py++)
|
||||
for (int px = 0; px < pixelsPerCell; px++)
|
||||
{
|
||||
int index = ((displayRow * pixelsPerCell + py) * width + col * pixelsPerCell + px) * 4;
|
||||
rgba[index] = red; rgba[index + 1] = green; rgba[index + 2] = blue; rgba[index + 3] = 255;
|
||||
}
|
||||
}
|
||||
return rgba;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using StbImageWriteSharp;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>编码 RGBA 像素并校验 PNG 文件头、必要块和全部 CRC 的内部写入器。</summary>
|
||||
internal static class ValidatedPngWriter
|
||||
{
|
||||
private static readonly byte[] Signature = { 137, 80, 78, 71, 13, 10, 26, 10 };
|
||||
private static readonly byte[] PhysType = { 112, 72, 89, 115 };
|
||||
|
||||
/// <summary>
|
||||
/// 将 RGBA 像素写入经过校验的 PNG 流。
|
||||
///
|
||||
/// 参数：rgba 为行主序、每像素四字节的红绿蓝透明度数组；width、height 为像素尺寸；output 为可写目标流。
|
||||
/// 返回：无。输入长度不等于 width × height × 4 或 PNG 校验失败时抛出异常。
|
||||
/// </summary>
|
||||
public static void Write(byte[] rgba, int width, int height, Stream output)
|
||||
{
|
||||
Write(rgba, width, height, output, 11811u);
|
||||
}
|
||||
|
||||
/// <summary>将 RGBA 像素写入带指定物理分辨率元数据的经过校验的 PNG 流。</summary>
|
||||
internal static void Write(byte[] rgba, int width, int height, Stream output, uint pixelsPerMeter)
|
||||
{
|
||||
if (rgba == null || output == null || width <= 0 || height <= 0 || pixelsPerMeter == 0u || rgba.Length != checked(width * height * 4))
|
||||
throw new ArgumentException("Invalid RGBA PNG input.");
|
||||
byte[] encoded;
|
||||
using (var memory = new MemoryStream())
|
||||
{
|
||||
new ImageWriter().WritePng(rgba, width, height, ColorComponents.RedGreenBlueAlpha, memory);
|
||||
encoded = memory.ToArray();
|
||||
}
|
||||
Validate(encoded);
|
||||
const int ihdrEndOffset = 8 + 4 + 4 + 13 + 4;
|
||||
using (var outputMemory = new MemoryStream())
|
||||
{
|
||||
outputMemory.Write(encoded, 0, ihdrEndOffset);
|
||||
var phys = new byte[9];
|
||||
WriteUInt32BigEndian(phys, 0, pixelsPerMeter);
|
||||
WriteUInt32BigEndian(phys, 4, pixelsPerMeter);
|
||||
phys[8] = 1;
|
||||
WriteChunk(outputMemory, PhysType, phys);
|
||||
outputMemory.Write(encoded, ihdrEndOffset, encoded.Length - ihdrEndOffset);
|
||||
encoded = outputMemory.ToArray();
|
||||
}
|
||||
Validate(encoded);
|
||||
output.Write(encoded, 0, encoded.Length);
|
||||
}
|
||||
|
||||
private static void Validate(byte[] png)
|
||||
{
|
||||
if (png == null || png.Length < 45) throw new InvalidDataException("PNG is truncated.");
|
||||
for (int i = 0; i < Signature.Length; i++) if (png[i] != Signature[i]) throw new InvalidDataException("PNG signature is invalid.");
|
||||
int offset = Signature.Length, ihdr = 0, iend = 0;
|
||||
while (offset < png.Length)
|
||||
{
|
||||
if (png.Length - offset < 12) throw new InvalidDataException("PNG chunk header is truncated.");
|
||||
uint length = ReadUInt32BigEndian(png, offset);
|
||||
long crcOffset = (long)offset + 8L + length;
|
||||
if (crcOffset + 4L > png.Length) throw new InvalidDataException("PNG chunk is truncated.");
|
||||
int typeOffset = offset + 4;
|
||||
uint expected = ReadUInt32BigEndian(png, (int)crcOffset);
|
||||
uint actual = ComputeCrc32(png, typeOffset, checked((int)length + 4));
|
||||
if (expected != actual) throw new InvalidDataException("PNG chunk CRC is invalid.");
|
||||
bool isIhdr = IsType(png, typeOffset, 73, 72, 68, 82);
|
||||
bool isIend = IsType(png, typeOffset, 73, 69, 78, 68);
|
||||
if (offset == Signature.Length && !isIhdr) throw new InvalidDataException("PNG must start with IHDR.");
|
||||
if (isIhdr) ihdr++;
|
||||
if (isIend) { iend++; if (crcOffset + 4L != png.Length) throw new InvalidDataException("PNG data follows IEND."); }
|
||||
offset = checked((int)crcOffset + 4);
|
||||
}
|
||||
if (ihdr != 1 || iend != 1) throw new InvalidDataException("PNG must contain exactly one IHDR and IEND.");
|
||||
}
|
||||
private static bool IsType(byte[] bytes, int offset, byte a, byte b, byte c, byte d) { return bytes[offset] == a && bytes[offset + 1] == b && bytes[offset + 2] == c && bytes[offset + 3] == d; }
|
||||
private static uint ReadUInt32BigEndian(byte[] bytes, int offset) { return ((uint)bytes[offset] << 24) | ((uint)bytes[offset + 1] << 16) | ((uint)bytes[offset + 2] << 8) | bytes[offset + 3]; }
|
||||
private static void WriteUInt32BigEndian(byte[] bytes, int offset, uint value) { bytes[offset] = (byte)(value >> 24); bytes[offset + 1] = (byte)(value >> 16); bytes[offset + 2] = (byte)(value >> 8); bytes[offset + 3] = (byte)value; }
|
||||
private static void WriteChunk(Stream output, byte[] type, byte[] data)
|
||||
{
|
||||
var length = new byte[4]; WriteUInt32BigEndian(length, 0, (uint)data.Length); output.Write(length, 0, 4); output.Write(type, 0, 4); output.Write(data, 0, data.Length);
|
||||
var crcBytes = new byte[4]; WriteUInt32BigEndian(crcBytes, 0, ComputeCrc32(type, 0, type.Length, data)); output.Write(crcBytes, 0, 4);
|
||||
}
|
||||
private static uint ComputeCrc32(byte[] data, int offset, int count) { return ComputeCrc32(data, offset, count, null); }
|
||||
private static uint ComputeCrc32(byte[] first, int offset, int count, byte[] second)
|
||||
{
|
||||
uint crc = 0xffffffffu;
|
||||
for (int i = 0; i < count; i++) crc = UpdateCrc(crc, first[offset + i]);
|
||||
if (second != null) for (int i = 0; i < second.Length; i++) crc = UpdateCrc(crc, second[i]);
|
||||
return crc ^ 0xffffffffu;
|
||||
}
|
||||
private static uint UpdateCrc(uint crc, byte value)
|
||||
{
|
||||
crc ^= value;
|
||||
for (int bit = 0; bit < 8; bit++) crc = (crc & 1u) == 0u ? crc >> 1 : 0xedb88320u ^ (crc >> 1);
|
||||
return crc;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user