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