fix: align smoothing feasibility and option flow
This commit is contained in:
@@ -13,7 +13,6 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
private const int Degree = 3;
|
private const int Degree = 3;
|
||||||
private const int SamplesPerSpan = 64;
|
private const int SamplesPerSpan = 64;
|
||||||
private const double StraightToleranceMeters = 1e-9d;
|
private const double StraightToleranceMeters = 1e-9d;
|
||||||
private const double EndpointTangentScale = 1d / 3d;
|
|
||||||
private const double EndpointProbeParameter = 1e-6d;
|
private const double EndpointProbeParameter = 1e-6d;
|
||||||
private const double MinimumTangentHandleLengthMeters = 1e-10d;
|
private const double MinimumTangentHandleLengthMeters = 1e-10d;
|
||||||
|
|
||||||
@@ -27,6 +26,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
CancellationToken cancellationToken)
|
CancellationToken cancellationToken)
|
||||||
{
|
{
|
||||||
if (input == null || input.OriginalPath == null ||
|
if (input == null || input.OriginalPath == null ||
|
||||||
|
input.Options == null ||
|
||||||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
||||||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
||||||
input.MinimumClearanceReserveMeters < 0d)
|
input.MinimumClearanceReserveMeters < 0d)
|
||||||
@@ -39,10 +39,19 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
{
|
{
|
||||||
cancellationToken.ThrowIfCancellationRequested();
|
cancellationToken.ThrowIfCancellationRequested();
|
||||||
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
||||||
if (!TrySmoothSegment(sourceSegment, effectiveStrength, input.MinimumClearanceReserveMeters,
|
if (!TrySmoothSegment(
|
||||||
cancellationToken, out IReadOnlyList<SmoothingPoint2D> points, out string reason))
|
sourceSegment,
|
||||||
|
effectiveStrength,
|
||||||
|
input.MinimumClearanceReserveMeters,
|
||||||
|
input.Options.CubicBSplineEndpointTangentScale,
|
||||||
|
cancellationToken,
|
||||||
|
out IReadOnlyList<SmoothingPoint2D> points,
|
||||||
|
out string reason,
|
||||||
|
out SmoothingCandidateStatus status))
|
||||||
{
|
{
|
||||||
return SmoothingCandidate.Failed(reason);
|
return status == SmoothingCandidateStatus.RetryableInfeasible
|
||||||
|
? SmoothingCandidate.RetryableInfeasible(reason)
|
||||||
|
: SmoothingCandidate.Failed(reason);
|
||||||
}
|
}
|
||||||
|
|
||||||
candidateSegments.Add(new PreparedDirectionSegment(
|
candidateSegments.Add(new PreparedDirectionSegment(
|
||||||
@@ -60,12 +69,15 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
PreparedDirectionSegment sourceSegment,
|
PreparedDirectionSegment sourceSegment,
|
||||||
double strength,
|
double strength,
|
||||||
double reserveMeters,
|
double reserveMeters,
|
||||||
|
double endpointTangentScale,
|
||||||
CancellationToken cancellationToken,
|
CancellationToken cancellationToken,
|
||||||
out IReadOnlyList<SmoothingPoint2D> result,
|
out IReadOnlyList<SmoothingPoint2D> result,
|
||||||
out string reason)
|
out string reason,
|
||||||
|
out SmoothingCandidateStatus status)
|
||||||
{
|
{
|
||||||
result = null;
|
result = null;
|
||||||
reason = string.Empty;
|
reason = string.Empty;
|
||||||
|
status = SmoothingCandidateStatus.Failed;
|
||||||
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
|
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
|
||||||
{
|
{
|
||||||
reason = "B 样条方向段为空。";
|
reason = "B 样条方向段为空。";
|
||||||
@@ -89,7 +101,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
|
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, endpointTangentScale,
|
||||||
cancellationToken, out Point2D[] controls, out reason))
|
cancellationToken, out Point2D[] controls, out reason))
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
@@ -99,23 +111,23 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
var sampled = new List<SmoothingPoint2D>();
|
var sampled = new List<SmoothingPoint2D>();
|
||||||
int spanCount = controls.Length - Degree;
|
int spanCount = controls.Length - Degree;
|
||||||
int uniformIntervals = spanCount * SamplesPerSpan;
|
int uniformIntervals = spanCount * SamplesPerSpan;
|
||||||
if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
|
if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
|
||||||
return false;
|
return false;
|
||||||
if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
||||||
sampled, cancellationToken, out reason))
|
sampled, cancellationToken, out reason, out status))
|
||||||
return false;
|
return false;
|
||||||
for (int index = 1; index < uniformIntervals; index++)
|
for (int index = 1; index < uniformIntervals; index++)
|
||||||
{
|
{
|
||||||
if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
|
if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
|
||||||
sampled, cancellationToken, out reason))
|
sampled, cancellationToken, out reason, out status))
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
||||||
sampled, cancellationToken, out reason))
|
sampled, cancellationToken, out reason, out status))
|
||||||
return false;
|
return false;
|
||||||
if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
|
if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
result = sampled;
|
result = sampled;
|
||||||
@@ -127,6 +139,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
TravelDirection direction,
|
TravelDirection direction,
|
||||||
double strength,
|
double strength,
|
||||||
double reserveMeters,
|
double reserveMeters,
|
||||||
|
double endpointTangentScale,
|
||||||
CancellationToken cancellationToken,
|
CancellationToken cancellationToken,
|
||||||
out Point2D[] controls,
|
out Point2D[] controls,
|
||||||
out string reason)
|
out string reason)
|
||||||
@@ -136,7 +149,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
controls[0] = Point2D.FromAnchor(anchors[0]);
|
controls[0] = Point2D.FromAnchor(anchors[0]);
|
||||||
controls[controls.Length - 1] = Point2D.FromAnchor(anchors[anchors.Count - 1]);
|
controls[controls.Length - 1] = Point2D.FromAnchor(anchors[anchors.Count - 1]);
|
||||||
|
|
||||||
double startHandleLength = Distance(anchors[0], anchors[1]) * EndpointTangentScale * strength;
|
double startHandleLength = Distance(anchors[0], anchors[1]) * endpointTangentScale * strength;
|
||||||
double startTravelHeading = GetTravelHeading(anchors[0], direction);
|
double startTravelHeading = GetTravelHeading(anchors[0], direction);
|
||||||
if (!TryConstrainTangentHandle(
|
if (!TryConstrainTangentHandle(
|
||||||
Point2D.FromAnchor(anchors[0]),
|
Point2D.FromAnchor(anchors[0]),
|
||||||
@@ -152,7 +165,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
}
|
}
|
||||||
|
|
||||||
int finalIndex = anchors.Count - 1;
|
int finalIndex = anchors.Count - 1;
|
||||||
double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * EndpointTangentScale * strength;
|
double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * endpointTangentScale * strength;
|
||||||
double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
|
double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
|
||||||
if (!TryConstrainTangentHandle(
|
if (!TryConstrainTangentHandle(
|
||||||
Point2D.FromAnchor(anchors[finalIndex]),
|
Point2D.FromAnchor(anchors[finalIndex]),
|
||||||
@@ -201,16 +214,35 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
double reserveMeters,
|
double reserveMeters,
|
||||||
List<SmoothingPoint2D> output,
|
List<SmoothingPoint2D> output,
|
||||||
CancellationToken cancellationToken,
|
CancellationToken cancellationToken,
|
||||||
out string reason)
|
out string reason,
|
||||||
|
out SmoothingCandidateStatus status)
|
||||||
{
|
{
|
||||||
reason = string.Empty;
|
reason = string.Empty;
|
||||||
|
status = SmoothingCandidateStatus.Failed;
|
||||||
cancellationToken.ThrowIfCancellationRequested();
|
cancellationToken.ThrowIfCancellationRequested();
|
||||||
Point2D evaluated = Evaluate(controls, knots, parameter);
|
Point2D evaluated = Evaluate(controls, knots, parameter);
|
||||||
SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter);
|
if (!NumericGuard.IsFinite(evaluated.X) || !NumericGuard.IsFinite(evaluated.Y))
|
||||||
|
{
|
||||||
|
reason = "B 样条评估产生非法数值。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
double targetArcLength = parameter * anchors[anchors.Count - 1].ArcLength;
|
||||||
|
if (!PathReferenceInterpolator.TryInterpolateByArcLength(anchors, targetArcLength,
|
||||||
|
out SmoothingPoint2D reference, out reason))
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
double displacement = Distance(evaluated, reference);
|
double displacement = Distance(evaluated, reference);
|
||||||
if (!NumericGuard.IsFinite(displacement) || displacement > GetAllowedRadius(reference, reserveMeters))
|
if (!NumericGuard.IsFinite(displacement))
|
||||||
|
{
|
||||||
|
reason = "B 样条评估点位移产生非法数值。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (displacement > GetAllowedRadius(reference, reserveMeters))
|
||||||
{
|
{
|
||||||
reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
|
reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
|
||||||
|
status = SmoothingCandidateStatus.RetryableInfeasible;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
bool endpoint = parameter == 0d || parameter == 1d;
|
bool endpoint = parameter == 0d || parameter == 1d;
|
||||||
@@ -298,27 +330,6 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
|||||||
return knots;
|
return knots;
|
||||||
}
|
}
|
||||||
|
|
||||||
private static SmoothingPoint2D InterpolateAnchor(IReadOnlyList<SmoothingPoint2D> anchors, double parameter)
|
|
||||||
{
|
|
||||||
if (parameter <= 0d) return anchors[0];
|
|
||||||
if (parameter >= 1d) return anchors[anchors.Count - 1];
|
|
||||||
|
|
||||||
double scaled = parameter * (anchors.Count - 1);
|
|
||||||
int leftIndex = (int)Math.Floor(scaled);
|
|
||||||
double ratio = scaled - leftIndex;
|
|
||||||
SmoothingPoint2D left = anchors[leftIndex];
|
|
||||||
SmoothingPoint2D right = anchors[leftIndex + 1];
|
|
||||||
return new SmoothingPoint2D(
|
|
||||||
left.X + ratio * (right.X - left.X),
|
|
||||||
left.Y + ratio * (right.Y - left.Y),
|
|
||||||
left.ArcLength + ratio * (right.ArcLength - left.ArcLength),
|
|
||||||
left.Heading + ratio * (right.Heading - left.Heading),
|
|
||||||
left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
|
|
||||||
left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
|
|
||||||
false,
|
|
||||||
SmoothedPathPointSource.Interpolated);
|
|
||||||
}
|
|
||||||
|
|
||||||
private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius)
|
private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius)
|
||||||
{
|
{
|
||||||
double deltaX = proposed.X - anchor.X;
|
double deltaX = proposed.X - anchor.X;
|
||||||
|
|||||||
@@ -13,13 +13,15 @@ internal sealed class SmoothingAlgorithmInput
|
|||||||
PlanningGridMap map,
|
PlanningGridMap map,
|
||||||
VehicleParameters vehicle,
|
VehicleParameters vehicle,
|
||||||
double maximumCollisionCheckStepMeters,
|
double maximumCollisionCheckStepMeters,
|
||||||
double minimumClearanceReserveMeters)
|
double minimumClearanceReserveMeters,
|
||||||
|
SmoothingOptionsSnapshot options)
|
||||||
{
|
{
|
||||||
OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath));
|
OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath));
|
||||||
Map = map ?? throw new ArgumentNullException(nameof(map));
|
Map = map ?? throw new ArgumentNullException(nameof(map));
|
||||||
Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle));
|
Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle));
|
||||||
MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters;
|
MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters;
|
||||||
MinimumClearanceReserveMeters = minimumClearanceReserveMeters;
|
MinimumClearanceReserveMeters = minimumClearanceReserveMeters;
|
||||||
|
Options = options ?? throw new ArgumentNullException(nameof(options));
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>已校验并按方向分段的原始路径。</summary>
|
/// <summary>已校验并按方向分段的原始路径。</summary>
|
||||||
@@ -36,4 +38,7 @@ internal sealed class SmoothingAlgorithmInput
|
|||||||
|
|
||||||
/// <summary>候选几何必须从原始保守净空中预留的最小安全余量,单位 m。</summary>
|
/// <summary>候选几何必须从原始保守净空中预留的最小安全余量,单位 m。</summary>
|
||||||
internal double MinimumClearanceReserveMeters { get; }
|
internal double MinimumClearanceReserveMeters { get; }
|
||||||
|
|
||||||
|
/// <summary>本次算法运行使用的已验证方法选项快照。</summary>
|
||||||
|
internal SmoothingOptionsSnapshot Options { get; }
|
||||||
}
|
}
|
||||||
|
|||||||
+31
-35
@@ -12,6 +12,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
|||||||
/// <summary>在有限强度计划内运行单一算法,并以共享分析和安全复核决定是否接受候选。</summary>
|
/// <summary>在有限强度计划内运行单一算法,并以共享分析和安全复核决定是否接受候选。</summary>
|
||||||
internal sealed class SmoothingAlgorithmRunner
|
internal sealed class SmoothingAlgorithmRunner
|
||||||
{
|
{
|
||||||
|
private static readonly double[] RetryStrengthScales = { 1d, 0.75d, 0.50d, 0.25d };
|
||||||
private readonly PathGeometryAnalyzer _analyzer;
|
private readonly PathGeometryAnalyzer _analyzer;
|
||||||
private readonly SmoothedPathValidator _validator;
|
private readonly SmoothedPathValidator _validator;
|
||||||
|
|
||||||
@@ -28,7 +29,7 @@ internal sealed class SmoothingAlgorithmRunner
|
|||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 依次尝试配置的有限强度比例。取消直接向上传播,由门面转换为最终状态;
|
/// 依次尝试配置的有限强度比例。取消直接向上传播,由门面转换为最终状态;
|
||||||
/// 数值失败和几何分析失败均不重试,以避免以较低强度掩盖算法退化。
|
/// 只有算法明确标记为可重试的不可行性才会使用较低强度;终止失败和统一复核失败均不重试。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
internal AlgorithmRunResult Run(
|
internal AlgorithmRunResult Run(
|
||||||
IPathSmoother smoother,
|
IPathSmoother smoother,
|
||||||
@@ -38,13 +39,10 @@ internal sealed class SmoothingAlgorithmRunner
|
|||||||
{
|
{
|
||||||
var attemptedStrengths = new List<double>();
|
var attemptedStrengths = new List<double>();
|
||||||
var failureReasons = new List<string>();
|
var failureReasons = new List<string>();
|
||||||
if (smoother == null || input == null || configuration == null)
|
if (smoother == null || input == null || input.Options == null || configuration == null)
|
||||||
return AlgorithmRunResult.Failed("平滑算法、输入或配置无效。", attemptedStrengths, failureReasons);
|
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 RetryStrengthScales)
|
||||||
foreach (double scale in configuration.RetryStrengthScales)
|
|
||||||
{
|
{
|
||||||
cancellationToken.ThrowIfCancellationRequested();
|
cancellationToken.ThrowIfCancellationRequested();
|
||||||
double effectiveStrength = configuration.SmoothingStrength * scale;
|
double effectiveStrength = configuration.SmoothingStrength * scale;
|
||||||
@@ -55,11 +53,22 @@ internal sealed class SmoothingAlgorithmRunner
|
|||||||
SmoothingCandidate candidate = smoother.Smooth(input, effectiveStrength, cancellationToken);
|
SmoothingCandidate candidate = smoother.Smooth(input, effectiveStrength, cancellationToken);
|
||||||
if (candidate == null)
|
if (candidate == null)
|
||||||
return AlgorithmRunResult.Failed("平滑算法未返回候选。", attemptedStrengths, failureReasons);
|
return AlgorithmRunResult.Failed("平滑算法未返回候选。", attemptedStrengths, failureReasons);
|
||||||
if (!candidate.Succeeded)
|
if (candidate.Status == SmoothingCandidateStatus.RetryableInfeasible)
|
||||||
|
{
|
||||||
|
failureReasons.Add(candidate.Reason);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (candidate.Status == SmoothingCandidateStatus.Failed)
|
||||||
{
|
{
|
||||||
failureReasons.Add(candidate.Reason);
|
failureReasons.Add(candidate.Reason);
|
||||||
return AlgorithmRunResult.Failed(candidate.Reason, attemptedStrengths, failureReasons);
|
return AlgorithmRunResult.Failed(candidate.Reason, attemptedStrengths, failureReasons);
|
||||||
}
|
}
|
||||||
|
if (candidate.Status != SmoothingCandidateStatus.Success)
|
||||||
|
{
|
||||||
|
const string unknownStatusReason = "平滑算法返回未知候选状态。";
|
||||||
|
failureReasons.Add(unknownStatusReason);
|
||||||
|
return AlgorithmRunResult.Failed(unknownStatusReason, attemptedStrengths, failureReasons);
|
||||||
|
}
|
||||||
|
|
||||||
if (!_analyzer.TryAnalyze(candidate.Segments, configuration.OutputSpacingMeters,
|
if (!_analyzer.TryAnalyze(candidate.Segments, configuration.OutputSpacingMeters,
|
||||||
out PathGeometryAnalysis analysis, out string reason))
|
out PathGeometryAnalysis analysis, out string reason))
|
||||||
@@ -77,11 +86,11 @@ internal sealed class SmoothingAlgorithmRunner
|
|||||||
attemptedStrengths, failureReasons);
|
attemptedStrengths, failureReasons);
|
||||||
}
|
}
|
||||||
|
|
||||||
rejectedCandidate = candidate;
|
|
||||||
failureReasons.Add(reason);
|
failureReasons.Add(reason);
|
||||||
|
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
|
||||||
}
|
}
|
||||||
|
|
||||||
return AlgorithmRunResult.Infeasible(rejectedCandidate, attemptedStrengths, failureReasons);
|
return AlgorithmRunResult.Infeasible(null, attemptedStrengths, failureReasons);
|
||||||
}
|
}
|
||||||
|
|
||||||
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
|
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
|
||||||
@@ -195,9 +204,9 @@ internal sealed class SmoothingAlgorithmRunner
|
|||||||
|
|
||||||
private enum TestScenario
|
private enum TestScenario
|
||||||
{
|
{
|
||||||
RejectAll,
|
RetryableInfeasible,
|
||||||
AcceptFirst,
|
AcceptFirst,
|
||||||
NumericalFailure,
|
TerminalFailed,
|
||||||
CancelBeforeNextAttempt,
|
CancelBeforeNextAttempt,
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -223,23 +232,22 @@ internal sealed class SmoothingAlgorithmRunner
|
|||||||
CancellationToken cancellationToken)
|
CancellationToken cancellationToken)
|
||||||
{
|
{
|
||||||
AttemptedStrengths.Add(effectiveStrength);
|
AttemptedStrengths.Add(effectiveStrength);
|
||||||
if (_scenario == TestScenario.NumericalFailure)
|
if (_scenario == TestScenario.TerminalFailed)
|
||||||
return SmoothingCandidate.Failed("确定性数值退化。");
|
return SmoothingCandidate.Failed("确定性数值退化。");
|
||||||
if (_scenario == TestScenario.AcceptFirst)
|
if (_scenario == TestScenario.AcceptFirst)
|
||||||
return CreateAcceptedCandidate();
|
return CreateAcceptedCandidate();
|
||||||
|
|
||||||
SmoothingCandidate rejected = CreateRejectedCandidate();
|
|
||||||
if (_scenario == TestScenario.CancelBeforeNextAttempt)
|
if (_scenario == TestScenario.CancelBeforeNextAttempt)
|
||||||
_cancellationSource.Cancel();
|
_cancellationSource.Cancel();
|
||||||
return rejected;
|
return SmoothingCandidate.RetryableInfeasible("确定性可重试不可行。" );
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private static TestScenario ParseScenario(string scenario)
|
private static TestScenario ParseScenario(string scenario)
|
||||||
{
|
{
|
||||||
if (string.Equals(scenario, nameof(TestScenario.RejectAll), StringComparison.Ordinal)) return TestScenario.RejectAll;
|
if (string.Equals(scenario, nameof(TestScenario.RetryableInfeasible), StringComparison.Ordinal)) return TestScenario.RetryableInfeasible;
|
||||||
if (string.Equals(scenario, nameof(TestScenario.AcceptFirst), StringComparison.Ordinal)) return TestScenario.AcceptFirst;
|
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.TerminalFailed), StringComparison.Ordinal)) return TestScenario.TerminalFailed;
|
||||||
if (string.Equals(scenario, nameof(TestScenario.CancelBeforeNextAttempt), StringComparison.Ordinal)) return TestScenario.CancelBeforeNextAttempt;
|
if (string.Equals(scenario, nameof(TestScenario.CancelBeforeNextAttempt), StringComparison.Ordinal)) return TestScenario.CancelBeforeNextAttempt;
|
||||||
throw new ArgumentOutOfRangeException(nameof(scenario));
|
throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||||
}
|
}
|
||||||
@@ -277,7 +285,13 @@ internal sealed class SmoothingAlgorithmRunner
|
|||||||
MaximumCurvaturePerMeter = 100d,
|
MaximumCurvaturePerMeter = 100d,
|
||||||
MinimumTurningRadiusMeters = 0.01d,
|
MinimumTurningRadiusMeters = 0.01d,
|
||||||
};
|
};
|
||||||
return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d, 0.02d);
|
return new SmoothingAlgorithmInput(
|
||||||
|
new PreparedPath(originalSegments),
|
||||||
|
mapResult.Map,
|
||||||
|
vehicle,
|
||||||
|
0.05d,
|
||||||
|
0.02d,
|
||||||
|
new SmoothingOptionsSnapshot(new PathSmoothingConfiguration()));
|
||||||
}
|
}
|
||||||
|
|
||||||
private static SmoothingCandidate CreateAcceptedCandidate()
|
private static SmoothingCandidate CreateAcceptedCandidate()
|
||||||
@@ -297,24 +311,6 @@ internal sealed class SmoothingAlgorithmRunner
|
|||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
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)
|
private static SmoothingPoint2D CreatePoint(double x, double y, double arcLength)
|
||||||
{
|
{
|
||||||
return new SmoothingPoint2D(
|
return new SmoothingPoint2D(
|
||||||
|
|||||||
@@ -5,18 +5,47 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|||||||
|
|
||||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||||
|
|
||||||
|
internal enum SmoothingCandidateStatus
|
||||||
|
{
|
||||||
|
Success,
|
||||||
|
RetryableInfeasible,
|
||||||
|
Failed,
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>平滑方法产生的原始方向段候选,尚未经过统一几何或安全复核。</summary>
|
/// <summary>平滑方法产生的原始方向段候选,尚未经过统一几何或安全复核。</summary>
|
||||||
internal sealed class SmoothingCandidate
|
internal sealed class SmoothingCandidate
|
||||||
{
|
{
|
||||||
private SmoothingCandidate(bool succeeded, IReadOnlyList<PreparedDirectionSegment> segments, string reason)
|
private SmoothingCandidate(
|
||||||
|
SmoothingCandidateStatus status,
|
||||||
|
IReadOnlyList<PreparedDirectionSegment> segments,
|
||||||
|
string reason)
|
||||||
{
|
{
|
||||||
Succeeded = succeeded;
|
Status = status;
|
||||||
Segments = CopyReadOnly(segments);
|
if (status == SmoothingCandidateStatus.Success)
|
||||||
Reason = reason ?? string.Empty;
|
{
|
||||||
|
if (segments == null || segments.Count == 0)
|
||||||
|
throw new ArgumentException("A successful smoothing candidate requires direction segments.", nameof(segments));
|
||||||
|
for (int index = 0; index < segments.Count; index++)
|
||||||
|
{
|
||||||
|
if (segments[index] == null || segments[index].Points == null || segments[index].Points.Count == 0)
|
||||||
|
throw new ArgumentException("A successful smoothing candidate requires complete direction segments.", nameof(segments));
|
||||||
|
}
|
||||||
|
Segments = CopyReadOnly(segments);
|
||||||
|
Reason = string.Empty;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (string.IsNullOrWhiteSpace(reason))
|
||||||
|
throw new ArgumentException("An unsuccessful smoothing candidate requires a reason.", nameof(reason));
|
||||||
|
Segments = CopyReadOnly<PreparedDirectionSegment>(null);
|
||||||
|
Reason = reason;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>候选是否成功产生有限的原始几何。</summary>
|
/// <summary>候选是否成功产生有限的原始几何。</summary>
|
||||||
internal bool Succeeded { get; }
|
internal bool Succeeded => Status == SmoothingCandidateStatus.Success;
|
||||||
|
|
||||||
|
/// <summary>候选的可重试性和终止性状态。</summary>
|
||||||
|
internal SmoothingCandidateStatus Status { get; }
|
||||||
|
|
||||||
/// <summary>候选方向段;失败候选始终为空。</summary>
|
/// <summary>候选方向段;失败候选始终为空。</summary>
|
||||||
internal IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
internal IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
||||||
@@ -27,17 +56,19 @@ internal sealed class SmoothingCandidate
|
|||||||
/// <summary>创建待统一分析和验证的成功候选。</summary>
|
/// <summary>创建待统一分析和验证的成功候选。</summary>
|
||||||
internal static SmoothingCandidate Success(IReadOnlyList<PreparedDirectionSegment> segments)
|
internal static SmoothingCandidate Success(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||||
{
|
{
|
||||||
if (segments == null || segments.Count == 0)
|
return new SmoothingCandidate(SmoothingCandidateStatus.Success, segments, string.Empty);
|
||||||
throw new ArgumentException("A successful smoothing candidate requires direction segments.", nameof(segments));
|
}
|
||||||
return new SmoothingCandidate(true, segments, string.Empty);
|
|
||||||
|
/// <summary>创建可由较低平滑强度重新尝试的不可行候选。</summary>
|
||||||
|
internal static SmoothingCandidate RetryableInfeasible(string reason)
|
||||||
|
{
|
||||||
|
return new SmoothingCandidate(SmoothingCandidateStatus.RetryableInfeasible, null, reason);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>创建不应重试的数值或构造失败候选。</summary>
|
/// <summary>创建不应重试的数值或构造失败候选。</summary>
|
||||||
internal static SmoothingCandidate Failed(string reason)
|
internal static SmoothingCandidate Failed(string reason)
|
||||||
{
|
{
|
||||||
if (string.IsNullOrWhiteSpace(reason))
|
return new SmoothingCandidate(SmoothingCandidateStatus.Failed, null, 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)
|
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||||
|
|||||||
@@ -0,0 +1,50 @@
|
|||||||
|
using System;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||||
|
|
||||||
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||||
|
|
||||||
|
/// <summary>供单次平滑算法运行使用的、已校验的不可变方法选项快照。</summary>
|
||||||
|
internal sealed class SmoothingOptionsSnapshot
|
||||||
|
{
|
||||||
|
internal SmoothingOptionsSnapshot(PathSmoothingConfiguration configuration)
|
||||||
|
{
|
||||||
|
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||||
|
|
||||||
|
CubicBSplineEndpointTangentScale = configuration.CubicBSpline.EndpointTangentScale;
|
||||||
|
BezierCornerHeadingThresholdRadians = configuration.LocalCubicBezier.CornerHeadingThresholdRadians;
|
||||||
|
BezierMaximumWindowLengthMeters = configuration.LocalCubicBezier.MaximumWindowLengthMeters;
|
||||||
|
BezierHandleLengthRatio = configuration.LocalCubicBezier.HandleLengthRatio;
|
||||||
|
QuinticKnotSpacingMeters = configuration.PiecewiseQuintic.KnotSpacingMeters;
|
||||||
|
QuinticMinimumKnotSpacingMeters = configuration.PiecewiseQuintic.MinimumKnotSpacingMeters;
|
||||||
|
|
||||||
|
ValidatePositiveFinite(CubicBSplineEndpointTangentScale, nameof(CubicBSplineEndpointTangentScale));
|
||||||
|
if (!NumericGuard.IsFinite(BezierCornerHeadingThresholdRadians) ||
|
||||||
|
BezierCornerHeadingThresholdRadians <= 0d || BezierCornerHeadingThresholdRadians > Math.PI)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(BezierCornerHeadingThresholdRadians));
|
||||||
|
}
|
||||||
|
ValidatePositiveFinite(BezierMaximumWindowLengthMeters, nameof(BezierMaximumWindowLengthMeters));
|
||||||
|
ValidatePositiveFinite(BezierHandleLengthRatio, nameof(BezierHandleLengthRatio));
|
||||||
|
ValidatePositiveFinite(QuinticKnotSpacingMeters, nameof(QuinticKnotSpacingMeters));
|
||||||
|
ValidatePositiveFinite(QuinticMinimumKnotSpacingMeters, nameof(QuinticMinimumKnotSpacingMeters));
|
||||||
|
if (QuinticKnotSpacingMeters < QuinticMinimumKnotSpacingMeters)
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(QuinticKnotSpacingMeters));
|
||||||
|
}
|
||||||
|
|
||||||
|
internal double CubicBSplineEndpointTangentScale { get; }
|
||||||
|
|
||||||
|
internal double BezierCornerHeadingThresholdRadians { get; }
|
||||||
|
|
||||||
|
internal double BezierMaximumWindowLengthMeters { get; }
|
||||||
|
|
||||||
|
internal double BezierHandleLengthRatio { get; }
|
||||||
|
|
||||||
|
internal double QuinticKnotSpacingMeters { get; }
|
||||||
|
|
||||||
|
internal double QuinticMinimumKnotSpacingMeters { get; }
|
||||||
|
|
||||||
|
private static void ValidatePositiveFinite(double value, string name)
|
||||||
|
{
|
||||||
|
if (!NumericGuard.IsPositiveFinite(value)) throw new ArgumentOutOfRangeException(name);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,102 @@
|
|||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||||
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||||
|
|
||||||
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||||
|
|
||||||
|
/// <summary>按方向段局部弧长插值平滑算法的原始路径参考。</summary>
|
||||||
|
internal static class PathReferenceInterpolator
|
||||||
|
{
|
||||||
|
internal static bool TryInterpolateByArcLength(
|
||||||
|
IReadOnlyList<SmoothingPoint2D> points,
|
||||||
|
double targetArcLength,
|
||||||
|
out SmoothingPoint2D reference,
|
||||||
|
out string reason)
|
||||||
|
{
|
||||||
|
reference = null;
|
||||||
|
reason = string.Empty;
|
||||||
|
if (points == null || points.Count == 0 || !NumericGuard.IsFinite(targetArcLength))
|
||||||
|
{
|
||||||
|
reason = "原始路径参考点或目标弧长无效。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int index = 0; index < points.Count; index++)
|
||||||
|
{
|
||||||
|
SmoothingPoint2D point = points[index];
|
||||||
|
if (!IsValid(point) || (index > 0 && point.ArcLength < points[index - 1].ArcLength))
|
||||||
|
{
|
||||||
|
reason = "原始路径参考点包含非法数值或非递增弧长。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
SmoothingPoint2D first = points[0];
|
||||||
|
SmoothingPoint2D last = points[points.Count - 1];
|
||||||
|
if (targetArcLength < first.ArcLength || targetArcLength > last.ArcLength)
|
||||||
|
{
|
||||||
|
reason = "目标弧长不在原始方向段范围内。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (targetArcLength == first.ArcLength)
|
||||||
|
{
|
||||||
|
reference = first;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
if (targetArcLength == last.ArcLength)
|
||||||
|
{
|
||||||
|
reference = last;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int rightIndex = 1; rightIndex < points.Count; rightIndex++)
|
||||||
|
{
|
||||||
|
SmoothingPoint2D left = points[rightIndex - 1];
|
||||||
|
SmoothingPoint2D right = points[rightIndex];
|
||||||
|
if (targetArcLength > right.ArcLength) continue;
|
||||||
|
if (targetArcLength == right.ArcLength)
|
||||||
|
{
|
||||||
|
reference = right;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
double interval = right.ArcLength - left.ArcLength;
|
||||||
|
if (!NumericGuard.IsPositiveFinite(interval))
|
||||||
|
{
|
||||||
|
reason = "原始路径参考点包含无法插值的重复弧长。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
double ratio = (targetArcLength - left.ArcLength) / interval;
|
||||||
|
reference = new SmoothingPoint2D(
|
||||||
|
left.X + ratio * (right.X - left.X),
|
||||||
|
left.Y + ratio * (right.Y - left.Y),
|
||||||
|
targetArcLength,
|
||||||
|
left.Heading + ratio * (right.Heading - left.Heading),
|
||||||
|
left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
|
||||||
|
left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
|
||||||
|
false,
|
||||||
|
SmoothedPathPointSource.Interpolated);
|
||||||
|
if (!IsValid(reference))
|
||||||
|
{
|
||||||
|
reference = null;
|
||||||
|
reason = "原始路径参考插值产生非法数值。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
reason = "原始路径参考无法定位目标弧长。";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static bool IsValid(SmoothingPoint2D point)
|
||||||
|
{
|
||||||
|
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||||
|
NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.Heading) &&
|
||||||
|
NumericGuard.IsFinite(point.UnwrappedHeading) && NumericGuard.IsFinite(point.BodyClearance) &&
|
||||||
|
point.ArcLength >= 0d && point.BodyClearance >= 0d;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,121 @@
|
|||||||
|
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-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||||
|
if ([Math]::Abs($Expected - $Actual) -gt 0.000000001) {
|
||||||
|
throw "$Message Expected=$Expected Actual=$Actual"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||||
|
try {
|
||||||
|
& $Action
|
||||||
|
}
|
||||||
|
catch {
|
||||||
|
return
|
||||||
|
}
|
||||||
|
|
||||||
|
throw $Message
|
||||||
|
}
|
||||||
|
|
||||||
|
function Get-RequiredType([string]$Name) {
|
||||||
|
return $assembly.GetType($Name, $true)
|
||||||
|
}
|
||||||
|
|
||||||
|
function Get-RequiredProperty($Type, [string]$Name) {
|
||||||
|
$property = $Type.GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
||||||
|
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
||||||
|
Assert-True (-not $property.CanWrite) ("Snapshot property must be get-only: " + $Name)
|
||||||
|
return $property
|
||||||
|
}
|
||||||
|
|
||||||
|
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||||
|
$algorithms = $root + 'Algorithms.'
|
||||||
|
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||||
|
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||||
|
|
||||||
|
$snapshotType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
||||||
|
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||||
|
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||||
|
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||||
|
$preparedPathType = Get-RequiredType ($root + 'Processing.PreparedPath')
|
||||||
|
$mapType = Get-RequiredType ($mapping + 'PlanningGridMap')
|
||||||
|
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||||
|
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
||||||
|
$pathSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
||||||
|
|
||||||
|
$snapshotConstructor = $snapshotType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||||
|
@($configurationType), $null)
|
||||||
|
Assert-True ($null -ne $snapshotConstructor) 'SmoothingOptionsSnapshot must be created from PathSmoothingConfiguration.'
|
||||||
|
|
||||||
|
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||||
|
@($preparedPathType, $mapType, $vehicleType, [double], [double], $snapshotType), $null)
|
||||||
|
Assert-True ($null -ne $inputConstructor) 'SmoothingAlgorithmInput must accept the immutable smoothing-options snapshot at its construction boundary.'
|
||||||
|
|
||||||
|
$optionNames = @(
|
||||||
|
'CubicBSplineEndpointTangentScale',
|
||||||
|
'BezierCornerHeadingThresholdRadians',
|
||||||
|
'BezierMaximumWindowLengthMeters',
|
||||||
|
'BezierHandleLengthRatio',
|
||||||
|
'QuinticKnotSpacingMeters',
|
||||||
|
'QuinticMinimumKnotSpacingMeters')
|
||||||
|
$optionProperties = @{}
|
||||||
|
foreach ($optionName in $optionNames) {
|
||||||
|
$optionProperties[$optionName] = Get-RequiredProperty $snapshotType $optionName
|
||||||
|
}
|
||||||
|
|
||||||
|
function New-Configuration {
|
||||||
|
return [Activator]::CreateInstance($configurationType)
|
||||||
|
}
|
||||||
|
|
||||||
|
function New-Snapshot($Configuration) {
|
||||||
|
return $snapshotConstructor.Invoke(@($Configuration))
|
||||||
|
}
|
||||||
|
|
||||||
|
# Request creation takes a configuration copy. Later mutations to either the source configuration
|
||||||
|
# or a configuration copy returned by the request must not alter the algorithm snapshot.
|
||||||
|
$configuration = New-Configuration
|
||||||
|
$configuration.CubicBSpline.EndpointTangentScale = [double]0.20
|
||||||
|
$configuration.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.40
|
||||||
|
$configuration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.80
|
||||||
|
$configuration.LocalCubicBezier.HandleLengthRatio = [double]0.25
|
||||||
|
$configuration.PiecewiseQuintic.KnotSpacingMeters = [double]0.60
|
||||||
|
$configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.15
|
||||||
|
$emptyCoarsePath = [Array]::CreateInstance($coarsePointType, 0)
|
||||||
|
$emptySegments = [Array]::CreateInstance($pathSegmentType, 0)
|
||||||
|
$request = [Activator]::CreateInstance($requestType, @($emptyCoarsePath, $emptySegments, $null, $null, $configuration))
|
||||||
|
$configuration.CubicBSpline.EndpointTangentScale = [double]0.90
|
||||||
|
$requestConfiguration = $request.Configuration
|
||||||
|
Assert-Near 0.20 $requestConfiguration.CubicBSpline.EndpointTangentScale 'Request configuration must remain independent from source-config mutations.'
|
||||||
|
$snapshot = New-Snapshot $requestConfiguration
|
||||||
|
$requestConfiguration.CubicBSpline.EndpointTangentScale = [double]0.70
|
||||||
|
Assert-Near 0.20 $optionProperties['CubicBSplineEndpointTangentScale'].GetValue($snapshot) 'Algorithm options must remain independent from request-configuration mutations.'
|
||||||
|
Assert-Near 0.40 $optionProperties['BezierCornerHeadingThresholdRadians'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||||
|
Assert-Near 0.80 $optionProperties['BezierMaximumWindowLengthMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||||
|
Assert-Near 0.25 $optionProperties['BezierHandleLengthRatio'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||||
|
Assert-Near 0.60 $optionProperties['QuinticKnotSpacingMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||||
|
Assert-Near 0.15 $optionProperties['QuinticMinimumKnotSpacingMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||||
|
|
||||||
|
function Assert-InvalidSnapshot([scriptblock]$Mutate, [string]$Message) {
|
||||||
|
$invalidConfiguration = New-Configuration
|
||||||
|
& $Mutate $invalidConfiguration
|
||||||
|
Assert-Throws { New-Snapshot $invalidConfiguration } $Message
|
||||||
|
}
|
||||||
|
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.CubicBSpline.EndpointTangentScale = [double]::NaN } 'Non-finite B-spline tangent scale must be rejected before retry.'
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.CubicBSpline.EndpointTangentScale = [double]0.0 } 'Non-positive B-spline tangent scale must be rejected before retry.'
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.0 } 'A zero Bézier heading threshold must be rejected before retry.'
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [Math]::PI + 0.0001 } 'A Bézier heading threshold above pi must be rejected before retry.'
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.MaximumWindowLengthMeters = [double]::PositiveInfinity } 'A non-finite Bézier window length must be rejected before retry.'
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.HandleLengthRatio = [double]0.0 } 'A non-positive Bézier handle ratio must be rejected before retry.'
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.0 } 'A non-positive quintic knot spacing must be rejected before retry.'
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]::NaN } 'A non-finite quintic minimum knot spacing must be rejected before retry.'
|
||||||
|
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.05; $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.10 } 'Quintic knot spacing below the configured minimum must be rejected before retry.'
|
||||||
|
|
||||||
|
Write-Output 'Path smoothing algorithm-input checks passed.'
|
||||||
@@ -62,7 +62,10 @@ function New-EmptyMap {
|
|||||||
return $map
|
return $map
|
||||||
}
|
}
|
||||||
|
|
||||||
function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
|
function New-AlgorithmInput(
|
||||||
|
[object[]]$Segments,
|
||||||
|
[double]$ReserveMeters,
|
||||||
|
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
||||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||||
$typedSegments.SetValue($Segments[$index], $index)
|
$typedSegments.SetValue($Segments[$index], $index)
|
||||||
@@ -74,16 +77,27 @@ function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
|
|||||||
$vehicle.SafetyMarginMeters = [double]0.0
|
$vehicle.SafetyMarginMeters = [double]0.0
|
||||||
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
||||||
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
||||||
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters))
|
$configuration = [Activator]::CreateInstance($configurationType)
|
||||||
|
$configuration.CubicBSpline.EndpointTangentScale = $EndpointTangentScale
|
||||||
|
$options = $optionsConstructor.Invoke(@($configuration))
|
||||||
|
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
||||||
}
|
}
|
||||||
|
|
||||||
function Invoke-Candidate([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
|
function Invoke-Candidate(
|
||||||
|
[object[]]$Segments,
|
||||||
|
[double]$ReserveMeters,
|
||||||
|
[double]$Strength = 1.0,
|
||||||
|
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
||||||
return $smoothMethod.Invoke($smoother, @(
|
return $smoothMethod.Invoke($smoother, @(
|
||||||
(New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None))
|
(New-AlgorithmInput $Segments $ReserveMeters $EndpointTangentScale), $Strength, [Threading.CancellationToken]::None))
|
||||||
}
|
}
|
||||||
|
|
||||||
function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
|
function Invoke-Smoothing(
|
||||||
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength
|
[object[]]$Segments,
|
||||||
|
[double]$ReserveMeters,
|
||||||
|
[double]$Strength = 1.0,
|
||||||
|
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
||||||
|
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength $EndpointTangentScale
|
||||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
|
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
|
||||||
return @(Get-PropertyValue $candidate 'Segments')
|
return @(Get-PropertyValue $candidate 'Segments')
|
||||||
}
|
}
|
||||||
@@ -137,6 +151,9 @@ $pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
|||||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||||
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||||
|
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
||||||
|
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
|
||||||
|
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||||
@@ -146,8 +163,12 @@ $mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.Plann
|
|||||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||||
|
|
||||||
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||||
@($preparedPathType, $mapType, $vehicleType, [double], [double]), $null)
|
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
|
||||||
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry the minimum clearance reserve for per-anchor movement limits.'
|
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable smoothing options and the minimum clearance reserve for per-anchor movement limits.'
|
||||||
|
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
||||||
|
Assert-True ($null -ne $optionsConstructor) 'B-spline tests must create an immutable options snapshot.'
|
||||||
|
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
|
||||||
|
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose arc-length interpolation.'
|
||||||
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
||||||
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
||||||
Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.'
|
Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.'
|
||||||
@@ -157,6 +178,20 @@ $forward = [Enum]::Parse($directionType, 'Forward')
|
|||||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||||
|
|
||||||
|
# Arc-length interpolation must bracket by local arc rather than by sample index.
|
||||||
|
$nonUniformPoints = [Array]::CreateInstance($pointType, 4)
|
||||||
|
$nonUniformPoints.SetValue((New-Point 0.0 0.0 0.000 0.0 1.0), 0)
|
||||||
|
$nonUniformPoints.SetValue((New-Point 5.0 0.0 0.050 0.1 0.9), 1)
|
||||||
|
$nonUniformPoints.SetValue((New-Point 10.0 0.0 0.100 0.2 0.8), 2)
|
||||||
|
$nonUniformPoints.SetValue((New-Point 20.0 0.0 0.125 0.3 0.7), 3)
|
||||||
|
$interpolateArguments = [object[]]@($nonUniformPoints, [double]0.1125, $null, $null)
|
||||||
|
Assert-True $interpolateMethod.Invoke($null, $interpolateArguments) 'Arc-length interpolation must accept a target within the final non-uniform interval.'
|
||||||
|
$arcReference = $interpolateArguments[2]
|
||||||
|
Assert-Near 15.0 $arcReference.X 0.000000001 'Target arc length 0.1125 must lie halfway through the final 0.100-0.125 interval, independent of point count.'
|
||||||
|
Assert-Near 0.1125 $arcReference.ArcLength 0.000000001 'Arc-length interpolation must preserve the requested target arc length.'
|
||||||
|
Assert-Near 0.25 $arcReference.Heading 0.000000001 'Arc-length interpolation must linearly interpolate heading.'
|
||||||
|
Assert-Near 0.75 $arcReference.BodyClearance 0.000000001 'Arc-length interpolation must linearly interpolate clearance.'
|
||||||
|
|
||||||
# Straight samples are returned exactly, so a straight is never distorted or densified.
|
# Straight samples are returned exactly, so a straight is never distorted or densified.
|
||||||
$straightSource = @(
|
$straightSource = @(
|
||||||
(New-Point 0.0 0.0 0.0 0.0 0.03),
|
(New-Point 0.0 0.0 0.0 0.0 0.03),
|
||||||
@@ -196,6 +231,11 @@ for ($index = 2; $index -lt $cornerPoints.Count; $index++) {
|
|||||||
Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
|
Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
|
||||||
}
|
}
|
||||||
|
|
||||||
|
# Endpoint tangent scale is an immutable option and must influence the clamped B-spline endpoint handle.
|
||||||
|
$shortHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.20)[0].Points
|
||||||
|
$longHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.60)[0].Points
|
||||||
|
Assert-True (($longHandlePoints[2].X - $shortHandlePoints[2].X) -gt 0.0001) 'A custom endpoint tangent scale must change the B-spline start handle and early curve samples.'
|
||||||
|
|
||||||
# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
|
# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
|
||||||
$allowedRadius = 0.06
|
$allowedRadius = 0.06
|
||||||
foreach ($point in $cornerPoints) {
|
foreach ($point in $cornerPoints) {
|
||||||
|
|||||||
@@ -36,7 +36,15 @@ $root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
|||||||
$algorithms = $root + 'Algorithms.'
|
$algorithms = $root + 'Algorithms.'
|
||||||
$runnerType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmRunner')
|
$runnerType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmRunner')
|
||||||
$smootherType = Get-RequiredType ($algorithms + 'IPathSmoother')
|
$smootherType = Get-RequiredType ($algorithms + 'IPathSmoother')
|
||||||
|
$candidateType = Get-RequiredType ($algorithms + 'SmoothingCandidate')
|
||||||
|
$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus')
|
||||||
Assert-False $smootherType.IsPublic 'IPathSmoother must remain internal to the algorithm assembly.'
|
Assert-False $smootherType.IsPublic 'IPathSmoother must remain internal to the algorithm assembly.'
|
||||||
|
Assert-Equal 3 ([Enum]::GetNames($candidateStatusType).Length) 'Smoothing candidate status must contain only the three defined feasibility states.'
|
||||||
|
Assert-Equal 'Success' ([Enum]::GetNames($candidateStatusType)[0]) 'Candidate status must expose Success.'
|
||||||
|
Assert-Equal 'RetryableInfeasible' ([Enum]::GetNames($candidateStatusType)[1]) 'Candidate status must expose RetryableInfeasible.'
|
||||||
|
Assert-Equal 'Failed' ([Enum]::GetNames($candidateStatusType)[2]) 'Candidate status must expose Failed.'
|
||||||
|
$retryableFactory = $candidateType.GetMethod('RetryableInfeasible', [Reflection.BindingFlags]'Static,NonPublic')
|
||||||
|
Assert-True ($null -ne $retryableFactory) 'SmoothingCandidate must create retryable infeasibility without executable geometry.'
|
||||||
|
|
||||||
$hooksType = $runnerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
$hooksType = $runnerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||||
Assert-True ($null -ne $hooksType) 'SmoothingAlgorithmRunner must expose its narrowly scoped nested TestHooks helper.'
|
Assert-True ($null -ne $hooksType) 'SmoothingAlgorithmRunner must expose its narrowly scoped nested TestHooks helper.'
|
||||||
@@ -47,12 +55,12 @@ function Invoke-Scenario([string]$Scenario) {
|
|||||||
return $executeMethod.Invoke($null, @($Scenario))
|
return $executeMethod.Invoke($null, @($Scenario))
|
||||||
}
|
}
|
||||||
|
|
||||||
$allRejected = Invoke-Scenario 'RejectAll'
|
$allRetryable = Invoke-Scenario 'RetryableInfeasible'
|
||||||
Assert-Equal 'Infeasible' $allRejected.Status 'All rejected finite candidates must produce an Infeasible runner result.'
|
Assert-Equal 'Infeasible' $allRetryable.Status 'Exhausted retryable infeasibility 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-AttemptedStrengths $allRetryable @(1.00, 0.75, 0.50, 0.25) 'Retryable infeasibility 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-Equal 0 $allRetryable.AcceptedPathPointCount 'An infeasible runner result must not retain a retryable candidate as an accepted path.'
|
||||||
Assert-True ($allRejected.RejectedComparisonCandidatePointCount -gt 0) 'Only comparison diagnostics may retain the last rejected candidate geometry.'
|
Assert-Equal 0 $allRetryable.RejectedComparisonCandidatePointCount 'Retryable infeasibility must not retain executable candidate geometry.'
|
||||||
Assert-Equal 4 $allRejected.FailureCount 'Every rejected validation attempt must retain its failure reason.'
|
Assert-Equal 4 $allRetryable.FailureCount 'Every retryable attempt must retain its failure reason.'
|
||||||
|
|
||||||
$accepted = Invoke-Scenario 'AcceptFirst'
|
$accepted = Invoke-Scenario 'AcceptFirst'
|
||||||
Assert-Equal 'Success' $accepted.Status 'The first safe candidate must be accepted.'
|
Assert-Equal 'Success' $accepted.Status 'The first safe candidate must be accepted.'
|
||||||
@@ -60,10 +68,10 @@ Assert-AttemptedStrengths $accepted @(1.00) 'The runner must stop immediately af
|
|||||||
Assert-True ($accepted.AcceptedPathPointCount -gt 0) 'A successful runner result must publish the validated path internally.'
|
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.'
|
Assert-Equal 0 $accepted.RejectedComparisonCandidatePointCount 'An accepted candidate must not create rejected comparison geometry.'
|
||||||
|
|
||||||
$numericalFailure = Invoke-Scenario 'NumericalFailure'
|
$terminalFailure = Invoke-Scenario 'TerminalFailed'
|
||||||
Assert-Equal 'Failed' $numericalFailure.Status 'A numerical candidate failure must stop the runner as Failed.'
|
Assert-Equal 'Failed' $terminalFailure.Status 'A terminal candidate failure must stop the runner as Failed.'
|
||||||
Assert-AttemptedStrengths $numericalFailure @(1.00) 'Numerical candidate failure must not retry at lower strength.'
|
Assert-AttemptedStrengths $terminalFailure @(1.00) 'Terminal candidate failure must run exactly once.'
|
||||||
Assert-Equal 0 $numericalFailure.RejectedComparisonCandidatePointCount 'A non-candidate numerical failure must not retain comparison geometry.'
|
Assert-Equal 0 $terminalFailure.RejectedComparisonCandidatePointCount 'A terminal failure must not retain comparison geometry.'
|
||||||
|
|
||||||
$cancelled = Invoke-Scenario 'CancelBeforeNextAttempt'
|
$cancelled = Invoke-Scenario 'CancelBeforeNextAttempt'
|
||||||
Assert-True $cancelled.CancellationPropagated 'Cancellation between attempts must propagate out of the runner.'
|
Assert-True $cancelled.CancellationPropagated 'Cancellation between attempts must propagate out of the runner.'
|
||||||
|
|||||||
Reference in New Issue
Block a user