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/Planning/EuclideanDistanceTransform.cs
+123
View File
@@ -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;
}
}
ClumsyPilot/ParkrobTrajplanner/Map/Planning/ObstacleDistanceField.cs
+70
View File
@@ -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(); }
}
ClumsyPilot/ParkrobTrajplanner/Map/Planning/PlanningGridMap.cs
+88
View File
@@ -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);
}
}
ClumsyPilot/ParkrobTrajplanner/Map/Planning/PlanningMapAdapter.cs
+45
View File
@@ -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;
}
}
ClumsyPilot/ParkrobTrajplanner/Map/Planning/PlanningMapCache.cs
+65
View File
@@ -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; } }
}