185 lines
11 KiB
C#
185 lines
11 KiB
C#
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);
|
|
}
|
|
}
|