First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: align smoothing feasibility and option flow

Browse Source
This commit is contained in:
梁薄云
2026-07-29 12:39:46 +08:00
parent fc9aff4d84
commit d670a9c821
9 changed files with 466 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs
+48 -37
View File
@@ -13,7 +13,6 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
private const int Degree = 3;
private const int SamplesPerSpan = 64;
private const double StraightToleranceMeters = 1e-9d;
private const double EndpointTangentScale = 1d / 3d;
private const double EndpointProbeParameter = 1e-6d;
private const double MinimumTangentHandleLengthMeters = 1e-10d;
@@ -27,6 +26,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
CancellationToken cancellationToken)
{
if (input == null || input.OriginalPath == null ||
input.Options == null ||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
input.MinimumClearanceReserveMeters < 0d)
@@ -39,10 +39,19 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
{
cancellationToken.ThrowIfCancellationRequested();
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
if (!TrySmoothSegment(sourceSegment, effectiveStrength, input.MinimumClearanceReserveMeters,
cancellationToken, out IReadOnlyList<SmoothingPoint2D> points, out string reason))
if (!TrySmoothSegment(
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(
@@ -60,12 +69,15 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
PreparedDirectionSegment sourceSegment,
double strength,
double reserveMeters,
double endpointTangentScale,
CancellationToken cancellationToken,
out IReadOnlyList<SmoothingPoint2D> result,
out string reason)
out string reason,
out SmoothingCandidateStatus status)
{
result = null;
reason = string.Empty;
status = SmoothingCandidateStatus.Failed;
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
{
reason = "B 样条方向段为空。";
@@ -89,7 +101,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
return true;
}
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, endpointTangentScale,
cancellationToken, out Point2D[] controls, out reason))
{
return false;
@@ -99,23 +111,23 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
var sampled = new List<SmoothingPoint2D>();
int spanCount = controls.Length - Degree;
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;
if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
sampled, cancellationToken, out reason))
sampled, cancellationToken, out reason, out status))
return false;
for (int index = 1; index < uniformIntervals; index++)
{
if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
sampled, cancellationToken, out reason))
sampled, cancellationToken, out reason, out status))
{
return false;
}
}
if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
sampled, cancellationToken, out reason))
sampled, cancellationToken, out reason, out status))
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;
result = sampled;
@@ -127,6 +139,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
TravelDirection direction,
double strength,
double reserveMeters,
double endpointTangentScale,
CancellationToken cancellationToken,
out Point2D[] controls,
out string reason)
@@ -136,7 +149,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
controls[0] = Point2D.FromAnchor(anchors[0]);
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);
if (!TryConstrainTangentHandle(
Point2D.FromAnchor(anchors[0]),
@@ -152,7 +165,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
}
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);
if (!TryConstrainTangentHandle(
Point2D.FromAnchor(anchors[finalIndex]),
@@ -201,16 +214,35 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
double reserveMeters,
List<SmoothingPoint2D> output,
CancellationToken cancellationToken,
out string reason)
out string reason,
out SmoothingCandidateStatus status)
{
reason = string.Empty;
status = SmoothingCandidateStatus.Failed;
cancellationToken.ThrowIfCancellationRequested();
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);
if (!NumericGuard.IsFinite(displacement) || displacement > GetAllowedRadius(reference, reserveMeters))
if (!NumericGuard.IsFinite(displacement))
{
reason = "B 样条评估点位移产生非法数值。";
return false;
}
if (displacement > GetAllowedRadius(reference, reserveMeters))
{
reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
status = SmoothingCandidateStatus.RetryableInfeasible;
return false;
}
bool endpoint = parameter == 0d || parameter == 1d;
@@ -298,27 +330,6 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
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)
{
double deltaX = proposed.X - anchor.X;