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/PlanningMapFactory.cs
+184
View File
@@ -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);
}
}