chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,234 @@
|
||||
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>Publishes independently validated Local G2 regional replacements through the dedicated service route.</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();
|
||||
|
||||
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, 0, 0d, 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, 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, 0, 0d, 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; }
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user