First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

chore: save current workspace progress

Browse Source
This commit is contained in:
梁薄云
2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/Map/Core/EnvironmentGridMap.cs
+92
View File
@@ -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(); }
}
ClumsyPilot/ParkrobTrajplanner/Map/Core/EnvironmentMapBuildResult.cs
+38
View File
@@ -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);
}
}
ClumsyPilot/ParkrobTrajplanner/Map/Core/EnvironmentMapBuilder.cs
+67
View File
@@ -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);
}
}
ClumsyPilot/ParkrobTrajplanner/Map/Core/MapBoundsMm.cs
+86
View File
@@ -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(); }
}
}
ClumsyPilot/ParkrobTrajplanner/Map/Core/MapBuildRequest.cs
+18
View File
@@ -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>();
}