544 lines
30 KiB
C#
544 lines
30 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using System.Linq;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
|
|
|
/// <summary>对一个局部 G2 替换候选执行区域质量门和完整路径安全复核。</summary>
|
|
internal sealed class LocalG2CandidateEvaluator
|
|
{
|
|
private const double CurvatureRangeTolerance = 1e-6d;
|
|
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
|
new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
|
private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
|
|
new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
|
|
private readonly PathGeometryAnalyzer _analyzer;
|
|
private readonly LocalG2PathSplicer _splicer;
|
|
private readonly SmoothedPathValidator _validator;
|
|
|
|
internal LocalG2CandidateEvaluator()
|
|
: this(new PathGeometryAnalyzer(), new LocalG2PathSplicer(), new SmoothedPathValidator())
|
|
{
|
|
}
|
|
|
|
internal LocalG2CandidateEvaluator(
|
|
PathGeometryAnalyzer analyzer,
|
|
LocalG2PathSplicer splicer,
|
|
SmoothedPathValidator validator)
|
|
{
|
|
_analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
|
|
_splicer = splicer ?? throw new ArgumentNullException(nameof(splicer));
|
|
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
|
}
|
|
|
|
internal LocalG2CandidateEvaluation Evaluate(
|
|
PreparedPath rawPath,
|
|
PreparedPath currentPath,
|
|
LocalG2SmoothingRegion region,
|
|
LocalG2CandidateGeometry candidate,
|
|
PathSmoothingRequest request,
|
|
LocalG2OptionsSnapshot options,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
int candidateIndex = candidate == null ? -1 : candidate.CandidateIndex;
|
|
if (!HasUsableInput(rawPath, currentPath, region, candidate, request, options))
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选评价输入无效。");
|
|
|
|
PreparedDirectionSegment currentSegment = currentPath.Segments[candidate.SegmentIndex];
|
|
if (!TryExtractWindow(currentSegment, candidate.StartArcLengthMeters, candidate.EndArcLengthMeters,
|
|
out IReadOnlyList<SmoothingPoint2D> rawWindow, out string reason) ||
|
|
!TryAnalyzeWindow(currentSegment, rawWindow, request.Configuration.OutputSpacingMeters,
|
|
out PathGeometryAnalysis rawAnalysis, out reason) ||
|
|
!TryAnalyzeWindow(currentSegment, candidate.RegionPoints, request.Configuration.OutputSpacingMeters,
|
|
out PathGeometryAnalysis candidateAnalysis, out reason) ||
|
|
!HasFiniteMetrics(rawAnalysis) || !HasFiniteMetrics(candidateAnalysis))
|
|
{
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
|
string.IsNullOrEmpty(reason) ? "局部 G2 区域几何分析失败。" : reason);
|
|
}
|
|
|
|
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double vehicleMaximumCurvature))
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "车辆曲率约束无效。");
|
|
if (candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter > vehicleMaximumCurvature + CurvatureRangeTolerance)
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureExceeded, "局部 G2 候选超过车辆曲率上限。");
|
|
|
|
GetCurvatureRange(rawAnalysis.Path, out double rawMinimumCurvature, out double rawMaximumCurvature);
|
|
if (ExceedsRawCurvatureRange(candidateAnalysis.Path, rawMinimumCurvature, rawMaximumCurvature))
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureOvershoot, "局部 G2 候选超出原始区域曲率范围。");
|
|
|
|
double maximumDeviation = MaximumDistanceToPolyline(candidate.RegionPoints, rawWindow);
|
|
if (!NumericGuard.IsFinite(maximumDeviation))
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选偏差计算失败。");
|
|
if (maximumDeviation > options.MaximumDeviationMeters)
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.DeviationExceeded, "局部 G2 候选偏离原始窗口过远。");
|
|
|
|
if (!_splicer.TryReplace(currentPath, candidate, out PreparedPath spliced, out reason) ||
|
|
!_analyzer.TryAnalyze(spliced.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis fullAnalysis, out reason) ||
|
|
!HasFiniteMetrics(fullAnalysis))
|
|
{
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
|
string.IsNullOrEmpty(reason) ? "局部 G2 完整路径几何分析失败。" : reason);
|
|
}
|
|
|
|
if (!_validator.TryValidate(fullAnalysis.Path, fullAnalysis.Segments, rawPath, request.Map, request.Vehicle,
|
|
request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
|
out double minimumClearance, out reason))
|
|
{
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.Collision,
|
|
string.IsNullOrEmpty(reason) ? "局部 G2 完整路径安全复核失败。" : reason);
|
|
}
|
|
if (!NumericGuard.IsFinite(minimumClearance) || minimumClearance < request.Configuration.MinimumClearanceReserveMeters)
|
|
return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientClearance, candidateIndex,
|
|
null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
|
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
|
|
candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
|
|
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选净空不足。");
|
|
|
|
if (candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter >
|
|
rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter *
|
|
(1d - options.MinimumPeakGradientImprovementRatio))
|
|
{
|
|
return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientImprovement, candidateIndex,
|
|
null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
|
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
|
|
candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
|
|
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选曲率导数峰值改善不足。");
|
|
}
|
|
if (candidateAnalysis.CurvatureVariationCost > rawAnalysis.CurvatureVariationCost *
|
|
(1d + options.MaximumVariationCostRegressionRatio))
|
|
{
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.VariationCostRegression, "局部 G2 候选曲率变化代价回退。");
|
|
}
|
|
|
|
if (!_analyzer.TryAnalyze(rawPath.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis rawFullAnalysis, out reason) ||
|
|
!HasFiniteMetrics(rawFullAnalysis))
|
|
{
|
|
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
|
string.IsNullOrEmpty(reason) ? "原始完整路径几何分析失败。" : reason);
|
|
}
|
|
|
|
return new LocalG2CandidateEvaluation(
|
|
true,
|
|
PathSmoothingRegionFailureReason.None,
|
|
candidateIndex,
|
|
spliced,
|
|
safePath,
|
|
fullAnalysis.Segments,
|
|
rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
|
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
|
rawAnalysis.CurvatureVariationCost,
|
|
candidateAnalysis.CurvatureVariationCost,
|
|
maximumDeviation,
|
|
minimumClearance,
|
|
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
|
Math.Abs(fullAnalysis.PathLengthMeters - rawFullAnalysis.PathLengthMeters),
|
|
"Accepted");
|
|
}
|
|
|
|
internal static LocalG2CandidateEvaluation SelectBest(IReadOnlyList<LocalG2CandidateEvaluation> evaluations)
|
|
{
|
|
if (evaluations == null) throw new ArgumentNullException(nameof(evaluations));
|
|
LocalG2CandidateEvaluation best = null;
|
|
for (int index = 0; index < evaluations.Count; index++)
|
|
{
|
|
LocalG2CandidateEvaluation evaluation = evaluations[index];
|
|
if (evaluation == null || !evaluation.Accepted) continue;
|
|
if (best == null || Compare(evaluation, best) < 0) best = evaluation;
|
|
}
|
|
return best ?? Rejected(-1, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "没有通过质量门的局部 G2 候选。");
|
|
}
|
|
|
|
private static int Compare(LocalG2CandidateEvaluation left, LocalG2CandidateEvaluation right)
|
|
{
|
|
int result = left.MaximumDeviationMeters.CompareTo(right.MaximumDeviationMeters);
|
|
if (result != 0) return result;
|
|
result = left.ResultPeakCurvatureDerivativePerSquareMeter.CompareTo(right.ResultPeakCurvatureDerivativePerSquareMeter);
|
|
if (result != 0) return result;
|
|
result = left.ResultCurvatureVariationCost.CompareTo(right.ResultCurvatureVariationCost);
|
|
if (result != 0) return result;
|
|
result = left.AbsolutePathLengthChangeMeters.CompareTo(right.AbsolutePathLengthChangeMeters);
|
|
return result != 0 ? result : left.CandidateIndex.CompareTo(right.CandidateIndex);
|
|
}
|
|
|
|
private static bool HasUsableInput(PreparedPath rawPath, PreparedPath currentPath, LocalG2SmoothingRegion region,
|
|
LocalG2CandidateGeometry candidate, PathSmoothingRequest request, LocalG2OptionsSnapshot options)
|
|
{
|
|
return rawPath != null && currentPath != null && region != null && candidate != null && request != null && options != null &&
|
|
request.Configuration != null && request.Map != null && request.Vehicle != null && candidate.SegmentIndex == region.SegmentIndex &&
|
|
candidate.SegmentIndex >= 0 && candidate.SegmentIndex < currentPath.Segments.Count &&
|
|
NumericGuard.IsPositiveFinite(request.Configuration.OutputSpacingMeters) &&
|
|
NumericGuard.IsPositiveFinite(request.Configuration.MaximumCollisionCheckStepMeters) &&
|
|
NumericGuard.IsFinite(request.Configuration.MinimumClearanceReserveMeters) &&
|
|
request.Configuration.MinimumClearanceReserveMeters >= 0d;
|
|
}
|
|
|
|
private static bool TryExtractWindow(PreparedDirectionSegment segment, double startArcLength, double endArcLength,
|
|
out IReadOnlyList<SmoothingPoint2D> window, out string reason)
|
|
{
|
|
window = null;
|
|
reason = string.Empty;
|
|
if (segment == null || !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, startArcLength, out SmoothingPoint2D start, out reason) ||
|
|
!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, endArcLength, out SmoothingPoint2D end, out reason)) return false;
|
|
var points = new List<SmoothingPoint2D> { start };
|
|
for (int index = 0; index < segment.Points.Count; index++)
|
|
{
|
|
SmoothingPoint2D point = segment.Points[index];
|
|
if (point.ArcLength > startArcLength && point.ArcLength < endArcLength) points.Add(point);
|
|
}
|
|
points.Add(end);
|
|
window = new ReadOnlyCollection<SmoothingPoint2D>(points);
|
|
return true;
|
|
}
|
|
|
|
private bool TryAnalyzeWindow(PreparedDirectionSegment source, IReadOnlyList<SmoothingPoint2D> points, double spacing,
|
|
out PathGeometryAnalysis analysis, out string reason)
|
|
{
|
|
var segment = new PreparedDirectionSegment(0, source.Direction, points, false, false,
|
|
source.Points[0].ArcLength == points[0].ArcLength ? source.StartVehicleCurvaturePerMeter : null);
|
|
return _analyzer.TryAnalyze(new[] { segment }, spacing, out analysis, out reason);
|
|
}
|
|
|
|
private static bool HasFiniteMetrics(PathGeometryAnalysis analysis)
|
|
{
|
|
return analysis != null && NumericGuard.IsFinite(analysis.PathLengthMeters) &&
|
|
NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvaturePerMeter) &&
|
|
NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter) &&
|
|
NumericGuard.IsFinite(analysis.CurvatureVariationCost) && analysis.Path != null && analysis.Path.Count >= 2;
|
|
}
|
|
|
|
private static void GetCurvatureRange(IReadOnlyList<SmoothedPathPoint> path, out double minimum, out double maximum)
|
|
{
|
|
minimum = double.PositiveInfinity;
|
|
maximum = double.NegativeInfinity;
|
|
for (int index = 0; index < path.Count; index++)
|
|
{
|
|
minimum = Math.Min(minimum, path[index].VehicleCurvature);
|
|
maximum = Math.Max(maximum, path[index].VehicleCurvature);
|
|
}
|
|
}
|
|
|
|
private static bool ExceedsRawCurvatureRange(IReadOnlyList<SmoothedPathPoint> candidate, double minimum, double maximum)
|
|
{
|
|
for (int index = 0; index < candidate.Count; index++)
|
|
{
|
|
double curvature = candidate[index].VehicleCurvature;
|
|
if (curvature < minimum - CurvatureRangeTolerance || curvature > maximum + CurvatureRangeTolerance) return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
private static double MaximumDistanceToPolyline(IReadOnlyList<SmoothingPoint2D> candidate, IReadOnlyList<SmoothingPoint2D> raw)
|
|
{
|
|
if (candidate == null || raw == null || candidate.Count == 0 || raw.Count < 2) return double.NaN;
|
|
double maximum = 0d;
|
|
for (int index = 0; index < candidate.Count; index++)
|
|
{
|
|
double nearest = double.PositiveInfinity;
|
|
for (int segment = 1; segment < raw.Count; segment++)
|
|
nearest = Math.Min(nearest, PointToSegmentDistance(candidate[index], raw[segment - 1], raw[segment]));
|
|
maximum = Math.Max(maximum, nearest);
|
|
}
|
|
return maximum;
|
|
}
|
|
|
|
private static double PointToSegmentDistance(SmoothingPoint2D point, SmoothingPoint2D start, SmoothingPoint2D end)
|
|
{
|
|
double dx = end.X - start.X;
|
|
double dy = end.Y - start.Y;
|
|
double lengthSquared = dx * dx + dy * dy;
|
|
if (!NumericGuard.IsPositiveFinite(lengthSquared)) return double.NaN;
|
|
double projection = ((point.X - start.X) * dx + (point.Y - start.Y) * dy) / lengthSquared;
|
|
projection = Math.Max(0d, Math.Min(1d, projection));
|
|
double nearestX = start.X + projection * dx;
|
|
double nearestY = start.Y + projection * dy;
|
|
double distanceX = point.X - nearestX;
|
|
double distanceY = point.Y - nearestY;
|
|
return Math.Sqrt(distanceX * distanceX + distanceY * distanceY);
|
|
}
|
|
|
|
private static LocalG2CandidateEvaluation Rejected(int candidateIndex, PathSmoothingRegionFailureReason failureReason, string reason)
|
|
{
|
|
return new LocalG2CandidateEvaluation(false, failureReason, candidateIndex, null, EmptyPath, EmptySegments,
|
|
0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, reason);
|
|
}
|
|
|
|
/// <summary>反射脚本使用的窄范围确定性质量门覆盖入口。</summary>
|
|
public static class TestHooks
|
|
{
|
|
public static EvaluationTestSnapshot Execute(string scenario)
|
|
{
|
|
LocalG2CandidateEvaluation evaluation;
|
|
switch (scenario)
|
|
{
|
|
case "TooFar":
|
|
PreparedPath tooFarPath = CreateWavyPath(1d);
|
|
evaluation = Evaluate(tooFarPath, CreateCandidate(tooFarPath, 0, 1.12d), 0d);
|
|
break;
|
|
case "Overshoot":
|
|
PreparedPath overshootPath = CreateStraightPath(1d);
|
|
evaluation = Evaluate(overshootPath, CreateCandidate(overshootPath, 0, 1.15d), 0d);
|
|
break;
|
|
case "NoOp":
|
|
PreparedPath noOpPath = CreateWavyPath(1d);
|
|
evaluation = Evaluate(noOpPath, CreateCandidateFromPath(noOpPath, 0), 0d);
|
|
break;
|
|
case "Oscillating":
|
|
PreparedPath oscillatingPath = CreateOscillationBaseline(1d);
|
|
evaluation = Evaluate(oscillatingPath, CreateOscillatingCandidate(oscillatingPath), 0d);
|
|
break;
|
|
case "LowClearance":
|
|
PreparedPath lowClearancePath = CreateWavyPath(0.27d);
|
|
evaluation = Evaluate(lowClearancePath, CreateCandidateFromPath(lowClearancePath, 0), 0.02d, false, true);
|
|
break;
|
|
case "Improved":
|
|
PreparedPath improvedPath = CreateWavyPath(1d);
|
|
evaluation = Evaluate(improvedPath, CreateCandidate(improvedPath, 0, 1d), 0d);
|
|
break;
|
|
case "SmallestDeviation":
|
|
PreparedPath smallestDeviationPath = CreateWavyPath(1d);
|
|
evaluation = Evaluate(smallestDeviationPath, CreateCandidate(smallestDeviationPath, 4, 1d), 100d);
|
|
break;
|
|
case "Best": evaluation = SelectBest(CreateWavyPath(1d)); break;
|
|
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
|
}
|
|
return new EvaluationTestSnapshot(evaluation.Accepted ? "Accepted" : "Rejected", evaluation.FailureReason.ToString(), evaluation.CandidateIndex,
|
|
evaluation.MinimumBodyClearanceMeters, evaluation.Reason);
|
|
}
|
|
|
|
private static LocalG2CandidateEvaluation Evaluate(PreparedPath path, LocalG2CandidateGeometry candidate, double minimumClearance,
|
|
bool useNearObstacle = false, bool useEmptyMap = false)
|
|
{
|
|
PathSmoothingConfiguration configuration = CreateConfiguration(minimumClearance);
|
|
PathSmoothingRequest request = new PathSmoothingRequest(null, null,
|
|
useEmptyMap ? CreateEmptyMap() : CreateMap(useNearObstacle), CreateVehicle(), configuration);
|
|
return new LocalG2CandidateEvaluator().Evaluate(path, path, CreateRegion(), candidate, request,
|
|
new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
|
}
|
|
|
|
private static LocalG2CandidateEvaluation SelectBest(PreparedPath path)
|
|
{
|
|
PathSmoothingConfiguration configuration = CreateConfiguration(0d);
|
|
PathSmoothingRequest request = new PathSmoothingRequest(null, null, CreateMap(false), CreateVehicle(), configuration);
|
|
var evaluator = new LocalG2CandidateEvaluator();
|
|
LocalG2CandidateEvaluation smallestDeviation = evaluator.Evaluate(path, path, CreateRegion(),
|
|
CreateCandidate(path, 4, 1d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
|
LocalG2CandidateEvaluation smootherButFarther = evaluator.Evaluate(path, path, CreateRegion(),
|
|
CreateCandidate(path, 5, 1.01d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
|
return LocalG2CandidateEvaluator.SelectBest(new[] { smootherButFarther, smallestDeviation });
|
|
}
|
|
|
|
private static PreparedPath CreateWavyPath(double startX)
|
|
{
|
|
return CreatePath(new[]
|
|
{
|
|
Point(startX, 1d), Point(startX + 0.10d, 1.04d), Point(startX + 0.20d, 1.08d),
|
|
Point(startX + 0.30d, 1.05d), Point(startX + 0.40d, 0.98d), Point(startX + 0.50d, 0.92d),
|
|
Point(startX + 0.60d, 0.98d), Point(startX + 0.70d, 1.05d), Point(startX + 0.80d, 1.08d),
|
|
Point(startX + 0.90d, 1.04d), Point(startX + 1d, 1d),
|
|
});
|
|
}
|
|
|
|
private static PreparedPath CreateStraightPath(double startX) => CreatePath(new[] { Point(startX, 1d), Point(startX + 0.5d, 1d), Point(startX + 1d, 1d) });
|
|
|
|
private static PreparedPath CreateOscillationBaseline(double startX)
|
|
{
|
|
return CreatePath(new[]
|
|
{
|
|
Point(startX, 1d), Point(startX + 0.05d, 1.022d), Point(startX + 0.10d, 1d),
|
|
Point(startX + 2.03d, 1d),
|
|
});
|
|
}
|
|
|
|
private static PreparedPath CreatePath(IReadOnlyList<SmoothingPoint2D> points)
|
|
{
|
|
var normalized = new List<SmoothingPoint2D>(points.Count);
|
|
double arcLength = 0d;
|
|
for (int index = 0; index < points.Count; index++)
|
|
{
|
|
if (index > 0)
|
|
{
|
|
double dx = points[index].X - points[index - 1].X;
|
|
double dy = points[index].Y - points[index - 1].Y;
|
|
arcLength += Math.Sqrt(dx * dx + dy * dy);
|
|
}
|
|
normalized.Add(new SmoothingPoint2D(points[index].X, points[index].Y, arcLength, 0d, 0d, 1d,
|
|
false, SmoothedPathPointSource.LocalG2Transition));
|
|
}
|
|
return new PreparedPath(new[] { new PreparedDirectionSegment(0, TravelDirection.Forward, normalized, false, false) });
|
|
}
|
|
|
|
private static LocalG2CandidateGeometry CreateCandidate(PreparedPath path, int index, double middleY)
|
|
{
|
|
IReadOnlyList<SmoothingPoint2D> source = path.Segments[0].Points;
|
|
SmoothingPoint2D start = source[0];
|
|
SmoothingPoint2D end = source[source.Count - 1];
|
|
return new LocalG2CandidateGeometry(index, 0, start.ArcLength, end.ArcLength, 0d, 0d,
|
|
new[] { Point(start.X, start.Y), Point((start.X + end.X) / 2d, middleY), Point(end.X, end.Y) },
|
|
0d, 0d, 0d, 0d, true);
|
|
}
|
|
|
|
private static LocalG2CandidateGeometry CreateCandidateFromPath(PreparedPath path, int index)
|
|
{
|
|
IReadOnlyList<SmoothingPoint2D> points = path.Segments[0].Points;
|
|
return new LocalG2CandidateGeometry(index, 0, points[0].ArcLength, points[points.Count - 1].ArcLength, 0d, 0d, points,
|
|
0d, 0d, 0d, 0d, true);
|
|
}
|
|
|
|
private static LocalG2CandidateGeometry CreateOscillatingCandidate(PreparedPath path)
|
|
{
|
|
IReadOnlyList<SmoothingPoint2D> source = path.Segments[0].Points;
|
|
SmoothingPoint2D start = source[0];
|
|
SmoothingPoint2D end = source[source.Count - 1];
|
|
var points = new List<SmoothingPoint2D> { Point(start.X, start.Y) };
|
|
SmoothingPoint2D straightStart = source[source.Count - 2];
|
|
points.Add(Point(straightStart.X, straightStart.Y));
|
|
for (int index = 1; index < 20; index++)
|
|
{
|
|
double x = straightStart.X + index * (end.X - straightStart.X) / 20d;
|
|
points.Add(Point(x, straightStart.Y + (index % 2 == 0 ? 0.004d : -0.004d)));
|
|
}
|
|
points.Add(Point(end.X, end.Y));
|
|
return new LocalG2CandidateGeometry(0, 0, start.ArcLength, end.ArcLength, 0d, 0d, points, 0d, 0d, 0d, 0d, true);
|
|
}
|
|
|
|
private static LocalG2SmoothingRegion CreateRegion()
|
|
{
|
|
return new LocalG2SmoothingRegion(0,
|
|
new[] { new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0d, 0d) },
|
|
0d, 1d, new[] { new LocalG2WindowVariant(0, 0d, 1d, 0d, 0d) });
|
|
}
|
|
|
|
private static PathSmoothingConfiguration CreateConfiguration(double minimumClearance)
|
|
{
|
|
var configuration = new PathSmoothingConfiguration
|
|
{
|
|
OutputSpacingMeters = 0.05d,
|
|
MaximumCollisionCheckStepMeters = 0.05d,
|
|
MinimumClearanceReserveMeters = minimumClearance,
|
|
};
|
|
configuration.LocalG2Quintic.MaximumDeviationMeters = 0.10d;
|
|
configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d;
|
|
configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d;
|
|
return configuration;
|
|
}
|
|
|
|
private static VehicleParameters CreateVehicle() => new VehicleParameters
|
|
{
|
|
LengthMeters = 0.20d, WidthMeters = 0.20d, SafetyMarginMeters = 0d,
|
|
MaximumCurvaturePerMeter = 1000000d, MinimumTurningRadiusMeters = 0.000001d,
|
|
};
|
|
|
|
private static PlanningGridMap CreateMap(bool useNearObstacle)
|
|
{
|
|
var request = new PlanningMapRequest
|
|
{
|
|
Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
|
|
ObstacleSources = new IMapObstacleSource[]
|
|
{
|
|
new ManualObstacleSource("local-g2-evaluator", 1, true, new IMapObstacle[]
|
|
{
|
|
useNearObstacle
|
|
? new AxisAlignedRectangleObstacle(100f, 150f, 900f, 1100f)
|
|
: new AxisAlignedRectangleObstacle(3000f, 3100f, 3000f, 3100f),
|
|
}),
|
|
},
|
|
};
|
|
PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
|
|
if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试地图创建失败。");
|
|
return result.Map;
|
|
}
|
|
|
|
private static PlanningGridMap CreateEmptyMap()
|
|
{
|
|
var request = new PlanningMapRequest
|
|
{
|
|
Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
|
|
ObstacleSources = Array.Empty<IMapObstacleSource>(), AllowExplicitEmptyMap = true,
|
|
};
|
|
PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
|
|
if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试空地图创建失败。");
|
|
return result.Map;
|
|
}
|
|
|
|
private static SmoothingPoint2D Point(double x, double y) =>
|
|
new SmoothingPoint2D(x, y, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.LocalG2Transition);
|
|
|
|
public sealed class EvaluationTestSnapshot
|
|
{
|
|
internal EvaluationTestSnapshot(string status, string failureReason, int candidateIndex, double minimumClearanceMeters, string reason)
|
|
{
|
|
Status = status;
|
|
FailureReason = failureReason;
|
|
CandidateIndex = candidateIndex;
|
|
MinimumClearanceMeters = minimumClearanceMeters;
|
|
Reason = reason;
|
|
}
|
|
public string Status { get; }
|
|
public string FailureReason { get; }
|
|
public int CandidateIndex { get; }
|
|
public double MinimumClearanceMeters { get; }
|
|
public string Reason { get; }
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>不可变的局部 G2 候选质量与安全评价结果。</summary>
|
|
internal sealed class LocalG2CandidateEvaluation
|
|
{
|
|
internal LocalG2CandidateEvaluation(bool accepted, PathSmoothingRegionFailureReason failureReason, int candidateIndex,
|
|
PreparedPath splicedPreparedPath, IReadOnlyList<SmoothedPathPoint> safePath, IReadOnlyList<SmoothedPathSegment> safeSegments,
|
|
double rawPeakCurvatureDerivativePerSquareMeter, double resultPeakCurvatureDerivativePerSquareMeter,
|
|
double rawCurvatureVariationCost, double resultCurvatureVariationCost, double maximumDeviationMeters,
|
|
double minimumBodyClearanceMeters, double maximumAbsoluteVehicleCurvaturePerMeter,
|
|
double absolutePathLengthChangeMeters, string reason)
|
|
{
|
|
Accepted = accepted;
|
|
FailureReason = failureReason;
|
|
CandidateIndex = candidateIndex;
|
|
SplicedPreparedPath = splicedPreparedPath;
|
|
SafePath = Copy(safePath);
|
|
SafeSegments = Copy(safeSegments);
|
|
RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter;
|
|
ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter;
|
|
RawCurvatureVariationCost = rawCurvatureVariationCost;
|
|
ResultCurvatureVariationCost = resultCurvatureVariationCost;
|
|
MaximumDeviationMeters = maximumDeviationMeters;
|
|
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
|
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
|
AbsolutePathLengthChangeMeters = absolutePathLengthChangeMeters;
|
|
Reason = reason ?? string.Empty;
|
|
}
|
|
|
|
internal bool Accepted { get; }
|
|
internal PathSmoothingRegionFailureReason FailureReason { get; }
|
|
internal int CandidateIndex { get; }
|
|
internal PreparedPath SplicedPreparedPath { get; }
|
|
internal IReadOnlyList<SmoothedPathPoint> SafePath { get; }
|
|
internal IReadOnlyList<SmoothedPathSegment> SafeSegments { get; }
|
|
internal double RawPeakCurvatureDerivativePerSquareMeter { get; }
|
|
internal double ResultPeakCurvatureDerivativePerSquareMeter { get; }
|
|
internal double RawCurvatureVariationCost { get; }
|
|
internal double ResultCurvatureVariationCost { get; }
|
|
internal double MaximumDeviationMeters { get; }
|
|
internal double MinimumBodyClearanceMeters { get; }
|
|
internal double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
|
internal double AbsolutePathLengthChangeMeters { get; }
|
|
internal string Reason { get; }
|
|
|
|
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
|
{
|
|
var copy = new List<T>(source == null ? 0 : source.Count);
|
|
if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
|
return new ReadOnlyCollection<T>(copy);
|
|
}
|
|
}
|