311 lines
13 KiB
C#
311 lines
13 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
|
|
|
/// <summary>正式单算法路径平滑入口，负责输入校验、有限重试与经过复核的粗路径回退。</summary>
|
|
public sealed class PathSmoothingService
|
|
{
|
|
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
|
|
private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
|
|
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
|
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
|
private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
|
|
private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother();
|
|
private readonly IPathSmoother _quintic = new PiecewiseQuinticSmoother();
|
|
|
|
/// <summary>执行一次经过完整安全复核的单算法平滑。</summary>
|
|
public PathSmoothingResult Smooth(
|
|
PathSmoothingRequest request,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
var stopwatch = Stopwatch.StartNew();
|
|
try
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (!TryValidateRequest(request, out PathSmoothingConfiguration configuration, out string reason))
|
|
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
|
|
|
if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
|
|
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
var input = new SmoothingAlgorithmInput(
|
|
preparedPath,
|
|
request.Map,
|
|
request.Vehicle,
|
|
configuration.MaximumCollisionCheckStepMeters,
|
|
configuration.MinimumClearanceReserveMeters,
|
|
new SmoothingOptionsSnapshot(configuration));
|
|
SmoothingAlgorithmRunner.AlgorithmRunResult runResult = _runner.Run(
|
|
Resolve(configuration.Method), input, configuration, cancellationToken);
|
|
int retryCount = GetRetryCount(runResult.AttemptedStrengths);
|
|
PathSmoothingDiagnostics diagnostics = new PathSmoothingDiagnostics(
|
|
runResult.Metrics,
|
|
stopwatch.Elapsed,
|
|
retryCount,
|
|
runResult.AcceptedStrength,
|
|
runResult.Reason);
|
|
|
|
if (runResult.Status == PathSmoothingStatus.Success)
|
|
{
|
|
return PathSmoothingResult.Success(
|
|
configuration.Method,
|
|
runResult.Path,
|
|
runResult.Segments,
|
|
diagnostics);
|
|
}
|
|
|
|
if (!configuration.AllowFallbackToCoarsePath)
|
|
return PathSmoothingResult.Failure(runResult.Status, diagnostics);
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (!TryCreateVerifiedFallback(
|
|
request,
|
|
configuration,
|
|
cancellationToken,
|
|
out IReadOnlyList<SmoothedPathPoint> fallbackPath,
|
|
out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
|
|
out PathQualityMetrics fallbackMetrics,
|
|
out reason))
|
|
{
|
|
return PathSmoothingResult.Failure(runResult.Status, diagnostics);
|
|
}
|
|
|
|
var fallbackDiagnostics = new PathSmoothingDiagnostics(
|
|
fallbackMetrics,
|
|
stopwatch.Elapsed,
|
|
retryCount,
|
|
runResult.AcceptedStrength,
|
|
runResult.Reason);
|
|
return PathSmoothingResult.Fallback(
|
|
configuration.Method,
|
|
fallbackPath,
|
|
fallbackSegments,
|
|
fallbackDiagnostics);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
return Failure(PathSmoothingStatus.Cancelled, stopwatch, 0, 0d, "路径平滑已取消。");
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
return Failure(PathSmoothingStatus.Failed, stopwatch, 0, 0d, exception.Message);
|
|
}
|
|
}
|
|
|
|
private bool TryCreateVerifiedFallback(
|
|
PathSmoothingRequest request,
|
|
PathSmoothingConfiguration configuration,
|
|
CancellationToken cancellationToken,
|
|
out IReadOnlyList<SmoothedPathPoint> fallbackPath,
|
|
out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
|
|
out PathQualityMetrics fallbackMetrics,
|
|
out string reason)
|
|
{
|
|
fallbackPath = null;
|
|
fallbackSegments = null;
|
|
fallbackMetrics = null;
|
|
reason = string.Empty;
|
|
|
|
// Reprepare from the immutable request instead of reusing the algorithm input: fallback is a
|
|
// separately published output and must repeat the coarse-path contract validation.
|
|
if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false;
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
if (!_analyzer.TryAnalyze(
|
|
ToFallbackSegments(revalidatedPath),
|
|
configuration.OutputSpacingMeters,
|
|
out PathGeometryAnalysis analysis,
|
|
out reason))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
IReadOnlyList<SmoothedPathPoint> fallbackSourcePath = ToFallbackPoints(analysis.Path);
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (!_validator.TryValidate(
|
|
fallbackSourcePath,
|
|
analysis.Segments,
|
|
revalidatedPath,
|
|
request.Map,
|
|
request.Vehicle,
|
|
configuration.MaximumCollisionCheckStepMeters,
|
|
out IReadOnlyList<SmoothedPathPoint> safePath,
|
|
out double minimumClearanceMeters,
|
|
out reason))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
fallbackPath = safePath;
|
|
fallbackSegments = analysis.Segments;
|
|
fallbackMetrics = CreateMetrics(analysis, minimumClearanceMeters);
|
|
return true;
|
|
}
|
|
|
|
private IPathSmoother Resolve(SmoothingMethod method)
|
|
{
|
|
return method switch
|
|
{
|
|
SmoothingMethod.CubicBSpline => _bSpline,
|
|
SmoothingMethod.LocalCubicBezier => _bezier,
|
|
SmoothingMethod.PiecewiseQuintic => _quintic,
|
|
_ => throw new ArgumentOutOfRangeException(nameof(method)),
|
|
};
|
|
}
|
|
|
|
private static bool TryValidateRequest(
|
|
PathSmoothingRequest request,
|
|
out PathSmoothingConfiguration configuration,
|
|
out string reason)
|
|
{
|
|
configuration = null;
|
|
reason = string.Empty;
|
|
if (request == null)
|
|
{
|
|
reason = "平滑请求为空。";
|
|
return false;
|
|
}
|
|
|
|
configuration = request.Configuration;
|
|
VehicleParameters vehicle = request.Vehicle;
|
|
if (configuration == null || request.Map == null || !request.Map.PlanningReady || vehicle == null)
|
|
{
|
|
reason = "平滑请求缺少可用的地图、车辆或配置。";
|
|
return false;
|
|
}
|
|
|
|
if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
|
|
!NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
|
|
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
|
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _))
|
|
{
|
|
reason = "平滑请求中的车辆几何或曲率约束无效。";
|
|
return false;
|
|
}
|
|
|
|
if (!Enum.IsDefined(typeof(SmoothingMethod), configuration.Method))
|
|
{
|
|
reason = "平滑方法无效。";
|
|
return false;
|
|
}
|
|
|
|
if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters) ||
|
|
!NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
|
|
!NumericGuard.IsFinite(configuration.MinimumClearanceReserveMeters) ||
|
|
configuration.MinimumClearanceReserveMeters < 0d ||
|
|
!NumericGuard.IsPositiveFinite(configuration.SmoothingStrength) ||
|
|
!NumericGuard.IsPositiveFinite(configuration.CubicBSpline.EndpointTangentScale) ||
|
|
!IsValidBezierThreshold(configuration.LocalCubicBezier.CornerHeadingThresholdRadians) ||
|
|
!NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.MaximumWindowLengthMeters) ||
|
|
!NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.HandleLengthRatio) ||
|
|
!NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.KnotSpacingMeters) ||
|
|
!NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.MinimumKnotSpacingMeters) ||
|
|
configuration.PiecewiseQuintic.KnotSpacingMeters < configuration.PiecewiseQuintic.MinimumKnotSpacingMeters)
|
|
{
|
|
reason = "平滑配置包含非法数值或不满足方法契约。";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool IsValidBezierThreshold(double thresholdRadians)
|
|
{
|
|
return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI;
|
|
}
|
|
|
|
private static IReadOnlyList<PreparedDirectionSegment> ToFallbackSegments(PreparedPath path)
|
|
{
|
|
var segments = new List<PreparedDirectionSegment>(path.Segments.Count);
|
|
for (int segmentIndex = 0; segmentIndex < path.Segments.Count; segmentIndex++)
|
|
{
|
|
PreparedDirectionSegment segment = path.Segments[segmentIndex];
|
|
var points = new List<SmoothingPoint2D>(segment.Points.Count);
|
|
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
|
{
|
|
SmoothingPoint2D point = segment.Points[pointIndex];
|
|
points.Add(new SmoothingPoint2D(
|
|
point.X,
|
|
point.Y,
|
|
point.ArcLength,
|
|
point.Heading,
|
|
point.UnwrappedHeading,
|
|
point.BodyClearance,
|
|
point.IsGearSwitchPoint,
|
|
SmoothedPathPointSource.CoarsePathFallback));
|
|
}
|
|
segments.Add(new PreparedDirectionSegment(
|
|
segment.SegmentIndex,
|
|
segment.Direction,
|
|
points,
|
|
segment.StartsAtGearSwitch,
|
|
segment.EndsAtGearSwitch));
|
|
}
|
|
return segments;
|
|
}
|
|
|
|
private static IReadOnlyList<SmoothedPathPoint> ToFallbackPoints(IReadOnlyList<SmoothedPathPoint> path)
|
|
{
|
|
var points = new List<SmoothedPathPoint>(path.Count);
|
|
for (int index = 0; index < path.Count; index++)
|
|
{
|
|
SmoothedPathPoint point = path[index];
|
|
points.Add(new SmoothedPathPoint(
|
|
point.X,
|
|
point.Y,
|
|
point.Heading,
|
|
point.UnwrappedHeading,
|
|
point.ArcLength,
|
|
point.Direction,
|
|
point.GeometricCurvature,
|
|
point.VehicleCurvature,
|
|
point.BodyClearance,
|
|
point.IsGearSwitchPoint,
|
|
SmoothedPathPointSource.CoarsePathFallback));
|
|
}
|
|
return points;
|
|
}
|
|
|
|
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
|
|
{
|
|
return new PathQualityMetrics(
|
|
true,
|
|
analysis.PathLengthMeters,
|
|
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
|
analysis.RootMeanSquareVehicleCurvaturePerMeter,
|
|
analysis.TotalAbsoluteCurvatureVariationPerMeter,
|
|
analysis.CurvatureVariationEnergy,
|
|
minimumClearanceMeters,
|
|
0d,
|
|
0d,
|
|
0d,
|
|
0d);
|
|
}
|
|
|
|
private static int GetRetryCount(IReadOnlyList<double> attemptedStrengths)
|
|
{
|
|
return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1;
|
|
}
|
|
|
|
private static PathSmoothingResult Failure(
|
|
PathSmoothingStatus status,
|
|
Stopwatch stopwatch,
|
|
int retryCount,
|
|
double acceptedStrength,
|
|
string reason)
|
|
{
|
|
return PathSmoothingResult.Failure(
|
|
status,
|
|
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, retryCount, acceptedStrength, reason));
|
|
}
|
|
}
|