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/PathSmoothing/Facade/PathSmoothingComparisonService.cs
+37 -46
View File
@@ -2,13 +2,16 @@ using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
/// <summary>以固定预热和五次测量隔离比较所有请求平滑方法的离线入口。</summary>
/// <summary>
/// 原始路径基线与一次 Local G2 平滑的离线比较入口。
/// 此类仅汇总已冻结请求的质量、确定性摘要和耗时，不参与实时控制或修改平滑结果。
/// </summary>
public sealed class PathSmoothingComparisonService
{
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
@@ -16,13 +19,16 @@ public sealed class PathSmoothingComparisonService
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
/// <summary>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
/// <summary>创建原始基线并比较一次 Local G2 运行的质量、耗时和推荐资格。</summary>
/// <param name="request">比较请求，包含待平滑的不可变请求快照和比较设置；为 <see langword="null"/> 时返回带失败基线的结果。</param>
/// <param name="cancellationToken">取消比较和重复计时的调用方令牌。</param>
/// <returns>包含原始基线、候选条目和推荐方法的不可变比较结果；取消时 <c>Cancelled</c> 为 <see langword="true"/>。</returns>
public PathSmoothingComparisonResult Compare(
PathSmoothingComparisonRequest request,
CancellationToken cancellationToken = default)
{
PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason);
var entries = new List<PathSmoothingComparisonEntry>();
var entries = new List<PathSmoothingComparisonEntry>(1);
if (cancellationToken.IsCancellationRequested)
return Cancelled(baseline, entries);
@@ -30,33 +36,26 @@ public sealed class PathSmoothingComparisonService
if (request == null || request.SmoothingRequest == null)
return new PathSmoothingComparisonResult(baseline, entries, null, false, baselineReason);
for (int methodIndex = 0; methodIndex < request.Methods.Count; methodIndex++)
if (!TryCompareLocalG2(
request.SmoothingRequest,
baseline.Metrics,
cancellationToken,
out PathSmoothingComparisonEntry entry))
{
if (cancellationToken.IsCancellationRequested)
return Cancelled(baseline, entries);
SmoothingMethod method = request.Methods[methodIndex];
if (!TryCompareMethod(
request.SmoothingRequest,
method,
baseline.Metrics,
cancellationToken,
out PathSmoothingComparisonEntry entry))
return Cancelled(baseline, entries);
entries.Add(entry);
return Cancelled(baseline, entries);
}
entries.Add(entry);
return new PathSmoothingComparisonResult(
baseline,
entries,
SmoothingMethodRanker.Rank(entries),
entry.IsEligibleForRecommendation ? SmoothingMethod.LocalG2Quintic : null,
false,
baselineReason);
}
private bool TryCompareMethod(
PathSmoothingRequest sourceRequest,
SmoothingMethod method,
private bool TryCompareLocalG2(
PathSmoothingRequest smoothingRequest,
PathQualityMetrics rawMetrics,
CancellationToken cancellationToken,
out PathSmoothingComparisonEntry entry)
@@ -64,9 +63,8 @@ public sealed class PathSmoothingComparisonService
entry = null;
try
{
PathSmoothingRequest methodRequest = CreateMethodRequest(sourceRequest, method);
cancellationToken.ThrowIfCancellationRequested();
PathSmoothingResult warmup = _smoothingService.Smooth(methodRequest, cancellationToken);
PathSmoothingResult warmup = _smoothingService.Smooth(smoothingRequest, cancellationToken);
if (warmup.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
var timings = new List<double>(5);
@@ -75,7 +73,7 @@ public sealed class PathSmoothingComparisonService
{
cancellationToken.ThrowIfCancellationRequested();
Stopwatch stopwatch = Stopwatch.StartNew();
PathSmoothingResult result = _smoothingService.Smooth(methodRequest, cancellationToken);
PathSmoothingResult result = _smoothingService.Smooth(smoothingRequest, cancellationToken);
stopwatch.Stop();
if (result.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
timings.Add(stopwatch.Elapsed.TotalMilliseconds);
@@ -88,12 +86,11 @@ public sealed class PathSmoothingComparisonService
string diagnostic = string.IsNullOrWhiteSpace(timing.Diagnostic)
? canonical.Diagnostics.TerminationReason
: timing.Diagnostic;
PathQualityMetrics metrics = canonical.Status == PathSmoothingStatus.Success
PathQualityMetrics metrics = PathSmoothingComparisonEntry.IsPublishedLocalG2Status(canonical.Status)
? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics)
: new PathQualityMetrics();
entry = PathSmoothingComparisonEntry.CreateCandidate(
method,
SmoothingMethod.LocalG2Quintic,
canonical.Status,
metrics,
timing,
@@ -110,7 +107,7 @@ public sealed class PathSmoothingComparisonService
catch (Exception exception)
{
entry = PathSmoothingComparisonEntry.CreateCandidate(
method,
SmoothingMethod.LocalG2Quintic,
PathSmoothingStatus.Failed,
new PathQualityMetrics(),
new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, exception.GetType().Name),
@@ -128,12 +125,17 @@ public sealed class PathSmoothingComparisonService
{
reason = string.Empty;
if (request == null || request.SmoothingRequest == null)
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求为空。", out reason);
return FailedBaseline(PathSmoothingStatus.InvalidInput, "Comparison request is required.", out reason);
PathSmoothingRequest smoothingRequest = request.SmoothingRequest;
PathSmoothingConfiguration configuration = smoothingRequest.Configuration;
if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null)
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求缺少可用的地图、车辆或配置。", out reason);
{
return FailedBaseline(
PathSmoothingStatus.InvalidInput,
"A planning-ready map, vehicle, and configuration are required.",
out reason);
}
if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason))
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
@@ -146,11 +148,13 @@ public sealed class PathSmoothingComparisonService
configuration.MaximumCollisionCheckStepMeters,
out RawPathBaseline rawPath,
out reason))
{
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
}
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, rawPath.Path, rawPath.Segments);
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.NotNeeded, null, rawPath.Path, rawPath.Segments);
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
PathSmoothingStatus.Success,
PathSmoothingStatus.NotNeeded,
rawPath.Metrics,
digest,
string.Empty,
@@ -177,20 +181,7 @@ public sealed class PathSmoothingComparisonService
PathSmoothingComparisonEntry baseline,
IReadOnlyList<PathSmoothingComparisonEntry> entries)
{
return new PathSmoothingComparisonResult(baseline, entries, null, true, "路径平滑比较已取消。");
}
private static PathSmoothingRequest CreateMethodRequest(PathSmoothingRequest source, SmoothingMethod method)
{
PathSmoothingConfiguration configuration = source.Configuration;
configuration.Method = method;
configuration.AllowFallbackToCoarsePath = false;
return new PathSmoothingRequest(
source.CoarsePath,
source.Segments,
source.Map,
source.Vehicle,
configuration);
return new PathSmoothingComparisonResult(baseline, entries, null, true, "Path smoothing comparison was cancelled.");
}
private static PathQualityMetrics NormalizeMetrics(