feat: add smoothing algorithm retry runner
This commit is contained in:
@@ -0,0 +1,14 @@
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>单一平滑方法生成原始几何候选的内部契约。</summary>
|
||||
internal interface IPathSmoother
|
||||
{
|
||||
SmoothingMethod Method { get; }
|
||||
|
||||
SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken);
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>单次算法运行共享的已预处理路径和独立复核上下文。</summary>
|
||||
internal sealed class SmoothingAlgorithmInput
|
||||
{
|
||||
internal SmoothingAlgorithmInput(
|
||||
PreparedPath originalPath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
double maximumCollisionCheckStepMeters)
|
||||
{
|
||||
OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath));
|
||||
Map = map ?? throw new ArgumentNullException(nameof(map));
|
||||
Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle));
|
||||
MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters;
|
||||
}
|
||||
|
||||
/// <summary>已校验并按方向分段的原始路径。</summary>
|
||||
internal PreparedPath OriginalPath { get; }
|
||||
|
||||
/// <summary>用于完整车体复核的不可变规划地图。</summary>
|
||||
internal PlanningGridMap Map { get; }
|
||||
|
||||
/// <summary>用于曲率和足迹复核的车辆参数快照。</summary>
|
||||
internal VehicleParameters Vehicle { get; }
|
||||
|
||||
/// <summary>连续车体碰撞检查的最大步长,单位 m。</summary>
|
||||
internal double MaximumCollisionCheckStepMeters { get; }
|
||||
}
|
||||
@@ -0,0 +1,446 @@
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>平滑方法产生的原始方向段候选,尚未经过统一几何或安全复核。</summary>
|
||||
internal sealed class SmoothingCandidate
|
||||
{
|
||||
private SmoothingCandidate(bool succeeded, IReadOnlyList<PreparedDirectionSegment> segments, string reason)
|
||||
{
|
||||
Succeeded = succeeded;
|
||||
Segments = CopyReadOnly(segments);
|
||||
Reason = reason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>候选是否成功产生有限的原始几何。</summary>
|
||||
internal bool Succeeded { get; }
|
||||
|
||||
/// <summary>候选方向段;失败候选始终为空。</summary>
|
||||
internal IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
||||
|
||||
/// <summary>失败或退化时的稳定说明;成功时为空。</summary>
|
||||
internal string Reason { get; }
|
||||
|
||||
/// <summary>创建待统一分析和验证的成功候选。</summary>
|
||||
internal static SmoothingCandidate Success(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("A successful smoothing candidate requires direction segments.", nameof(segments));
|
||||
return new SmoothingCandidate(true, segments, string.Empty);
|
||||
}
|
||||
|
||||
/// <summary>创建不应重试的数值或构造失败候选。</summary>
|
||||
internal static SmoothingCandidate Failed(string reason)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(reason))
|
||||
throw new ArgumentException("A failed smoothing candidate requires a reason.", nameof(reason));
|
||||
return new SmoothingCandidate(false, null, 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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,73 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000000001) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Assert-AttemptedStrengths($Snapshot, [double[]]$Expected, [string]$Message) {
|
||||
Assert-Equal $Expected.Length $Snapshot.AttemptedStrengths.Count ($Message + ' count')
|
||||
for ($index = 0; $index -lt $Expected.Length; $index++) {
|
||||
Assert-Near $Expected[$index] $Snapshot.AttemptedStrengths[$index] ($Message + " index=$index")
|
||||
}
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$runnerType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmRunner')
|
||||
$smootherType = Get-RequiredType ($algorithms + 'IPathSmoother')
|
||||
Assert-False $smootherType.IsPublic 'IPathSmoother must remain internal to the algorithm assembly.'
|
||||
|
||||
$hooksType = $runnerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||
Assert-True ($null -ne $hooksType) 'SmoothingAlgorithmRunner must expose its narrowly scoped nested TestHooks helper.'
|
||||
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
|
||||
Assert-True ($null -ne $executeMethod) 'TestHooks must expose deterministic scenario execution for reflection tests.'
|
||||
|
||||
function Invoke-Scenario([string]$Scenario) {
|
||||
return $executeMethod.Invoke($null, @($Scenario))
|
||||
}
|
||||
|
||||
$allRejected = Invoke-Scenario 'RejectAll'
|
||||
Assert-Equal 'Infeasible' $allRejected.Status 'All rejected finite candidates must produce an Infeasible runner result.'
|
||||
Assert-AttemptedStrengths $allRejected @(1.00, 0.75, 0.50, 0.25) 'Rejected candidates must use the finite retry schedule exactly.'
|
||||
Assert-Equal 0 $allRejected.AcceptedPathPointCount 'An infeasible runner result must not retain a rejected candidate as an accepted path.'
|
||||
Assert-True ($allRejected.RejectedComparisonCandidatePointCount -gt 0) 'Only comparison diagnostics may retain the last rejected candidate geometry.'
|
||||
Assert-Equal 4 $allRejected.FailureCount 'Every rejected validation attempt must retain its failure reason.'
|
||||
|
||||
$accepted = Invoke-Scenario 'AcceptFirst'
|
||||
Assert-Equal 'Success' $accepted.Status 'The first safe candidate must be accepted.'
|
||||
Assert-AttemptedStrengths $accepted @(1.00) 'The runner must stop immediately after the first accepted candidate.'
|
||||
Assert-True ($accepted.AcceptedPathPointCount -gt 0) 'A successful runner result must publish the validated path internally.'
|
||||
Assert-Equal 0 $accepted.RejectedComparisonCandidatePointCount 'An accepted candidate must not create rejected comparison geometry.'
|
||||
|
||||
$numericalFailure = Invoke-Scenario 'NumericalFailure'
|
||||
Assert-Equal 'Failed' $numericalFailure.Status 'A numerical candidate failure must stop the runner as Failed.'
|
||||
Assert-AttemptedStrengths $numericalFailure @(1.00) 'Numerical candidate failure must not retry at lower strength.'
|
||||
Assert-Equal 0 $numericalFailure.RejectedComparisonCandidatePointCount 'A non-candidate numerical failure must not retain comparison geometry.'
|
||||
|
||||
$cancelled = Invoke-Scenario 'CancelBeforeNextAttempt'
|
||||
Assert-True $cancelled.CancellationPropagated 'Cancellation between attempts must propagate out of the runner.'
|
||||
Assert-AttemptedStrengths $cancelled @(1.00) 'Cancellation before the next attempt must prevent another smoother call.'
|
||||
Assert-Equal 0 $cancelled.AcceptedPathPointCount 'A cancelled run must not publish a partial path.'
|
||||
|
||||
Write-Output 'Path smoothing retry runner checks passed.'
|
||||
Reference in New Issue
Block a user