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/LocalG2/LocalG2CandidateEvaluator.cs
+24 -3
View File
@@ -12,7 +12,7 @@ using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>对一个局部 G2 替换候选执行区域质量门和完整路径安全复核。</summary>
/// <summary>对局部 G2 替换候选执行区域质量门、整条路径几何重算及碰撞安全复核。</summary>
internal sealed class LocalG2CandidateEvaluator
{
private const double CurvatureRangeTolerance = 1e-6d;
@@ -25,11 +25,16 @@ internal sealed class LocalG2CandidateEvaluator
private readonly LocalG2PathSplicer _splicer;
private readonly SmoothedPathValidator _validator;
/// <summary>使用默认几何分析、路径拼接和安全校验器创建候选评估器。</summary>
internal LocalG2CandidateEvaluator()
: this(new PathGeometryAnalyzer(), new LocalG2PathSplicer(), new SmoothedPathValidator())
{
}
/// <summary>使用注入的依赖创建候选评估器，便于复用统一的几何与碰撞判定规则。</summary>
/// <param name="analyzer">将预处理路径转换为带曲率指标的 <see cref="PathGeometryAnalysis"/> 的分析器。</param>
/// <param name="splicer">将候选窗口替换回当前路径并维持换向拓扑的拼接器。</param>
/// <param name="validator">对完整候选路径执行碰撞、净空和轨迹不变量检查的校验器。</param>
internal LocalG2CandidateEvaluator(
PathGeometryAnalyzer analyzer,
LocalG2PathSplicer splicer,
@@ -40,6 +45,15 @@ internal sealed class LocalG2CandidateEvaluator
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
}
/// <summary>评估一个局部 G2 候选，仅在全部质量门与完整路径安全复核通过时接受。</summary>
/// <param name="rawPath">未经局部替换的基线路径；用于约束曲率范围和总长度变化。</param>
/// <param name="currentPath">已接受先前窗口替换的当前路径；本候选将在其指定方向段内拼接。</param>
/// <param name="region">候选所属的单一行驶方向平滑区域。</param>
/// <param name="candidate">待评估候选的窗口边界与几何点；坐标和弧长单位为 m。</param>
/// <param name="request">平滑请求，提供地图、车辆模型、采样间距 m 和安全阈值。</param>
/// <param name="options">局部 G2 质量门选项，包括偏差 m、曲率导数 1/m² 和代价回退阈值。</param>
/// <param name="cancellationToken">取消令牌；取消时抛出 <see cref="OperationCanceledException"/>。</param>
/// <returns>包含接受状态、失败原因和度量值的 <see cref="LocalG2CandidateEvaluation"/>；拒绝时不发布候选路径。</returns>
internal LocalG2CandidateEvaluation Evaluate(
PreparedPath rawPath,
PreparedPath currentPath,
@@ -67,7 +81,9 @@ internal sealed class LocalG2CandidateEvaluator
string.IsNullOrEmpty(reason) ? "局部 G2 区域几何分析失败。" : reason);
}
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double vehicleMaximumCurvature))
if (!CurvatureLimitPolicy.TryGetAllowedMaximumVehicleCurvaturePerMeter(
request.Vehicle, request.Configuration.CurvatureLimitRadiusToleranceMeters,
out double vehicleMaximumCurvature))
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "车辆曲率约束无效。");
if (candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter > vehicleMaximumCurvature + CurvatureRangeTolerance)
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureExceeded, "局部 G2 候选超过车辆曲率上限。");
@@ -91,7 +107,9 @@ internal sealed class LocalG2CandidateEvaluator
}
if (!_validator.TryValidate(fullAnalysis.Path, fullAnalysis.Segments, rawPath, request.Map, request.Vehicle,
request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
request.Configuration.MaximumCollisionCheckStepMeters,
request.Configuration.CurvatureLimitRadiusToleranceMeters,
out IReadOnlyList<SmoothedPathPoint> safePath,
out double minimumClearance, out reason))
{
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.Collision,
@@ -145,6 +163,9 @@ internal sealed class LocalG2CandidateEvaluator
"Accepted");
}
/// <summary>在已通过质量门的候选中按偏差、峰值曲率导数、变化代价、长度变化和索引稳定选优。</summary>
/// <param name="evaluations">待比较的候选评估结果集合；仅 <c>Accepted</c> 为 <see langword="true"/> 的项可入选。</param>
/// <returns>最优的已接受评估；若没有合格项，返回带失败原因的拒绝评估。</returns>
internal static LocalG2CandidateEvaluation SelectBest(IReadOnlyList<LocalG2CandidateEvaluation> evaluations)
{
if (evaluations == null) throw new ArgumentNullException(nameof(evaluations));
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PreSmoothingPipeline.cs
+242
View File
@@ -0,0 +1,242 @@
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Diagnostics;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>通过专用服务路径生成、回滚并发布经独立验证的 Local G2 局部区域替换；不发布未经最终复核的候选。</summary>
internal sealed class LocalG2PreSmoothingPipeline
{
private readonly CurvatureTransitionDetector _detector = new CurvatureTransitionDetector();
private readonly LocalG2WindowPlanner _windowPlanner = new LocalG2WindowPlanner();
private readonly LocalG2CandidateBuilder _builder = new LocalG2CandidateBuilder();
private readonly LocalG2CandidateEvaluator _evaluator = new LocalG2CandidateEvaluator();
private readonly LocalG2RegionWorkOrder _workOrder = new LocalG2RegionWorkOrder();
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
/// <summary>在预处理路径上检测曲率事件、评估候选、按确定顺序拼接，并执行全局回滚复核。</summary>
/// <param name="request">不可变平滑请求，提供地图、车辆、配置和路径上下文。</param>
/// <param name="preparedPath">按方向段准备并重采样的原始路径；局部弧长单位为 m。</param>
/// <param name="rawBaseline">已独立复核的原始路径质量基线。</param>
/// <param name="cancellationToken">调用方取消令牌；取消时返回空路径的 Cancelled 结果。</param>
/// <returns>完整、部分、无需或保持原样时均只发布最终复核路径；失败/取消时不发布部分候选。</returns>
internal PathSmoothingResult Smooth(
PathSmoothingRequest request,
PreparedPath preparedPath,
RawPathBaseline rawBaseline,
CancellationToken cancellationToken)
{
var stopwatch = Stopwatch.StartNew();
try
{
cancellationToken.ThrowIfCancellationRequested();
if (request == null || preparedPath == null || rawBaseline == null ||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvature))
{
return Failure(PathSmoothingStatus.Failed, stopwatch, "局部 G2 预平滑输入无效。");
}
var options = new LocalG2OptionsSnapshot(request.Configuration);
if (!_detector.TryDetect(request, maximumCurvature, options, out IReadOnlyList<CurvatureTransition> transitions, out string reason) ||
!_windowPlanner.TryPlan(preparedPath, transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out reason))
{
return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
}
IReadOnlyList<LocalG2SmoothingRegion> reportOrder =
new ReadOnlyCollection<LocalG2SmoothingRegion>(new List<LocalG2SmoothingRegion>(regions));
if (!_workOrder.TryCreate(reportOrder, out IReadOnlyList<LocalG2SmoothingRegion> workRegions, out string orderReason))
return Failure(PathSmoothingStatus.Failed, stopwatch, orderReason);
PreparedPath current = preparedPath;
var reportsByRegion = new Dictionary<LocalG2SmoothingRegion, PathSmoothingRegionReport>();
var accepted = new List<AcceptedRegion>();
int improvedCount = 0;
foreach (LocalG2SmoothingRegion region in workRegions)
{
cancellationToken.ThrowIfCancellationRequested();
IReadOnlyList<LocalG2CandidateGeometry> candidates = _builder.Build(
preparedPath.Segments[region.SegmentIndex], region,
request.Configuration.OutputSpacingMeters, options, cancellationToken);
var evaluations = new List<LocalG2CandidateEvaluation>();
for (int candidateIndex = 0; candidateIndex < candidates.Count; candidateIndex++)
evaluations.Add(_evaluator.Evaluate(
preparedPath, current, region, candidates[candidateIndex], request, options, cancellationToken));
LocalG2CandidateEvaluation best = LocalG2CandidateEvaluator.SelectBest(evaluations);
if (best.Accepted)
{
accepted.Add(new AcceptedRegion(region, current, best));
current = best.SplicedPreparedPath;
improvedCount++;
reportsByRegion.Add(region, CreateImprovedReport(region, candidates.Count, best));
}
else
{
reportsByRegion.Add(region, CreateRetainedReport(region, candidates.Count, best));
}
}
if (!TryValidateFinal(current, preparedPath, rawBaseline, request, options, out IReadOnlyList<SmoothedPathPoint> path,
out IReadOnlyList<SmoothedPathSegment> segments, out PathQualityMetrics metrics, out reason))
{
for (int index = accepted.Count - 1; index >= 0; index--)
{
AcceptedRegion rollback = accepted[index];
current = rollback.Before;
reportsByRegion[rollback.Region] = CreateRollbackReport(rollback.Region, rollback.Evaluation);
improvedCount--;
if (TryValidateFinal(current, preparedPath, rawBaseline, request, options, out path, out segments, out metrics, out reason))
break;
}
}
if (metrics == null)
return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
var reports = new List<PathSmoothingRegionReport>(reportOrder.Count);
for (int reportIndex = 0; reportIndex < reportOrder.Count; reportIndex++)
reports.Add(reportsByRegion[reportOrder[reportIndex]]);
PathSmoothingStatus status;
if (transitions.Count == 0) status = PathSmoothingStatus.NotNeeded;
else if (improvedCount == regions.Count) status = PathSmoothingStatus.Complete;
else if (improvedCount > 0) status = PathSmoothingStatus.PartialImprovement;
else status = PathSmoothingStatus.Unchanged;
return PathSmoothingResult.PublishLocalG2(
status,
path,
segments,
new PathSmoothingDiagnostics(metrics, stopwatch.Elapsed, reason ?? string.Empty),
reports);
}
catch (OperationCanceledException)
{
return Failure(PathSmoothingStatus.Cancelled, stopwatch, "路径平滑已取消。");
}
catch (Exception exception)
{
return Failure(PathSmoothingStatus.Failed, stopwatch, exception.Message);
}
}
private bool TryValidateFinal(
PreparedPath current,
PreparedPath rawPath,
RawPathBaseline rawBaseline,
PathSmoothingRequest request,
LocalG2OptionsSnapshot options,
out IReadOnlyList<SmoothedPathPoint> path,
out IReadOnlyList<SmoothedPathSegment> segments,
out PathQualityMetrics metrics,
out string reason)
{
path = null;
segments = null;
metrics = null;
reason = string.Empty;
if (!_analyzer.TryAnalyze(current.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason) ||
!_validator.TryValidate(analysis.Path, analysis.Segments, rawPath, request.Map, request.Vehicle,
request.Configuration.MaximumCollisionCheckStepMeters,
request.Configuration.CurvatureLimitRadiusToleranceMeters,
out IReadOnlyList<SmoothedPathPoint> safePath,
out double minimumClearance, out reason) ||
minimumClearance < request.Configuration.MinimumClearanceReserveMeters ||
analysis.CurvatureVariationCost > rawBaseline.Metrics.CurvatureVariationCost *
(1d + options.MaximumVariationCostRegressionRatio))
{
if (string.IsNullOrEmpty(reason)) reason = "局部 G2 完整路径复核未通过。";
return false;
}
path = safePath;
segments = analysis.Segments;
metrics = new PathQualityMetrics(
true,
analysis.PathLengthMeters,
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
analysis.RootMeanSquareVehicleCurvaturePerMeter,
analysis.TotalAbsoluteCurvatureVariationPerMeter,
analysis.CurvatureVariationCost,
minimumClearance,
0d, 0d, 0d, 0d);
return true;
}
private static PathSmoothingRegionReport CreateImprovedReport(
LocalG2SmoothingRegion region,
int candidateCount,
LocalG2CandidateEvaluation evaluation) =>
CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.Improved, PathSmoothingRegionFailureReason.None);
private static PathSmoothingRegionReport CreateRetainedReport(
LocalG2SmoothingRegion region,
int candidateCount,
LocalG2CandidateEvaluation evaluation) =>
CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.RetainedOriginal, evaluation.FailureReason);
private static PathSmoothingRegionReport CreateRollbackReport(
LocalG2SmoothingRegion region,
LocalG2CandidateEvaluation evaluation) =>
CreateReport(region, region.WindowVariants.Count, evaluation, PathSmoothingRegionStatus.RetainedOriginal,
PathSmoothingRegionFailureReason.GlobalValidationRollback);
private static PathSmoothingRegionReport CreateReport(
LocalG2SmoothingRegion region,
int candidateCount,
LocalG2CandidateEvaluation evaluation,
PathSmoothingRegionStatus status,
PathSmoothingRegionFailureReason failureReason)
{
LocalG2WindowVariant window = region.WindowVariants[0];
var jumps = new List<double>(region.Transitions.Count);
for (int index = 0; index < region.Transitions.Count; index++)
jumps.Add(region.Transitions[index].RightVehicleCurvaturePerMeter - region.Transitions[index].LeftVehicleCurvaturePerMeter);
return new PathSmoothingRegionReport(
region.SegmentIndex,
window.StartArcLengthMeters,
window.EndArcLengthMeters,
jumps,
window.EndArcLengthMeters - window.StartArcLengthMeters,
window.EndArcLengthMeters - window.StartArcLengthMeters,
window.LeftWindowLengthMeters,
window.RightWindowLengthMeters,
candidateCount,
evaluation.CandidateIndex,
status,
failureReason,
evaluation.RawPeakCurvatureDerivativePerSquareMeter,
evaluation.ResultPeakCurvatureDerivativePerSquareMeter,
evaluation.RawCurvatureVariationCost,
evaluation.ResultCurvatureVariationCost,
evaluation.MaximumDeviationMeters,
evaluation.MinimumBodyClearanceMeters,
evaluation.MaximumAbsoluteVehicleCurvaturePerMeter);
}
private static PathSmoothingResult Failure(PathSmoothingStatus status, Stopwatch stopwatch, string reason) =>
PathSmoothingResult.Failure(status,
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, reason));
private sealed class AcceptedRegion
{
internal AcceptedRegion(LocalG2SmoothingRegion region, PreparedPath before, LocalG2CandidateEvaluation evaluation)
{
Region = region;
Before = before;
Evaluation = evaluation;
}
internal LocalG2SmoothingRegion Region { get; }
internal PreparedPath Before { get; }
internal LocalG2CandidateEvaluation Evaluation { get; }
}
}