447 lines
18 KiB
C#
447 lines
18 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
|
|
|
/// <summary>在有限强度计划内运行单一算法，并以共享分析和安全复核决定是否接受候选。</summary>
|
|
internal sealed class SmoothingAlgorithmRunner
|
|
{
|
|
private readonly PathGeometryAnalyzer _analyzer;
|
|
private readonly SmoothedPathValidator _validator;
|
|
|
|
internal SmoothingAlgorithmRunner()
|
|
: this(new PathGeometryAnalyzer(), new SmoothedPathValidator())
|
|
{
|
|
}
|
|
|
|
internal SmoothingAlgorithmRunner(PathGeometryAnalyzer analyzer, SmoothedPathValidator validator)
|
|
{
|
|
_analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
|
|
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 依次尝试配置的有限强度比例。取消直接向上传播，由门面转换为最终状态；
|
|
/// 数值失败和几何分析失败均不重试，以避免以较低强度掩盖算法退化。
|
|
/// </summary>
|
|
internal AlgorithmRunResult Run(
|
|
IPathSmoother smoother,
|
|
SmoothingAlgorithmInput input,
|
|
PathSmoothingConfiguration configuration,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
var attemptedStrengths = new List<double>();
|
|
var failureReasons = new List<string>();
|
|
if (smoother == null || input == null || configuration == null)
|
|
return AlgorithmRunResult.Failed("平滑算法、输入或配置无效。", attemptedStrengths, failureReasons);
|
|
if (configuration.RetryStrengthScales == null || configuration.RetryStrengthScales.Count == 0)
|
|
return AlgorithmRunResult.Failed("平滑强度重试计划为空。", attemptedStrengths, failureReasons);
|
|
|
|
SmoothingCandidate rejectedCandidate = null;
|
|
foreach (double scale in configuration.RetryStrengthScales)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
double effectiveStrength = configuration.SmoothingStrength * scale;
|
|
if (!IsPositiveFinite(effectiveStrength))
|
|
return AlgorithmRunResult.Failed("平滑强度或重试比例无效。", attemptedStrengths, failureReasons);
|
|
|
|
attemptedStrengths.Add(effectiveStrength);
|
|
SmoothingCandidate candidate = smoother.Smooth(input, effectiveStrength, cancellationToken);
|
|
if (candidate == null)
|
|
return AlgorithmRunResult.Failed("平滑算法未返回候选。", attemptedStrengths, failureReasons);
|
|
if (!candidate.Succeeded)
|
|
{
|
|
failureReasons.Add(candidate.Reason);
|
|
return AlgorithmRunResult.Failed(candidate.Reason, attemptedStrengths, failureReasons);
|
|
}
|
|
|
|
if (!_analyzer.TryAnalyze(candidate.Segments, configuration.OutputSpacingMeters,
|
|
out PathGeometryAnalysis analysis, out string reason))
|
|
{
|
|
failureReasons.Add(reason);
|
|
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
|
|
}
|
|
|
|
if (_validator.TryValidate(analysis.Path, analysis.Segments, input.OriginalPath, input.Map, input.Vehicle,
|
|
input.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
|
out double minimumClearanceMeters, out reason))
|
|
{
|
|
return AlgorithmRunResult.Success(safePath, analysis.Segments,
|
|
CreateMetrics(analysis, minimumClearanceMeters), effectiveStrength,
|
|
attemptedStrengths, failureReasons);
|
|
}
|
|
|
|
rejectedCandidate = candidate;
|
|
failureReasons.Add(reason);
|
|
}
|
|
|
|
return AlgorithmRunResult.Infeasible(rejectedCandidate, attemptedStrengths, failureReasons);
|
|
}
|
|
|
|
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 bool IsPositiveFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reflection-only deterministic coverage seam. It is nested in an internal runner and intentionally
|
|
/// does not construct or register a production smoothing method.
|
|
/// </summary>
|
|
public static class TestHooks
|
|
{
|
|
/// <summary>执行一个固定的内部假平滑器场景并返回可反射读取的快照。</summary>
|
|
public static RunnerTestSnapshot Execute(string scenario)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
|
|
|
|
var cancellationSource = new CancellationTokenSource();
|
|
var smoother = new DeterministicTestSmoother(ParseScenario(scenario), cancellationSource);
|
|
var runner = new SmoothingAlgorithmRunner();
|
|
SmoothingAlgorithmInput input = CreateTestInput();
|
|
var configuration = new PathSmoothingConfiguration();
|
|
try
|
|
{
|
|
AlgorithmRunResult result = runner.Run(smoother, input, configuration, cancellationSource.Token);
|
|
return RunnerTestSnapshot.FromResult(result, false);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
return new RunnerTestSnapshot(
|
|
"OperationCanceledException",
|
|
smoother.AttemptedStrengths,
|
|
0,
|
|
0,
|
|
0,
|
|
true);
|
|
}
|
|
finally
|
|
{
|
|
cancellationSource.Dispose();
|
|
}
|
|
}
|
|
|
|
/// <summary>供 PowerShell 断言使用的不可变执行摘要。</summary>
|
|
public sealed class RunnerTestSnapshot
|
|
{
|
|
internal RunnerTestSnapshot(
|
|
string status,
|
|
IReadOnlyList<double> attemptedStrengths,
|
|
int acceptedPathPointCount,
|
|
int rejectedComparisonCandidatePointCount,
|
|
int failureCount,
|
|
bool cancellationPropagated)
|
|
{
|
|
Status = status ?? string.Empty;
|
|
AttemptedStrengths = CopyReadOnly(attemptedStrengths);
|
|
AcceptedPathPointCount = acceptedPathPointCount;
|
|
RejectedComparisonCandidatePointCount = rejectedComparisonCandidatePointCount;
|
|
FailureCount = failureCount;
|
|
CancellationPropagated = cancellationPropagated;
|
|
}
|
|
|
|
public string Status { get; }
|
|
public IReadOnlyList<double> AttemptedStrengths { get; }
|
|
public int AcceptedPathPointCount { get; }
|
|
public int RejectedComparisonCandidatePointCount { get; }
|
|
public int FailureCount { get; }
|
|
public bool CancellationPropagated { get; }
|
|
|
|
internal static RunnerTestSnapshot FromResult(AlgorithmRunResult result, bool cancellationPropagated)
|
|
{
|
|
int rejectedPointCount = result.RejectedComparisonCandidate == null
|
|
? 0
|
|
: CountPoints(result.RejectedComparisonCandidate.Segments);
|
|
return new RunnerTestSnapshot(
|
|
result.Status.ToString(),
|
|
result.AttemptedStrengths,
|
|
result.Path.Count,
|
|
rejectedPointCount,
|
|
result.FailureReasons.Count,
|
|
cancellationPropagated);
|
|
}
|
|
|
|
private static IReadOnlyList<T> CopyReadOnly<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);
|
|
}
|
|
}
|
|
|
|
private enum TestScenario
|
|
{
|
|
RejectAll,
|
|
AcceptFirst,
|
|
NumericalFailure,
|
|
CancelBeforeNextAttempt,
|
|
}
|
|
|
|
private sealed class DeterministicTestSmoother : IPathSmoother
|
|
{
|
|
private readonly TestScenario _scenario;
|
|
private readonly CancellationTokenSource _cancellationSource;
|
|
|
|
internal DeterministicTestSmoother(TestScenario scenario, CancellationTokenSource cancellationSource)
|
|
{
|
|
_scenario = scenario;
|
|
_cancellationSource = cancellationSource;
|
|
AttemptedStrengths = new List<double>();
|
|
}
|
|
|
|
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
|
|
|
internal List<double> AttemptedStrengths { get; }
|
|
|
|
public SmoothingCandidate Smooth(
|
|
SmoothingAlgorithmInput input,
|
|
double effectiveStrength,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
AttemptedStrengths.Add(effectiveStrength);
|
|
if (_scenario == TestScenario.NumericalFailure)
|
|
return SmoothingCandidate.Failed("确定性数值退化。");
|
|
if (_scenario == TestScenario.AcceptFirst)
|
|
return CreateAcceptedCandidate();
|
|
|
|
SmoothingCandidate rejected = CreateRejectedCandidate();
|
|
if (_scenario == TestScenario.CancelBeforeNextAttempt)
|
|
_cancellationSource.Cancel();
|
|
return rejected;
|
|
}
|
|
}
|
|
|
|
private static TestScenario ParseScenario(string scenario)
|
|
{
|
|
if (string.Equals(scenario, nameof(TestScenario.RejectAll), StringComparison.Ordinal)) return TestScenario.RejectAll;
|
|
if (string.Equals(scenario, nameof(TestScenario.AcceptFirst), StringComparison.Ordinal)) return TestScenario.AcceptFirst;
|
|
if (string.Equals(scenario, nameof(TestScenario.NumericalFailure), StringComparison.Ordinal)) return TestScenario.NumericalFailure;
|
|
if (string.Equals(scenario, nameof(TestScenario.CancelBeforeNextAttempt), StringComparison.Ordinal)) return TestScenario.CancelBeforeNextAttempt;
|
|
throw new ArgumentOutOfRangeException(nameof(scenario));
|
|
}
|
|
|
|
private static SmoothingAlgorithmInput CreateTestInput()
|
|
{
|
|
var mapRequest = new PlanningMapRequest
|
|
{
|
|
Bounds = new MapBoundsMm(0f, 5000f, 0f, 5000f),
|
|
ResolutionMm = 50f,
|
|
AllowExplicitEmptyMap = true,
|
|
};
|
|
PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(mapRequest);
|
|
if (!mapResult.Succeeded || mapResult.Map == null)
|
|
throw new InvalidOperationException("The runner test hook could not create its empty map.");
|
|
|
|
var originalSegments = new List<PreparedDirectionSegment>
|
|
{
|
|
new PreparedDirectionSegment(
|
|
0,
|
|
TravelDirection.Forward,
|
|
new List<SmoothingPoint2D>
|
|
{
|
|
CreatePoint(0.5d, 0.5d, 0d),
|
|
CreatePoint(1.5d, 0.5d, 1d),
|
|
},
|
|
false,
|
|
false),
|
|
};
|
|
var vehicle = new VehicleParameters
|
|
{
|
|
LengthMeters = 0.20d,
|
|
WidthMeters = 0.20d,
|
|
SafetyMarginMeters = 0d,
|
|
MaximumCurvaturePerMeter = 100d,
|
|
MinimumTurningRadiusMeters = 0.01d,
|
|
};
|
|
return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d, 0.02d);
|
|
}
|
|
|
|
private static SmoothingCandidate CreateAcceptedCandidate()
|
|
{
|
|
return SmoothingCandidate.Success(new List<PreparedDirectionSegment>
|
|
{
|
|
new PreparedDirectionSegment(
|
|
0,
|
|
TravelDirection.Forward,
|
|
new List<SmoothingPoint2D>
|
|
{
|
|
CreatePoint(0.5d, 0.5d, 0d),
|
|
CreatePoint(1.5d, 0.5d, 1d),
|
|
},
|
|
false,
|
|
false),
|
|
});
|
|
}
|
|
|
|
private static SmoothingCandidate CreateRejectedCandidate()
|
|
{
|
|
return SmoothingCandidate.Success(new List<PreparedDirectionSegment>
|
|
{
|
|
new PreparedDirectionSegment(
|
|
0,
|
|
TravelDirection.Forward,
|
|
new List<SmoothingPoint2D>
|
|
{
|
|
CreatePoint(0.5d, 0.5d, 0d),
|
|
CreatePoint(0.05d, 0.5d, 0.45d),
|
|
CreatePoint(1.5d, 0.5d, 1.90d),
|
|
},
|
|
false,
|
|
false),
|
|
});
|
|
}
|
|
|
|
private static SmoothingPoint2D CreatePoint(double x, double y, double arcLength)
|
|
{
|
|
return new SmoothingPoint2D(
|
|
x,
|
|
y,
|
|
arcLength,
|
|
0d,
|
|
0d,
|
|
1d,
|
|
false,
|
|
SmoothedPathPointSource.Anchor);
|
|
}
|
|
|
|
private static int CountPoints(IReadOnlyList<PreparedDirectionSegment> segments)
|
|
{
|
|
int count = 0;
|
|
for (int index = 0; index < segments.Count; index++) count += segments[index].Points.Count;
|
|
return count;
|
|
}
|
|
}
|
|
|
|
/// <summary>内部运行结果；只有成功路径可被正式门面发布，拒绝候选仅供比较诊断读取。</summary>
|
|
internal sealed class AlgorithmRunResult
|
|
{
|
|
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 AlgorithmRunResult(
|
|
PathSmoothingStatus status,
|
|
IReadOnlyList<SmoothedPathPoint> path,
|
|
IReadOnlyList<SmoothedPathSegment> segments,
|
|
PathQualityMetrics metrics,
|
|
double acceptedStrength,
|
|
SmoothingCandidate rejectedComparisonCandidate,
|
|
IReadOnlyList<double> attemptedStrengths,
|
|
IReadOnlyList<string> failureReasons,
|
|
string reason)
|
|
{
|
|
Status = status;
|
|
Path = path ?? EmptyPath;
|
|
Segments = segments ?? EmptySegments;
|
|
Metrics = metrics ?? new PathQualityMetrics();
|
|
AcceptedStrength = acceptedStrength;
|
|
RejectedComparisonCandidate = rejectedComparisonCandidate;
|
|
AttemptedStrengths = CopyReadOnly(attemptedStrengths);
|
|
FailureReasons = CopyReadOnly(failureReasons);
|
|
Reason = reason ?? string.Empty;
|
|
}
|
|
|
|
internal PathSmoothingStatus Status { get; }
|
|
internal IReadOnlyList<SmoothedPathPoint> Path { get; }
|
|
internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
|
internal PathQualityMetrics Metrics { get; }
|
|
internal double AcceptedStrength { get; }
|
|
internal SmoothingCandidate RejectedComparisonCandidate { get; }
|
|
internal IReadOnlyList<double> AttemptedStrengths { get; }
|
|
internal IReadOnlyList<string> FailureReasons { get; }
|
|
internal string Reason { get; }
|
|
|
|
internal static AlgorithmRunResult Success(
|
|
IReadOnlyList<SmoothedPathPoint> path,
|
|
IReadOnlyList<SmoothedPathSegment> segments,
|
|
PathQualityMetrics metrics,
|
|
double acceptedStrength,
|
|
IReadOnlyList<double> attemptedStrengths,
|
|
IReadOnlyList<string> failureReasons)
|
|
{
|
|
return new AlgorithmRunResult(
|
|
PathSmoothingStatus.Success,
|
|
CopyReadOnly(path),
|
|
CopyReadOnly(segments),
|
|
metrics,
|
|
acceptedStrength,
|
|
null,
|
|
attemptedStrengths,
|
|
failureReasons,
|
|
string.Empty);
|
|
}
|
|
|
|
internal static AlgorithmRunResult Infeasible(
|
|
SmoothingCandidate rejectedComparisonCandidate,
|
|
IReadOnlyList<double> attemptedStrengths,
|
|
IReadOnlyList<string> failureReasons)
|
|
{
|
|
string reason = failureReasons == null || failureReasons.Count == 0
|
|
? "所有有限平滑尝试均未通过复核。"
|
|
: failureReasons[failureReasons.Count - 1];
|
|
return new AlgorithmRunResult(
|
|
PathSmoothingStatus.Infeasible,
|
|
null,
|
|
null,
|
|
null,
|
|
0d,
|
|
rejectedComparisonCandidate,
|
|
attemptedStrengths,
|
|
failureReasons,
|
|
reason);
|
|
}
|
|
|
|
internal static AlgorithmRunResult Failed(
|
|
string reason,
|
|
IReadOnlyList<double> attemptedStrengths,
|
|
IReadOnlyList<string> failureReasons)
|
|
{
|
|
return new AlgorithmRunResult(
|
|
PathSmoothingStatus.Failed,
|
|
null,
|
|
null,
|
|
null,
|
|
0d,
|
|
null,
|
|
attemptedStrengths,
|
|
failureReasons,
|
|
reason);
|
|
}
|
|
|
|
private static IReadOnlyList<T> CopyReadOnly<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);
|
|
}
|
|
}
|
|
}
|