71 lines
3.4 KiB
C#
71 lines
3.4 KiB
C#
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(); }
|
|
}
|