feat: add smoothing algorithm retry runner

This commit is contained in:
梁薄云
2026-07-29 09:37:50 +08:00
parent 69ef08dff5
commit 83940f9749
5 changed files with 619 additions and 0 deletions
@@ -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.'