chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -1,412 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>按方向段独立生成夹持三次 B 样条原始几何候选。</summary>
|
||||
internal sealed class CubicBSplineSmoother : IPathSmoother
|
||||
{
|
||||
private const int Degree = 3;
|
||||
private const int SamplesPerSpan = 64;
|
||||
private const double StraightToleranceMeters = 1e-9d;
|
||||
private const double EndpointProbeParameter = 1e-6d;
|
||||
private const double MinimumTangentHandleLengthMeters = 1e-10d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null || input.OriginalPath == null ||
|
||||
input.Options == null ||
|
||||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
||||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
||||
input.MinimumClearanceReserveMeters < 0d)
|
||||
{
|
||||
return SmoothingCandidate.Failed("B 样条输入、强度或净空预留无效。");
|
||||
}
|
||||
|
||||
var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
||||
if (!TrySmoothSegment(
|
||||
sourceSegment,
|
||||
effectiveStrength,
|
||||
input.MinimumClearanceReserveMeters,
|
||||
input.Options.CubicBSplineEndpointTangentScale,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> points,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status))
|
||||
{
|
||||
return status == SmoothingCandidateStatus.RetryableInfeasible
|
||||
? SmoothingCandidate.RetryableInfeasible(reason)
|
||||
: SmoothingCandidate.Failed(reason);
|
||||
}
|
||||
|
||||
candidateSegments.Add(new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
points,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch,
|
||||
sourceSegment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
return SmoothingCandidate.Success(candidateSegments);
|
||||
}
|
||||
|
||||
private static bool TrySmoothSegment(
|
||||
PreparedDirectionSegment sourceSegment,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
double endpointTangentScale,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> result,
|
||||
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 样条方向段为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
||||
for (int index = 0; index < anchors.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!IsValidAnchor(anchors[index]))
|
||||
{
|
||||
reason = "B 样条方向段包含非法锚点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (anchors.Count <= Degree || IsStraight(anchors))
|
||||
{
|
||||
result = anchors;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, endpointTangentScale,
|
||||
cancellationToken, out Point2D[] controls, out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double[] knots = CreateClampedKnots(controls.Length);
|
||||
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, out status))
|
||||
return false;
|
||||
if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
||||
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, out status))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
||||
sampled, cancellationToken, out reason, out status))
|
||||
return false;
|
||||
if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
|
||||
return false;
|
||||
|
||||
result = sampled;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateControls(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
TravelDirection direction,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
double endpointTangentScale,
|
||||
CancellationToken cancellationToken,
|
||||
out Point2D[] controls,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
controls = new Point2D[anchors.Count];
|
||||
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 startTravelHeading = GetTravelHeading(anchors[0], direction);
|
||||
if (!TryConstrainTangentHandle(
|
||||
Point2D.FromAnchor(anchors[0]),
|
||||
Math.Cos(startTravelHeading),
|
||||
Math.Sin(startTravelHeading),
|
||||
startHandleLength,
|
||||
anchors[1],
|
||||
GetAllowedRadius(anchors[1], reserveMeters),
|
||||
out controls[1]))
|
||||
{
|
||||
reason = "B 样条起点切向手柄无法同时满足相邻锚点移动范围。";
|
||||
return false;
|
||||
}
|
||||
|
||||
int finalIndex = anchors.Count - 1;
|
||||
double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * endpointTangentScale * strength;
|
||||
double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
|
||||
if (!TryConstrainTangentHandle(
|
||||
Point2D.FromAnchor(anchors[finalIndex]),
|
||||
-Math.Cos(endTravelHeading),
|
||||
-Math.Sin(endTravelHeading),
|
||||
endHandleLength,
|
||||
anchors[finalIndex - 1],
|
||||
GetAllowedRadius(anchors[finalIndex - 1], reserveMeters),
|
||||
out controls[finalIndex - 1]))
|
||||
{
|
||||
reason = "B 样条终点切向手柄无法同时满足相邻锚点移动范围。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 2; index < finalIndex - 1; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D previous = anchors[index - 1];
|
||||
SmoothingPoint2D current = anchors[index];
|
||||
SmoothingPoint2D next = anchors[index + 1];
|
||||
Point2D target = new Point2D(
|
||||
(previous.X + current.X + next.X) / 3d,
|
||||
(previous.Y + current.Y + next.Y) / 3d);
|
||||
Point2D proposed = new Point2D(
|
||||
current.X + strength * (target.X - current.X),
|
||||
current.Y + strength * (target.Y - current.Y));
|
||||
controls[index] = ClampDisplacement(current, proposed, GetAllowedRadius(current, reserveMeters));
|
||||
}
|
||||
|
||||
for (int index = 0; index < controls.Length; index++)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(controls[index].X) || !NumericGuard.IsFinite(controls[index].Y))
|
||||
{
|
||||
reason = "B 样条控制点构造产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAddSample(
|
||||
double parameter,
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
Point2D[] controls,
|
||||
double[] knots,
|
||||
double reserveMeters,
|
||||
List<SmoothingPoint2D> output,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Point2D evaluated = Evaluate(controls, knots, 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))
|
||||
{
|
||||
reason = "B 样条评估点位移产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
if (displacement > GetAllowedRadius(reference, reserveMeters))
|
||||
{
|
||||
reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
|
||||
status = SmoothingCandidateStatus.RetryableInfeasible;
|
||||
return false;
|
||||
}
|
||||
bool endpoint = parameter == 0d || parameter == 1d;
|
||||
output.Add(new SmoothingPoint2D(
|
||||
evaluated.X,
|
||||
evaluated.Y,
|
||||
reference.ArcLength,
|
||||
reference.Heading,
|
||||
reference.UnwrappedHeading,
|
||||
reference.BodyClearance,
|
||||
endpoint && reference.IsGearSwitchPoint,
|
||||
endpoint ? reference.Source : SmoothedPathPointSource.Interpolated));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryConstrainTangentHandle(
|
||||
Point2D endpoint,
|
||||
double rayDirectionX,
|
||||
double rayDirectionY,
|
||||
double desiredLength,
|
||||
SmoothingPoint2D adjacentAnchor,
|
||||
double allowedRadius,
|
||||
out Point2D control)
|
||||
{
|
||||
control = default;
|
||||
double offsetX = adjacentAnchor.X - endpoint.X;
|
||||
double offsetY = adjacentAnchor.Y - endpoint.Y;
|
||||
double projectedLength = offsetX * rayDirectionX + offsetY * rayDirectionY;
|
||||
double perpendicularX = offsetX - projectedLength * rayDirectionX;
|
||||
double perpendicularY = offsetY - projectedLength * rayDirectionY;
|
||||
double discriminant = allowedRadius * allowedRadius -
|
||||
(perpendicularX * perpendicularX + perpendicularY * perpendicularY);
|
||||
if (!NumericGuard.IsFinite(discriminant) || discriminant < 0d) return false;
|
||||
|
||||
double halfInterval = Math.Sqrt(discriminant);
|
||||
double minimumLength = Math.Max(MinimumTangentHandleLengthMeters, projectedLength - halfInterval);
|
||||
double maximumLength = projectedLength + halfInterval;
|
||||
if (!NumericGuard.IsFinite(maximumLength) || maximumLength < minimumLength) return false;
|
||||
|
||||
double constrainedLength = Math.Max(minimumLength, Math.Min(desiredLength, maximumLength));
|
||||
control = new Point2D(
|
||||
endpoint.X + constrainedLength * rayDirectionX,
|
||||
endpoint.Y + constrainedLength * rayDirectionY);
|
||||
return NumericGuard.IsFinite(control.X) && NumericGuard.IsFinite(control.Y);
|
||||
}
|
||||
|
||||
private static Point2D Evaluate(Point2D[] controls, double[] knots, double parameter)
|
||||
{
|
||||
if (parameter <= 0d) return controls[0];
|
||||
if (parameter >= 1d) return controls[controls.Length - 1];
|
||||
|
||||
var point = new Point2D(0d, 0d);
|
||||
for (int index = 0; index < controls.Length; index++)
|
||||
{
|
||||
double basis = EvaluateBasis(index, Degree, parameter, knots);
|
||||
point = new Point2D(point.X + basis * controls[index].X, point.Y + basis * controls[index].Y);
|
||||
}
|
||||
return point;
|
||||
}
|
||||
|
||||
private static double EvaluateBasis(int index, int degree, double parameter, double[] knots)
|
||||
{
|
||||
if (degree == 0)
|
||||
return knots[index] <= parameter && parameter < knots[index + 1] ? 1d : 0d;
|
||||
|
||||
double left = 0d;
|
||||
double leftDenominator = knots[index + degree] - knots[index];
|
||||
if (leftDenominator > 0d)
|
||||
left = (parameter - knots[index]) / leftDenominator * EvaluateBasis(index, degree - 1, parameter, knots);
|
||||
|
||||
double right = 0d;
|
||||
double rightDenominator = knots[index + degree + 1] - knots[index + 1];
|
||||
if (rightDenominator > 0d)
|
||||
right = (knots[index + degree + 1] - parameter) / rightDenominator *
|
||||
EvaluateBasis(index + 1, degree - 1, parameter, knots);
|
||||
return left + right;
|
||||
}
|
||||
|
||||
private static double[] CreateClampedKnots(int controlCount)
|
||||
{
|
||||
var knots = new double[controlCount + Degree + 1];
|
||||
for (int index = Degree + 1; index < controlCount; index++)
|
||||
knots[index] = (double)(index - Degree) / (controlCount - Degree);
|
||||
for (int index = controlCount; index < knots.Length; index++) knots[index] = 1d;
|
||||
return knots;
|
||||
}
|
||||
|
||||
private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius)
|
||||
{
|
||||
double deltaX = proposed.X - anchor.X;
|
||||
double deltaY = proposed.Y - anchor.Y;
|
||||
double distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= allowedRadius) return proposed;
|
||||
if (distance == 0d || allowedRadius == 0d) return Point2D.FromAnchor(anchor);
|
||||
double scale = allowedRadius / distance;
|
||||
return new Point2D(anchor.X + deltaX * scale, anchor.Y + deltaY * scale);
|
||||
}
|
||||
|
||||
private static bool IsStraight(IReadOnlyList<SmoothingPoint2D> anchors)
|
||||
{
|
||||
if (anchors.Count < 3) return true;
|
||||
SmoothingPoint2D first = anchors[0];
|
||||
SmoothingPoint2D last = anchors[anchors.Count - 1];
|
||||
double directionX = last.X - first.X;
|
||||
double directionY = last.Y - first.Y;
|
||||
double length = Math.Sqrt(directionX * directionX + directionY * directionY);
|
||||
if (!NumericGuard.IsFinite(length) || length <= StraightToleranceMeters) return false;
|
||||
for (int index = 1; index < anchors.Count - 1; index++)
|
||||
{
|
||||
double offsetX = anchors[index].X - first.X;
|
||||
double offsetY = anchors[index].Y - first.Y;
|
||||
double perpendicularDeviation = Math.Abs(directionX * offsetY - directionY * offsetX) / length;
|
||||
if (perpendicularDeviation >= StraightToleranceMeters) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidAnchor(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.BodyClearance >= 0d;
|
||||
}
|
||||
|
||||
private static double GetAllowedRadius(SmoothingPoint2D anchor, double reserveMeters)
|
||||
{
|
||||
return Math.Max(0d, anchor.BodyClearance - reserveMeters);
|
||||
}
|
||||
|
||||
private static double GetTravelHeading(SmoothingPoint2D point, TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward ? point.Heading : point.Heading - Math.PI;
|
||||
}
|
||||
|
||||
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double deltaX = right.X - left.X;
|
||||
double deltaY = right.Y - left.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private static double Distance(Point2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double deltaX = right.X - left.X;
|
||||
double deltaY = right.Y - left.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private readonly struct Point2D
|
||||
{
|
||||
internal Point2D(double x, double y)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
}
|
||||
|
||||
internal double X { get; }
|
||||
internal double Y { get; }
|
||||
|
||||
internal static Point2D FromAnchor(SmoothingPoint2D anchor)
|
||||
{
|
||||
return new Point2D(anchor.X, anchor.Y);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,14 +0,0 @@
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>单一平滑方法生成原始几何候选的内部契约。</summary>
|
||||
internal interface IPathSmoother
|
||||
{
|
||||
SmoothingMethod Method { get; }
|
||||
|
||||
SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken);
|
||||
}
|
||||
@@ -1,427 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>在方向段内以局部三次 Bézier 连接替换明显转角。</summary>
|
||||
internal sealed class LocalCubicBezierSmoother : IPathSmoother
|
||||
{
|
||||
private const double WindowToleranceMeters = 1e-9d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.LocalCubicBezier;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null || input.OriginalPath == null || input.Options == null ||
|
||||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
||||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
||||
input.MinimumClearanceReserveMeters < 0d)
|
||||
{
|
||||
return SmoothingCandidate.Failed("Bézier 输入、强度或净空预留无效。");
|
||||
}
|
||||
|
||||
var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
||||
if (!TrySmoothSegment(
|
||||
sourceSegment,
|
||||
input.Options.BezierCornerHeadingThresholdRadians,
|
||||
input.Options.BezierMaximumWindowLengthMeters,
|
||||
input.Options.BezierHandleLengthRatio,
|
||||
effectiveStrength,
|
||||
input.MinimumClearanceReserveMeters,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> points,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status))
|
||||
{
|
||||
return status == SmoothingCandidateStatus.RetryableInfeasible
|
||||
? SmoothingCandidate.RetryableInfeasible(reason)
|
||||
: SmoothingCandidate.Failed(reason);
|
||||
}
|
||||
|
||||
candidateSegments.Add(new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
points,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch,
|
||||
sourceSegment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
return SmoothingCandidate.Success(candidateSegments);
|
||||
}
|
||||
|
||||
private static bool TrySmoothSegment(
|
||||
PreparedDirectionSegment sourceSegment,
|
||||
double cornerThresholdRadians,
|
||||
double maximumWindowLengthMeters,
|
||||
double handleLengthRatio,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> result,
|
||||
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ézier 方向段为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
||||
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
|
||||
if (anchors.Count < 3)
|
||||
{
|
||||
result = anchors;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!TryCreateMergedWindows(
|
||||
anchors,
|
||||
cornerThresholdRadians,
|
||||
maximumWindowLengthMeters,
|
||||
cancellationToken,
|
||||
out List<Window> windows,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (windows.Count == 0)
|
||||
{
|
||||
result = anchors;
|
||||
return true;
|
||||
}
|
||||
|
||||
var output = new List<SmoothingPoint2D>(anchors.Count);
|
||||
int anchorIndex = 0;
|
||||
for (int windowIndex = 0; windowIndex < windows.Count; windowIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Window window = windows[windowIndex];
|
||||
while (anchorIndex <= window.StartIndex)
|
||||
{
|
||||
output.Add(anchors[anchorIndex]);
|
||||
anchorIndex++;
|
||||
}
|
||||
|
||||
if (!TryAppendWindowInterior(
|
||||
anchors,
|
||||
window,
|
||||
handleLengthRatio,
|
||||
strength,
|
||||
reserveMeters,
|
||||
output,
|
||||
cancellationToken,
|
||||
out reason,
|
||||
out status))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
output.Add(anchors[window.EndIndex]);
|
||||
anchorIndex = window.EndIndex + 1;
|
||||
}
|
||||
|
||||
while (anchorIndex < anchors.Count)
|
||||
{
|
||||
output.Add(anchors[anchorIndex]);
|
||||
anchorIndex++;
|
||||
}
|
||||
|
||||
result = output;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateAnchors(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int index = 0; index < anchors.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D point = anchors[index];
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
|
||||
!NumericGuard.IsFinite(point.BodyClearance) || point.BodyClearance < 0d ||
|
||||
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
|
||||
{
|
||||
reason = "Bézier 方向段包含非有限、非递增弧长或无效净空的锚点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateMergedWindows(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
double cornerThresholdRadians,
|
||||
double maximumWindowLengthMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out List<Window> windows,
|
||||
out string reason)
|
||||
{
|
||||
windows = new List<Window>();
|
||||
var candidates = new List<Window>();
|
||||
reason = string.Empty;
|
||||
for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!TryGetTravelTangent(anchors[cornerIndex - 1], anchors[cornerIndex], out Point2D entryTangent) ||
|
||||
!TryGetTravelTangent(anchors[cornerIndex], anchors[cornerIndex + 1], out Point2D exitTangent))
|
||||
{
|
||||
reason = "Bézier 转角包含零长度或非有限行进切向。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double cross = entryTangent.X * exitTangent.Y - entryTangent.Y * exitTangent.X;
|
||||
double dot = entryTangent.X * exitTangent.X + entryTangent.Y * exitTangent.Y;
|
||||
double turnRadians = Math.Atan2(Math.Abs(cross), dot);
|
||||
if (!NumericGuard.IsFinite(turnRadians))
|
||||
{
|
||||
reason = "Bézier 转角计算产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
if (turnRadians < cornerThresholdRadians) continue;
|
||||
|
||||
int startIndex = cornerIndex - 1;
|
||||
int endIndex = cornerIndex + 1;
|
||||
double windowLength = anchors[endIndex].ArcLength - anchors[startIndex].ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(windowLength))
|
||||
{
|
||||
reason = "Bézier 局部窗口弧长无效。";
|
||||
return false;
|
||||
}
|
||||
if (windowLength > maximumWindowLengthMeters + WindowToleranceMeters) continue;
|
||||
if (ContainsGearSwitch(anchors, startIndex, endIndex)) continue;
|
||||
|
||||
candidates.Add(new Window(startIndex, endIndex));
|
||||
}
|
||||
|
||||
MergeBoundedConnectedWindows(anchors, candidates, maximumWindowLengthMeters, windows);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void MergeBoundedConnectedWindows(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
IReadOnlyList<Window> candidates,
|
||||
double maximumWindowLengthMeters,
|
||||
List<Window> windows)
|
||||
{
|
||||
int candidateIndex = 0;
|
||||
while (candidateIndex < candidates.Count)
|
||||
{
|
||||
Window merged = candidates[candidateIndex];
|
||||
candidateIndex++;
|
||||
while (candidateIndex < candidates.Count &&
|
||||
candidates[candidateIndex].StartIndex <= merged.EndIndex + 1)
|
||||
{
|
||||
merged = new Window(merged.StartIndex,
|
||||
Math.Max(merged.EndIndex, candidates[candidateIndex].EndIndex));
|
||||
candidateIndex++;
|
||||
}
|
||||
|
||||
double mergedLength = anchors[merged.EndIndex].ArcLength - anchors[merged.StartIndex].ArcLength;
|
||||
if (mergedLength <= maximumWindowLengthMeters + WindowToleranceMeters)
|
||||
{
|
||||
windows.Add(merged);
|
||||
}
|
||||
// A connected group that exceeds the cap is declined as a whole. Splitting it into
|
||||
// adjacent local curves would introduce unrequested joins; accepting it would violate
|
||||
// the maximum-window contract. Its original anchors therefore remain unchanged.
|
||||
}
|
||||
}
|
||||
|
||||
private static bool TryAppendWindowInterior(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
Window window,
|
||||
double handleLengthRatio,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
List<SmoothingPoint2D> output,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
SmoothingPoint2D p0 = anchors[window.StartIndex];
|
||||
SmoothingPoint2D p3 = anchors[window.EndIndex];
|
||||
if (!TryGetTravelTangent(p0, anchors[window.StartIndex + 1], out Point2D entryTangent) ||
|
||||
!TryGetTravelTangent(anchors[window.EndIndex - 1], p3, out Point2D exitTangent))
|
||||
{
|
||||
reason = "Bézier 窗口端点包含无效行进切向。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double arcLength = p3.ArcLength - p0.ArcLength;
|
||||
double chordLength = Distance(p0, p3);
|
||||
double handleLength = chordLength * handleLengthRatio * strength;
|
||||
if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(chordLength) ||
|
||||
!NumericGuard.IsPositiveFinite(handleLength))
|
||||
{
|
||||
reason = "Bézier 窗口弧长、端点弦长或控制柄长度无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D control1 = new Point2D(
|
||||
p0.X + entryTangent.X * handleLength,
|
||||
p0.Y + entryTangent.Y * handleLength);
|
||||
Point2D control2 = new Point2D(
|
||||
p3.X - exitTangent.X * handleLength,
|
||||
p3.Y - exitTangent.Y * handleLength);
|
||||
if (!IsFinite(control1) || !IsFinite(control2))
|
||||
{
|
||||
reason = "Bézier 控制点产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = window.StartIndex + 1; index < window.EndIndex; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D anchor = anchors[index];
|
||||
double parameter = (anchor.ArcLength - p0.ArcLength) / arcLength;
|
||||
if (!NumericGuard.IsFinite(parameter) || parameter <= 0d || parameter >= 1d)
|
||||
{
|
||||
reason = "Bézier 窗口参数无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D evaluated = Evaluate(p0, control1, control2, p3, parameter);
|
||||
if (!IsFinite(evaluated))
|
||||
{
|
||||
reason = "Bézier 评估产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double referenceArcLength = p0.ArcLength + parameter * arcLength;
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
||||
anchors,
|
||||
referenceArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double displacement = Distance(evaluated, reference);
|
||||
if (!NumericGuard.IsFinite(displacement))
|
||||
{
|
||||
reason = "Bézier 评估点位移产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
|
||||
if (displacement > allowedDisplacement)
|
||||
{
|
||||
reason = "Bézier 评估点超过对应原始弧长参考点的允许移动范围。";
|
||||
status = SmoothingCandidateStatus.RetryableInfeasible;
|
||||
return false;
|
||||
}
|
||||
|
||||
output.Add(new SmoothingPoint2D(
|
||||
evaluated.X,
|
||||
evaluated.Y,
|
||||
reference.ArcLength,
|
||||
reference.Heading,
|
||||
reference.UnwrappedHeading,
|
||||
reference.BodyClearance,
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ContainsGearSwitch(IReadOnlyList<SmoothingPoint2D> anchors, int startIndex, int endIndex)
|
||||
{
|
||||
for (int index = startIndex; index <= endIndex; index++)
|
||||
{
|
||||
if (anchors[index].IsGearSwitchPoint) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool TryGetTravelTangent(SmoothingPoint2D start, SmoothingPoint2D end, out Point2D tangent)
|
||||
{
|
||||
tangent = default;
|
||||
double deltaX = end.X - start.X;
|
||||
double deltaY = end.Y - start.Y;
|
||||
double length = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsPositiveFinite(length)) return false;
|
||||
tangent = new Point2D(deltaX / length, deltaY / length);
|
||||
return IsFinite(tangent);
|
||||
}
|
||||
|
||||
private static Point2D Evaluate(SmoothingPoint2D p0, Point2D p1, Point2D p2, SmoothingPoint2D p3, double parameter)
|
||||
{
|
||||
double oneMinusParameter = 1d - parameter;
|
||||
double p0Weight = oneMinusParameter * oneMinusParameter * oneMinusParameter;
|
||||
double p1Weight = 3d * oneMinusParameter * oneMinusParameter * parameter;
|
||||
double p2Weight = 3d * oneMinusParameter * parameter * parameter;
|
||||
double p3Weight = parameter * parameter * parameter;
|
||||
return new Point2D(
|
||||
p0Weight * p0.X + p1Weight * p1.X + p2Weight * p2.X + p3Weight * p3.X,
|
||||
p0Weight * p0.Y + p1Weight * p1.Y + p2Weight * p2.Y + p3Weight * p3.Y);
|
||||
}
|
||||
|
||||
private static bool IsFinite(Point2D point)
|
||||
{
|
||||
return NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y);
|
||||
}
|
||||
|
||||
private static double Distance(Point2D point, SmoothingPoint2D reference)
|
||||
{
|
||||
double deltaX = point.X - reference.X;
|
||||
double deltaY = point.Y - reference.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double deltaX = left.X - right.X;
|
||||
double deltaY = left.Y - right.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private readonly struct Window
|
||||
{
|
||||
internal Window(int startIndex, int endIndex)
|
||||
{
|
||||
StartIndex = startIndex;
|
||||
EndIndex = endIndex;
|
||||
}
|
||||
|
||||
internal int StartIndex { get; }
|
||||
|
||||
internal int EndIndex { get; }
|
||||
}
|
||||
|
||||
private readonly struct Point2D
|
||||
{
|
||||
internal Point2D(double x, double y)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
}
|
||||
|
||||
internal double X { get; }
|
||||
|
||||
internal double Y { get; }
|
||||
}
|
||||
}
|
||||
@@ -1,557 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>按方向段局部弧长构造 C2 连续的分段五次 Hermite 原始几何候选。</summary>
|
||||
internal sealed class PiecewiseQuinticSmoother : IPathSmoother
|
||||
{
|
||||
private const int SamplesPerInterval = 8;
|
||||
private const double DoubleMachineEpsilon = 2.2204460492503131e-16d;
|
||||
private const double EndpointNormalizationUlps = 32d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.PiecewiseQuintic;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null || input.OriginalPath == null || input.Options == null ||
|
||||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
||||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
||||
input.MinimumClearanceReserveMeters < 0d ||
|
||||
!NumericGuard.IsPositiveFinite(input.Options.QuinticKnotSpacingMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(input.Options.QuinticMinimumKnotSpacingMeters) ||
|
||||
input.Options.QuinticKnotSpacingMeters < input.Options.QuinticMinimumKnotSpacingMeters)
|
||||
{
|
||||
return SmoothingCandidate.Failed("五次 Hermite 输入、强度、净空预留或结点间距无效。");
|
||||
}
|
||||
|
||||
var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
||||
if (!TrySmoothSegment(
|
||||
sourceSegment,
|
||||
effectiveStrength,
|
||||
input.MinimumClearanceReserveMeters,
|
||||
input.Options.QuinticKnotSpacingMeters,
|
||||
input.Options.QuinticMinimumKnotSpacingMeters,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> points,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status))
|
||||
{
|
||||
return status == SmoothingCandidateStatus.RetryableInfeasible
|
||||
? SmoothingCandidate.RetryableInfeasible(reason)
|
||||
: SmoothingCandidate.Failed(reason);
|
||||
}
|
||||
|
||||
candidateSegments.Add(new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
points,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch,
|
||||
sourceSegment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
return SmoothingCandidate.Success(candidateSegments);
|
||||
}
|
||||
|
||||
private static bool TrySmoothSegment(
|
||||
PreparedDirectionSegment sourceSegment,
|
||||
double effectiveStrength,
|
||||
double reserveMeters,
|
||||
double knotSpacingMeters,
|
||||
double minimumKnotSpacingMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> result,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
result = null;
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count < 2)
|
||||
{
|
||||
reason = "五次 Hermite 方向段至少需要两个锚点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
||||
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
|
||||
|
||||
if (!TryCreateKnots(
|
||||
anchors,
|
||||
sourceSegment.Direction,
|
||||
effectiveStrength,
|
||||
knotSpacingMeters,
|
||||
minimumKnotSpacingMeters,
|
||||
cancellationToken,
|
||||
out List<Knot> knots,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Each physical acceleration is blended once into its shared knot and then scaled by
|
||||
// the left/right local interval independently. Reusing this value is what makes the
|
||||
// curve C2 with respect to local arc length, even for nonuniform final intervals.
|
||||
if (!TryAssignSharedAccelerations(knots, out reason)) return false;
|
||||
if (!TryCreateIntervals(knots, out List<QuinticInterval> intervals, out reason)) return false;
|
||||
|
||||
var sampled = new List<SmoothingPoint2D>(1 + intervals.Count * SamplesPerInterval);
|
||||
for (int intervalIndex = 0; intervalIndex < intervals.Count; intervalIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
QuinticInterval interval = intervals[intervalIndex];
|
||||
int firstSample = intervalIndex == 0 ? 0 : 1;
|
||||
for (int sampleIndex = firstSample; sampleIndex <= SamplesPerInterval; sampleIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
double parameter = (double)sampleIndex / SamplesPerInterval;
|
||||
double referenceArcLength = interval.Start.ArcLength + parameter * interval.Length;
|
||||
if (!NumericGuard.IsFinite(referenceArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 采样参考弧长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
||||
anchors,
|
||||
referenceArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D evaluated;
|
||||
if (sampleIndex == 0)
|
||||
evaluated = interval.Start.Position;
|
||||
else if (sampleIndex == SamplesPerInterval)
|
||||
evaluated = interval.End.Position;
|
||||
else if (!interval.TryEvaluate(parameter, out evaluated, out Point2D derivative, out Point2D secondDerivative))
|
||||
{
|
||||
reason = "五次 Hermite 采样产生非有限位置或导数。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double displacement = Distance(evaluated, reference);
|
||||
if (!NumericGuard.IsFinite(displacement))
|
||||
{
|
||||
reason = "五次 Hermite 采样位移无效。";
|
||||
return false;
|
||||
}
|
||||
double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
|
||||
if (displacement > allowedDisplacement)
|
||||
{
|
||||
reason = "五次 Hermite 采样点超过对应局部弧长参考点的允许移动范围。";
|
||||
status = SmoothingCandidateStatus.RetryableInfeasible;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool firstEndpoint = intervalIndex == 0 && sampleIndex == 0;
|
||||
bool lastEndpoint = intervalIndex == intervals.Count - 1 && sampleIndex == SamplesPerInterval;
|
||||
if (firstEndpoint)
|
||||
{
|
||||
sampled.Add(anchors[0]);
|
||||
}
|
||||
else if (lastEndpoint)
|
||||
{
|
||||
sampled.Add(anchors[anchors.Count - 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sampled.Add(new SmoothingPoint2D(
|
||||
evaluated.X,
|
||||
evaluated.Y,
|
||||
reference.ArcLength,
|
||||
reference.Heading,
|
||||
reference.UnwrappedHeading,
|
||||
reference.BodyClearance,
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
result = sampled;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateAnchors(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int index = 0; index < anchors.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D point = anchors[index];
|
||||
if (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 ||
|
||||
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 方向段包含非有限、非递增弧长或无效净空的锚点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateKnots(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
TravelDirection direction,
|
||||
double effectiveStrength,
|
||||
double knotSpacingMeters,
|
||||
double minimumKnotSpacingMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out List<Knot> knots,
|
||||
out string reason)
|
||||
{
|
||||
knots = new List<Knot>();
|
||||
reason = string.Empty;
|
||||
double startArcLength = anchors[0].ArcLength;
|
||||
double endArcLength = anchors[anchors.Count - 1].ArcLength;
|
||||
double totalLength = endArcLength - startArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(totalLength) || totalLength < minimumKnotSpacingMeters)
|
||||
{
|
||||
reason = "五次 Hermite 方向段短于配置的最小结点间距。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryCreateKnot(anchors[0], direction, effectiveStrength, out Knot first))
|
||||
{
|
||||
reason = "五次 Hermite 起点结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(first);
|
||||
|
||||
double endpointTolerance = GetEndpointNormalizationTolerance(
|
||||
startArcLength,
|
||||
endArcLength,
|
||||
knotSpacingMeters);
|
||||
double previousArcLength = startArcLength;
|
||||
for (long knotOrdinal = 1L; ; knotOrdinal++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
double targetArcLength = startArcLength + knotOrdinal * knotSpacingMeters;
|
||||
if (!NumericGuard.IsFinite(targetArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 内部结点弧长无效。";
|
||||
return false;
|
||||
}
|
||||
if (targetArcLength >= endArcLength - endpointTolerance) break;
|
||||
if (targetArcLength <= previousArcLength)
|
||||
{
|
||||
reason = "五次 Hermite 内部结点无法在浮点弧长尺度上保持递增。";
|
||||
return false;
|
||||
}
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
||||
anchors,
|
||||
targetArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out reason) ||
|
||||
!TryCreateKnot(reference, direction, effectiveStrength, out Knot knot))
|
||||
{
|
||||
if (string.IsNullOrEmpty(reason)) reason = "五次 Hermite 内部结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(knot);
|
||||
previousArcLength = targetArcLength;
|
||||
}
|
||||
|
||||
if (!TryCreateKnot(anchors[anchors.Count - 1], direction, effectiveStrength, out Knot last))
|
||||
{
|
||||
reason = "五次 Hermite 终点结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(last);
|
||||
|
||||
for (int index = 1; index < knots.Count; index++)
|
||||
{
|
||||
double intervalLength = knots[index].ArcLength - knots[index - 1].ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(intervalLength) || intervalLength < minimumKnotSpacingMeters)
|
||||
{
|
||||
reason = "五次 Hermite 结点间隔无效或短于配置的最小间距。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double GetEndpointNormalizationTolerance(
|
||||
double startArcLength,
|
||||
double endArcLength,
|
||||
double knotSpacingMeters)
|
||||
{
|
||||
double magnitude = Math.Max(
|
||||
Math.Abs(startArcLength),
|
||||
Math.Max(Math.Abs(endArcLength), Math.Abs(knotSpacingMeters)));
|
||||
return EndpointNormalizationUlps * DoubleMachineEpsilon * magnitude;
|
||||
}
|
||||
|
||||
private static bool TryCreateKnot(
|
||||
SmoothingPoint2D reference,
|
||||
TravelDirection direction,
|
||||
double effectiveStrength,
|
||||
out Knot knot)
|
||||
{
|
||||
knot = default;
|
||||
if (reference == null || !NumericGuard.IsFinite(reference.X) || !NumericGuard.IsFinite(reference.Y) ||
|
||||
!NumericGuard.IsFinite(reference.ArcLength) || !NumericGuard.IsFinite(reference.Heading))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double travelHeading = direction == TravelDirection.Forward
|
||||
? reference.Heading
|
||||
: reference.Heading - Math.PI;
|
||||
double tangentX = Math.Cos(travelHeading);
|
||||
double tangentY = Math.Sin(travelHeading);
|
||||
if (!NumericGuard.IsFinite(tangentX) || !NumericGuard.IsFinite(tangentY)) return false;
|
||||
|
||||
var velocity = new Point2D(tangentX * effectiveStrength, tangentY * effectiveStrength);
|
||||
if (!velocity.IsFinite) return false;
|
||||
knot = new Knot(reference.ArcLength, new Point2D(reference.X, reference.Y), velocity);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAssignSharedAccelerations(List<Knot> knots, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int index = 0; index < knots.Count; index++)
|
||||
{
|
||||
Point2D acceleration;
|
||||
if (index == 0)
|
||||
{
|
||||
if (!TryAcceleration(knots[0], knots[1], out acceleration))
|
||||
{
|
||||
reason = "五次 Hermite 起点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (index == knots.Count - 1)
|
||||
{
|
||||
if (!TryAcceleration(knots[index - 1], knots[index], out acceleration))
|
||||
{
|
||||
reason = "五次 Hermite 终点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!TryAcceleration(knots[index - 1], knots[index], out Point2D left) ||
|
||||
!TryAcceleration(knots[index], knots[index + 1], out Point2D right))
|
||||
{
|
||||
reason = "五次 Hermite 共享结点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
acceleration = new Point2D((left.X + right.X) / 2d, (left.Y + right.Y) / 2d);
|
||||
if (!acceleration.IsFinite)
|
||||
{
|
||||
reason = "五次 Hermite 共享结点加速度混合产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
knots[index] = knots[index].WithAcceleration(acceleration);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAcceleration(Knot start, Knot end, out Point2D acceleration)
|
||||
{
|
||||
acceleration = default;
|
||||
double intervalLength = end.ArcLength - start.ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(intervalLength)) return false;
|
||||
acceleration = new Point2D(
|
||||
(end.Velocity.X - start.Velocity.X) / intervalLength,
|
||||
(end.Velocity.Y - start.Velocity.Y) / intervalLength);
|
||||
return acceleration.IsFinite;
|
||||
}
|
||||
|
||||
private static bool TryCreateIntervals(
|
||||
IReadOnlyList<Knot> knots,
|
||||
out List<QuinticInterval> intervals,
|
||||
out string reason)
|
||||
{
|
||||
intervals = new List<QuinticInterval>(knots.Count - 1);
|
||||
reason = string.Empty;
|
||||
for (int index = 1; index < knots.Count; index++)
|
||||
{
|
||||
if (!QuinticInterval.TryCreate(knots[index - 1], knots[index], out QuinticInterval interval))
|
||||
{
|
||||
reason = "五次 Hermite 系数、端点导数或结点区间无效。";
|
||||
return false;
|
||||
}
|
||||
intervals.Add(interval);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double Distance(Point2D point, SmoothingPoint2D reference)
|
||||
{
|
||||
double deltaX = point.X - reference.X;
|
||||
double deltaY = point.Y - reference.Y;
|
||||
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
}
|
||||
|
||||
private readonly struct Knot
|
||||
{
|
||||
internal Knot(double arcLength, Point2D position, Point2D velocity)
|
||||
{
|
||||
ArcLength = arcLength;
|
||||
Position = position;
|
||||
Velocity = velocity;
|
||||
Acceleration = default;
|
||||
}
|
||||
|
||||
internal double ArcLength { get; }
|
||||
|
||||
internal Point2D Position { get; }
|
||||
|
||||
internal Point2D Velocity { get; }
|
||||
|
||||
internal Point2D Acceleration { get; }
|
||||
|
||||
internal Knot WithAcceleration(Point2D acceleration)
|
||||
{
|
||||
return new Knot(ArcLength, Position, Velocity, acceleration);
|
||||
}
|
||||
|
||||
private Knot(double arcLength, Point2D position, Point2D velocity, Point2D acceleration)
|
||||
{
|
||||
ArcLength = arcLength;
|
||||
Position = position;
|
||||
Velocity = velocity;
|
||||
Acceleration = acceleration;
|
||||
}
|
||||
}
|
||||
|
||||
private readonly struct QuinticInterval
|
||||
{
|
||||
private QuinticInterval(Knot start, Knot end, Point2D c0, Point2D c1, Point2D c2, Point2D c3, Point2D c4, Point2D c5)
|
||||
{
|
||||
Start = start;
|
||||
End = end;
|
||||
Length = end.ArcLength - start.ArcLength;
|
||||
_c0 = c0;
|
||||
_c1 = c1;
|
||||
_c2 = c2;
|
||||
_c3 = c3;
|
||||
_c4 = c4;
|
||||
_c5 = c5;
|
||||
}
|
||||
|
||||
private readonly Point2D _c0;
|
||||
private readonly Point2D _c1;
|
||||
private readonly Point2D _c2;
|
||||
private readonly Point2D _c3;
|
||||
private readonly Point2D _c4;
|
||||
private readonly Point2D _c5;
|
||||
|
||||
internal Knot Start { get; }
|
||||
|
||||
internal Knot End { get; }
|
||||
|
||||
internal double Length { get; }
|
||||
|
||||
internal static bool TryCreate(Knot start, Knot end, out QuinticInterval interval)
|
||||
{
|
||||
interval = default;
|
||||
double length = end.ArcLength - start.ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(length) || !start.Position.IsFinite || !end.Position.IsFinite ||
|
||||
!start.Velocity.IsFinite || !end.Velocity.IsFinite ||
|
||||
!start.Acceleration.IsFinite || !end.Acceleration.IsFinite)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D c0 = start.Position;
|
||||
Point2D c1 = Scale(start.Velocity, length);
|
||||
Point2D c2 = Scale(start.Acceleration, length * length / 2d);
|
||||
Point2D difference = Subtract(end.Position, start.Position);
|
||||
Point2D endVelocity = Scale(end.Velocity, length);
|
||||
Point2D startAcceleration = Scale(start.Acceleration, length * length);
|
||||
Point2D endAcceleration = Scale(end.Acceleration, length * length);
|
||||
Point2D c3 = Add(
|
||||
Add(Scale(difference, 10d), Scale(c1, -6d)),
|
||||
Add(Scale(endVelocity, -4d), Add(Scale(startAcceleration, -1.5d), Scale(endAcceleration, 0.5d))));
|
||||
Point2D c4 = Add(
|
||||
Add(Scale(difference, -15d), Scale(c1, 8d)),
|
||||
Add(Scale(endVelocity, 7d), Add(Scale(startAcceleration, 1.5d), Scale(endAcceleration, -1d))));
|
||||
Point2D c5 = Add(
|
||||
Add(Scale(difference, 6d), Add(Scale(c1, -3d), Scale(endVelocity, -3d))),
|
||||
Add(Scale(startAcceleration, -0.5d), Scale(endAcceleration, 0.5d)));
|
||||
if (!c0.IsFinite || !c1.IsFinite || !c2.IsFinite || !c3.IsFinite || !c4.IsFinite || !c5.IsFinite)
|
||||
return false;
|
||||
|
||||
interval = new QuinticInterval(start, end, c0, c1, c2, c3, c4, c5);
|
||||
return interval.TryEvaluate(0d, out _, out _, out _) && interval.TryEvaluate(1d, out _, out _, out _);
|
||||
}
|
||||
|
||||
internal bool TryEvaluate(double parameter, out Point2D position, out Point2D derivative, out Point2D secondDerivative)
|
||||
{
|
||||
position = default;
|
||||
derivative = default;
|
||||
secondDerivative = default;
|
||||
if (!NumericGuard.IsFinite(parameter) || parameter < 0d || parameter > 1d) return false;
|
||||
|
||||
double t2 = parameter * parameter;
|
||||
double t3 = t2 * parameter;
|
||||
double t4 = t3 * parameter;
|
||||
double t5 = t4 * parameter;
|
||||
position = Add(Add(Add(_c0, Scale(_c1, parameter)), Add(Scale(_c2, t2), Scale(_c3, t3))),
|
||||
Add(Scale(_c4, t4), Scale(_c5, t5)));
|
||||
derivative = Add(Add(_c1, Scale(_c2, 2d * parameter)),
|
||||
Add(Scale(_c3, 3d * t2), Add(Scale(_c4, 4d * t3), Scale(_c5, 5d * t4))));
|
||||
secondDerivative = Add(Scale(_c2, 2d),
|
||||
Add(Scale(_c3, 6d * parameter), Add(Scale(_c4, 12d * t2), Scale(_c5, 20d * t3))));
|
||||
return position.IsFinite && derivative.IsFinite && secondDerivative.IsFinite;
|
||||
}
|
||||
}
|
||||
|
||||
private readonly struct Point2D
|
||||
{
|
||||
internal Point2D(double x, double y)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
}
|
||||
|
||||
internal double X { get; }
|
||||
|
||||
internal double Y { get; }
|
||||
|
||||
internal bool IsFinite => NumericGuard.IsFinite(X) && NumericGuard.IsFinite(Y);
|
||||
}
|
||||
|
||||
private static Point2D Add(Point2D left, Point2D right)
|
||||
{
|
||||
return new Point2D(left.X + right.X, left.Y + right.Y);
|
||||
}
|
||||
|
||||
private static Point2D Subtract(Point2D left, Point2D right)
|
||||
{
|
||||
return new Point2D(left.X - right.X, left.Y - right.Y);
|
||||
}
|
||||
|
||||
private static Point2D Scale(Point2D point, double scale)
|
||||
{
|
||||
return new Point2D(point.X * scale, point.Y * scale);
|
||||
}
|
||||
}
|
||||
@@ -1,44 +0,0 @@
|
||||
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,
|
||||
double minimumClearanceReserveMeters,
|
||||
SmoothingOptionsSnapshot options)
|
||||
{
|
||||
OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath));
|
||||
Map = map ?? throw new ArgumentNullException(nameof(map));
|
||||
Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle));
|
||||
MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters;
|
||||
MinimumClearanceReserveMeters = minimumClearanceReserveMeters;
|
||||
Options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
}
|
||||
|
||||
/// <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; }
|
||||
|
||||
/// <summary>候选几何必须从原始保守净空中预留的最小安全余量，单位 m。</summary>
|
||||
internal double MinimumClearanceReserveMeters { get; }
|
||||
|
||||
/// <summary>本次算法运行使用的已验证方法选项快照。</summary>
|
||||
internal SmoothingOptionsSnapshot Options { get; }
|
||||
}
|
||||
@@ -1,443 +0,0 @@
|
||||
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 static readonly double[] RetryStrengthScales = { 1d, 0.75d, 0.50d, 0.25d };
|
||||
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 || input.Options == null || configuration == null)
|
||||
return AlgorithmRunResult.Failed("平滑算法、输入或配置无效。", attemptedStrengths, failureReasons);
|
||||
|
||||
foreach (double scale in 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.Status == SmoothingCandidateStatus.RetryableInfeasible)
|
||||
{
|
||||
failureReasons.Add(candidate.Reason);
|
||||
continue;
|
||||
}
|
||||
if (candidate.Status == SmoothingCandidateStatus.Failed)
|
||||
{
|
||||
failureReasons.Add(candidate.Reason);
|
||||
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,
|
||||
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);
|
||||
}
|
||||
|
||||
failureReasons.Add(reason);
|
||||
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
return AlgorithmRunResult.Infeasible(null, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
|
||||
{
|
||||
return new PathQualityMetrics(
|
||||
true,
|
||||
analysis.PathLengthMeters,
|
||||
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
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
|
||||
{
|
||||
RetryableInfeasible,
|
||||
AcceptFirst,
|
||||
TerminalFailed,
|
||||
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.TerminalFailed)
|
||||
return SmoothingCandidate.Failed("确定性数值退化。");
|
||||
if (_scenario == TestScenario.AcceptFirst)
|
||||
return CreateAcceptedCandidate();
|
||||
|
||||
if (_scenario == TestScenario.CancelBeforeNextAttempt)
|
||||
_cancellationSource.Cancel();
|
||||
return SmoothingCandidate.RetryableInfeasible("确定性可重试不可行。" );
|
||||
}
|
||||
}
|
||||
|
||||
private static TestScenario ParseScenario(string scenario)
|
||||
{
|
||||
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.TerminalFailed), StringComparison.Ordinal)) return TestScenario.TerminalFailed;
|
||||
if (string.Equals(scenario, nameof(TestScenario.CancelBeforeNextAttempt), StringComparison.Ordinal)) return TestScenario.CancelBeforeNextAttempt;
|
||||
throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
|
||||
private static SmoothingAlgorithmInput CreateTestInput()
|
||||
{
|
||||
var mapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 5000f, 0f, 5000f),
|
||||
ResolutionMm = 50f,
|
||||
AllowExplicitEmptyMap = true,
|
||||
};
|
||||
PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(mapRequest);
|
||||
if (!mapResult.Succeeded || mapResult.Map == null)
|
||||
throw new InvalidOperationException("The runner test hook could not create its empty map.");
|
||||
|
||||
var originalSegments = new List<PreparedDirectionSegment>
|
||||
{
|
||||
new PreparedDirectionSegment(
|
||||
0,
|
||||
TravelDirection.Forward,
|
||||
new List<SmoothingPoint2D>
|
||||
{
|
||||
CreatePoint(0.5d, 0.5d, 0d),
|
||||
CreatePoint(1.5d, 0.5d, 1d),
|
||||
},
|
||||
false,
|
||||
false),
|
||||
};
|
||||
var vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.20d,
|
||||
WidthMeters = 0.20d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 100d,
|
||||
MinimumTurningRadiusMeters = 0.01d,
|
||||
};
|
||||
return new SmoothingAlgorithmInput(
|
||||
new PreparedPath(originalSegments),
|
||||
mapResult.Map,
|
||||
vehicle,
|
||||
0.05d,
|
||||
0.02d,
|
||||
new SmoothingOptionsSnapshot(new PathSmoothingConfiguration()));
|
||||
}
|
||||
|
||||
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 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,83 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
internal enum SmoothingCandidateStatus
|
||||
{
|
||||
Success,
|
||||
RetryableInfeasible,
|
||||
Failed,
|
||||
}
|
||||
|
||||
/// <summary>平滑方法产生的原始方向段候选，尚未经过统一几何或安全复核。</summary>
|
||||
internal sealed class SmoothingCandidate
|
||||
{
|
||||
private SmoothingCandidate(
|
||||
SmoothingCandidateStatus status,
|
||||
IReadOnlyList<PreparedDirectionSegment> segments,
|
||||
string reason)
|
||||
{
|
||||
Status = status;
|
||||
if (status == SmoothingCandidateStatus.Success)
|
||||
{
|
||||
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>
|
||||
internal bool Succeeded => Status == SmoothingCandidateStatus.Success;
|
||||
|
||||
/// <summary>候选的可重试性和终止性状态。</summary>
|
||||
internal SmoothingCandidateStatus Status { get; }
|
||||
|
||||
/// <summary>候选方向段；失败候选始终为空。</summary>
|
||||
internal IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
||||
|
||||
/// <summary>失败或退化时的稳定说明；成功时为空。</summary>
|
||||
internal string Reason { get; }
|
||||
|
||||
/// <summary>创建待统一分析和验证的成功候选。</summary>
|
||||
internal static SmoothingCandidate Success(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.Success, segments, string.Empty);
|
||||
}
|
||||
|
||||
/// <summary>创建可由较低平滑强度重新尝试的不可行候选。</summary>
|
||||
internal static SmoothingCandidate RetryableInfeasible(string reason)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.RetryableInfeasible, null, reason);
|
||||
}
|
||||
|
||||
/// <summary>创建不应重试的数值或构造失败候选。</summary>
|
||||
internal static SmoothingCandidate Failed(string reason)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.Failed, 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);
|
||||
}
|
||||
}
|
||||
@@ -1,50 +0,0 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
-60
@@ -1,60 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>同一粗路径的离线平滑比较请求。</summary>
|
||||
public sealed class PathSmoothingComparisonRequest
|
||||
{
|
||||
private static readonly SmoothingMethod[] DefaultMethods =
|
||||
{
|
||||
SmoothingMethod.CubicBSpline,
|
||||
SmoothingMethod.LocalCubicBezier,
|
||||
SmoothingMethod.PiecewiseQuintic,
|
||||
};
|
||||
|
||||
/// <summary>创建比较请求，并固定原始输入与方法顺序。</summary>
|
||||
public PathSmoothingComparisonRequest(
|
||||
PathSmoothingRequest smoothingRequest,
|
||||
IReadOnlyList<SmoothingMethod> methods = null)
|
||||
{
|
||||
SmoothingRequest = CopyRequest(smoothingRequest);
|
||||
Methods = CopyMethods(methods ?? DefaultMethods);
|
||||
}
|
||||
|
||||
/// <summary>所有方法共享的不可变粗路径、地图、车辆和配置快照。</summary>
|
||||
public PathSmoothingRequest SmoothingRequest { get; }
|
||||
|
||||
/// <summary>按调用方指定稳定顺序运行的方法集合。</summary>
|
||||
public IReadOnlyList<SmoothingMethod> Methods { get; }
|
||||
|
||||
private static PathSmoothingRequest CopyRequest(PathSmoothingRequest source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
source.Configuration);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingMethod> CopyMethods(IReadOnlyList<SmoothingMethod> source)
|
||||
{
|
||||
var copy = new List<SmoothingMethod>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
SmoothingMethod method = source[index];
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
|
||||
throw new ArgumentOutOfRangeException(nameof(source), "比较方法无效。");
|
||||
if (copy.Contains(method))
|
||||
throw new ArgumentException("比较方法不能重复。", nameof(source));
|
||||
copy.Add(method);
|
||||
}
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingMethod>(copy);
|
||||
}
|
||||
}
|
||||
@@ -1,60 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>按公开字典序选择唯一的推荐平滑方法。</summary>
|
||||
public static class SmoothingMethodRanker
|
||||
{
|
||||
/// <summary>从当前场景的可行且确定性候选中选择最佳方法；没有合格候选时返回空。</summary>
|
||||
public static SmoothingMethod? Rank(IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
||||
{
|
||||
PathSmoothingComparisonEntry best = null;
|
||||
if (entries == null) return null;
|
||||
|
||||
for (int index = 0; index < entries.Count; index++)
|
||||
{
|
||||
PathSmoothingComparisonEntry candidate = entries[index];
|
||||
if (candidate == null || !candidate.IsEligibleForRecommendation) continue;
|
||||
if (best == null || Compare(candidate, best) < 0) best = candidate;
|
||||
}
|
||||
return best == null ? (SmoothingMethod?)null : best.Method;
|
||||
}
|
||||
|
||||
private static int Compare(PathSmoothingComparisonEntry left, PathSmoothingComparisonEntry right)
|
||||
{
|
||||
int comparison = CompareAscending(left.Metrics.CurvatureVariationEnergy, right.Metrics.CurvatureVariationEnergy);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(
|
||||
left.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
right.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareDescending(left.Metrics.MinimumBodyClearanceMeters, right.Metrics.MinimumBodyClearanceMeters);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(left.Metrics.LengthChangePercent, right.Metrics.LengthChangePercent);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(left.Timing.MedianElapsedMilliseconds, right.Timing.MedianElapsedMilliseconds);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
return ((int)left.Method.Value).CompareTo((int)right.Method.Value);
|
||||
}
|
||||
|
||||
private static int CompareAscending(double left, double right)
|
||||
{
|
||||
return Normalize(left).CompareTo(Normalize(right));
|
||||
}
|
||||
|
||||
private static int CompareDescending(double left, double right)
|
||||
{
|
||||
return Normalize(right).CompareTo(Normalize(left));
|
||||
}
|
||||
|
||||
private static double Normalize(double value)
|
||||
{
|
||||
return double.IsNaN(value) || double.IsInfinity(value) ? double.PositiveInfinity : value;
|
||||
}
|
||||
}
|
||||
@@ -1,8 +0,0 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>三次 B 样条平滑参数。</summary>
|
||||
public sealed class CubicBSplineOptions
|
||||
{
|
||||
/// <summary>端点切向控制柄相对于相邻弦长的比例。</summary>
|
||||
public double EndpointTangentScale { get; set; } = 1d / 3d;
|
||||
}
|
||||
@@ -1,16 +0,0 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>局部三次 Bézier 平滑参数。</summary>
|
||||
public sealed class LocalCubicBezierOptions
|
||||
{
|
||||
/// <summary>判定为明显转角的最小航向变化，单位 rad。</summary>
|
||||
public double CornerHeadingThresholdRadians { get; set; } = Math.PI / 18d;
|
||||
|
||||
/// <summary>单个局部平滑窗口的最大弧长，单位 m。</summary>
|
||||
public double MaximumWindowLengthMeters { get; set; } = 0.60d;
|
||||
|
||||
/// <summary>控制柄相对于窗口局部弦长的比例。</summary>
|
||||
public double HandleLengthRatio { get; set; } = 1d / 3d;
|
||||
}
|
||||
+11
-35
@@ -1,53 +1,29 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的公共配置；所有距离使用 m。</summary>
|
||||
/// <summary>Local G2 路径平滑管线共享配置；所有距离单位为 m，曲率相关限制使用 1/m。</summary>
|
||||
public sealed class PathSmoothingConfiguration
|
||||
{
|
||||
/// <summary>创建带有安全默认值的平滑配置。</summary>
|
||||
/// <summary>创建采用首版安全采样、碰撞步长、净空和曲率容差默认值的可编辑配置。</summary>
|
||||
public PathSmoothingConfiguration()
|
||||
{
|
||||
OutputSpacingMeters = 0.05d;
|
||||
OutputSpacingMeters = 0.025d;
|
||||
MaximumCollisionCheckStepMeters = 0.025d;
|
||||
MinimumClearanceReserveMeters = 0.02d;
|
||||
SmoothingStrength = 1d;
|
||||
AllowFallbackToCoarsePath = true;
|
||||
RetryStrengthScales = new ReadOnlyCollection<double>(
|
||||
new List<double> { 1d, 0.75d, 0.50d, 0.25d });
|
||||
MinimumClearanceReserveMeters = 0d;
|
||||
CurvatureLimitRadiusToleranceMeters = 0.002d;
|
||||
}
|
||||
|
||||
/// <summary>正式单算法入口使用的方法。</summary>
|
||||
public SmoothingMethod Method { get; set; }
|
||||
|
||||
/// <summary>输出路径的目标弧长采样间距，单位 m。</summary>
|
||||
/// <summary>发布路径沿弧长的目标采样间距，单位 m；必须为有限正数。</summary>
|
||||
public double OutputSpacingMeters { get; set; }
|
||||
|
||||
/// <summary>扫掠碰撞检查的最大步长，单位 m。</summary>
|
||||
/// <summary>连续车体扫掠复核允许的最大中心步长，单位 m；较大值会降低碰撞检查分辨率。</summary>
|
||||
public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
|
||||
/// <summary>平滑候选必须在最小净空之外保留的额外余量，单位 m。</summary>
|
||||
/// <summary>除无碰撞外还要求保留的最小额外净空，单位 m。</summary>
|
||||
public double MinimumClearanceReserveMeters { get; set; }
|
||||
|
||||
/// <summary>算法初始平滑强度。</summary>
|
||||
public double SmoothingStrength { get; set; }
|
||||
/// <summary>曲率上限复核时允许相对名义最小转弯半径的缩减容差，单位 m。</summary>
|
||||
public double CurvatureLimitRadiusToleranceMeters { get; set; }
|
||||
|
||||
/// <summary>所有平滑尝试失败时是否允许发布经过复核的原粗路径。</summary>
|
||||
public bool AllowFallbackToCoarsePath { get; set; }
|
||||
|
||||
/// <summary>有限且严格递减的平滑强度重试比例。</summary>
|
||||
public IReadOnlyList<double> RetryStrengthScales { get; }
|
||||
|
||||
/// <summary>三次 B 样条专用参数。</summary>
|
||||
public CubicBSplineOptions CubicBSpline { get; } = new CubicBSplineOptions();
|
||||
|
||||
/// <summary>局部三次 Bézier 专用参数。</summary>
|
||||
public LocalCubicBezierOptions LocalCubicBezier { get; } = new LocalCubicBezierOptions();
|
||||
|
||||
/// <summary>分段五次多项式专用参数。</summary>
|
||||
public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions();
|
||||
|
||||
/// <summary>局部 G2 五次过渡专用参数。</summary>
|
||||
/// <summary>局部 G2 五次过渡窗口与候选筛选选项；返回同一配置对象的可编辑子配置。</summary>
|
||||
public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions();
|
||||
}
|
||||
|
||||
+10
-54
@@ -2,79 +2,35 @@ using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>一次平滑尝试的不可变诊断信息。</summary>
|
||||
/// <summary>一次 Local G2 平滑尝试的不可变诊断快照，适用于成功、取消和失败结果。</summary>
|
||||
public sealed class PathSmoothingDiagnostics
|
||||
{
|
||||
/// <summary>创建不含路径指标的默认诊断信息。</summary>
|
||||
/// <summary>创建不可行、零耗时且没有终止原因的默认诊断快照。</summary>
|
||||
public PathSmoothingDiagnostics()
|
||||
: this(new PathQualityMetrics(), TimeSpan.Zero, 0, 0d, string.Empty)
|
||||
: this(new PathQualityMetrics(), TimeSpan.Zero, string.Empty)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建完整的平滑诊断快照。</summary>
|
||||
/// <summary>创建平滑诊断快照。</summary>
|
||||
/// <param name="metrics">路径可行性、曲率、净空和相对变化的质量指标。</param>
|
||||
/// <param name="elapsed">从服务入口到终止的累计耗时。</param>
|
||||
/// <param name="terminationReason">可供调用方记录的成功、取消或失败原因；为 <see langword="null"/> 时为空字符串。</param>
|
||||
public PathSmoothingDiagnostics(
|
||||
PathQualityMetrics metrics,
|
||||
TimeSpan elapsed,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string terminationReason = null)
|
||||
{
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
Elapsed = elapsed;
|
||||
RetryCount = retryCount;
|
||||
AcceptedStrength = acceptedStrength;
|
||||
TerminationReason = terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>使用所有测量量创建平滑诊断快照。</summary>
|
||||
public PathSmoothingDiagnostics(
|
||||
bool isFeasible,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters,
|
||||
double lengthChangePercent,
|
||||
double peakCurvatureChangePercent,
|
||||
double curvatureVariationChangePercent,
|
||||
double minimumClearanceChangeMeters,
|
||||
TimeSpan elapsed,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string terminationReason = null)
|
||||
: this(
|
||||
new PathQualityMetrics(
|
||||
isFeasible,
|
||||
pathLengthMeters,
|
||||
maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
rootMeanSquareVehicleCurvaturePerMeter,
|
||||
totalAbsoluteCurvatureVariationPerMeter,
|
||||
curvatureVariationEnergy,
|
||||
minimumBodyClearanceMeters,
|
||||
lengthChangePercent,
|
||||
peakCurvatureChangePercent,
|
||||
curvatureVariationChangePercent,
|
||||
minimumClearanceChangeMeters),
|
||||
elapsed,
|
||||
retryCount,
|
||||
acceptedStrength,
|
||||
terminationReason)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>路径质量指标；始终非空。</summary>
|
||||
/// <summary>本次尝试的质量指标；未提供时为全零且不可行的默认指标。</summary>
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
|
||||
/// <summary>从算法入口到返回诊断的耗时。</summary>
|
||||
/// <summary>本次尝试的累计耗时，类型为 <see cref="TimeSpan"/>。</summary>
|
||||
public TimeSpan Elapsed { get; }
|
||||
|
||||
/// <summary>已执行的安全强度重试次数。</summary>
|
||||
public int RetryCount { get; }
|
||||
|
||||
/// <summary>通过复核的平滑强度；未接受候选时为零。</summary>
|
||||
public double AcceptedStrength { get; }
|
||||
|
||||
/// <summary>面向调用方的稳定终止说明。</summary>
|
||||
/// <summary>终止原因文本；不承载可消费的部分路径。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
|
||||
@@ -5,13 +5,23 @@ using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑所需的原始粗路径、复核上下文和配置。</summary>
|
||||
/// <summary>
|
||||
/// 一次路径平滑所需的不可变输入快照。
|
||||
/// 粗路径、方向段、车辆和配置均在构造时复制；地图保留为调用方已冻结的 <see cref="PlanningGridMap"/> 快照。
|
||||
/// </summary>
|
||||
public sealed class PathSmoothingRequest
|
||||
{
|
||||
private readonly VehicleParameters _vehicle;
|
||||
private readonly PathSmoothingConfiguration _configuration;
|
||||
|
||||
/// <summary>创建路径平滑请求，并复制粗路径和方向分段集合。</summary>
|
||||
/// <summary>
|
||||
/// 创建路径平滑请求，并防御性复制可变的路径、分段、车辆和配置输入。
|
||||
/// </summary>
|
||||
/// <param name="coarsePath">按起点到终点顺序排列的粗路径点集合；位置与弧长单位为 m，航向单位为 rad。</param>
|
||||
/// <param name="segments">与 <paramref name="coarsePath"/> 对应的前进/倒车包含式方向分段集合。</param>
|
||||
/// <param name="map">用于完整车体碰撞与净空复核的已准备不可变规划地图快照。</param>
|
||||
/// <param name="vehicle">车辆长宽、安全余量和曲率约束；长度单位为 m，曲率单位为 1/m。</param>
|
||||
/// <param name="configuration">本次平滑的采样、净空、曲率和 Local G2 配置；构造后不会与调用方共享可变实例。</param>
|
||||
public PathSmoothingRequest(
|
||||
IReadOnlyList<CoarsePathPoint> coarsePath,
|
||||
IReadOnlyList<PathSegment> segments,
|
||||
@@ -26,13 +36,13 @@ public sealed class PathSmoothingRequest
|
||||
_configuration = CopyConfiguration(configuration);
|
||||
}
|
||||
|
||||
/// <summary>原始粗路径的不可变快照。</summary>
|
||||
/// <summary>原始粗路径的只读快照，按起点到终点顺序排列；位置与弧长单位为 m，航向单位为 rad。</summary>
|
||||
public IReadOnlyList<CoarsePathPoint> CoarsePath { get; }
|
||||
|
||||
/// <summary>原始粗路径方向分段的不可变快照。</summary>
|
||||
/// <summary>原始粗路径方向分段的只读快照；每段标识连续前进或倒车区间。</summary>
|
||||
public IReadOnlyList<PathSegment> Segments { get; }
|
||||
|
||||
/// <summary>用于平滑后完整车体复核的规划栅格地图。</summary>
|
||||
/// <summary>用于平滑后完整车体碰撞和净空复核的已冻结规划栅格地图快照。</summary>
|
||||
public PlanningGridMap Map { get; }
|
||||
|
||||
/// <summary>车辆几何与最大曲率约束的不可变快照副本。</summary>
|
||||
@@ -70,19 +80,11 @@ public sealed class PathSmoothingRequest
|
||||
if (source == null) return null;
|
||||
var copy = new PathSmoothingConfiguration
|
||||
{
|
||||
Method = source.Method,
|
||||
OutputSpacingMeters = source.OutputSpacingMeters,
|
||||
MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
|
||||
MinimumClearanceReserveMeters = source.MinimumClearanceReserveMeters,
|
||||
SmoothingStrength = source.SmoothingStrength,
|
||||
AllowFallbackToCoarsePath = source.AllowFallbackToCoarsePath,
|
||||
CurvatureLimitRadiusToleranceMeters = source.CurvatureLimitRadiusToleranceMeters,
|
||||
};
|
||||
copy.CubicBSpline.EndpointTangentScale = source.CubicBSpline.EndpointTangentScale;
|
||||
copy.LocalCubicBezier.CornerHeadingThresholdRadians = source.LocalCubicBezier.CornerHeadingThresholdRadians;
|
||||
copy.LocalCubicBezier.MaximumWindowLengthMeters = source.LocalCubicBezier.MaximumWindowLengthMeters;
|
||||
copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio;
|
||||
copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters;
|
||||
copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters;
|
||||
copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters;
|
||||
copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters;
|
||||
copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters;
|
||||
|
||||
@@ -4,7 +4,10 @@ using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的最终不可变结果。</summary>
|
||||
/// <summary>
|
||||
/// Local G2 路径平滑管线产生的不可变结果。
|
||||
/// 只有可发布状态携带完整、已验证的路径和方向段；失败、取消和无效输入结果始终提供空集合，不能被当作部分路径消费。
|
||||
/// </summary>
|
||||
public sealed class PathSmoothingResult
|
||||
{
|
||||
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
||||
@@ -30,64 +33,34 @@ public sealed class PathSmoothingResult
|
||||
Diagnostics = diagnostics ?? new PathSmoothingDiagnostics(
|
||||
new PathQualityMetrics(),
|
||||
TimeSpan.Zero,
|
||||
0,
|
||||
0d,
|
||||
"No smoothing diagnostics were supplied.");
|
||||
}
|
||||
|
||||
/// <summary>最终发布状态。</summary>
|
||||
/// <summary>本次平滑的终止状态；决定 <see cref="Path"/> 和 <see cref="Segments"/> 是否可消费。</summary>
|
||||
public PathSmoothingStatus Status { get; }
|
||||
|
||||
/// <summary>成功或回退时的实际（或尝试）平滑方法；失败时为空。</summary>
|
||||
/// <summary>成功发布路径时实际采用的平滑方法；失败结果为 <see langword="null"/>。</summary>
|
||||
public SmoothingMethod? Method { get; }
|
||||
|
||||
/// <summary>成功或经过复核的回退路径；其他状态始终为空且不可变。</summary>
|
||||
/// <summary>成功时按起点到终点排列的不可变平滑路径；位置和弧长单位为 m，航向单位为 rad；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>覆盖 <see cref="Path"/> 的方向分段；其他状态始终为空且不可变。</summary>
|
||||
/// <summary>成功时覆盖 <see cref="Path"/> 的不可变方向段集合；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>局部 G2 各检测区域的不可变报告；传统算法结果为空。</summary>
|
||||
/// <summary>各 Local G2 区域的不可变处理报告；失败结果不包含区域发布记录。</summary>
|
||||
public IReadOnlyList<PathSmoothingRegionReport> RegionReports { get; }
|
||||
|
||||
/// <summary>本次平滑的质量和终止诊断；始终非空。</summary>
|
||||
/// <summary>质量指标、耗时和终止原因；始终存在，供调用方诊断成功或失败。</summary>
|
||||
public PathSmoothingDiagnostics Diagnostics { get; }
|
||||
|
||||
/// <summary>创建已通过所有复核的平滑结果。</summary>
|
||||
public static PathSmoothingResult Success(
|
||||
SmoothingMethod method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
ValidatePublishedResult(method, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
PathSmoothingStatus.Success,
|
||||
method,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
EmptyRegionReports);
|
||||
}
|
||||
|
||||
/// <summary>创建经过完整复核的原始粗路径回退结果。</summary>
|
||||
public static PathSmoothingResult Fallback(
|
||||
SmoothingMethod attemptedMethod,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
ValidatePublishedResult(attemptedMethod, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
PathSmoothingStatus.FallbackToCoarsePath,
|
||||
attemptedMethod,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
EmptyRegionReports);
|
||||
}
|
||||
|
||||
/// <summary>发布经过完整复核的局部 G2 预平滑结果。</summary>
|
||||
/// <summary>发布已经过独立几何、曲率、净空和连续碰撞复核的 Local G2 结果。</summary>
|
||||
/// <param name="status">可发布状态，只能是完整、部分改进、不需要平滑或保持原样之一。</param>
|
||||
/// <param name="path">按起点到终点顺序排列的完整平滑路径；位置与弧长单位为 m，航向单位为 rad。</param>
|
||||
/// <param name="segments">覆盖完整路径的前进/倒车方向段集合。</param>
|
||||
/// <param name="diagnostics">包含可行质量指标和终止说明的诊断快照。</param>
|
||||
/// <param name="regionReports">每个局部区域的不可变处理报告集合，不能为 <see langword="null"/>。</param>
|
||||
/// <returns>携带防御性复制路径、分段和区域报告的不可变可消费结果。</returns>
|
||||
public static PathSmoothingResult PublishLocalG2(
|
||||
PathSmoothingStatus status,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
@@ -99,11 +72,13 @@ public sealed class PathSmoothingResult
|
||||
status != PathSmoothingStatus.PartialImprovement &&
|
||||
status != PathSmoothingStatus.NotNeeded &&
|
||||
status != PathSmoothingStatus.Unchanged)
|
||||
{
|
||||
throw new ArgumentException("Use a Local G2 publication status.", nameof(status));
|
||||
}
|
||||
if (regionReports == null)
|
||||
throw new ArgumentNullException(nameof(regionReports));
|
||||
|
||||
ValidatePublishedResult(SmoothingMethod.LocalG2Quintic, path, segments, diagnostics);
|
||||
ValidatePublishedResult(path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
status,
|
||||
SmoothingMethod.LocalG2Quintic,
|
||||
@@ -113,29 +88,29 @@ public sealed class PathSmoothingResult
|
||||
CopyReadOnly(regionReports));
|
||||
}
|
||||
|
||||
/// <summary>创建不发布路径的失败、不可行、取消或输入无效结果。</summary>
|
||||
/// <summary>创建明确不发布路径的失败、取消或无效输入结果。</summary>
|
||||
/// <param name="status">非可发布的终止状态。</param>
|
||||
/// <param name="diagnostics">失败原因、质量指标和耗时；为 <see langword="null"/> 时生成默认诊断。</param>
|
||||
/// <returns>路径、方向段和区域报告均为空的不可变结果，不能作为部分路径使用。</returns>
|
||||
public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
if (status == PathSmoothingStatus.Success ||
|
||||
status == PathSmoothingStatus.FallbackToCoarsePath ||
|
||||
status == PathSmoothingStatus.Complete ||
|
||||
if (status == PathSmoothingStatus.Complete ||
|
||||
status == PathSmoothingStatus.PartialImprovement ||
|
||||
status == PathSmoothingStatus.NotNeeded ||
|
||||
status == PathSmoothingStatus.Unchanged)
|
||||
throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status));
|
||||
{
|
||||
throw new ArgumentException("Use PublishLocalG2 to publish a path.", nameof(status));
|
||||
}
|
||||
if (!Enum.IsDefined(typeof(PathSmoothingStatus), status))
|
||||
throw new ArgumentOutOfRangeException(nameof(status));
|
||||
return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics, EmptyRegionReports);
|
||||
}
|
||||
|
||||
private static void ValidatePublishedResult(
|
||||
SmoothingMethod method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
|
||||
throw new ArgumentOutOfRangeException(nameof(method));
|
||||
if (path == null || path.Count == 0)
|
||||
throw new ArgumentException("Published smoothing results require a non-empty path.", nameof(path));
|
||||
if (segments == null || segments.Count == 0)
|
||||
@@ -147,8 +122,7 @@ public sealed class PathSmoothingResult
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,10 +1,8 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的最终发布状态。</summary>
|
||||
/// <summary>路径平滑的最终发布状态；只有 Complete、PartialImprovement、NotNeeded 和 Unchanged 可携带完整路径。</summary>
|
||||
public enum PathSmoothingStatus
|
||||
{
|
||||
Success,
|
||||
FallbackToCoarsePath,
|
||||
InvalidInput,
|
||||
Infeasible,
|
||||
Failed,
|
||||
|
||||
@@ -1,11 +0,0 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>分段五次 Hermite 平滑参数。</summary>
|
||||
public sealed class PiecewiseQuinticOptions
|
||||
{
|
||||
/// <summary>相邻内部结点的目标距离，单位 m。</summary>
|
||||
public double KnotSpacingMeters { get; set; } = 0.50d;
|
||||
|
||||
/// <summary>允许创建内部结点的最小间距，单位 m。</summary>
|
||||
public double MinimumKnotSpacingMeters { get; set; } = 0.10d;
|
||||
}
|
||||
@@ -1,11 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>平滑路径点的来源。</summary>
|
||||
/// <summary>平滑路径点的来源；用于区分保留锚点、常规重采样、换向锚点和 Local G2 过渡几何。</summary>
|
||||
public enum SmoothedPathPointSource
|
||||
{
|
||||
/// <summary>直接保留的原始路径锚点。</summary>
|
||||
Anchor,
|
||||
/// <summary>在同一方向段内按弧长插值得到的常规采样点。</summary>
|
||||
Interpolated,
|
||||
/// <summary>精确保留的换向边界点；不能与相邻方向段按坐标合并。</summary>
|
||||
GearSwitch,
|
||||
CoarsePathFallback,
|
||||
LocalG2Transition,
|
||||
/// <summary>由 Local G2 五次过渡曲线生成的候选或发布点。</summary>
|
||||
LocalG2Transition = 4,
|
||||
}
|
||||
|
||||
@@ -1,10 +1,7 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>支持的粗路径平滑方法。</summary>
|
||||
/// <summary>本模块可以发布的路径平滑方法枚举；当前仅支持 Local G2 五次过渡。</summary>
|
||||
public enum SmoothingMethod
|
||||
{
|
||||
CubicBSpline,
|
||||
LocalCubicBezier,
|
||||
PiecewiseQuintic,
|
||||
LocalG2Quintic,
|
||||
}
|
||||
|
||||
+37
-46
@@ -2,13 +2,16 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
/// <summary>以固定预热和五次测量隔离比较所有请求平滑方法的离线入口。</summary>
|
||||
/// <summary>
|
||||
/// 原始路径基线与一次 Local G2 平滑的离线比较入口。
|
||||
/// 此类仅汇总已冻结请求的质量、确定性摘要和耗时，不参与实时控制或修改平滑结果。
|
||||
/// </summary>
|
||||
public sealed class PathSmoothingComparisonService
|
||||
{
|
||||
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
|
||||
@@ -16,13 +19,16 @@ public sealed class PathSmoothingComparisonService
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
|
||||
/// <summary>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
|
||||
/// <summary>创建原始基线并比较一次 Local G2 运行的质量、耗时和推荐资格。</summary>
|
||||
/// <param name="request">比较请求，包含待平滑的不可变请求快照和比较设置；为 <see langword="null"/> 时返回带失败基线的结果。</param>
|
||||
/// <param name="cancellationToken">取消比较和重复计时的调用方令牌。</param>
|
||||
/// <returns>包含原始基线、候选条目和推荐方法的不可变比较结果；取消时 <c>Cancelled</c> 为 <see langword="true"/>。</returns>
|
||||
public PathSmoothingComparisonResult Compare(
|
||||
PathSmoothingComparisonRequest request,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason);
|
||||
var entries = new List<PathSmoothingComparisonEntry>();
|
||||
var entries = new List<PathSmoothingComparisonEntry>(1);
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Cancelled(baseline, entries);
|
||||
@@ -30,33 +36,26 @@ public sealed class PathSmoothingComparisonService
|
||||
if (request == null || request.SmoothingRequest == null)
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, false, baselineReason);
|
||||
|
||||
for (int methodIndex = 0; methodIndex < request.Methods.Count; methodIndex++)
|
||||
if (!TryCompareLocalG2(
|
||||
request.SmoothingRequest,
|
||||
baseline.Metrics,
|
||||
cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry))
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Cancelled(baseline, entries);
|
||||
|
||||
SmoothingMethod method = request.Methods[methodIndex];
|
||||
if (!TryCompareMethod(
|
||||
request.SmoothingRequest,
|
||||
method,
|
||||
baseline.Metrics,
|
||||
cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry))
|
||||
return Cancelled(baseline, entries);
|
||||
entries.Add(entry);
|
||||
return Cancelled(baseline, entries);
|
||||
}
|
||||
|
||||
entries.Add(entry);
|
||||
return new PathSmoothingComparisonResult(
|
||||
baseline,
|
||||
entries,
|
||||
SmoothingMethodRanker.Rank(entries),
|
||||
entry.IsEligibleForRecommendation ? SmoothingMethod.LocalG2Quintic : null,
|
||||
false,
|
||||
baselineReason);
|
||||
}
|
||||
|
||||
private bool TryCompareMethod(
|
||||
PathSmoothingRequest sourceRequest,
|
||||
SmoothingMethod method,
|
||||
private bool TryCompareLocalG2(
|
||||
PathSmoothingRequest smoothingRequest,
|
||||
PathQualityMetrics rawMetrics,
|
||||
CancellationToken cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry)
|
||||
@@ -64,9 +63,8 @@ public sealed class PathSmoothingComparisonService
|
||||
entry = null;
|
||||
try
|
||||
{
|
||||
PathSmoothingRequest methodRequest = CreateMethodRequest(sourceRequest, method);
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PathSmoothingResult warmup = _smoothingService.Smooth(methodRequest, cancellationToken);
|
||||
PathSmoothingResult warmup = _smoothingService.Smooth(smoothingRequest, cancellationToken);
|
||||
if (warmup.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
|
||||
|
||||
var timings = new List<double>(5);
|
||||
@@ -75,7 +73,7 @@ public sealed class PathSmoothingComparisonService
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Stopwatch stopwatch = Stopwatch.StartNew();
|
||||
PathSmoothingResult result = _smoothingService.Smooth(methodRequest, cancellationToken);
|
||||
PathSmoothingResult result = _smoothingService.Smooth(smoothingRequest, cancellationToken);
|
||||
stopwatch.Stop();
|
||||
if (result.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
|
||||
timings.Add(stopwatch.Elapsed.TotalMilliseconds);
|
||||
@@ -88,12 +86,11 @@ public sealed class PathSmoothingComparisonService
|
||||
string diagnostic = string.IsNullOrWhiteSpace(timing.Diagnostic)
|
||||
? canonical.Diagnostics.TerminationReason
|
||||
: timing.Diagnostic;
|
||||
|
||||
PathQualityMetrics metrics = canonical.Status == PathSmoothingStatus.Success
|
||||
PathQualityMetrics metrics = PathSmoothingComparisonEntry.IsPublishedLocalG2Status(canonical.Status)
|
||||
? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics)
|
||||
: new PathQualityMetrics();
|
||||
entry = PathSmoothingComparisonEntry.CreateCandidate(
|
||||
method,
|
||||
SmoothingMethod.LocalG2Quintic,
|
||||
canonical.Status,
|
||||
metrics,
|
||||
timing,
|
||||
@@ -110,7 +107,7 @@ public sealed class PathSmoothingComparisonService
|
||||
catch (Exception exception)
|
||||
{
|
||||
entry = PathSmoothingComparisonEntry.CreateCandidate(
|
||||
method,
|
||||
SmoothingMethod.LocalG2Quintic,
|
||||
PathSmoothingStatus.Failed,
|
||||
new PathQualityMetrics(),
|
||||
new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, exception.GetType().Name),
|
||||
@@ -128,12 +125,17 @@ public sealed class PathSmoothingComparisonService
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (request == null || request.SmoothingRequest == null)
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求为空。", out reason);
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "Comparison request is required.", out reason);
|
||||
|
||||
PathSmoothingRequest smoothingRequest = request.SmoothingRequest;
|
||||
PathSmoothingConfiguration configuration = smoothingRequest.Configuration;
|
||||
if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null)
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求缺少可用的地图、车辆或配置。", out reason);
|
||||
{
|
||||
return FailedBaseline(
|
||||
PathSmoothingStatus.InvalidInput,
|
||||
"A planning-ready map, vehicle, and configuration are required.",
|
||||
out reason);
|
||||
}
|
||||
|
||||
if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason))
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
|
||||
@@ -146,11 +148,13 @@ public sealed class PathSmoothingComparisonService
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline rawPath,
|
||||
out reason))
|
||||
{
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
|
||||
}
|
||||
|
||||
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, rawPath.Path, rawPath.Segments);
|
||||
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.NotNeeded, null, rawPath.Path, rawPath.Segments);
|
||||
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
|
||||
PathSmoothingStatus.Success,
|
||||
PathSmoothingStatus.NotNeeded,
|
||||
rawPath.Metrics,
|
||||
digest,
|
||||
string.Empty,
|
||||
@@ -177,20 +181,7 @@ public sealed class PathSmoothingComparisonService
|
||||
PathSmoothingComparisonEntry baseline,
|
||||
IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
||||
{
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, true, "路径平滑比较已取消。");
|
||||
}
|
||||
|
||||
private static PathSmoothingRequest CreateMethodRequest(PathSmoothingRequest source, SmoothingMethod method)
|
||||
{
|
||||
PathSmoothingConfiguration configuration = source.Configuration;
|
||||
configuration.Method = method;
|
||||
configuration.AllowFallbackToCoarsePath = false;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
configuration);
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, true, "Path smoothing comparison was cancelled.");
|
||||
}
|
||||
|
||||
private static PathQualityMetrics NormalizeMetrics(
|
||||
|
||||
@@ -1,28 +1,30 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
/// <summary>正式单算法路径平滑入口，负责输入校验、有限重试与经过复核的粗路径回退。</summary>
|
||||
/// <summary>
|
||||
/// Local G2 路径平滑的公开、同步业务入口。
|
||||
/// 它只消费调用方冻结的请求，依次执行输入校验、预处理、原始基线、局部 G2 和独立复核；从不发布未经验证的部分路径。
|
||||
/// </summary>
|
||||
public sealed class PathSmoothingService
|
||||
{
|
||||
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
|
||||
private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother();
|
||||
private readonly IPathSmoother _quintic = new PiecewiseQuinticSmoother();
|
||||
private readonly LocalG2PreSmoothingPipeline _localG2Pipeline = new LocalG2PreSmoothingPipeline();
|
||||
|
||||
/// <summary>执行一次经过完整安全复核的单算法平滑。</summary>
|
||||
/// <summary>在请求携带的不可变粗路径、地图和车辆快照上执行一次 Local G2 平滑。</summary>
|
||||
/// <param name="request">平滑输入快照，包含粗路径/方向段、规划地图、车辆和配置；路径单位为 m/rad，曲率单位为 1/m。</param>
|
||||
/// <param name="cancellationToken">调用方取消令牌；取消时返回 <see cref="PathSmoothingStatus.Cancelled"/>，不会发布部分路径。</param>
|
||||
/// <returns>成功时携带完整独立复核路径的结果；输入、预处理、碰撞、曲率或运行失败时路径和分段为空，原因位于诊断中。</returns>
|
||||
public PathSmoothingResult Smooth(
|
||||
PathSmoothingRequest request,
|
||||
CancellationToken cancellationToken = default)
|
||||
@@ -32,10 +34,10 @@ public sealed class PathSmoothingService
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!TryValidateRequest(request, out PathSmoothingConfiguration configuration, out string reason))
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, reason);
|
||||
|
||||
if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, reason);
|
||||
|
||||
if (!RawPathBaselineBuilder.TryCreate(
|
||||
request,
|
||||
@@ -44,126 +46,24 @@ public sealed class PathSmoothingService
|
||||
configuration.OutputSpacingMeters,
|
||||
_validator,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out _,
|
||||
out RawPathBaseline rawBaseline,
|
||||
out reason))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, reason);
|
||||
}
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
var input = new SmoothingAlgorithmInput(
|
||||
preparedPath,
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
configuration.MinimumClearanceReserveMeters,
|
||||
new SmoothingOptionsSnapshot(configuration));
|
||||
SmoothingAlgorithmRunner.AlgorithmRunResult runResult = _runner.Run(
|
||||
Resolve(configuration.Method), input, configuration, cancellationToken);
|
||||
int retryCount = GetRetryCount(runResult.AttemptedStrengths);
|
||||
PathSmoothingDiagnostics diagnostics = new PathSmoothingDiagnostics(
|
||||
runResult.Metrics,
|
||||
stopwatch.Elapsed,
|
||||
retryCount,
|
||||
runResult.AcceptedStrength,
|
||||
runResult.Reason);
|
||||
|
||||
if (runResult.Status == PathSmoothingStatus.Success)
|
||||
{
|
||||
return PathSmoothingResult.Success(
|
||||
configuration.Method,
|
||||
runResult.Path,
|
||||
runResult.Segments,
|
||||
diagnostics);
|
||||
}
|
||||
|
||||
if (!configuration.AllowFallbackToCoarsePath)
|
||||
return PathSmoothingResult.Failure(runResult.Status, diagnostics);
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!TryCreateVerifiedFallback(
|
||||
request,
|
||||
configuration,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothedPathPoint> fallbackPath,
|
||||
out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
|
||||
out PathQualityMetrics fallbackMetrics,
|
||||
out reason))
|
||||
{
|
||||
return PathSmoothingResult.Failure(runResult.Status, diagnostics);
|
||||
}
|
||||
|
||||
var fallbackDiagnostics = new PathSmoothingDiagnostics(
|
||||
fallbackMetrics,
|
||||
stopwatch.Elapsed,
|
||||
retryCount,
|
||||
runResult.AcceptedStrength,
|
||||
runResult.Reason);
|
||||
return PathSmoothingResult.Fallback(
|
||||
configuration.Method,
|
||||
fallbackPath,
|
||||
fallbackSegments,
|
||||
fallbackDiagnostics);
|
||||
return _localG2Pipeline.Smooth(request, preparedPath, rawBaseline, cancellationToken);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Cancelled, stopwatch, 0, 0d, "路径平滑已取消。");
|
||||
return Failure(PathSmoothingStatus.Cancelled, stopwatch, "Path smoothing was cancelled.");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, 0, 0d, exception.Message);
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryCreateVerifiedFallback(
|
||||
PathSmoothingRequest request,
|
||||
PathSmoothingConfiguration configuration,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothedPathPoint> fallbackPath,
|
||||
out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
|
||||
out PathQualityMetrics fallbackMetrics,
|
||||
out string reason)
|
||||
{
|
||||
fallbackPath = null;
|
||||
fallbackSegments = null;
|
||||
fallbackMetrics = null;
|
||||
reason = string.Empty;
|
||||
|
||||
// Reprepare from the immutable request instead of reusing the algorithm input: fallback is a
|
||||
// separately published output and must repeat the coarse-path contract validation.
|
||||
if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false;
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
if (!RawPathBaselineBuilder.TryCreate(
|
||||
request,
|
||||
revalidatedPath,
|
||||
_analyzer,
|
||||
configuration.OutputSpacingMeters,
|
||||
_validator,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline rawPath,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
fallbackPath = ToFallbackPoints(rawPath.Path);
|
||||
fallbackSegments = rawPath.Segments;
|
||||
fallbackMetrics = rawPath.Metrics;
|
||||
return true;
|
||||
}
|
||||
|
||||
private IPathSmoother Resolve(SmoothingMethod method)
|
||||
{
|
||||
return method switch
|
||||
{
|
||||
SmoothingMethod.CubicBSpline => _bSpline,
|
||||
SmoothingMethod.LocalCubicBezier => _bezier,
|
||||
SmoothingMethod.PiecewiseQuintic => _quintic,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(method)),
|
||||
};
|
||||
}
|
||||
|
||||
private static bool TryValidateRequest(
|
||||
PathSmoothingRequest request,
|
||||
out PathSmoothingConfiguration configuration,
|
||||
@@ -173,7 +73,7 @@ public sealed class PathSmoothingService
|
||||
reason = string.Empty;
|
||||
if (request == null)
|
||||
{
|
||||
reason = "平滑请求为空。";
|
||||
reason = "Path smoothing request is required.";
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -181,7 +81,7 @@ public sealed class PathSmoothingService
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
if (configuration == null || request.Map == null || !request.Map.PlanningReady || vehicle == null)
|
||||
{
|
||||
reason = "平滑请求缺少可用的地图、车辆或配置。";
|
||||
reason = "A planning-ready map, vehicle, and configuration are required.";
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -190,13 +90,7 @@ public sealed class PathSmoothingService
|
||||
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _))
|
||||
{
|
||||
reason = "平滑请求中的车辆几何或曲率约束无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), configuration.Method))
|
||||
{
|
||||
reason = "平滑方法无效。";
|
||||
reason = "Vehicle geometry or curvature constraints are invalid.";
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -204,63 +98,48 @@ public sealed class PathSmoothingService
|
||||
!NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
|
||||
!NumericGuard.IsFinite(configuration.MinimumClearanceReserveMeters) ||
|
||||
configuration.MinimumClearanceReserveMeters < 0d ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.SmoothingStrength) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.CubicBSpline.EndpointTangentScale) ||
|
||||
!IsValidBezierThreshold(configuration.LocalCubicBezier.CornerHeadingThresholdRadians) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.MaximumWindowLengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.HandleLengthRatio) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.KnotSpacingMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.MinimumKnotSpacingMeters) ||
|
||||
configuration.PiecewiseQuintic.KnotSpacingMeters < configuration.PiecewiseQuintic.MinimumKnotSpacingMeters)
|
||||
!CurvatureLimitPolicy.TryGetAllowedMaximumVehicleCurvaturePerMeter(
|
||||
vehicle, configuration.CurvatureLimitRadiusToleranceMeters, out _))
|
||||
{
|
||||
reason = "平滑配置包含非法数值或不满足方法契约。";
|
||||
reason = "Shared Local G2 configuration values are invalid.";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
||||
return IsValidLocalG2Options(configuration.LocalG2Quintic, out reason);
|
||||
}
|
||||
|
||||
private static bool IsValidBezierThreshold(double thresholdRadians)
|
||||
private static bool IsValidLocalG2Options(LocalG2QuinticOptions options, out string reason)
|
||||
{
|
||||
return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothedPathPoint> ToFallbackPoints(IReadOnlyList<SmoothedPathPoint> path)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>(path.Count);
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
reason = string.Empty;
|
||||
if (options != null &&
|
||||
NumericGuard.IsPositiveFinite(options.MinimumWindowLengthMeters) &&
|
||||
NumericGuard.IsFinite(options.PreferredWindowLengthMeters) &&
|
||||
options.PreferredWindowLengthMeters >= options.MinimumWindowLengthMeters &&
|
||||
NumericGuard.IsFinite(options.MaximumWindowLengthMeters) &&
|
||||
options.MaximumWindowLengthMeters >= options.PreferredWindowLengthMeters &&
|
||||
NumericGuard.IsPositiveFinite(options.MaximumDeviationMeters) &&
|
||||
NumericGuard.IsPositiveFinite(options.AbsoluteCurvatureJumpFloorPerMeter) &&
|
||||
NumericGuard.IsFinite(options.CurvatureJumpRatioOfMaximum) &&
|
||||
options.CurvatureJumpRatioOfMaximum > 0d && options.CurvatureJumpRatioOfMaximum <= 1d &&
|
||||
NumericGuard.IsFinite(options.MinimumPeakGradientImprovementRatio) &&
|
||||
options.MinimumPeakGradientImprovementRatio > 0d && options.MinimumPeakGradientImprovementRatio < 1d &&
|
||||
NumericGuard.IsFinite(options.MaximumVariationCostRegressionRatio) &&
|
||||
options.MaximumVariationCostRegressionRatio >= 0d && options.MaximumCandidatesPerRegion >= 1)
|
||||
{
|
||||
SmoothedPathPoint point = path[index];
|
||||
points.Add(new SmoothedPathPoint(
|
||||
point.X,
|
||||
point.Y,
|
||||
point.Heading,
|
||||
point.UnwrappedHeading,
|
||||
point.ArcLength,
|
||||
point.Direction,
|
||||
point.GeometricCurvature,
|
||||
point.VehicleCurvature,
|
||||
point.VehicleCurvatureDerivative,
|
||||
point.BodyClearance,
|
||||
point.IsGearSwitchPoint,
|
||||
SmoothedPathPointSource.CoarsePathFallback));
|
||||
return true;
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
private static int GetRetryCount(IReadOnlyList<double> attemptedStrengths)
|
||||
{
|
||||
return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1;
|
||||
reason = "Local G2 configuration values are invalid.";
|
||||
return false;
|
||||
}
|
||||
|
||||
private static PathSmoothingResult Failure(
|
||||
PathSmoothingStatus status,
|
||||
Stopwatch stopwatch,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string reason)
|
||||
{
|
||||
return PathSmoothingResult.Failure(
|
||||
status,
|
||||
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, retryCount, acceptedStrength, reason));
|
||||
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, reason));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -12,7 +12,7 @@ using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>对一个局部 G2 替换候选执行区域质量门和完整路径安全复核。</summary>
|
||||
/// <summary>对局部 G2 替换候选执行区域质量门、整条路径几何重算及碰撞安全复核。</summary>
|
||||
internal sealed class LocalG2CandidateEvaluator
|
||||
{
|
||||
private const double CurvatureRangeTolerance = 1e-6d;
|
||||
@@ -25,11 +25,16 @@ internal sealed class LocalG2CandidateEvaluator
|
||||
private readonly LocalG2PathSplicer _splicer;
|
||||
private readonly SmoothedPathValidator _validator;
|
||||
|
||||
/// <summary>使用默认几何分析、路径拼接和安全校验器创建候选评估器。</summary>
|
||||
internal LocalG2CandidateEvaluator()
|
||||
: this(new PathGeometryAnalyzer(), new LocalG2PathSplicer(), new SmoothedPathValidator())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>使用注入的依赖创建候选评估器，便于复用统一的几何与碰撞判定规则。</summary>
|
||||
/// <param name="analyzer">将预处理路径转换为带曲率指标的 <see cref="PathGeometryAnalysis"/> 的分析器。</param>
|
||||
/// <param name="splicer">将候选窗口替换回当前路径并维持换向拓扑的拼接器。</param>
|
||||
/// <param name="validator">对完整候选路径执行碰撞、净空和轨迹不变量检查的校验器。</param>
|
||||
internal LocalG2CandidateEvaluator(
|
||||
PathGeometryAnalyzer analyzer,
|
||||
LocalG2PathSplicer splicer,
|
||||
@@ -40,6 +45,15 @@ internal sealed class LocalG2CandidateEvaluator
|
||||
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
||||
}
|
||||
|
||||
/// <summary>评估一个局部 G2 候选，仅在全部质量门与完整路径安全复核通过时接受。</summary>
|
||||
/// <param name="rawPath">未经局部替换的基线路径；用于约束曲率范围和总长度变化。</param>
|
||||
/// <param name="currentPath">已接受先前窗口替换的当前路径；本候选将在其指定方向段内拼接。</param>
|
||||
/// <param name="region">候选所属的单一行驶方向平滑区域。</param>
|
||||
/// <param name="candidate">待评估候选的窗口边界与几何点；坐标和弧长单位为 m。</param>
|
||||
/// <param name="request">平滑请求，提供地图、车辆模型、采样间距 m 和安全阈值。</param>
|
||||
/// <param name="options">局部 G2 质量门选项，包括偏差 m、曲率导数 1/m² 和代价回退阈值。</param>
|
||||
/// <param name="cancellationToken">取消令牌；取消时抛出 <see cref="OperationCanceledException"/>。</param>
|
||||
/// <returns>包含接受状态、失败原因和度量值的 <see cref="LocalG2CandidateEvaluation"/>；拒绝时不发布候选路径。</returns>
|
||||
internal LocalG2CandidateEvaluation Evaluate(
|
||||
PreparedPath rawPath,
|
||||
PreparedPath currentPath,
|
||||
@@ -67,7 +81,9 @@ internal sealed class LocalG2CandidateEvaluator
|
||||
string.IsNullOrEmpty(reason) ? "局部 G2 区域几何分析失败。" : reason);
|
||||
}
|
||||
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double vehicleMaximumCurvature))
|
||||
if (!CurvatureLimitPolicy.TryGetAllowedMaximumVehicleCurvaturePerMeter(
|
||||
request.Vehicle, request.Configuration.CurvatureLimitRadiusToleranceMeters,
|
||||
out double vehicleMaximumCurvature))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "车辆曲率约束无效。");
|
||||
if (candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter > vehicleMaximumCurvature + CurvatureRangeTolerance)
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureExceeded, "局部 G2 候选超过车辆曲率上限。");
|
||||
@@ -91,7 +107,9 @@ internal sealed class LocalG2CandidateEvaluator
|
||||
}
|
||||
|
||||
if (!_validator.TryValidate(fullAnalysis.Path, fullAnalysis.Segments, rawPath, request.Map, request.Vehicle,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters,
|
||||
request.Configuration.CurvatureLimitRadiusToleranceMeters,
|
||||
out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearance, out reason))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.Collision,
|
||||
@@ -145,6 +163,9 @@ internal sealed class LocalG2CandidateEvaluator
|
||||
"Accepted");
|
||||
}
|
||||
|
||||
/// <summary>在已通过质量门的候选中按偏差、峰值曲率导数、变化代价、长度变化和索引稳定选优。</summary>
|
||||
/// <param name="evaluations">待比较的候选评估结果集合；仅 <c>Accepted</c> 为 <see langword="true"/> 的项可入选。</param>
|
||||
/// <returns>最优的已接受评估；若没有合格项，返回带失败原因的拒绝评估。</returns>
|
||||
internal static LocalG2CandidateEvaluation SelectBest(IReadOnlyList<LocalG2CandidateEvaluation> evaluations)
|
||||
{
|
||||
if (evaluations == null) throw new ArgumentNullException(nameof(evaluations));
|
||||
|
||||
@@ -0,0 +1,242 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>通过专用服务路径生成、回滚并发布经独立验证的 Local G2 局部区域替换；不发布未经最终复核的候选。</summary>
|
||||
internal sealed class LocalG2PreSmoothingPipeline
|
||||
{
|
||||
private readonly CurvatureTransitionDetector _detector = new CurvatureTransitionDetector();
|
||||
private readonly LocalG2WindowPlanner _windowPlanner = new LocalG2WindowPlanner();
|
||||
private readonly LocalG2CandidateBuilder _builder = new LocalG2CandidateBuilder();
|
||||
private readonly LocalG2CandidateEvaluator _evaluator = new LocalG2CandidateEvaluator();
|
||||
private readonly LocalG2RegionWorkOrder _workOrder = new LocalG2RegionWorkOrder();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
|
||||
/// <summary>在预处理路径上检测曲率事件、评估候选、按确定顺序拼接，并执行全局回滚复核。</summary>
|
||||
/// <param name="request">不可变平滑请求，提供地图、车辆、配置和路径上下文。</param>
|
||||
/// <param name="preparedPath">按方向段准备并重采样的原始路径；局部弧长单位为 m。</param>
|
||||
/// <param name="rawBaseline">已独立复核的原始路径质量基线。</param>
|
||||
/// <param name="cancellationToken">调用方取消令牌；取消时返回空路径的 Cancelled 结果。</param>
|
||||
/// <returns>完整、部分、无需或保持原样时均只发布最终复核路径；失败/取消时不发布部分候选。</returns>
|
||||
internal PathSmoothingResult Smooth(
|
||||
PathSmoothingRequest request,
|
||||
PreparedPath preparedPath,
|
||||
RawPathBaseline rawBaseline,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
try
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (request == null || preparedPath == null || rawBaseline == null ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvature))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, "局部 G2 预平滑输入无效。");
|
||||
}
|
||||
|
||||
var options = new LocalG2OptionsSnapshot(request.Configuration);
|
||||
if (!_detector.TryDetect(request, maximumCurvature, options, out IReadOnlyList<CurvatureTransition> transitions, out string reason) ||
|
||||
!_windowPlanner.TryPlan(preparedPath, transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out reason))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
|
||||
}
|
||||
|
||||
IReadOnlyList<LocalG2SmoothingRegion> reportOrder =
|
||||
new ReadOnlyCollection<LocalG2SmoothingRegion>(new List<LocalG2SmoothingRegion>(regions));
|
||||
if (!_workOrder.TryCreate(reportOrder, out IReadOnlyList<LocalG2SmoothingRegion> workRegions, out string orderReason))
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, orderReason);
|
||||
|
||||
PreparedPath current = preparedPath;
|
||||
var reportsByRegion = new Dictionary<LocalG2SmoothingRegion, PathSmoothingRegionReport>();
|
||||
var accepted = new List<AcceptedRegion>();
|
||||
int improvedCount = 0;
|
||||
|
||||
foreach (LocalG2SmoothingRegion region in workRegions)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
IReadOnlyList<LocalG2CandidateGeometry> candidates = _builder.Build(
|
||||
preparedPath.Segments[region.SegmentIndex], region,
|
||||
request.Configuration.OutputSpacingMeters, options, cancellationToken);
|
||||
var evaluations = new List<LocalG2CandidateEvaluation>();
|
||||
for (int candidateIndex = 0; candidateIndex < candidates.Count; candidateIndex++)
|
||||
evaluations.Add(_evaluator.Evaluate(
|
||||
preparedPath, current, region, candidates[candidateIndex], request, options, cancellationToken));
|
||||
|
||||
LocalG2CandidateEvaluation best = LocalG2CandidateEvaluator.SelectBest(evaluations);
|
||||
if (best.Accepted)
|
||||
{
|
||||
accepted.Add(new AcceptedRegion(region, current, best));
|
||||
current = best.SplicedPreparedPath;
|
||||
improvedCount++;
|
||||
reportsByRegion.Add(region, CreateImprovedReport(region, candidates.Count, best));
|
||||
}
|
||||
else
|
||||
{
|
||||
reportsByRegion.Add(region, CreateRetainedReport(region, candidates.Count, best));
|
||||
}
|
||||
}
|
||||
|
||||
if (!TryValidateFinal(current, preparedPath, rawBaseline, request, options, out IReadOnlyList<SmoothedPathPoint> path,
|
||||
out IReadOnlyList<SmoothedPathSegment> segments, out PathQualityMetrics metrics, out reason))
|
||||
{
|
||||
for (int index = accepted.Count - 1; index >= 0; index--)
|
||||
{
|
||||
AcceptedRegion rollback = accepted[index];
|
||||
current = rollback.Before;
|
||||
reportsByRegion[rollback.Region] = CreateRollbackReport(rollback.Region, rollback.Evaluation);
|
||||
improvedCount--;
|
||||
if (TryValidateFinal(current, preparedPath, rawBaseline, request, options, out path, out segments, out metrics, out reason))
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (metrics == null)
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
|
||||
|
||||
var reports = new List<PathSmoothingRegionReport>(reportOrder.Count);
|
||||
for (int reportIndex = 0; reportIndex < reportOrder.Count; reportIndex++)
|
||||
reports.Add(reportsByRegion[reportOrder[reportIndex]]);
|
||||
|
||||
PathSmoothingStatus status;
|
||||
if (transitions.Count == 0) status = PathSmoothingStatus.NotNeeded;
|
||||
else if (improvedCount == regions.Count) status = PathSmoothingStatus.Complete;
|
||||
else if (improvedCount > 0) status = PathSmoothingStatus.PartialImprovement;
|
||||
else status = PathSmoothingStatus.Unchanged;
|
||||
return PathSmoothingResult.PublishLocalG2(
|
||||
status,
|
||||
path,
|
||||
segments,
|
||||
new PathSmoothingDiagnostics(metrics, stopwatch.Elapsed, reason ?? string.Empty),
|
||||
reports);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Cancelled, stopwatch, "路径平滑已取消。");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryValidateFinal(
|
||||
PreparedPath current,
|
||||
PreparedPath rawPath,
|
||||
RawPathBaseline rawBaseline,
|
||||
PathSmoothingRequest request,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<SmoothedPathPoint> path,
|
||||
out IReadOnlyList<SmoothedPathSegment> segments,
|
||||
out PathQualityMetrics metrics,
|
||||
out string reason)
|
||||
{
|
||||
path = null;
|
||||
segments = null;
|
||||
metrics = null;
|
||||
reason = string.Empty;
|
||||
if (!_analyzer.TryAnalyze(current.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason) ||
|
||||
!_validator.TryValidate(analysis.Path, analysis.Segments, rawPath, request.Map, request.Vehicle,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters,
|
||||
request.Configuration.CurvatureLimitRadiusToleranceMeters,
|
||||
out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearance, out reason) ||
|
||||
minimumClearance < request.Configuration.MinimumClearanceReserveMeters ||
|
||||
analysis.CurvatureVariationCost > rawBaseline.Metrics.CurvatureVariationCost *
|
||||
(1d + options.MaximumVariationCostRegressionRatio))
|
||||
{
|
||||
if (string.IsNullOrEmpty(reason)) reason = "局部 G2 完整路径复核未通过。";
|
||||
return false;
|
||||
}
|
||||
|
||||
path = safePath;
|
||||
segments = analysis.Segments;
|
||||
metrics = new PathQualityMetrics(
|
||||
true,
|
||||
analysis.PathLengthMeters,
|
||||
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
analysis.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
analysis.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
analysis.CurvatureVariationCost,
|
||||
minimumClearance,
|
||||
0d, 0d, 0d, 0d);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static PathSmoothingRegionReport CreateImprovedReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.Improved, PathSmoothingRegionFailureReason.None);
|
||||
|
||||
private static PathSmoothingRegionReport CreateRetainedReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.RetainedOriginal, evaluation.FailureReason);
|
||||
|
||||
private static PathSmoothingRegionReport CreateRollbackReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, region.WindowVariants.Count, evaluation, PathSmoothingRegionStatus.RetainedOriginal,
|
||||
PathSmoothingRegionFailureReason.GlobalValidationRollback);
|
||||
|
||||
private static PathSmoothingRegionReport CreateReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation,
|
||||
PathSmoothingRegionStatus status,
|
||||
PathSmoothingRegionFailureReason failureReason)
|
||||
{
|
||||
LocalG2WindowVariant window = region.WindowVariants[0];
|
||||
var jumps = new List<double>(region.Transitions.Count);
|
||||
for (int index = 0; index < region.Transitions.Count; index++)
|
||||
jumps.Add(region.Transitions[index].RightVehicleCurvaturePerMeter - region.Transitions[index].LeftVehicleCurvaturePerMeter);
|
||||
return new PathSmoothingRegionReport(
|
||||
region.SegmentIndex,
|
||||
window.StartArcLengthMeters,
|
||||
window.EndArcLengthMeters,
|
||||
jumps,
|
||||
window.EndArcLengthMeters - window.StartArcLengthMeters,
|
||||
window.EndArcLengthMeters - window.StartArcLengthMeters,
|
||||
window.LeftWindowLengthMeters,
|
||||
window.RightWindowLengthMeters,
|
||||
candidateCount,
|
||||
evaluation.CandidateIndex,
|
||||
status,
|
||||
failureReason,
|
||||
evaluation.RawPeakCurvatureDerivativePerSquareMeter,
|
||||
evaluation.ResultPeakCurvatureDerivativePerSquareMeter,
|
||||
evaluation.RawCurvatureVariationCost,
|
||||
evaluation.ResultCurvatureVariationCost,
|
||||
evaluation.MaximumDeviationMeters,
|
||||
evaluation.MinimumBodyClearanceMeters,
|
||||
evaluation.MaximumAbsoluteVehicleCurvaturePerMeter);
|
||||
}
|
||||
|
||||
private static PathSmoothingResult Failure(PathSmoothingStatus status, Stopwatch stopwatch, string reason) =>
|
||||
PathSmoothingResult.Failure(status,
|
||||
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, reason));
|
||||
|
||||
private sealed class AcceptedRegion
|
||||
{
|
||||
internal AcceptedRegion(LocalG2SmoothingRegion region, PreparedPath before, LocalG2CandidateEvaluation evaluation)
|
||||
{
|
||||
Region = region;
|
||||
Before = before;
|
||||
Evaluation = evaluation;
|
||||
}
|
||||
|
||||
internal LocalG2SmoothingRegion Region { get; }
|
||||
internal PreparedPath Before { get; }
|
||||
internal LocalG2CandidateEvaluation Evaluation { get; }
|
||||
}
|
||||
}
|
||||
+12
-15
@@ -2,12 +2,11 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
/// <summary>原始基线或一种平滑方法的不可变比较条目。</summary>
|
||||
/// <summary>Immutable raw-baseline or Local G2 report entry.</summary>
|
||||
public sealed class PathSmoothingComparisonEntry
|
||||
{
|
||||
/// <summary>为测试、离线分析和排序创建不携带路径几何的候选条目。</summary>
|
||||
public PathSmoothingComparisonEntry(
|
||||
SmoothingMethod method,
|
||||
PathSmoothingStatus status,
|
||||
@@ -34,47 +33,45 @@ public sealed class PathSmoothingComparisonEntry
|
||||
IsRawPathBaseline = isRawPathBaseline;
|
||||
Status = status;
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
Timing = timing ?? new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, "未提供计时结果。");
|
||||
Timing = timing ?? new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, "No timing result was supplied.");
|
||||
StableGeometryDigest = stableGeometryDigest ?? string.Empty;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
Path = CopyReadOnly(path);
|
||||
Segments = CopyReadOnly(segments);
|
||||
}
|
||||
|
||||
/// <summary>候选所代表的方法；原始粗路径基线为空。</summary>
|
||||
public SmoothingMethod? Method { get; }
|
||||
|
||||
/// <summary>是否为单独分析的原始粗路径基线。</summary>
|
||||
public bool IsRawPathBaseline { get; }
|
||||
|
||||
/// <summary>本条目的最终状态。</summary>
|
||||
public PathSmoothingStatus Status { get; }
|
||||
|
||||
/// <summary>使用原始基线规范化后的质量指标。</summary>
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
|
||||
/// <summary>方法的五次测量计时；基线不参与计时排名。</summary>
|
||||
public SmoothingTimingSummary Timing { get; }
|
||||
|
||||
/// <summary>由状态、分段元数据和完整路径 IEEE 754 位模式生成的 SHA-256 摘要。</summary>
|
||||
public string StableGeometryDigest { get; }
|
||||
|
||||
/// <summary>面向报告和诊断的稳定说明。</summary>
|
||||
public string Diagnostic { get; }
|
||||
|
||||
/// <summary>仅供比较与报告读取的正式路径；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>覆盖 <see cref="Path"/> 的方向段；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>条目能否参与方法推荐。</summary>
|
||||
public bool IsEligibleForRecommendation =>
|
||||
!IsRawPathBaseline &&
|
||||
Status == PathSmoothingStatus.Success &&
|
||||
IsPublishedLocalG2Status(Status) &&
|
||||
Metrics.IsFeasible &&
|
||||
Timing.IsDeterministic;
|
||||
|
||||
internal static bool IsPublishedLocalG2Status(PathSmoothingStatus status)
|
||||
{
|
||||
return status == PathSmoothingStatus.Complete ||
|
||||
status == PathSmoothingStatus.PartialImprovement ||
|
||||
status == PathSmoothingStatus.NotNeeded ||
|
||||
status == PathSmoothingStatus.Unchanged;
|
||||
}
|
||||
|
||||
internal static PathSmoothingComparisonEntry CreateCandidate(
|
||||
SmoothingMethod method,
|
||||
PathSmoothingStatus status,
|
||||
+33
@@ -0,0 +1,33 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
/// <summary>Immutable request for comparing a coarse-path baseline with Local G2 output.</summary>
|
||||
public sealed class PathSmoothingComparisonRequest
|
||||
{
|
||||
private static readonly IReadOnlyList<SmoothingMethod> LocalG2OnlyMethods =
|
||||
new ReadOnlyCollection<SmoothingMethod>(new[] { SmoothingMethod.LocalG2Quintic });
|
||||
|
||||
public PathSmoothingComparisonRequest(PathSmoothingRequest smoothingRequest)
|
||||
{
|
||||
SmoothingRequest = CopyRequest(smoothingRequest);
|
||||
Methods = LocalG2OnlyMethods;
|
||||
}
|
||||
|
||||
public PathSmoothingRequest SmoothingRequest { get; }
|
||||
|
||||
/// <summary>The report always contains the sole supported method, Local G2 quintic.</summary>
|
||||
public IReadOnlyList<SmoothingMethod> Methods { get; }
|
||||
|
||||
private static PathSmoothingRequest CopyRequest(PathSmoothingRequest source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
source.Configuration);
|
||||
}
|
||||
}
|
||||
+3
-7
@@ -2,9 +2,9 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
/// <summary>一次离线比较的不可变基线、方法条目和推荐结论。</summary>
|
||||
/// <summary>Immutable raw-path and Local G2 comparison result.</summary>
|
||||
public sealed class PathSmoothingComparisonResult
|
||||
{
|
||||
internal PathSmoothingComparisonResult(
|
||||
@@ -21,19 +21,15 @@ public sealed class PathSmoothingComparisonResult
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>独立分析的原始粗路径；不属于任何候选方法。</summary>
|
||||
public PathSmoothingComparisonEntry RawPathBaseline { get; }
|
||||
|
||||
/// <summary>每个请求方法恰有一个条目；取消时可能只包含已完成的方法。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonEntry> Entries { get; }
|
||||
|
||||
/// <summary>按公开字典序选择的方法；没有合格方法或取消时为空。</summary>
|
||||
/// <summary>Local G2 when its published result is deterministic; otherwise null.</summary>
|
||||
public SmoothingMethod? RecommendedMethod { get; }
|
||||
|
||||
/// <summary>比较是否在启动后续方法前被取消。</summary>
|
||||
public bool IsCancelled { get; }
|
||||
|
||||
/// <summary>整个比较的稳定状态说明。</summary>
|
||||
public string Diagnostic { get; }
|
||||
|
||||
private static IReadOnlyList<PathSmoothingComparisonEntry> CopyReadOnly(
|
||||
+1
-1
@@ -2,7 +2,7 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
/// <summary>一个方法的固定五次计时样本和确定性结论。</summary>
|
||||
public sealed class SmoothingTimingSummary
|
||||
+1
-1
@@ -4,7 +4,7 @@ using System.IO;
|
||||
using System.Security.Cryptography;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
/// <summary>为重复执行结果生成与进程无关的稳定几何 SHA-256 摘要。</summary>
|
||||
public static class StableGeometryDigest
|
||||
@@ -0,0 +1,12 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>Fixed dimensions and colors for Local G2 comparison reports.</summary>
|
||||
public static class IeeeFigureStyle
|
||||
{
|
||||
public const double FigureWidthPoints = 7.16d * 72d;
|
||||
public const double FigureHeightPoints = 5.20d * 72d;
|
||||
public const string RawColor = "#4D4D4D";
|
||||
public const string LocalG2Color = "#0072B2";
|
||||
public const string LocalG2DiagnosticColor = "#B1373E";
|
||||
public const string LimitColor = "#CC79A7";
|
||||
}
|
||||
+42
@@ -0,0 +1,42 @@
|
||||
using System;
|
||||
using System.Globalization;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>把共享图形模型的指标表写为带 BOM 的 UTF-8 CSV。</summary>
|
||||
public sealed class SmoothingCsvWriter
|
||||
{
|
||||
private const string Header = "ScenarioId,Method,Status,PathLengthMeters,MaximumAbsoluteVehicleCurvaturePerMeter,RootMeanSquareVehicleCurvaturePerMeter,TotalAbsoluteCurvatureVariationPerMeter,CurvatureVariationEnergy,MinimumBodyClearanceMeters,MedianElapsedMilliseconds,TimingSampleCount,IsDeterministic";
|
||||
|
||||
public byte[] Write(SmoothingFigureModel model)
|
||||
{
|
||||
if (model == null) throw new ArgumentNullException(nameof(model));
|
||||
var text = new StringBuilder(Header).Append("\r\n");
|
||||
for (int index = 0; index < model.MetricRows.Count; index++)
|
||||
{
|
||||
SmoothingFigureMetricRow row = model.MetricRows[index];
|
||||
PathQualityMetrics metrics = row.Metrics;
|
||||
text.Append(Field(model.ScenarioId)).Append(',').Append(Field(row.Method)).Append(',').Append(Field(row.Status.ToString())).Append(',')
|
||||
.Append(Number(metrics.PathLengthMeters)).Append(',').Append(Number(metrics.MaximumAbsoluteVehicleCurvaturePerMeter)).Append(',')
|
||||
.Append(Number(metrics.RootMeanSquareVehicleCurvaturePerMeter)).Append(',').Append(Number(metrics.TotalAbsoluteCurvatureVariationPerMeter)).Append(',')
|
||||
.Append(Number(metrics.CurvatureVariationEnergy)).Append(',').Append(Number(metrics.MinimumBodyClearanceMeters)).Append(',')
|
||||
.Append(Number(row.Timing == null ? 0d : row.Timing.MedianElapsedMilliseconds)).Append(',')
|
||||
.Append(row.Timing == null ? 0 : row.Timing.MeasuredElapsedMilliseconds.Count).Append(',')
|
||||
.Append(row.Timing != null && row.Timing.IsDeterministic ? "true" : "false").Append("\r\n");
|
||||
}
|
||||
byte[] body = new UTF8Encoding(false).GetBytes(text.ToString());
|
||||
byte[] preamble = new UTF8Encoding(true).GetPreamble();
|
||||
var output = new byte[preamble.Length + body.Length];
|
||||
Buffer.BlockCopy(preamble, 0, output, 0, preamble.Length);
|
||||
Buffer.BlockCopy(body, 0, output, preamble.Length, body.Length);
|
||||
return output;
|
||||
}
|
||||
|
||||
private static string Number(double value) { return value.ToString("0.#################", CultureInfo.InvariantCulture); }
|
||||
private static string Field(string value)
|
||||
{
|
||||
string text = value ?? string.Empty;
|
||||
return text.IndexOfAny(new[] { ',', '\"', '\r', '\n' }) < 0 ? text : "\"" + text.Replace("\"", "\"\"") + "\"";
|
||||
}
|
||||
}
|
||||
+111
@@ -0,0 +1,111 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>单张 SVG/PNG 共享的不可变绘图视图。</summary>
|
||||
public sealed class SmoothingFigureDefinition
|
||||
{
|
||||
internal SmoothingFigureDefinition(
|
||||
SmoothingFigureKind kind,
|
||||
string fileStem,
|
||||
string title,
|
||||
SmoothingFigureModel model,
|
||||
bool showsMapContext,
|
||||
IReadOnlyList<SmoothingFigureSeriesView> series,
|
||||
double worldXMinMeters,
|
||||
double worldXMaxMeters,
|
||||
double worldYMinMeters,
|
||||
double worldYMaxMeters,
|
||||
IReadOnlyList<double> xTicks,
|
||||
IReadOnlyList<double> yTicks,
|
||||
double curvatureArcLengthMaximumMeters,
|
||||
double curvatureMinimumPerMeter,
|
||||
double curvatureMaximumPerMeter,
|
||||
IReadOnlyList<double> curvatureArcLengthTicks,
|
||||
IReadOnlyList<double> curvatureTicks)
|
||||
{
|
||||
Kind = kind;
|
||||
FileStem = fileStem ?? string.Empty;
|
||||
Title = title ?? string.Empty;
|
||||
Model = model ?? throw new ArgumentNullException(nameof(model));
|
||||
ShowsMapContext = showsMapContext;
|
||||
Series = Copy(series);
|
||||
WorldXMinMeters = worldXMinMeters;
|
||||
WorldXMaxMeters = worldXMaxMeters;
|
||||
WorldYMinMeters = worldYMinMeters;
|
||||
WorldYMaxMeters = worldYMaxMeters;
|
||||
XTicks = Copy(xTicks);
|
||||
YTicks = Copy(yTicks);
|
||||
CurvatureArcLengthMaximumMeters = curvatureArcLengthMaximumMeters;
|
||||
CurvatureMinimumPerMeter = curvatureMinimumPerMeter;
|
||||
CurvatureMaximumPerMeter = curvatureMaximumPerMeter;
|
||||
CurvatureArcLengthTicks = Copy(curvatureArcLengthTicks);
|
||||
CurvatureTicks = Copy(curvatureTicks);
|
||||
}
|
||||
|
||||
public SmoothingFigureKind Kind { get; }
|
||||
public string FileStem { get; }
|
||||
public string Title { get; }
|
||||
public SmoothingFigureModel Model { get; }
|
||||
public bool ShowsMapContext { get; }
|
||||
public bool IsCurvatureFigure => Kind == SmoothingFigureKind.CurvatureComparison;
|
||||
public double FigureWidthPoints => Model.FigureWidthPoints;
|
||||
public double FigureHeightPoints => Model.FigureHeightPoints;
|
||||
public double PlotXPoints => 68d;
|
||||
public double PlotYPoints => 44d;
|
||||
public double PlotWidthPoints => 400d;
|
||||
public double PlotHeightPoints => 245d;
|
||||
public double LegendYPoints => LegendEntries.Count > 4 ? 332d : 340d;
|
||||
public IReadOnlyList<SmoothingFigureSeriesView> Series { get; }
|
||||
public IReadOnlyList<SmoothingFigureLegendEntry> LegendEntries => BuildLegend(Series);
|
||||
public double WorldXMinMeters { get; }
|
||||
public double WorldXMaxMeters { get; }
|
||||
public double WorldYMinMeters { get; }
|
||||
public double WorldYMaxMeters { get; }
|
||||
public double WorldScalePointsPerMeter => Math.Min(PlotWidthPoints / (WorldXMaxMeters - WorldXMinMeters), PlotHeightPoints / (WorldYMaxMeters - WorldYMinMeters));
|
||||
public IReadOnlyList<double> XTicks { get; }
|
||||
public IReadOnlyList<double> YTicks { get; }
|
||||
public double CurvatureArcLengthMaximumMeters { get; }
|
||||
public double CurvatureMinimumPerMeter { get; }
|
||||
public double CurvatureMaximumPerMeter { get; }
|
||||
public IReadOnlyList<double> CurvatureArcLengthTicks { get; }
|
||||
public IReadOnlyList<double> CurvatureTicks { get; }
|
||||
|
||||
private static IReadOnlyList<T> Copy<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 static IReadOnlyList<SmoothingFigureLegendEntry> BuildLegend(IReadOnlyList<SmoothingFigureSeriesView> views)
|
||||
{
|
||||
var entries = new List<SmoothingFigureLegendEntry>(views == null ? 0 : views.Count);
|
||||
if (views != null)
|
||||
{
|
||||
for (int index = 0; index < views.Count; index++)
|
||||
{
|
||||
SmoothingFigureSeries series = views[index].Series;
|
||||
entries.Add(new SmoothingFigureLegendEntry(series.Label + " (" + series.Status + ")", series.Color, string.Empty));
|
||||
}
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingFigureLegendEntry>(entries);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>某条路径在特定图中的显示透明度。</summary>
|
||||
public sealed class SmoothingFigureSeriesView
|
||||
{
|
||||
internal SmoothingFigureSeriesView(SmoothingFigureSeries series, double opacity, double pointRadiusPoints = 1.35d)
|
||||
{
|
||||
Series = series ?? throw new ArgumentNullException(nameof(series));
|
||||
Opacity = opacity < 0d ? 0d : (opacity > 1d ? 1d : opacity);
|
||||
PointRadiusPoints = pointRadiusPoints > 0d ? pointRadiusPoints : 1.35d;
|
||||
}
|
||||
|
||||
public SmoothingFigureSeries Series { get; }
|
||||
public double Opacity { get; }
|
||||
public double PointRadiusPoints { get; }
|
||||
}
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>Stable report figures for the raw-path and Local G2 workflow.</summary>
|
||||
public enum SmoothingFigureKind
|
||||
{
|
||||
CoarsePathOverview,
|
||||
AllPathsComparison,
|
||||
LocalG2Overview,
|
||||
CurvatureComparison,
|
||||
LocalG2DiagnosticCandidate,
|
||||
}
|
||||
+207
@@ -0,0 +1,207 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>SVG 和 PNG 共享的不可变路径平滑报告图形模型。</summary>
|
||||
public sealed class SmoothingFigureModel
|
||||
{
|
||||
internal SmoothingFigureModel(
|
||||
string scenarioId,
|
||||
string scenarioLabel,
|
||||
double worldXMinMeters,
|
||||
double worldXMaxMeters,
|
||||
double worldYMinMeters,
|
||||
double worldYMaxMeters,
|
||||
double pathPanelX,
|
||||
double pathPanelY,
|
||||
double pathPanelWidth,
|
||||
double pathPanelHeight,
|
||||
double curvaturePanelX,
|
||||
double curvaturePanelY,
|
||||
double curvaturePanelWidth,
|
||||
double curvaturePanelHeight,
|
||||
double metricsPanelX,
|
||||
double metricsPanelY,
|
||||
double metricsPanelWidth,
|
||||
double metricsPanelHeight,
|
||||
IReadOnlyList<SmoothingFigureObstacle> obstacles,
|
||||
IReadOnlyList<SmoothingFigureSeries> series,
|
||||
IReadOnlyList<SmoothingFigureMetricRow> metricRows,
|
||||
SmoothingFigurePoint start,
|
||||
SmoothingFigurePoint goal)
|
||||
{
|
||||
ScenarioId = scenarioId ?? string.Empty;
|
||||
ScenarioLabel = scenarioLabel ?? string.Empty;
|
||||
WorldXMinMeters = worldXMinMeters;
|
||||
WorldXMaxMeters = worldXMaxMeters;
|
||||
WorldYMinMeters = worldYMinMeters;
|
||||
WorldYMaxMeters = worldYMaxMeters;
|
||||
PathPanelX = pathPanelX;
|
||||
PathPanelY = pathPanelY;
|
||||
PathPanelWidth = pathPanelWidth;
|
||||
PathPanelHeight = pathPanelHeight;
|
||||
CurvaturePanelX = curvaturePanelX;
|
||||
CurvaturePanelY = curvaturePanelY;
|
||||
CurvaturePanelWidth = curvaturePanelWidth;
|
||||
CurvaturePanelHeight = curvaturePanelHeight;
|
||||
MetricsPanelX = metricsPanelX;
|
||||
MetricsPanelY = metricsPanelY;
|
||||
MetricsPanelWidth = metricsPanelWidth;
|
||||
MetricsPanelHeight = metricsPanelHeight;
|
||||
Obstacles = Copy(obstacles);
|
||||
Series = Copy(series);
|
||||
MetricRows = Copy(metricRows);
|
||||
Start = start ?? throw new ArgumentNullException(nameof(start));
|
||||
Goal = goal ?? throw new ArgumentNullException(nameof(goal));
|
||||
}
|
||||
|
||||
public string ScenarioId { get; }
|
||||
public string ScenarioLabel { get; }
|
||||
public double FigureWidthPoints => IeeeFigureStyle.FigureWidthPoints;
|
||||
public double FigureHeightPoints => IeeeFigureStyle.FigureHeightPoints;
|
||||
public string PathPanelLabel => "(a)";
|
||||
public string CurvaturePanelLabel => "(b)";
|
||||
public string MetricsPanelLabel => "(c)";
|
||||
public double WorldXMinMeters { get; }
|
||||
public double WorldXMaxMeters { get; }
|
||||
public double WorldYMinMeters { get; }
|
||||
public double WorldYMaxMeters { get; }
|
||||
public double PathPanelX { get; }
|
||||
public double PathPanelY { get; }
|
||||
public double PathPanelWidth { get; }
|
||||
public double PathPanelHeight { get; }
|
||||
public double CurvaturePanelX { get; }
|
||||
public double CurvaturePanelY { get; }
|
||||
public double CurvaturePanelWidth { get; }
|
||||
public double CurvaturePanelHeight { get; }
|
||||
public double MetricsPanelX { get; }
|
||||
public double MetricsPanelY { get; }
|
||||
public double MetricsPanelWidth { get; }
|
||||
public double MetricsPanelHeight { get; }
|
||||
public double PathScaleX { get; internal set; }
|
||||
public double PathScaleY { get; internal set; }
|
||||
public IReadOnlyList<SmoothingFigureObstacle> Obstacles { get; }
|
||||
public IReadOnlyList<SmoothingFigureSeries> Series { get; }
|
||||
public IReadOnlyList<SmoothingFigureLegendEntry> LegendEntries => BuildLegend(Series);
|
||||
public IReadOnlyList<SmoothingFigureMetricRow> MetricRows { get; }
|
||||
public SmoothingFigurePoint Start { get; }
|
||||
public SmoothingFigurePoint Goal { get; }
|
||||
|
||||
internal SmoothingFigureModel WithAdditionalSeries(SmoothingFigureSeries series)
|
||||
{
|
||||
if (series == null) throw new ArgumentNullException(nameof(series));
|
||||
var combined = new List<SmoothingFigureSeries>(Series.Count + 1);
|
||||
for (int index = 0; index < Series.Count; index++)
|
||||
{
|
||||
if (Series[index].Key == series.Key)
|
||||
throw new ArgumentException("Figure series keys must be unique.", nameof(series));
|
||||
combined.Add(Series[index]);
|
||||
}
|
||||
combined.Add(series);
|
||||
var copy = new SmoothingFigureModel(
|
||||
ScenarioId, ScenarioLabel, WorldXMinMeters, WorldXMaxMeters, WorldYMinMeters, WorldYMaxMeters,
|
||||
PathPanelX, PathPanelY, PathPanelWidth, PathPanelHeight,
|
||||
CurvaturePanelX, CurvaturePanelY, CurvaturePanelWidth, CurvaturePanelHeight,
|
||||
MetricsPanelX, MetricsPanelY, MetricsPanelWidth, MetricsPanelHeight,
|
||||
Obstacles, combined, MetricRows, Start, Goal)
|
||||
{
|
||||
PathScaleX = PathScaleX,
|
||||
PathScaleY = PathScaleY,
|
||||
};
|
||||
return copy;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Copy<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 static IReadOnlyList<SmoothingFigureLegendEntry> BuildLegend(IReadOnlyList<SmoothingFigureSeries> series)
|
||||
{
|
||||
var legend = new List<SmoothingFigureLegendEntry>(series == null ? 0 : series.Count);
|
||||
if (series != null)
|
||||
{
|
||||
for (int index = 0; index < series.Count; index++)
|
||||
legend.Add(new SmoothingFigureLegendEntry(series[index].Label, series[index].Color, series[index].DashArray));
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingFigureLegendEntry>(legend);
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigurePoint
|
||||
{
|
||||
public SmoothingFigurePoint(double xMeters, double yMeters, double arcLengthMeters, double vehicleCurvaturePerMeter)
|
||||
{
|
||||
X = xMeters; Y = yMeters; ArcLength = arcLengthMeters; VehicleCurvature = vehicleCurvaturePerMeter;
|
||||
}
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double ArcLength { get; }
|
||||
public double VehicleCurvature { get; }
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigureObstacle
|
||||
{
|
||||
public SmoothingFigureObstacle(double xMeters, double yMeters, double widthMeters, double heightMeters)
|
||||
{
|
||||
X = xMeters; Y = yMeters; Width = widthMeters; Height = heightMeters;
|
||||
}
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double Width { get; }
|
||||
public double Height { get; }
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigureSeries
|
||||
{
|
||||
internal SmoothingFigureSeries(SmoothingMethod? method, string key, string label, PathSmoothingStatus status, string color, string dashArray,
|
||||
bool isRawPathBaseline, IReadOnlyList<SmoothingFigurePoint> points, IReadOnlyList<SmoothingFigurePoint> violationMarkers)
|
||||
{
|
||||
Method = method; Key = key ?? string.Empty; Label = label ?? string.Empty; Status = status; Color = color ?? string.Empty;
|
||||
DashArray = dashArray ?? string.Empty; IsRawPathBaseline = isRawPathBaseline; Points = Copy(points); ViolationMarkers = Copy(violationMarkers);
|
||||
}
|
||||
public SmoothingMethod? Method { get; }
|
||||
public string Key { get; }
|
||||
public string Label { get; }
|
||||
public PathSmoothingStatus Status { get; }
|
||||
public string Color { get; }
|
||||
public string DashArray { get; }
|
||||
public bool IsRawPathBaseline { get; }
|
||||
public bool IsCurveVisible => Points.Count > 0;
|
||||
public IReadOnlyList<SmoothingFigurePoint> Points { get; }
|
||||
public IReadOnlyList<SmoothingFigurePoint> ViolationMarkers { get; }
|
||||
private static IReadOnlyList<T> Copy<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);
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigureLegendEntry
|
||||
{
|
||||
internal SmoothingFigureLegendEntry(string label, string color, string dashArray) { Label = label ?? string.Empty; Color = color ?? string.Empty; DashArray = dashArray ?? string.Empty; }
|
||||
public string Label { get; }
|
||||
public string Color { get; }
|
||||
public string DashArray { get; }
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigureMetricRow
|
||||
{
|
||||
internal SmoothingFigureMetricRow(string method, string label, PathSmoothingStatus status, PathQualityMetrics metrics,
|
||||
SmoothingTimingSummary timing)
|
||||
{
|
||||
Method = method ?? string.Empty; Label = label ?? string.Empty; Status = status; Metrics = metrics ?? new PathQualityMetrics();
|
||||
Timing = timing;
|
||||
}
|
||||
public string Method { get; }
|
||||
public string Label { get; }
|
||||
public PathSmoothingStatus Status { get; }
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
public SmoothingTimingSummary Timing { get; }
|
||||
}
|
||||
+180
@@ -0,0 +1,180 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>Transforms the raw-path and Local G2 results into shared report-figure data.</summary>
|
||||
public sealed class SmoothingFigureModelBuilder
|
||||
{
|
||||
private const double MarginPoints = 18d;
|
||||
private const double PanelGapPoints = 12d;
|
||||
|
||||
public SmoothingFigureModel Build(
|
||||
PathSmoothingComparisonResult comparison,
|
||||
PlanningGridMap map,
|
||||
Pose2D start,
|
||||
Pose2D goal,
|
||||
string scenarioId,
|
||||
string scenarioLabel)
|
||||
{
|
||||
if (comparison == null) throw new ArgumentNullException(nameof(comparison));
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (start == null) throw new ArgumentNullException(nameof(start));
|
||||
if (goal == null) throw new ArgumentNullException(nameof(goal));
|
||||
|
||||
double worldXMin = map.Bounds.XMin / 1000d;
|
||||
double worldXMax = map.Bounds.XMax / 1000d;
|
||||
double worldYMin = map.Bounds.YMin / 1000d;
|
||||
double worldYMax = map.Bounds.YMax / 1000d;
|
||||
double innerWidth = IeeeFigureStyle.FigureWidthPoints - 2d * MarginPoints;
|
||||
double innerHeight = IeeeFigureStyle.FigureHeightPoints - 2d * MarginPoints;
|
||||
double pathWidth = innerWidth * 0.60d;
|
||||
double rightWidth = innerWidth - pathWidth - PanelGapPoints;
|
||||
double rightHeight = (innerHeight - PanelGapPoints) / 2d;
|
||||
|
||||
var model = new SmoothingFigureModel(
|
||||
scenarioId, scenarioLabel, worldXMin, worldXMax, worldYMin, worldYMax,
|
||||
MarginPoints, MarginPoints, pathWidth, innerHeight,
|
||||
MarginPoints + pathWidth + PanelGapPoints, MarginPoints, rightWidth, rightHeight,
|
||||
MarginPoints + pathWidth + PanelGapPoints, MarginPoints + rightHeight + PanelGapPoints, rightWidth, rightHeight,
|
||||
BuildObstacles(map), BuildSeries(comparison, map), BuildMetricRows(comparison),
|
||||
new SmoothingFigurePoint(start.X, start.Y, 0d, 0d), new SmoothingFigurePoint(goal.X, goal.Y, 0d, 0d));
|
||||
|
||||
double worldWidth = worldXMax - worldXMin;
|
||||
double worldHeight = worldYMax - worldYMin;
|
||||
if (worldWidth <= 0d || worldHeight <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(map), "Map bounds must be finite and non-degenerate.");
|
||||
double scale = Math.Min(pathWidth / worldWidth, innerHeight / worldHeight);
|
||||
model.PathScaleX = scale;
|
||||
model.PathScaleY = scale;
|
||||
return model;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureObstacle> BuildObstacles(PlanningGridMap map)
|
||||
{
|
||||
var runs = new List<ObstacleRun>();
|
||||
double resolution = map.ResolutionMeters;
|
||||
double xMin = map.Bounds.XMin / 1000d;
|
||||
double yMin = map.Bounds.YMin / 1000d;
|
||||
for (int row = 0; row < map.Rows; row++)
|
||||
{
|
||||
int runStart = -1;
|
||||
for (int column = 0; column <= map.Cols; column++)
|
||||
{
|
||||
bool occupied = column < map.Cols && map.IsOccupied(row, column);
|
||||
if (occupied && runStart < 0) { runStart = column; continue; }
|
||||
if (!occupied && runStart >= 0)
|
||||
{
|
||||
AddOrExtendRun(runs, xMin + runStart * resolution, yMin + row * resolution,
|
||||
(column - runStart) * resolution, resolution);
|
||||
runStart = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
var obstacles = new List<SmoothingFigureObstacle>(runs.Count);
|
||||
for (int index = 0; index < runs.Count; index++)
|
||||
{
|
||||
ObstacleRun run = runs[index];
|
||||
obstacles.Add(new SmoothingFigureObstacle(run.X, run.Y, run.Width, run.Height));
|
||||
}
|
||||
return obstacles;
|
||||
}
|
||||
|
||||
private static void AddOrExtendRun(IList<ObstacleRun> runs, double x, double y, double width, double height)
|
||||
{
|
||||
for (int index = runs.Count - 1; index >= 0; index--)
|
||||
{
|
||||
ObstacleRun candidate = runs[index];
|
||||
if (NearlyEqual(candidate.X, x) && NearlyEqual(candidate.Width, width) && NearlyEqual(candidate.Y + candidate.Height, y))
|
||||
{
|
||||
candidate.Height += height;
|
||||
return;
|
||||
}
|
||||
}
|
||||
runs.Add(new ObstacleRun(x, y, width, height));
|
||||
}
|
||||
|
||||
private static bool NearlyEqual(double first, double second) => Math.Abs(first - second) < 1e-9d;
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureSeries> BuildSeries(PathSmoothingComparisonResult comparison, PlanningGridMap map)
|
||||
{
|
||||
return new List<SmoothingFigureSeries>(2)
|
||||
{
|
||||
CreateSeries(comparison.RawPathBaseline, null, "raw", "Raw coarse path", IeeeFigureStyle.RawColor, string.Empty, true, map),
|
||||
CreateSeries(FindLocalG2(comparison), SmoothingMethod.LocalG2Quintic, "local-g2", "Local G2", IeeeFigureStyle.LocalG2Color, string.Empty, false, map),
|
||||
};
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureMetricRow> BuildMetricRows(PathSmoothingComparisonResult comparison)
|
||||
{
|
||||
return new List<SmoothingFigureMetricRow>
|
||||
{
|
||||
CreateRow(comparison.RawPathBaseline, "RawPath", "Raw coarse path"),
|
||||
CreateRow(FindLocalG2(comparison), "LocalG2Quintic", "Local G2"),
|
||||
};
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonEntry FindLocalG2(PathSmoothingComparisonResult comparison)
|
||||
{
|
||||
for (int index = 0; index < comparison.Entries.Count; index++)
|
||||
{
|
||||
PathSmoothingComparisonEntry entry = comparison.Entries[index];
|
||||
if (entry.Method == SmoothingMethod.LocalG2Quintic) return entry;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private static SmoothingFigureMetricRow CreateRow(PathSmoothingComparisonEntry entry, string method, string label)
|
||||
{
|
||||
return entry == null
|
||||
? new SmoothingFigureMetricRow(method, label, PathSmoothingStatus.Failed, new PathQualityMetrics(), null)
|
||||
: new SmoothingFigureMetricRow(method, label, entry.Status, entry.Metrics, entry.Timing);
|
||||
}
|
||||
|
||||
private static SmoothingFigureSeries CreateSeries(
|
||||
PathSmoothingComparisonEntry entry,
|
||||
SmoothingMethod? method,
|
||||
string key,
|
||||
string label,
|
||||
string color,
|
||||
string dashArray,
|
||||
bool isRawPathBaseline,
|
||||
PlanningGridMap map)
|
||||
{
|
||||
var points = new List<SmoothingFigurePoint>();
|
||||
var violations = new List<SmoothingFigurePoint>();
|
||||
PathSmoothingStatus status = entry == null ? PathSmoothingStatus.Failed : entry.Status;
|
||||
if (entry != null)
|
||||
{
|
||||
for (int index = 0; index < entry.Path.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint point = entry.Path[index];
|
||||
var figurePoint = new SmoothingFigurePoint(point.X, point.Y, point.ArcLength, point.VehicleCurvature);
|
||||
points.Add(figurePoint);
|
||||
if (status == PathSmoothingStatus.Infeasible && map.IsOccupiedWorld(point.X, point.Y)) violations.Add(figurePoint);
|
||||
}
|
||||
}
|
||||
if (status == PathSmoothingStatus.Infeasible && points.Count > 0 && violations.Count == 0)
|
||||
violations.Add(points[points.Count / 2]);
|
||||
return new SmoothingFigureSeries(method, key, label, status, color, dashArray, isRawPathBaseline, points, violations);
|
||||
}
|
||||
|
||||
private sealed class ObstacleRun
|
||||
{
|
||||
public ObstacleRun(double x, double y, double width, double height)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
Width = width;
|
||||
Height = height;
|
||||
}
|
||||
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double Width { get; }
|
||||
public double Height { get; set; }
|
||||
}
|
||||
}
|
||||
+265
@@ -0,0 +1,265 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>Builds the stable four-figure Local G2 report set and an optional diagnostic figure.</summary>
|
||||
public sealed class SmoothingFigureSetBuilder
|
||||
{
|
||||
private const double MinimumExtentMeters = 0.25d;
|
||||
private const double PaddingFraction = 0.10d;
|
||||
|
||||
public SmoothingFigureSet Build(SmoothingFigureModel model)
|
||||
{
|
||||
if (model == null) throw new ArgumentNullException(nameof(model));
|
||||
SmoothingFigureSeries raw = Find(model, "raw");
|
||||
SmoothingFigureSeries localG2 = Find(model, "local-g2");
|
||||
var figures = new List<SmoothingFigureDefinition>
|
||||
{
|
||||
BuildOverhead(SmoothingFigureKind.CoarsePathOverview, "01-coarse-path-overview", "Raw coarse path", model, true, View(raw, 1d)),
|
||||
BuildOverhead(SmoothingFigureKind.AllPathsComparison, "02-all-paths-comparison", "Raw path and Local G2", model, false, View(raw, 1d), View(localG2, 1d)),
|
||||
BuildOverhead(SmoothingFigureKind.LocalG2Overview, "03-local-g2-overview", "Local G2 smoothing", model, true, View(raw, 0.28d), View(localG2, 1d)),
|
||||
BuildCurvature(model, View(raw, 1d), View(localG2, 1d)),
|
||||
};
|
||||
if (TryFind(model, "local-g2-diagnostic", out SmoothingFigureSeries diagnostic))
|
||||
{
|
||||
figures.Add(BuildOverhead(
|
||||
SmoothingFigureKind.LocalG2DiagnosticCandidate,
|
||||
"05-local-g2-diagnostic-candidate",
|
||||
"Local G2 diagnostic candidate (not published)",
|
||||
model,
|
||||
true,
|
||||
View(raw, 1d, 2.10d),
|
||||
View(diagnostic, 1d)));
|
||||
}
|
||||
return new SmoothingFigureSet(figures);
|
||||
}
|
||||
|
||||
private static SmoothingFigureDefinition BuildOverhead(
|
||||
SmoothingFigureKind kind,
|
||||
string stem,
|
||||
string title,
|
||||
SmoothingFigureModel model,
|
||||
bool mapContext,
|
||||
params SmoothingFigureSeriesView[] series)
|
||||
{
|
||||
Bounds bounds = CalculateWorldBounds(model, mapContext, series);
|
||||
return new SmoothingFigureDefinition(
|
||||
kind, stem, title, model, mapContext, series,
|
||||
bounds.XMin, bounds.XMax, bounds.YMin, bounds.YMax,
|
||||
BuildTicks(bounds.XMin, bounds.XMax), BuildTicks(bounds.YMin, bounds.YMax),
|
||||
1d, -1d, 1d, Array.Empty<double>(), Array.Empty<double>());
|
||||
}
|
||||
|
||||
private static SmoothingFigureDefinition BuildCurvature(
|
||||
SmoothingFigureModel model,
|
||||
params SmoothingFigureSeriesView[] series)
|
||||
{
|
||||
double arcMaximum = 0d;
|
||||
double curvatureMinimum = 0d;
|
||||
double curvatureMaximum = 0d;
|
||||
for (int viewIndex = 0; viewIndex < series.Length; viewIndex++)
|
||||
{
|
||||
IReadOnlyList<SmoothingFigurePoint> points = series[viewIndex].Series.Points;
|
||||
for (int pointIndex = 0; pointIndex < points.Count; pointIndex++)
|
||||
{
|
||||
SmoothingFigurePoint point = points[pointIndex];
|
||||
if (point.ArcLength > arcMaximum) arcMaximum = point.ArcLength;
|
||||
if (point.VehicleCurvature < curvatureMinimum) curvatureMinimum = point.VehicleCurvature;
|
||||
if (point.VehicleCurvature > curvatureMaximum) curvatureMaximum = point.VehicleCurvature;
|
||||
}
|
||||
}
|
||||
arcMaximum = ExpandMaximum(arcMaximum, MinimumExtentMeters);
|
||||
ExpandRange(ref curvatureMinimum, ref curvatureMaximum, MinimumExtentMeters);
|
||||
return new SmoothingFigureDefinition(
|
||||
SmoothingFigureKind.CurvatureComparison,
|
||||
"04-curvature-comparison",
|
||||
"Vehicle-curvature comparison",
|
||||
model,
|
||||
false,
|
||||
series,
|
||||
0d,
|
||||
1d,
|
||||
0d,
|
||||
1d,
|
||||
Array.Empty<double>(),
|
||||
Array.Empty<double>(),
|
||||
arcMaximum,
|
||||
curvatureMinimum,
|
||||
curvatureMaximum,
|
||||
BuildTicks(0d, arcMaximum),
|
||||
BuildTicks(curvatureMinimum, curvatureMaximum));
|
||||
}
|
||||
|
||||
private static Bounds CalculateWorldBounds(
|
||||
SmoothingFigureModel model,
|
||||
bool includesEndpoints,
|
||||
IReadOnlyList<SmoothingFigureSeriesView> series)
|
||||
{
|
||||
bool hasPoint = false;
|
||||
double xMin = 0d;
|
||||
double xMax = 0d;
|
||||
double yMin = 0d;
|
||||
double yMax = 0d;
|
||||
for (int viewIndex = 0; viewIndex < series.Count; viewIndex++)
|
||||
{
|
||||
IReadOnlyList<SmoothingFigurePoint> points = series[viewIndex].Series.Points;
|
||||
for (int pointIndex = 0; pointIndex < points.Count; pointIndex++)
|
||||
Include(points[pointIndex].X, points[pointIndex].Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
|
||||
}
|
||||
if (includesEndpoints)
|
||||
{
|
||||
Include(model.Start.X, model.Start.Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
|
||||
Include(model.Goal.X, model.Goal.Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
|
||||
}
|
||||
if (!hasPoint)
|
||||
{
|
||||
xMin = model.WorldXMinMeters;
|
||||
xMax = model.WorldXMaxMeters;
|
||||
yMin = model.WorldYMinMeters;
|
||||
yMax = model.WorldYMaxMeters;
|
||||
}
|
||||
ExpandRange(ref xMin, ref xMax, MinimumExtentMeters);
|
||||
ExpandRange(ref yMin, ref yMax, MinimumExtentMeters);
|
||||
double xPadding = (xMax - xMin) * PaddingFraction;
|
||||
double yPadding = (yMax - yMin) * PaddingFraction;
|
||||
xMin -= xPadding;
|
||||
xMax += xPadding;
|
||||
yMin -= yPadding;
|
||||
yMax += yPadding;
|
||||
const double desiredAspect = 400d / 245d;
|
||||
double width = xMax - xMin;
|
||||
double height = yMax - yMin;
|
||||
if (width / height < desiredAspect)
|
||||
{
|
||||
double halfWidth = height * desiredAspect / 2d;
|
||||
double center = (xMin + xMax) / 2d;
|
||||
xMin = center - halfWidth;
|
||||
xMax = center + halfWidth;
|
||||
}
|
||||
else
|
||||
{
|
||||
double halfHeight = width / desiredAspect / 2d;
|
||||
double center = (yMin + yMax) / 2d;
|
||||
yMin = center - halfHeight;
|
||||
yMax = center + halfHeight;
|
||||
}
|
||||
return new Bounds(xMin, xMax, yMin, yMax);
|
||||
}
|
||||
|
||||
private static void Include(double x, double y, ref bool hasPoint, ref double xMin, ref double xMax, ref double yMin, ref double yMax)
|
||||
{
|
||||
if (!hasPoint)
|
||||
{
|
||||
hasPoint = true;
|
||||
xMin = x;
|
||||
xMax = x;
|
||||
yMin = y;
|
||||
yMax = y;
|
||||
return;
|
||||
}
|
||||
if (x < xMin) xMin = x;
|
||||
if (x > xMax) xMax = x;
|
||||
if (y < yMin) yMin = y;
|
||||
if (y > yMax) yMax = y;
|
||||
}
|
||||
|
||||
private static void ExpandRange(ref double minimum, ref double maximum, double minimumExtent)
|
||||
{
|
||||
double extent = maximum - minimum;
|
||||
if (extent >= minimumExtent) return;
|
||||
double center = (minimum + maximum) / 2d;
|
||||
minimum = center - minimumExtent / 2d;
|
||||
maximum = center + minimumExtent / 2d;
|
||||
}
|
||||
|
||||
private static double ExpandMaximum(double value, double minimum)
|
||||
{
|
||||
return value < minimum ? minimum : value * (1d + PaddingFraction);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> BuildTicks(double minimum, double maximum)
|
||||
{
|
||||
double span = maximum - minimum;
|
||||
if (span <= 0d) return new[] { minimum, maximum };
|
||||
double roughStep = span / 5d;
|
||||
double magnitude = Math.Pow(10d, Math.Floor(Math.Log10(roughStep)));
|
||||
double normalized = roughStep / magnitude;
|
||||
double nice = normalized <= 1d ? 1d : (normalized <= 2d ? 2d : (normalized <= 5d ? 5d : 10d));
|
||||
double step = nice * magnitude;
|
||||
var ticks = new List<double>();
|
||||
double first = Math.Ceiling(minimum / step) * step;
|
||||
for (double value = first; value <= maximum + step * 0.001d; value += step) ticks.Add(value);
|
||||
if (ticks.Count < 2)
|
||||
{
|
||||
ticks.Clear();
|
||||
ticks.Add(minimum);
|
||||
ticks.Add(maximum);
|
||||
}
|
||||
return new ReadOnlyCollection<double>(ticks);
|
||||
}
|
||||
|
||||
private static SmoothingFigureSeries Find(SmoothingFigureModel model, string key)
|
||||
{
|
||||
for (int index = 0; index < model.Series.Count; index++)
|
||||
{
|
||||
if (model.Series[index].Key == key) return model.Series[index];
|
||||
}
|
||||
throw new InvalidOperationException("Figure model is missing path series: " + key);
|
||||
}
|
||||
|
||||
private static bool TryFind(SmoothingFigureModel model, string key, out SmoothingFigureSeries series)
|
||||
{
|
||||
for (int index = 0; index < model.Series.Count; index++)
|
||||
{
|
||||
if (model.Series[index].Key == key)
|
||||
{
|
||||
series = model.Series[index];
|
||||
return true;
|
||||
}
|
||||
}
|
||||
series = null;
|
||||
return false;
|
||||
}
|
||||
|
||||
private static SmoothingFigureSeriesView View(
|
||||
SmoothingFigureSeries series,
|
||||
double opacity,
|
||||
double pointRadiusPoints = 1.35d)
|
||||
{
|
||||
return new SmoothingFigureSeriesView(series, opacity, pointRadiusPoints);
|
||||
}
|
||||
|
||||
private readonly struct Bounds
|
||||
{
|
||||
public Bounds(double xMin, double xMax, double yMin, double yMax)
|
||||
{
|
||||
XMin = xMin;
|
||||
XMax = xMax;
|
||||
YMin = yMin;
|
||||
YMax = yMax;
|
||||
}
|
||||
|
||||
public double XMin { get; }
|
||||
public double XMax { get; }
|
||||
public double YMin { get; }
|
||||
public double YMax { get; }
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>Stable normal figure order; an optional Local G2 diagnostic figure is appended.</summary>
|
||||
public sealed class SmoothingFigureSet
|
||||
{
|
||||
internal SmoothingFigureSet(IReadOnlyList<SmoothingFigureDefinition> figures)
|
||||
{
|
||||
var copy = new List<SmoothingFigureDefinition>(figures == null ? 0 : figures.Count);
|
||||
if (figures != null)
|
||||
{
|
||||
for (int index = 0; index < figures.Count; index++) copy.Add(figures[index]);
|
||||
}
|
||||
Figures = new ReadOnlyCollection<SmoothingFigureDefinition>(copy);
|
||||
}
|
||||
|
||||
public IReadOnlyList<SmoothingFigureDefinition> Figures { get; }
|
||||
}
|
||||
+140
@@ -0,0 +1,140 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Drawing.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>按准确族名解析报告所需字体，并提供混合中英文文本的共同基线度量。</summary>
|
||||
public sealed class SmoothingFontResolver
|
||||
{
|
||||
public bool TryResolve(
|
||||
string chineseFamilyName,
|
||||
string latinFamilyName,
|
||||
out SmoothingFontResolution resolution,
|
||||
out string reason)
|
||||
{
|
||||
resolution = null;
|
||||
reason = string.Empty;
|
||||
if (string.IsNullOrWhiteSpace(chineseFamilyName) || string.IsNullOrWhiteSpace(latinFamilyName))
|
||||
{
|
||||
reason = "中文和拉丁字体族名均不能为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var collection = new InstalledFontCollection();
|
||||
FontFamily chinese = FindExact(collection.Families, chineseFamilyName);
|
||||
FontFamily latin = FindExact(collection.Families, latinFamilyName);
|
||||
if (chinese == null || latin == null)
|
||||
{
|
||||
collection.Dispose();
|
||||
reason = "未安装报告所需的精确字体族：" + (chinese == null ? chineseFamilyName : latinFamilyName) + "。";
|
||||
return false;
|
||||
}
|
||||
|
||||
resolution = new SmoothingFontResolution(collection, chinese, latin, chineseFamilyName, latinFamilyName);
|
||||
return true;
|
||||
}
|
||||
|
||||
public RectangleF MeasureMixedText(SmoothingFontResolution resolution, string text, float points)
|
||||
{
|
||||
if (resolution == null) throw new ArgumentNullException(nameof(resolution));
|
||||
if (string.IsNullOrEmpty(text) || points <= 0f) return RectangleF.Empty;
|
||||
using (var bitmap = new Bitmap(1, 1))
|
||||
using (Graphics graphics = Graphics.FromImage(bitmap))
|
||||
using (var format = (StringFormat)StringFormat.GenericTypographic.Clone())
|
||||
{
|
||||
format.FormatFlags |= StringFormatFlags.MeasureTrailingSpaces;
|
||||
float width = 0f, height = 0f;
|
||||
foreach (TextRun run in SplitRuns(text))
|
||||
{
|
||||
using (Font font = resolution.CreateFont(run.IsChinese ? resolution.ChineseFamily : resolution.LatinFamily, points))
|
||||
{
|
||||
SizeF size = graphics.MeasureString(run.Text, font, PointF.Empty, format);
|
||||
width += size.Width;
|
||||
height = Math.Max(height, size.Height);
|
||||
}
|
||||
}
|
||||
return new RectangleF(0f, 0f, width, height);
|
||||
}
|
||||
}
|
||||
|
||||
internal static IReadOnlyList<TextRun> SplitRuns(string text)
|
||||
{
|
||||
var runs = new List<TextRun>();
|
||||
if (string.IsNullOrEmpty(text)) return runs;
|
||||
int start = 0;
|
||||
bool isChinese = IsChinese(text[0]);
|
||||
for (int index = 1; index < text.Length; index++)
|
||||
{
|
||||
bool currentIsChinese = IsChinese(text[index]);
|
||||
if (currentIsChinese == isChinese) continue;
|
||||
runs.Add(new TextRun(text.Substring(start, index - start), isChinese));
|
||||
start = index;
|
||||
isChinese = currentIsChinese;
|
||||
}
|
||||
runs.Add(new TextRun(text.Substring(start), isChinese));
|
||||
return runs;
|
||||
}
|
||||
|
||||
private static FontFamily FindExact(IReadOnlyList<FontFamily> families, string name)
|
||||
{
|
||||
for (int index = 0; index < families.Count; index++)
|
||||
{
|
||||
FontFamily family = families[index];
|
||||
if (string.Equals(family.Name, name, StringComparison.Ordinal) ||
|
||||
string.Equals(family.GetName(1033), name, StringComparison.Ordinal)) return family;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private static bool IsChinese(char value)
|
||||
{
|
||||
return (value >= 0x3400 && value <= 0x4dbf) || (value >= 0x4e00 && value <= 0x9fff) ||
|
||||
(value >= 0xf900 && value <= 0xfaff) || value == 0x3002 || value == 0xff0c || value == 0xff1a;
|
||||
}
|
||||
|
||||
internal sealed class TextRun
|
||||
{
|
||||
public TextRun(string text, bool isChinese) { Text = text; IsChinese = isChinese; }
|
||||
public string Text { get; }
|
||||
public bool IsChinese { get; }
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>一次报告导出所使用的精确字体族；释放时同时释放字体集合。</summary>
|
||||
public sealed class SmoothingFontResolution : IDisposable
|
||||
{
|
||||
private readonly InstalledFontCollection _collection;
|
||||
private readonly string _chineseFamilyName;
|
||||
private readonly string _latinFamilyName;
|
||||
|
||||
internal SmoothingFontResolution(
|
||||
InstalledFontCollection collection,
|
||||
FontFamily chineseFamily,
|
||||
FontFamily latinFamily,
|
||||
string chineseFamilyName,
|
||||
string latinFamilyName)
|
||||
{
|
||||
_collection = collection ?? throw new ArgumentNullException(nameof(collection));
|
||||
ChineseFamily = chineseFamily ?? throw new ArgumentNullException(nameof(chineseFamily));
|
||||
LatinFamily = latinFamily ?? throw new ArgumentNullException(nameof(latinFamily));
|
||||
_chineseFamilyName = chineseFamilyName ?? throw new ArgumentNullException(nameof(chineseFamilyName));
|
||||
_latinFamilyName = latinFamilyName ?? throw new ArgumentNullException(nameof(latinFamilyName));
|
||||
}
|
||||
|
||||
public string ChineseFamilyName => _chineseFamilyName;
|
||||
public string LatinFamilyName => _latinFamilyName;
|
||||
internal FontFamily ChineseFamily { get; }
|
||||
internal FontFamily LatinFamily { get; }
|
||||
|
||||
internal Font CreateFont(FontFamily family, float points)
|
||||
{
|
||||
return new Font(family, points, FontStyle.Regular, GraphicsUnit.Point);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
_collection.Dispose();
|
||||
}
|
||||
}
|
||||
+287
@@ -0,0 +1,287 @@
|
||||
using System;
|
||||
using System.Drawing;
|
||||
using System.Drawing.Drawing2D;
|
||||
using System.Drawing.Imaging;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Runtime.InteropServices;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>仅 Windows 运行时使用 GDI+ 将每张共享图形定义渲染为 600 dpi PNG；轨迹仅绘制离散点。</summary>
|
||||
public sealed class SmoothingPngRenderer
|
||||
{
|
||||
public const int WidthPixels = 4296;
|
||||
public const int HeightPixels = 3120;
|
||||
public const uint PixelsPerMeter = 23622u;
|
||||
private const float PixelsPerPoint = 600f / 72f;
|
||||
|
||||
public byte[] Render(SmoothingFigureModel model, SmoothingFontResolution fonts)
|
||||
{
|
||||
if (model == null) throw new ArgumentNullException(nameof(model));
|
||||
return Render(new SmoothingFigureSetBuilder().Build(model).Figures[1], fonts);
|
||||
}
|
||||
|
||||
public byte[] Render(SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
if (figure == null) throw new ArgumentNullException(nameof(figure));
|
||||
if (fonts == null) throw new ArgumentNullException(nameof(fonts));
|
||||
using (var bitmap = new Bitmap(WidthPixels, HeightPixels, PixelFormat.Format32bppArgb))
|
||||
{
|
||||
bitmap.SetResolution(600f, 600f);
|
||||
using (Graphics graphics = Graphics.FromImage(bitmap))
|
||||
{
|
||||
graphics.Clear(Color.White);
|
||||
graphics.SmoothingMode = SmoothingMode.AntiAlias;
|
||||
graphics.InterpolationMode = InterpolationMode.HighQualityBicubic;
|
||||
graphics.PixelOffsetMode = PixelOffsetMode.HighQuality;
|
||||
DrawFigure(graphics, figure, fonts);
|
||||
}
|
||||
return Encode(bitmap);
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawFigure(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
DrawMixedText(graphics, fonts, figure.Title + " — " + figure.Model.ScenarioLabel, PointX(figure.PlotXPoints), PointY(12d), 12f, Color.Black);
|
||||
if (figure.IsCurvatureFigure) DrawCurvatureAxes(graphics, figure, fonts); else DrawOverheadAxes(graphics, figure, fonts);
|
||||
GraphicsState state = graphics.Save();
|
||||
graphics.SetClip(new RectangleF(PointX(figure.PlotXPoints), PointY(figure.PlotYPoints), PointX(figure.PlotWidthPoints), PointY(figure.PlotHeightPoints)));
|
||||
if (!figure.IsCurvatureFigure && figure.ShowsMapContext) DrawObstacles(graphics, figure);
|
||||
if (figure.IsCurvatureFigure) DrawCurvaturePoints(graphics, figure); else DrawPathPoints(graphics, figure);
|
||||
if (!figure.IsCurvatureFigure && figure.ShowsMapContext) DrawStartGoal(graphics, figure);
|
||||
graphics.Restore(state);
|
||||
DrawLegend(graphics, figure, fonts);
|
||||
}
|
||||
|
||||
private static void DrawOverheadAxes(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
DrawPlotFrame(graphics, figure);
|
||||
using (var grid = new Pen(Color.FromArgb(230, 230, 230), 0.5f * PixelsPerPoint))
|
||||
{
|
||||
for (int index = 0; index < figure.XTicks.Count; index++)
|
||||
{
|
||||
float x = WorldX(figure, figure.XTicks[index]);
|
||||
graphics.DrawLine(grid, x, PointY(figure.PlotYPoints), x, PointY(figure.PlotYPoints + figure.PlotHeightPoints));
|
||||
DrawMixedText(graphics, fonts, Number(figure.XTicks[index]), x - 10f * PixelsPerPoint, PointY(figure.PlotYPoints + figure.PlotHeightPoints + 5d), 8f, Color.Black);
|
||||
}
|
||||
for (int index = 0; index < figure.YTicks.Count; index++)
|
||||
{
|
||||
float y = WorldY(figure, figure.YTicks[index]);
|
||||
graphics.DrawLine(grid, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y);
|
||||
DrawMixedText(graphics, fonts, Number(figure.YTicks[index]), PointX(figure.PlotXPoints - 34d), y - 5f * PixelsPerPoint, 8f, Color.Black);
|
||||
}
|
||||
}
|
||||
DrawMixedText(graphics, fonts, "X (m)", PointX(figure.PlotXPoints + figure.PlotWidthPoints / 2d - 11d), PointY(figure.PlotYPoints + figure.PlotHeightPoints + 20d), 10f, Color.Black);
|
||||
DrawVerticalText(graphics, fonts, "Y (m)", PointX(12d), PointY(figure.PlotYPoints + figure.PlotHeightPoints / 2d + 17d), 10f, Color.Black);
|
||||
}
|
||||
|
||||
private static void DrawCurvatureAxes(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
DrawPlotFrame(graphics, figure);
|
||||
using (var grid = new Pen(Color.FromArgb(230, 230, 230), 0.5f * PixelsPerPoint))
|
||||
{
|
||||
for (int index = 0; index < figure.CurvatureArcLengthTicks.Count; index++)
|
||||
{
|
||||
float x = CurvatureX(figure, figure.CurvatureArcLengthTicks[index]);
|
||||
graphics.DrawLine(grid, x, PointY(figure.PlotYPoints), x, PointY(figure.PlotYPoints + figure.PlotHeightPoints));
|
||||
DrawMixedText(graphics, fonts, Number(figure.CurvatureArcLengthTicks[index]), x - 10f * PixelsPerPoint, PointY(figure.PlotYPoints + figure.PlotHeightPoints + 5d), 8f, Color.Black);
|
||||
}
|
||||
for (int index = 0; index < figure.CurvatureTicks.Count; index++)
|
||||
{
|
||||
float y = CurvatureY(figure, figure.CurvatureTicks[index]);
|
||||
graphics.DrawLine(grid, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y);
|
||||
DrawMixedText(graphics, fonts, Number(figure.CurvatureTicks[index]), PointX(figure.PlotXPoints - 34d), y - 5f * PixelsPerPoint, 8f, Color.Black);
|
||||
}
|
||||
}
|
||||
if (figure.CurvatureMinimumPerMeter < 0d && figure.CurvatureMaximumPerMeter > 0d)
|
||||
{
|
||||
float y = CurvatureY(figure, 0d);
|
||||
using (var zero = new Pen(Color.FromArgb(77, 77, 77), 0.65f * PixelsPerPoint))
|
||||
graphics.DrawLine(zero, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y);
|
||||
}
|
||||
DrawMixedText(graphics, fonts, "s (m)", PointX(figure.PlotXPoints + figure.PlotWidthPoints / 2d - 10d), PointY(figure.PlotYPoints + figure.PlotHeightPoints + 20d), 10f, Color.Black);
|
||||
DrawVerticalText(graphics, fonts, "κ (m⁻¹)", PointX(12d), PointY(figure.PlotYPoints + figure.PlotHeightPoints / 2d + 22d), 10f, Color.Black);
|
||||
}
|
||||
|
||||
private static void DrawPlotFrame(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
using (var border = new Pen(Color.Black, 0.75f * PixelsPerPoint))
|
||||
graphics.DrawRectangle(border, PointX(figure.PlotXPoints), PointY(figure.PlotYPoints), PointX(figure.PlotWidthPoints), PointY(figure.PlotHeightPoints));
|
||||
}
|
||||
|
||||
private static void DrawObstacles(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
using (var fill = new SolidBrush(Color.FromArgb(217, 217, 217)))
|
||||
using (var outline = new Pen(Color.FromArgb(128, 128, 128), 0.35f * PixelsPerPoint))
|
||||
{
|
||||
for (int index = 0; index < figure.Model.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFigureObstacle obstacle = figure.Model.Obstacles[index];
|
||||
float x = WorldX(figure, obstacle.X);
|
||||
float y = WorldY(figure, obstacle.Y + obstacle.Height);
|
||||
float width = (float)(obstacle.Width * figure.WorldScalePointsPerMeter * PixelsPerPoint);
|
||||
float height = (float)(obstacle.Height * figure.WorldScalePointsPerMeter * PixelsPerPoint);
|
||||
graphics.FillRectangle(fill, x, y, width, height);
|
||||
graphics.DrawRectangle(outline, x, y, width, height);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawPathPoints(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++)
|
||||
{
|
||||
SmoothingFigureSeriesView view = figure.Series[viewIndex];
|
||||
DrawPoints(graphics, view, point => new PointF(WorldX(figure, point.X), WorldY(figure, point.Y)));
|
||||
DrawViolations(graphics, figure, view);
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawCurvaturePoints(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++)
|
||||
{
|
||||
SmoothingFigureSeriesView view = figure.Series[viewIndex];
|
||||
DrawPoints(graphics, view, point => new PointF(CurvatureX(figure, point.ArcLength), CurvatureY(figure, point.VehicleCurvature)));
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawPoints(Graphics graphics, SmoothingFigureSeriesView view, Func<SmoothingFigurePoint, PointF> transform)
|
||||
{
|
||||
if (!view.Series.IsCurveVisible) return;
|
||||
using (var fill = new SolidBrush(WithOpacity(ColorFromHex(view.Series.Color), view.Opacity)))
|
||||
{
|
||||
float radius = (float)(view.PointRadiusPoints * PixelsPerPoint);
|
||||
for (int index = 0; index < view.Series.Points.Count; index++)
|
||||
{
|
||||
PointF point = transform(view.Series.Points[index]);
|
||||
graphics.FillEllipse(fill, point.X - radius, point.Y - radius, radius * 2f, radius * 2f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawViolations(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFigureSeriesView view)
|
||||
{
|
||||
if (view.Series.ViolationMarkers.Count == 0) return;
|
||||
using (var marker = new Pen(ColorFromHex(view.Series.Color), 1.1f * PixelsPerPoint))
|
||||
{
|
||||
float radius = 3f * PixelsPerPoint;
|
||||
for (int index = 0; index < view.Series.ViolationMarkers.Count; index++)
|
||||
{
|
||||
SmoothingFigurePoint point = view.Series.ViolationMarkers[index];
|
||||
float x = WorldX(figure, point.X), y = WorldY(figure, point.Y);
|
||||
graphics.DrawLine(marker, x - radius, y - radius, x + radius, y + radius);
|
||||
graphics.DrawLine(marker, x - radius, y + radius, x + radius, y - radius);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawStartGoal(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
float startX = WorldX(figure, figure.Model.Start.X), startY = WorldY(figure, figure.Model.Start.Y), radius = 3.2f * PixelsPerPoint;
|
||||
using (var startBrush = new SolidBrush(Color.FromArgb(240, 228, 66)))
|
||||
using (var border = new Pen(Color.Black, 0.8f * PixelsPerPoint))
|
||||
using (var goalBrush = new SolidBrush(ColorFromHex(IeeeFigureStyle.LimitColor)))
|
||||
{
|
||||
graphics.FillEllipse(startBrush, startX - radius, startY - radius, radius * 2f, radius * 2f);
|
||||
graphics.DrawEllipse(border, startX - radius, startY - radius, radius * 2f, radius * 2f);
|
||||
float goalX = WorldX(figure, figure.Model.Goal.X), goalY = WorldY(figure, figure.Model.Goal.Y), diamond = 4f * PixelsPerPoint;
|
||||
PointF[] points = { new PointF(goalX, goalY - diamond), new PointF(goalX + diamond, goalY), new PointF(goalX, goalY + diamond), new PointF(goalX - diamond, goalY) };
|
||||
graphics.FillPolygon(goalBrush, points);
|
||||
graphics.DrawPolygon(border, points);
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawLegend(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
const double columnWidth = 198d;
|
||||
for (int index = 0; index < figure.LegendEntries.Count; index++)
|
||||
{
|
||||
SmoothingFigureLegendEntry entry = figure.LegendEntries[index];
|
||||
int column = index % 2, row = index / 2;
|
||||
float x = PointX(figure.PlotXPoints + column * columnWidth);
|
||||
float y = PointY(figure.LegendYPoints + row * 15d - 3d);
|
||||
using (var fill = new SolidBrush(ColorFromHex(entry.Color))) graphics.FillEllipse(fill, x, y - 2.2f * PixelsPerPoint, 4.4f * PixelsPerPoint, 4.4f * PixelsPerPoint);
|
||||
DrawMixedText(graphics, fonts, entry.Label, x + 10f * PixelsPerPoint, y - 5f * PixelsPerPoint, 8.5f, Color.Black);
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawVerticalText(Graphics graphics, SmoothingFontResolution fonts, string text, float x, float y, float points, Color color)
|
||||
{
|
||||
GraphicsState state = graphics.Save();
|
||||
graphics.TranslateTransform(x, y);
|
||||
graphics.RotateTransform(-90f);
|
||||
DrawMixedText(graphics, fonts, text, 0f, 0f, points, color);
|
||||
graphics.Restore(state);
|
||||
}
|
||||
|
||||
private static void DrawMixedText(Graphics graphics, SmoothingFontResolution fonts, string text, float x, float top, float points, Color color)
|
||||
{
|
||||
var runs = SmoothingFontResolver.SplitRuns(text);
|
||||
float emPixels = points * PixelsPerPoint, maxAscent = 0f;
|
||||
for (int index = 0; index < runs.Count; index++)
|
||||
{
|
||||
FontFamily family = runs[index].IsChinese ? fonts.ChineseFamily : fonts.LatinFamily;
|
||||
maxAscent = Math.Max(maxAscent, family.GetCellAscent(FontStyle.Regular) * emPixels / family.GetEmHeight(FontStyle.Regular));
|
||||
}
|
||||
float baseline = top + maxAscent;
|
||||
using (var brush = new SolidBrush(color))
|
||||
using (var format = (StringFormat)StringFormat.GenericTypographic.Clone())
|
||||
{
|
||||
format.FormatFlags |= StringFormatFlags.MeasureTrailingSpaces;
|
||||
for (int index = 0; index < runs.Count; index++)
|
||||
{
|
||||
FontFamily family = runs[index].IsChinese ? fonts.ChineseFamily : fonts.LatinFamily;
|
||||
using (Font font = fonts.CreateFont(family, points))
|
||||
{
|
||||
float runTop = baseline - family.GetCellAscent(FontStyle.Regular) * emPixels / family.GetEmHeight(FontStyle.Regular);
|
||||
graphics.DrawString(runs[index].Text, font, brush, x, runTop, format);
|
||||
x += graphics.MeasureString(runs[index].Text, font, PointF.Empty, format).Width;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static byte[] Encode(Bitmap bitmap)
|
||||
{
|
||||
Rectangle rectangle = new Rectangle(0, 0, bitmap.Width, bitmap.Height);
|
||||
BitmapData data = bitmap.LockBits(rectangle, ImageLockMode.ReadOnly, PixelFormat.Format32bppArgb);
|
||||
try
|
||||
{
|
||||
int sourceLength = checked(data.Stride * bitmap.Height);
|
||||
var source = new byte[sourceLength];
|
||||
Marshal.Copy(data.Scan0, source, 0, source.Length);
|
||||
var rgba = new byte[checked(bitmap.Width * bitmap.Height * 4)];
|
||||
for (int y = 0; y < bitmap.Height; y++)
|
||||
{
|
||||
int sourceRow = y * data.Stride, outputRow = y * bitmap.Width * 4;
|
||||
for (int x = 0; x < bitmap.Width; x++)
|
||||
{
|
||||
int sourceOffset = sourceRow + x * 4, outputOffset = outputRow + x * 4;
|
||||
rgba[outputOffset] = source[sourceOffset + 2];
|
||||
rgba[outputOffset + 1] = source[sourceOffset + 1];
|
||||
rgba[outputOffset + 2] = source[sourceOffset];
|
||||
rgba[outputOffset + 3] = source[sourceOffset + 3];
|
||||
}
|
||||
}
|
||||
using (var output = new MemoryStream())
|
||||
{
|
||||
ValidatedPngWriter.Write(rgba, bitmap.Width, bitmap.Height, output, PixelsPerMeter);
|
||||
return output.ToArray();
|
||||
}
|
||||
}
|
||||
finally { bitmap.UnlockBits(data); }
|
||||
}
|
||||
|
||||
private static Color ColorFromHex(string value) { return ColorTranslator.FromHtml(value); }
|
||||
private static Color WithOpacity(Color color, double opacity) { return Color.FromArgb((int)Math.Round(255d * opacity), color.R, color.G, color.B); }
|
||||
private static float PointX(double points) { return (float)(points * PixelsPerPoint); }
|
||||
private static float PointY(double points) { return (float)(points * PixelsPerPoint); }
|
||||
private static float WorldX(SmoothingFigureDefinition figure, double x) { return PointX(figure.PlotXPoints + (x - figure.WorldXMinMeters) * figure.WorldScalePointsPerMeter); }
|
||||
private static float WorldY(SmoothingFigureDefinition figure, double y) { return PointY(figure.PlotYPoints + figure.PlotHeightPoints - (y - figure.WorldYMinMeters) * figure.WorldScalePointsPerMeter); }
|
||||
private static float CurvatureX(SmoothingFigureDefinition figure, double arcLength) { return PointX(figure.PlotXPoints + arcLength / figure.CurvatureArcLengthMaximumMeters * figure.PlotWidthPoints); }
|
||||
private static float CurvatureY(SmoothingFigureDefinition figure, double curvature) { return PointY(figure.PlotYPoints + figure.PlotHeightPoints - (curvature - figure.CurvatureMinimumPerMeter) / (figure.CurvatureMaximumPerMeter - figure.CurvatureMinimumPerMeter) * figure.PlotHeightPoints); }
|
||||
private static string Number(double value) { return value.ToString("0.###", CultureInfo.InvariantCulture); }
|
||||
}
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>一次报告导出的共享图形模型、目标目录与精确字体要求。</summary>
|
||||
public sealed class SmoothingReportExportRequest
|
||||
{
|
||||
public SmoothingFigureModel Model { get; set; }
|
||||
public string OutputDirectory { get; set; }
|
||||
public string FileStem { get; set; }
|
||||
public string ChineseFontFamilyName { get; set; } = "SimSun";
|
||||
public string LatinFontFamilyName { get; set; } = "Times New Roman";
|
||||
}
|
||||
+49
@@ -0,0 +1,49 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>报告导出结果的稳定状态。</summary>
|
||||
public enum SmoothingReportExportStatus
|
||||
{
|
||||
Success,
|
||||
InvalidInput,
|
||||
FontUnavailable,
|
||||
Failed,
|
||||
}
|
||||
|
||||
/// <summary>六张常规 SVG、六张常规 PNG 与一个 CSV 的发布结果；增强模型携带诊断序列时追加可选 07-local-g2-diagnostic-candidate，失败时不返回部分输出路径。</summary>
|
||||
public sealed class SmoothingReportExportResult
|
||||
{
|
||||
internal SmoothingReportExportResult(SmoothingReportExportStatus status, string reason, IReadOnlyList<string> svgPaths, IReadOnlyList<string> pngPaths, string csvPath)
|
||||
{
|
||||
Status = status;
|
||||
Reason = reason ?? string.Empty;
|
||||
SvgPaths = Copy(svgPaths);
|
||||
PngPaths = Copy(pngPaths);
|
||||
CsvPath = csvPath ?? string.Empty;
|
||||
}
|
||||
|
||||
public SmoothingReportExportStatus Status { get; }
|
||||
public string Reason { get; }
|
||||
public IReadOnlyList<string> SvgPaths { get; }
|
||||
public IReadOnlyList<string> PngPaths { get; }
|
||||
public string CsvPath { get; }
|
||||
|
||||
internal static SmoothingReportExportResult Success(IReadOnlyList<string> svgPaths, IReadOnlyList<string> pngPaths, string csvPath)
|
||||
{
|
||||
return new SmoothingReportExportResult(SmoothingReportExportStatus.Success, string.Empty, svgPaths, pngPaths, csvPath);
|
||||
}
|
||||
|
||||
internal static SmoothingReportExportResult Failure(SmoothingReportExportStatus status, string reason)
|
||||
{
|
||||
return new SmoothingReportExportResult(status, reason, new string[0], new string[0], string.Empty);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<string> Copy(IReadOnlyList<string> source)
|
||||
{
|
||||
var copy = new List<string>(source == null ? 0 : source.Count);
|
||||
if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index] ?? string.Empty);
|
||||
return new ReadOnlyCollection<string>(copy);
|
||||
}
|
||||
}
|
||||
+130
@@ -0,0 +1,130 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>以同级临时文件生成六张常规 SVG、六张常规 PNG 和 CSV 后成组发布；增强模型携带诊断序列时追加可选 07-local-g2-diagnostic-candidate。</summary>
|
||||
public sealed class SmoothingReportExporter
|
||||
{
|
||||
private readonly SmoothingFontResolver _fontResolver = new SmoothingFontResolver();
|
||||
private readonly SmoothingFigureSetBuilder _figureSetBuilder = new SmoothingFigureSetBuilder();
|
||||
private readonly SmoothingSvgRenderer _svgRenderer = new SmoothingSvgRenderer();
|
||||
private readonly SmoothingPngRenderer _pngRenderer = new SmoothingPngRenderer();
|
||||
private readonly SmoothingCsvWriter _csvWriter = new SmoothingCsvWriter();
|
||||
|
||||
public SmoothingReportExportResult Export(SmoothingReportExportRequest request)
|
||||
{
|
||||
if (request == null || request.Model == null || string.IsNullOrWhiteSpace(request.OutputDirectory) || !IsFileStem(request.FileStem))
|
||||
return SmoothingReportExportResult.Failure(SmoothingReportExportStatus.InvalidInput, "报告模型、输出目录或文件名无效。");
|
||||
if (!_fontResolver.TryResolve(request.ChineseFontFamilyName, request.LatinFontFamilyName, out SmoothingFontResolution availableFonts, out string fontReason))
|
||||
return SmoothingReportExportResult.Failure(SmoothingReportExportStatus.FontUnavailable, fontReason);
|
||||
availableFonts.Dispose();
|
||||
|
||||
var pending = new List<PendingFile>();
|
||||
var svgPaths = new List<string>();
|
||||
var pngPaths = new List<string>();
|
||||
string csvPath = Path.Combine(request.OutputDirectory, request.FileStem + ".csv");
|
||||
try
|
||||
{
|
||||
SmoothingFigureSet figures = _figureSetBuilder.Build(request.Model);
|
||||
for (int index = 0; index < figures.Figures.Count; index++)
|
||||
{
|
||||
SmoothingFigureDefinition figure = figures.Figures[index];
|
||||
string svgPath = Path.Combine(request.OutputDirectory, figure.FileStem + ".svg");
|
||||
string pngPath = Path.Combine(request.OutputDirectory, figure.FileStem + ".png");
|
||||
svgPaths.Add(svgPath);
|
||||
pngPaths.Add(pngPath);
|
||||
pending.Add(new PendingFile(svgPath, Encoding.UTF8.GetBytes(_svgRenderer.Render(figure))));
|
||||
if (!_fontResolver.TryResolve(request.ChineseFontFamilyName, request.LatinFontFamilyName, out SmoothingFontResolution renderFonts, out fontReason))
|
||||
throw new InvalidOperationException(fontReason);
|
||||
byte[] png;
|
||||
using (renderFonts) png = _pngRenderer.Render(figure, renderFonts);
|
||||
pending.Add(new PendingFile(pngPath, png));
|
||||
}
|
||||
pending.Add(new PendingFile(csvPath, _csvWriter.Write(request.Model)));
|
||||
Directory.CreateDirectory(request.OutputDirectory);
|
||||
for (int index = 0; index < pending.Count; index++) File.WriteAllBytes(pending[index].TemporaryPath, pending[index].Content);
|
||||
PublishAll(pending);
|
||||
DeleteIfExists(Path.Combine(request.OutputDirectory, "comparison.svg"));
|
||||
DeleteIfExists(Path.Combine(request.OutputDirectory, "comparison.png"));
|
||||
return SmoothingReportExportResult.Success(svgPaths, pngPaths, csvPath);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
RestorePublishedFiles(pending);
|
||||
return SmoothingReportExportResult.Failure(SmoothingReportExportStatus.Failed, exception.Message);
|
||||
}
|
||||
finally
|
||||
{
|
||||
for (int index = 0; index < pending.Count; index++)
|
||||
{
|
||||
DeleteIfExists(pending[index].TemporaryPath);
|
||||
DeleteIfExists(pending[index].BackupPath);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void PublishAll(IReadOnlyList<PendingFile> files)
|
||||
{
|
||||
string transaction = Guid.NewGuid().ToString("N");
|
||||
for (int index = 0; index < files.Count; index++)
|
||||
{
|
||||
PendingFile file = files[index];
|
||||
file.ExistedBeforePublish = File.Exists(file.FinalPath);
|
||||
file.BackupPath = file.ExistedBeforePublish ? file.FinalPath + ".backup-" + transaction : string.Empty;
|
||||
if (file.ExistedBeforePublish) File.Replace(file.TemporaryPath, file.FinalPath, file.BackupPath);
|
||||
else File.Move(file.TemporaryPath, file.FinalPath);
|
||||
file.Published = true;
|
||||
}
|
||||
for (int index = 0; index < files.Count; index++) DeleteIfExists(files[index].BackupPath);
|
||||
}
|
||||
|
||||
private static void RestorePublishedFiles(IReadOnlyList<PendingFile> files)
|
||||
{
|
||||
for (int index = files.Count - 1; index >= 0; index--)
|
||||
{
|
||||
PendingFile file = files[index];
|
||||
if (!file.Published) continue;
|
||||
try
|
||||
{
|
||||
if (file.ExistedBeforePublish && File.Exists(file.BackupPath))
|
||||
{
|
||||
if (File.Exists(file.FinalPath)) File.Replace(file.BackupPath, file.FinalPath, null);
|
||||
else File.Move(file.BackupPath, file.FinalPath);
|
||||
}
|
||||
else if (!file.ExistedBeforePublish) DeleteIfExists(file.FinalPath);
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IsFileStem(string value)
|
||||
{
|
||||
return !string.IsNullOrWhiteSpace(value) && value.IndexOfAny(Path.GetInvalidFileNameChars()) < 0 && value.IndexOf(Path.DirectorySeparatorChar) < 0 && value.IndexOf(Path.AltDirectorySeparatorChar) < 0;
|
||||
}
|
||||
|
||||
private static void DeleteIfExists(string path)
|
||||
{
|
||||
if (!string.IsNullOrEmpty(path) && File.Exists(path)) File.Delete(path);
|
||||
}
|
||||
|
||||
private sealed class PendingFile
|
||||
{
|
||||
public PendingFile(string finalPath, byte[] content)
|
||||
{
|
||||
FinalPath = finalPath;
|
||||
Content = content;
|
||||
TemporaryPath = finalPath + ".tmp";
|
||||
BackupPath = string.Empty;
|
||||
}
|
||||
|
||||
public string FinalPath { get; }
|
||||
public byte[] Content { get; }
|
||||
public string TemporaryPath { get; }
|
||||
public string BackupPath { get; set; }
|
||||
public bool ExistedBeforePublish { get; set; }
|
||||
public bool Published { get; set; }
|
||||
}
|
||||
}
|
||||
+189
@@ -0,0 +1,189 @@
|
||||
using System;
|
||||
using System.Globalization;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
/// <summary>把单张共享图形定义渲染为 UTF-8 XML 可编辑 SVG；轨迹仅由离散点组成。</summary>
|
||||
public sealed class SmoothingSvgRenderer
|
||||
{
|
||||
public string Render(SmoothingFigureModel model)
|
||||
{
|
||||
if (model == null) throw new ArgumentNullException(nameof(model));
|
||||
return Render(new SmoothingFigureSetBuilder().Build(model).Figures[1]);
|
||||
}
|
||||
|
||||
public string Render(SmoothingFigureDefinition figure)
|
||||
{
|
||||
if (figure == null) throw new ArgumentNullException(nameof(figure));
|
||||
var svg = new StringBuilder();
|
||||
svg.Append("<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<svg xmlns=\"http://www.w3.org/2000/svg\" width=\"")
|
||||
.Append(Number(figure.FigureWidthPoints)).Append("pt\" height=\"").Append(Number(figure.FigureHeightPoints))
|
||||
.Append("pt\" viewBox=\"0 0 ").Append(Number(figure.FigureWidthPoints)).Append(' ').Append(Number(figure.FigureHeightPoints)).Append("\">\n")
|
||||
.Append("<rect width=\"100%\" height=\"100%\" fill=\"#FFFFFF\"/>\n")
|
||||
.Append("<clipPath id=\"plot-clip\"><rect x=\"").Append(Number(figure.PlotXPoints)).Append("\" y=\"").Append(Number(figure.PlotYPoints))
|
||||
.Append("\" width=\"").Append(Number(figure.PlotWidthPoints)).Append("\" height=\"").Append(Number(figure.PlotHeightPoints)).Append("\"/></clipPath>\n");
|
||||
AppendTitle(svg, figure);
|
||||
if (figure.IsCurvatureFigure) AppendCurvatureAxes(svg, figure); else AppendOverheadAxes(svg, figure);
|
||||
svg.Append("<g clip-path=\"url(#plot-clip)\">\n");
|
||||
if (!figure.IsCurvatureFigure && figure.ShowsMapContext) AppendObstacles(svg, figure);
|
||||
if (figure.IsCurvatureFigure) AppendCurvaturePoints(svg, figure); else AppendPathPoints(svg, figure);
|
||||
if (!figure.IsCurvatureFigure && figure.ShowsMapContext) AppendStartGoal(svg, figure);
|
||||
svg.Append("</g>\n");
|
||||
AppendLegend(svg, figure);
|
||||
svg.Append("</svg>");
|
||||
return svg.ToString();
|
||||
}
|
||||
|
||||
private static void AppendTitle(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints)).Append("\" y=\"23\" font-size=\"12\"><tspan font-family=\"SimSun\">")
|
||||
.Append(Escape(figure.Title)).Append("</tspan><tspan font-family=\"Times New Roman\"> — ").Append(Escape(figure.Model.ScenarioLabel)).Append("</tspan></text>\n");
|
||||
}
|
||||
|
||||
private static void AppendOverheadAxes(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
AppendPlotFrame(svg, figure);
|
||||
for (int index = 0; index < figure.XTicks.Count; index++)
|
||||
{
|
||||
double x = WorldX(figure, figure.XTicks[index]);
|
||||
AppendGridLine(svg, x, figure.PlotYPoints, x, figure.PlotYPoints + figure.PlotHeightPoints);
|
||||
svg.Append("<text x=\"").Append(Number(x)).Append("\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints + 15d))
|
||||
.Append("\" text-anchor=\"middle\" font-family=\"Times New Roman\" font-size=\"8\">").Append(Number(figure.XTicks[index])).Append("</text>\n");
|
||||
}
|
||||
for (int index = 0; index < figure.YTicks.Count; index++)
|
||||
{
|
||||
double y = WorldY(figure, figure.YTicks[index]);
|
||||
AppendGridLine(svg, figure.PlotXPoints, y, figure.PlotXPoints + figure.PlotWidthPoints, y);
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints - 8d)).Append("\" y=\"").Append(Number(y + 3d))
|
||||
.Append("\" text-anchor=\"end\" font-family=\"Times New Roman\" font-size=\"8\">").Append(Number(figure.YTicks[index])).Append("</text>\n");
|
||||
}
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints + figure.PlotWidthPoints / 2d)).Append("\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints + 31d))
|
||||
.Append("\" text-anchor=\"middle\" font-family=\"Times New Roman\" font-size=\"10\">X (m)</text>\n")
|
||||
.Append("<text x=\"19\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints / 2d)).Append("\" text-anchor=\"middle\" transform=\"rotate(-90 19 ")
|
||||
.Append(Number(figure.PlotYPoints + figure.PlotHeightPoints / 2d)).Append(")\" font-family=\"Times New Roman\" font-size=\"10\">Y (m)</text>\n");
|
||||
}
|
||||
|
||||
private static void AppendCurvatureAxes(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
AppendPlotFrame(svg, figure);
|
||||
for (int index = 0; index < figure.CurvatureArcLengthTicks.Count; index++)
|
||||
{
|
||||
double x = CurvatureX(figure, figure.CurvatureArcLengthTicks[index]);
|
||||
AppendGridLine(svg, x, figure.PlotYPoints, x, figure.PlotYPoints + figure.PlotHeightPoints);
|
||||
svg.Append("<text x=\"").Append(Number(x)).Append("\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints + 15d))
|
||||
.Append("\" text-anchor=\"middle\" font-family=\"Times New Roman\" font-size=\"8\">").Append(Number(figure.CurvatureArcLengthTicks[index])).Append("</text>\n");
|
||||
}
|
||||
for (int index = 0; index < figure.CurvatureTicks.Count; index++)
|
||||
{
|
||||
double y = CurvatureY(figure, figure.CurvatureTicks[index]);
|
||||
AppendGridLine(svg, figure.PlotXPoints, y, figure.PlotXPoints + figure.PlotWidthPoints, y);
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints - 8d)).Append("\" y=\"").Append(Number(y + 3d))
|
||||
.Append("\" text-anchor=\"end\" font-family=\"Times New Roman\" font-size=\"8\">").Append(Number(figure.CurvatureTicks[index])).Append("</text>\n");
|
||||
}
|
||||
if (figure.CurvatureMinimumPerMeter < 0d && figure.CurvatureMaximumPerMeter > 0d)
|
||||
{
|
||||
double zero = CurvatureY(figure, 0d);
|
||||
svg.Append("<line x1=\"").Append(Number(figure.PlotXPoints)).Append("\" y1=\"").Append(Number(zero)).Append("\" x2=\"")
|
||||
.Append(Number(figure.PlotXPoints + figure.PlotWidthPoints)).Append("\" y2=\"").Append(Number(zero)).Append("\" stroke=\"#4D4D4D\" stroke-width=\"0.65\"/>\n");
|
||||
}
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints + figure.PlotWidthPoints / 2d)).Append("\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints + 31d))
|
||||
.Append("\" text-anchor=\"middle\" font-family=\"Times New Roman\" font-size=\"10\">s (m)</text>\n")
|
||||
.Append("<text x=\"19\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints / 2d)).Append("\" text-anchor=\"middle\" transform=\"rotate(-90 19 ")
|
||||
.Append(Number(figure.PlotYPoints + figure.PlotHeightPoints / 2d)).Append(")\" font-family=\"Times New Roman\" font-size=\"10\">κ (m⁻¹)</text>\n");
|
||||
}
|
||||
|
||||
private static void AppendPlotFrame(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
svg.Append("<rect x=\"").Append(Number(figure.PlotXPoints)).Append("\" y=\"").Append(Number(figure.PlotYPoints)).Append("\" width=\"")
|
||||
.Append(Number(figure.PlotWidthPoints)).Append("\" height=\"").Append(Number(figure.PlotHeightPoints)).Append("\" fill=\"#FFFFFF\" stroke=\"#000000\" stroke-width=\"0.75\"/>\n");
|
||||
}
|
||||
|
||||
private static void AppendGridLine(StringBuilder svg, double x1, double y1, double x2, double y2)
|
||||
{
|
||||
svg.Append("<line x1=\"").Append(Number(x1)).Append("\" y1=\"").Append(Number(y1)).Append("\" x2=\"").Append(Number(x2)).Append("\" y2=\"")
|
||||
.Append(Number(y2)).Append("\" stroke=\"#E6E6E6\" stroke-width=\"0.5\"/>\n");
|
||||
}
|
||||
|
||||
private static void AppendObstacles(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int index = 0; index < figure.Model.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFigureObstacle obstacle = figure.Model.Obstacles[index];
|
||||
svg.Append("<rect class=\"obstacle\" x=\"").Append(Number(WorldX(figure, obstacle.X))).Append("\" y=\"")
|
||||
.Append(Number(WorldY(figure, obstacle.Y + obstacle.Height))).Append("\" width=\"").Append(Number(obstacle.Width * figure.WorldScalePointsPerMeter))
|
||||
.Append("\" height=\"").Append(Number(obstacle.Height * figure.WorldScalePointsPerMeter)).Append("\" fill=\"#D9D9D9\" stroke=\"#808080\" stroke-width=\"0.35\"/>\n");
|
||||
}
|
||||
}
|
||||
|
||||
private static void AppendPathPoints(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++)
|
||||
{
|
||||
SmoothingFigureSeriesView view = figure.Series[viewIndex];
|
||||
AppendPoints(svg, view, point => WorldX(figure, point.X), point => WorldY(figure, point.Y));
|
||||
for (int markerIndex = 0; markerIndex < view.Series.ViolationMarkers.Count; markerIndex++) AppendCross(svg, figure, view.Series.ViolationMarkers[markerIndex], view.Series.Color);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AppendCurvaturePoints(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++)
|
||||
{
|
||||
SmoothingFigureSeriesView view = figure.Series[viewIndex];
|
||||
AppendPoints(svg, view, point => CurvatureX(figure, point.ArcLength), point => CurvatureY(figure, point.VehicleCurvature));
|
||||
}
|
||||
}
|
||||
|
||||
private static void AppendPoints(StringBuilder svg, SmoothingFigureSeriesView view, Func<SmoothingFigurePoint, double> x, Func<SmoothingFigurePoint, double> y)
|
||||
{
|
||||
if (!view.Series.IsCurveVisible) return;
|
||||
svg.Append("<g class=\"trajectory-series\" data-series=\"").Append(Escape(view.Series.Key)).Append("\" fill=\"").Append(view.Series.Color).Append("\" opacity=\"").Append(Number(view.Opacity)).Append("\">\n");
|
||||
for (int index = 0; index < view.Series.Points.Count; index++)
|
||||
{
|
||||
SmoothingFigurePoint point = view.Series.Points[index];
|
||||
svg.Append("<circle class=\"trajectory-point\" cx=\"").Append(Number(x(point))).Append("\" cy=\"").Append(Number(y(point))).Append("\" r=\"").Append(Number(view.PointRadiusPoints)).Append("\"/>\n");
|
||||
}
|
||||
svg.Append("</g>\n");
|
||||
}
|
||||
|
||||
private static void AppendCross(StringBuilder svg, SmoothingFigureDefinition figure, SmoothingFigurePoint point, string color)
|
||||
{
|
||||
double x = WorldX(figure, point.X), y = WorldY(figure, point.Y), radius = 3d;
|
||||
svg.Append("<g class=\"violation-cross\" stroke=\"").Append(color).Append("\" stroke-width=\"1.1\"><line x1=\"").Append(Number(x - radius)).Append("\" y1=\"")
|
||||
.Append(Number(y - radius)).Append("\" x2=\"").Append(Number(x + radius)).Append("\" y2=\"").Append(Number(y + radius)).Append("\"/><line x1=\"")
|
||||
.Append(Number(x - radius)).Append("\" y1=\"").Append(Number(y + radius)).Append("\" x2=\"").Append(Number(x + radius)).Append("\" y2=\"").Append(Number(y - radius)).Append("\"/></g>\n");
|
||||
}
|
||||
|
||||
private static void AppendStartGoal(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
svg.Append("<circle class=\"start-marker\" cx=\"").Append(Number(WorldX(figure, figure.Model.Start.X))).Append("\" cy=\"").Append(Number(WorldY(figure, figure.Model.Start.Y)))
|
||||
.Append("\" r=\"3.2\" fill=\"#F0E442\" stroke=\"#000000\" stroke-width=\"0.8\"/>\n");
|
||||
double x = WorldX(figure, figure.Model.Goal.X), y = WorldY(figure, figure.Model.Goal.Y), radius = 4d;
|
||||
svg.Append("<polygon class=\"goal-marker\" points=\"").Append(Number(x)).Append(',').Append(Number(y - radius)).Append(' ').Append(Number(x + radius)).Append(',').Append(Number(y)).Append(' ')
|
||||
.Append(Number(x)).Append(',').Append(Number(y + radius)).Append(' ').Append(Number(x - radius)).Append(',').Append(Number(y)).Append("\" fill=\"#CC79A7\" stroke=\"#000000\" stroke-width=\"0.8\"/>\n");
|
||||
}
|
||||
|
||||
private static void AppendLegend(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
double columnWidth = 198d;
|
||||
for (int index = 0; index < figure.LegendEntries.Count; index++)
|
||||
{
|
||||
SmoothingFigureLegendEntry entry = figure.LegendEntries[index];
|
||||
int column = index % 2;
|
||||
int row = index / 2;
|
||||
double x = figure.PlotXPoints + column * columnWidth;
|
||||
double y = figure.LegendYPoints + row * 15d;
|
||||
svg.Append("<circle class=\"legend-point\" cx=\"").Append(Number(x + 3d)).Append("\" cy=\"").Append(Number(y - 3d)).Append("\" r=\"2.2\" fill=\"")
|
||||
.Append(entry.Color).Append("\"/>\n<text x=\"").Append(Number(x + 10d)).Append("\" y=\"").Append(Number(y)).Append("\" font-size=\"8.5\"><tspan font-family=\"SimSun\">")
|
||||
.Append(Escape(entry.Label)).Append("</tspan></text>\n");
|
||||
}
|
||||
}
|
||||
|
||||
private static double WorldX(SmoothingFigureDefinition figure, double x) { return figure.PlotXPoints + (x - figure.WorldXMinMeters) * figure.WorldScalePointsPerMeter; }
|
||||
private static double WorldY(SmoothingFigureDefinition figure, double y) { return figure.PlotYPoints + figure.PlotHeightPoints - (y - figure.WorldYMinMeters) * figure.WorldScalePointsPerMeter; }
|
||||
private static double CurvatureX(SmoothingFigureDefinition figure, double arcLength) { return figure.PlotXPoints + arcLength / figure.CurvatureArcLengthMaximumMeters * figure.PlotWidthPoints; }
|
||||
private static double CurvatureY(SmoothingFigureDefinition figure, double curvature) { return figure.PlotYPoints + figure.PlotHeightPoints - (curvature - figure.CurvatureMinimumPerMeter) / (figure.CurvatureMaximumPerMeter - figure.CurvatureMinimumPerMeter) * figure.PlotHeightPoints; }
|
||||
private static string Number(double value) { return value.ToString("0.###", CultureInfo.InvariantCulture); }
|
||||
private static string Escape(string value) { return (value ?? string.Empty).Replace("&", "&").Replace("<", "<").Replace(">", ">").Replace("\"", """).Replace("'", "'"); }
|
||||
}
|
||||
@@ -37,6 +37,7 @@ internal static class RawPathBaselineBuilder
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
maximumCollisionCheckStepMeters,
|
||||
request.Configuration.CurvatureLimitRadiusToleranceMeters,
|
||||
out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearanceMeters,
|
||||
out reason))
|
||||
@@ -55,6 +56,7 @@ internal static class RawPathBaselineBuilder
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
maximumCollisionCheckStepMeters,
|
||||
request.Configuration.CurvatureLimitRadiusToleranceMeters,
|
||||
out safePath,
|
||||
out minimumClearanceMeters,
|
||||
out reason))
|
||||
@@ -146,7 +148,7 @@ internal static class RawPathBaselineBuilder
|
||||
double geometricCurvature = directionSign * point.VehicleCurvature;
|
||||
SmoothedPathPointSource source = point.IsGearSwitchPoint
|
||||
? SmoothedPathPointSource.GearSwitch
|
||||
: SmoothedPathPointSource.CoarsePathFallback;
|
||||
: SmoothedPathPointSource.Anchor;
|
||||
path.Add(new SmoothedPathPoint(
|
||||
point.X,
|
||||
point.Y,
|
||||
|
||||
@@ -0,0 +1,137 @@
|
||||
# PathSmoothing 路径平滑（Local G2）
|
||||
|
||||
## 模块说明（Module Overview）
|
||||
|
||||
`PathSmoothing` 是 Hybrid A* 成功输出后的空间参考路径后处理模块。它只保留 Local G2 五次过渡算法：在同一行驶方向的粗路径段内检测车辆曲率跳变，尝试以局部五次曲线替换，并对整条结果进行车辆曲率、全车体碰撞、净空和曲率变化代价复核。
|
||||
|
||||
该模块不参与粗路径搜索接受条件，也不做时间参数化、速度、加速度、转向速率或动态障碍物处理。下游只能消费本模块已经发布的空间路径。
|
||||
|
||||
## 文件结构（File Structure）
|
||||
|
||||
```text
|
||||
PathSmoothing/
|
||||
├── Contracts/ # 请求、结果、配置、状态、质量指标与区域报告
|
||||
├── Facade/ # 正式平滑入口和离线比较入口
|
||||
├── LocalG2/ # 跳变检测、窗口规划、候选生成、评估、拼接
|
||||
├── Processing/ # 粗路径预处理、原始基线和几何分析
|
||||
├── Validation/ # 全车体碰撞与车辆曲率复核
|
||||
├── Output/
|
||||
│ ├── Comparison/ # 原始路径与 Local G2 的稳定比较模型和摘要
|
||||
│ └── Visualization/ # SVG、PNG、CSV 报告模型和导出器
|
||||
├── Test/ # 固定夹具、演示入口和 Local G2 诊断证据
|
||||
│ └── Fixtures/ # 版本化的成功粗路径快照
|
||||
└── README.md
|
||||
```
|
||||
|
||||
`Algorithms/`、旧的 `Comparison/`、旧的 `Visualization/` 以及 B-spline、局部 Bezier、分段五次选项均已移除。输出相关代码统一归入 `Output/`，与 `CoarsePath/Output/` 的组织方式保持一致。
|
||||
|
||||
## 平滑数据流（Smoothing Data Flow）
|
||||
|
||||
```text
|
||||
成功的 CoarsePath
|
||||
-> PathSmoothingRequest 快照
|
||||
-> 输入校验与 PathSmoothingPreprocessor
|
||||
-> RawPathBaselineBuilder 原始路径全局复核
|
||||
-> CurvatureTransitionDetector（仅同向 PathSegment 内）
|
||||
-> LocalG2WindowPlanner
|
||||
-> LocalG2CandidateBuilder / LocalG2CandidateEvaluator
|
||||
-> LocalG2PathSplicer
|
||||
-> 全路径曲率、碰撞、净空、代价复核
|
||||
-> PathSmoothingResult.PublishLocalG2
|
||||
```
|
||||
|
||||
Local G2 不跨换向段建立窗口。它处理的是同一前进或倒退段内、由运动原语摆角变化等原因形成的车辆曲率跳变，而不是把换向点作为平滑对象。
|
||||
|
||||
## 结果状态与发布规则（Result Status and Publication Rules）
|
||||
|
||||
下列四个状态都会发布已复核的 `Path` 和 `Segments`，可由下游消费：
|
||||
|
||||
| 状态 | 含义 |
|
||||
| --- | --- |
|
||||
| `Complete` | 检测到的所有 Local G2 区域都被接受并替换。 |
|
||||
| `PartialImprovement` | 至少一个区域被替换，其余区域保留原始几何。 |
|
||||
| `NotNeeded` | 没有检测到同向段内的曲率跳变；发布已复核的原始路径。 |
|
||||
| `Unchanged` | 检测到区域，但所有候选都被拒绝；发布已复核的原始路径，并保留 `RegionReports`。 |
|
||||
|
||||
`InvalidInput`、`Cancelled`、`Failed` 和 `Infeasible` 不发布可消费路径。`Unchanged` 不是失败，它表示“安全门或质量门没有接受新候选”，不是“原始粗路径未经验证地回退”。
|
||||
|
||||
### 区域报告与拒绝原因
|
||||
|
||||
`PathSmoothingRegionReport` 按检测顺序记录每个 Local G2 区域。区域状态为 `Improved` 或 `RetainedOriginal`；后者可进一步给出 `WindowUnavailable`、`CandidateGenerationFailed`、`Collision`、`InsufficientClearance`、`CurvatureExceeded`、`CurvatureOvershoot`、`DeviationExceeded`、`InsufficientImprovement`、`VariationCostRegression` 或 `GlobalValidationRollback`。
|
||||
|
||||
候选先进行全车体碰撞复核，再检查净空余量。`MinimumClearanceReserveMeters = 0d` 只取消额外净空储备，不会取消车辆曲率、碰撞、数值有效性、偏差或曲率变化代价门。
|
||||
|
||||
## 坐标与单位（Coordinates and Units）
|
||||
|
||||
- 路径位置、弧长、窗口长度、偏差、净空和碰撞检查步长：m
|
||||
- 航向：rad
|
||||
- 车辆曲率：`1/m`
|
||||
- 车辆曲率导数：`1/m^2`
|
||||
- 地图构建输入仍沿用 mm；`PlanningGridMap` 的路径复核查询使用 m
|
||||
|
||||
发布路径保留成功粗路径的首尾位姿，因此末端误差继承粗规划器已接受的目标容差。
|
||||
|
||||
## 最小调用示例（Minimal Call Example）
|
||||
|
||||
```csharp
|
||||
if (coarseResult.PlanningResult.Status != PlanningStatus.Success)
|
||||
return;
|
||||
|
||||
var request = new PathSmoothingRequest(
|
||||
coarseResult.PlanningResult.Path,
|
||||
coarseResult.PlanningResult.Segments,
|
||||
coarseResult.MapResult.Map,
|
||||
job.Vehicle,
|
||||
new PathSmoothingConfiguration());
|
||||
|
||||
PathSmoothingResult result = new PathSmoothingService().Smooth(request, cancellationToken);
|
||||
bool published = result.Status == PathSmoothingStatus.Complete ||
|
||||
result.Status == PathSmoothingStatus.PartialImprovement ||
|
||||
result.Status == PathSmoothingStatus.NotNeeded ||
|
||||
result.Status == PathSmoothingStatus.Unchanged;
|
||||
|
||||
if (published)
|
||||
ConsumeSpatialReference(result.Path, result.Segments);
|
||||
```
|
||||
|
||||
`PathSmoothingConfiguration` 没有算法选择开关；正式入口始终执行 Local G2。
|
||||
|
||||
## 详细使用指南（Detailed Usage Guide）
|
||||
|
||||
1. 长期持有 `PathSmoothingService`，只向它传入成功的粗路径、方向分段、规划地图和车辆参数。
|
||||
2. 使用 `PathSmoothingConfiguration` 调整输出采样、碰撞检查步长和 Local G2 窗口/质量门参数。
|
||||
3. `MinimumClearanceReserveMeters` 当前默认 `0d`。需要额外净空时显式设置正值；碰撞约束始终存在。
|
||||
4. 按上表判断结果是否已发布，随后消费 `Path` 和 `Segments`；不要以 `Failed` 或 `Cancelled` 的空路径继续规划。
|
||||
5. 对 `PartialImprovement` 或 `Unchanged`，读取 `RegionReports` 判断被保留的区域以及拒绝原因。
|
||||
|
||||
Local G2 关键选项位于 `LocalG2Quintic`：最小、首选和最大窗口长度，最大偏差，曲率跳变阈值，峰值曲率导数改善比，曲率变化代价回退比，以及每区候选数。
|
||||
|
||||
## 固定夹具报告与可视化（Fixture Reports and Visualization）
|
||||
|
||||
`SmoothingScenarioFixtureLoader.LoadAndVerify` 会校验 JSON 夹具的版本和指纹；夹具报告不重新运行 Hybrid A*。离线比较始终输出“原始粗路径 + Local G2”两组数据。
|
||||
|
||||
`SmoothingReportExporter.Export(SmoothingReportExportRequest)` 为每个场景写入 CSV、SVG 和 PNG。常规图固定为四张：
|
||||
|
||||
1. `01-coarse-path-overview`
|
||||
2. `02-all-paths-comparison`
|
||||
3. `03-local-g2-overview`
|
||||
4. `04-curvature-comparison`
|
||||
|
||||
当模型显式附带诊断候选时，额外写入 `05-local-g2-diagnostic-candidate`。诊断候选仅用于解释拒绝，不是发布路径。
|
||||
|
||||
报告默认写入 `ClumsyPilot/obj/path_smoothing_reports` 的调用方指定子目录。PNG 需要 Windows 的 `SimSun` 与 `Times New Roman`；若字体不可用，导出返回 `FontUnavailable`，SVG 和 CSV 仍可单独使用。
|
||||
|
||||
## 常见错误（Common Errors）
|
||||
|
||||
- 将未成功的 CoarsePath 直接传入平滑：应先检查 `PlanningStatus.Success`。
|
||||
- 把 `Unchanged` 当作失败：它仍然包含已复核的原始路径和区域报告。
|
||||
- 认为净空余量为 0 会关闭碰撞检查：它只取消额外储备，不能绕过全车体碰撞和车辆曲率门。
|
||||
- 在换向点两侧强行连续平滑：当前窗口检测明确不跨方向段。
|
||||
- 使用过期夹具：修改粗路径场景或规划配置后，应按夹具生成/验证脚本更新快照。
|
||||
|
||||
## 第一版限制（First-Version Limits）
|
||||
|
||||
- 仅平滑同向段内的车辆曲率跳变；不跨换向点。
|
||||
- 仅包含 Local G2 五次局部过渡，不提供多算法比较或算法回退。
|
||||
- 只输出几何空间路径；没有时间、速度、加速度、转向速率和动态障碍物约束。
|
||||
- Local G2 候选必须通过严格的全路径复核；在当前夹具上，`Unchanged` 和 `Failed` 仍是有价值的诊断结果，而不是自动放宽安全约束的信号。
|
||||
+8
-8
@@ -4,7 +4,7 @@
|
||||
{
|
||||
"id": "straight",
|
||||
"fixtureVersion": 1,
|
||||
"configurationFingerprint": "sha256:d8c625157c21789a8bd33052a7f5927e4b1f697a2567cf3644b37ce00e164c51",
|
||||
"configurationFingerprint": "sha256:d42ce997e1e51d421fe923c5819a332706007d900cc74a39d4b0ac88c6ac4a21",
|
||||
"map": {
|
||||
"xMinMm": 0.0,
|
||||
"xMaxMm": 6000.0,
|
||||
@@ -1914,7 +1914,7 @@
|
||||
{
|
||||
"id": "single-turn",
|
||||
"fixtureVersion": 1,
|
||||
"configurationFingerprint": "sha256:a0218dbe66746b347139161d545656d6a55516f8e87769b4f9f17c5cf8d5d84f",
|
||||
"configurationFingerprint": "sha256:49571e2eeb2e58f0a5dc0a56e61e530700c3da7d60e2a9c66809721963e5d9f8",
|
||||
"map": {
|
||||
"xMinMm": -7000.0,
|
||||
"xMaxMm": 11000.0,
|
||||
@@ -3020,7 +3020,7 @@
|
||||
{
|
||||
"id": "s-bend",
|
||||
"fixtureVersion": 1,
|
||||
"configurationFingerprint": "sha256:aa1ae45d914e3ddc181bef415d71659fb357f9e2288e3eecec52376e6ca663bc",
|
||||
"configurationFingerprint": "sha256:810b601e9090644292e4eb5652379f5f0db6fa7158aaf6e4faeae4b337ad5ec5",
|
||||
"map": {
|
||||
"xMinMm": 0.0,
|
||||
"xMaxMm": 6000.0,
|
||||
@@ -6715,7 +6715,7 @@
|
||||
{
|
||||
"id": "large-heading-change",
|
||||
"fixtureVersion": 1,
|
||||
"configurationFingerprint": "sha256:5a5b66530a8782e81b456102602112d3e9f41f031d976e7daad035f9fa70311b",
|
||||
"configurationFingerprint": "sha256:44b6c95a3b553bb04df3912d5cf7c9110f4d629e37551b2c130d8d7f22706fce",
|
||||
"map": {
|
||||
"xMinMm": -7000.0,
|
||||
"xMaxMm": 11000.0,
|
||||
@@ -8253,7 +8253,7 @@
|
||||
{
|
||||
"id": "rectangle-detour",
|
||||
"fixtureVersion": 1,
|
||||
"configurationFingerprint": "sha256:de5a9261b3b073e4324526ecaf0dfc9251aa4b02f85e3c07abc0c63cfe72c85c",
|
||||
"configurationFingerprint": "sha256:6ad46b2782bc519b17d62dac9e238542a88f4265fb7dd3a1ebe84c04313cf6ce",
|
||||
"map": {
|
||||
"xMinMm": 0.0,
|
||||
"xMaxMm": 6000.0,
|
||||
@@ -12158,7 +12158,7 @@
|
||||
{
|
||||
"id": "multi-obstacle-detour",
|
||||
"fixtureVersion": 1,
|
||||
"configurationFingerprint": "sha256:278442046751c251baea0961aeb6d1bb4976498b2abb321754e5a5e941b5d2ce",
|
||||
"configurationFingerprint": "sha256:d062cfcf4fec6c0119575bb0c139697d8e4d5164e958cf511c0200d0f8d9fb5d",
|
||||
"map": {
|
||||
"xMinMm": 0.0,
|
||||
"xMaxMm": 6000.0,
|
||||
@@ -15589,7 +15589,7 @@
|
||||
{
|
||||
"id": "narrow-corridor",
|
||||
"fixtureVersion": 1,
|
||||
"configurationFingerprint": "sha256:4ddf1423d67649faa9f52ae5ca8455c594e79ddf48374a4759a214cb5075c1b6",
|
||||
"configurationFingerprint": "sha256:5ec976b32eec84abdd781b8cedeb21d07dfd6809ea61c108bcce8cff0b9d3a37",
|
||||
"map": {
|
||||
"xMinMm": 0.0,
|
||||
"xMaxMm": 6000.0,
|
||||
@@ -17520,7 +17520,7 @@
|
||||
{
|
||||
"id": "forward-reverse-switch",
|
||||
"fixtureVersion": 1,
|
||||
"configurationFingerprint": "sha256:68563bf572a2fb3d9db25e7639bf488e36f83c990dbf5578373e48f3ed6270ed",
|
||||
"configurationFingerprint": "sha256:86e1436370027ad30a7a205a78da291f81345e6acff2c0bb4f2256803ad9d962",
|
||||
"map": {
|
||||
"xMinMm": 0.0,
|
||||
"xMaxMm": 6000.0,
|
||||
|
||||
+171
@@ -0,0 +1,171 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Security.Cryptography;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
public sealed class LocalG2DiagnosticVisualizationDemo
|
||||
{
|
||||
private readonly LocalG2DiagnosticEvidenceLoader _evidenceLoader = new LocalG2DiagnosticEvidenceLoader();
|
||||
private readonly PathSmoothingComparisonService _comparisonService = new PathSmoothingComparisonService();
|
||||
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
|
||||
private readonly LocalG2PathSplicer _splicer = new LocalG2PathSplicer();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothingFigureModelBuilder _figureBuilder = new SmoothingFigureModelBuilder();
|
||||
private readonly SmoothingReportExporter _exporter = new SmoothingReportExporter();
|
||||
|
||||
public SmoothingReportExportResult Export(
|
||||
string fixturePath,
|
||||
string evidencePath,
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
LocalG2DiagnosticEvidence evidence = _evidenceLoader.LoadAndVerify(evidencePath);
|
||||
RequireFixtureHash(fixturePath, evidence.FixtureSha256);
|
||||
PathSmoothingComparisonRequest request = FindFixtureRequest(fixturePath, evidence.ScenarioId);
|
||||
PathSmoothingComparisonResult comparison = _comparisonService.Compare(request, cancellationToken);
|
||||
PathSmoothingComparisonEntry localG2 = FindEntry(comparison, SmoothingMethod.LocalG2Quintic);
|
||||
Require(localG2 != null && localG2.Status == PathSmoothingStatus.Unchanged,
|
||||
"Strict Local G2 result must be Unchanged for the diagnostic evidence.");
|
||||
RequireSameGeometry(comparison.RawPathBaseline.Path, localG2.Path);
|
||||
Require(_preprocessor.TryPrepare(request.SmoothingRequest, out PreparedPath prepared, out string reason), reason);
|
||||
LocalG2CandidateGeometry candidate = CreateCandidate(evidence);
|
||||
Require(_splicer.TryReplace(prepared, candidate, out PreparedPath spliced, out reason), reason);
|
||||
Require(_analyzer.TryAnalyze(spliced.Segments, request.SmoothingRequest.Configuration.OutputSpacingMeters,
|
||||
out PathGeometryAnalysis analysis, out reason), reason);
|
||||
CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
|
||||
CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
|
||||
SmoothingFigureModel normal = _figureBuilder.Build(
|
||||
comparison,
|
||||
request.SmoothingRequest.Map,
|
||||
new Pose2D(first.X, first.Y, first.Heading),
|
||||
new Pose2D(last.X, last.Y, last.Heading),
|
||||
evidence.ScenarioId,
|
||||
evidence.ScenarioId);
|
||||
SmoothingFigureModel augmented = normal.WithAdditionalSeries(CreateDiagnosticSeries(analysis.Path));
|
||||
return _exporter.Export(new SmoothingReportExportRequest
|
||||
{
|
||||
Model = augmented,
|
||||
OutputDirectory = outputDirectory,
|
||||
FileStem = "comparison",
|
||||
});
|
||||
}
|
||||
|
||||
private static void RequireFixtureHash(string fixturePath, string expectedFixtureSha256)
|
||||
{
|
||||
byte[] fixtureBytes = File.ReadAllBytes(fixturePath);
|
||||
byte[] hash;
|
||||
using (SHA256 sha256 = SHA256.Create())
|
||||
hash = sha256.ComputeHash(fixtureBytes);
|
||||
string actualFixtureSha256 = BitConverter.ToString(hash).Replace("-", string.Empty).ToLowerInvariant();
|
||||
Require(string.Equals(actualFixtureSha256, expectedFixtureSha256, StringComparison.Ordinal),
|
||||
"Diagnostic evidence fixture SHA-256 does not match the supplied fixture.");
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonRequest FindFixtureRequest(string fixturePath, string scenarioId)
|
||||
{
|
||||
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
|
||||
IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
|
||||
Require(fixtures.Count == requests.Count, "Fixture requests do not match fixture records.");
|
||||
|
||||
int fixtureIndex = -1;
|
||||
for (int index = 0; index < fixtures.Count; index++)
|
||||
{
|
||||
if (!string.Equals(fixtures[index].Id, scenarioId, StringComparison.Ordinal)) continue;
|
||||
Require(fixtureIndex < 0, "Diagnostic fixture ID must be unique: " + scenarioId);
|
||||
fixtureIndex = index;
|
||||
}
|
||||
Require(fixtureIndex >= 0, "Diagnostic fixture ID was not found: " + scenarioId);
|
||||
return requests[fixtureIndex];
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonEntry FindEntry(PathSmoothingComparisonResult comparison, SmoothingMethod method)
|
||||
{
|
||||
if (comparison == null) return null;
|
||||
for (int index = 0; index < comparison.Entries.Count; index++)
|
||||
{
|
||||
if (comparison.Entries[index].Method == method) return comparison.Entries[index];
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private static LocalG2CandidateGeometry CreateCandidate(LocalG2DiagnosticEvidence evidence)
|
||||
{
|
||||
Require(evidence != null && evidence.CandidatePoints != null, "Diagnostic evidence is required.");
|
||||
var points = new List<SmoothingPoint2D>(evidence.CandidatePoints.Count);
|
||||
for (int index = 0; index < evidence.CandidatePoints.Count; index++)
|
||||
{
|
||||
LocalG2DiagnosticEvidencePoint point = evidence.CandidatePoints[index];
|
||||
points.Add(new SmoothingPoint2D(
|
||||
point.X.Value, point.Y.Value, point.ReferenceArcLengthMeters.Value,
|
||||
point.HeadingRadians.Value, point.UnwrappedHeadingRadians.Value,
|
||||
0d, false, SmoothedPathPointSource.LocalG2Transition));
|
||||
}
|
||||
return new LocalG2CandidateGeometry(
|
||||
evidence.CandidateIndex.Value,
|
||||
evidence.SegmentIndex.Value,
|
||||
evidence.WindowStartArcLengthMeters.Value,
|
||||
evidence.WindowEndArcLengthMeters.Value,
|
||||
0d,
|
||||
0d,
|
||||
points,
|
||||
evidence.StartVehicleCurvaturePerMeter.Value,
|
||||
evidence.EndVehicleCurvaturePerMeter.Value,
|
||||
evidence.StartGeometricCurvaturePerMeter.Value,
|
||||
evidence.EndGeometricCurvaturePerMeter.Value,
|
||||
true);
|
||||
}
|
||||
|
||||
private static SmoothingFigureSeries CreateDiagnosticSeries(IReadOnlyList<SmoothedPathPoint> path)
|
||||
{
|
||||
var points = new List<SmoothingFigurePoint>(path == null ? 0 : path.Count);
|
||||
if (path != null)
|
||||
{
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint point = path[index];
|
||||
points.Add(new SmoothingFigurePoint(point.X, point.Y, point.ArcLength, point.VehicleCurvature));
|
||||
}
|
||||
}
|
||||
return new SmoothingFigureSeries(
|
||||
null,
|
||||
"local-g2-diagnostic",
|
||||
"G2 诊断候选（净空拒绝，未发布）",
|
||||
PathSmoothingStatus.Infeasible,
|
||||
IeeeFigureStyle.LocalG2DiagnosticColor,
|
||||
string.Empty,
|
||||
false,
|
||||
points,
|
||||
Array.Empty<SmoothingFigurePoint>());
|
||||
}
|
||||
|
||||
private static void RequireSameGeometry(IReadOnlyList<SmoothedPathPoint> expected, IReadOnlyList<SmoothedPathPoint> actual)
|
||||
{
|
||||
Require(expected != null && actual != null && expected.Count == actual.Count,
|
||||
"Strict Local G2 output must retain raw-path geometry.");
|
||||
for (int index = 0; index < expected.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint left = expected[index];
|
||||
SmoothedPathPoint right = actual[index];
|
||||
Require(left.X == right.X && left.Y == right.Y && left.Heading == right.Heading &&
|
||||
left.UnwrappedHeading == right.UnwrappedHeading && left.ArcLength == right.ArcLength &&
|
||||
left.Direction == right.Direction && left.GeometricCurvature == right.GeometricCurvature &&
|
||||
left.VehicleCurvature == right.VehicleCurvature && left.VehicleCurvatureDerivative == right.VehicleCurvatureDerivative &&
|
||||
left.BodyClearance == right.BodyClearance && left.IsGearSwitchPoint == right.IsGearSwitchPoint && left.Source == right.Source,
|
||||
"Strict Local G2 output must retain raw-path geometry.");
|
||||
}
|
||||
}
|
||||
|
||||
private static void Require(bool condition, string reason)
|
||||
{
|
||||
if (!condition) throw new InvalidOperationException(reason);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>开发人员离线导出八个快速夹具和四个 Hybrid A* 端到端场景的比较报告。</summary>
|
||||
public sealed class PathSmoothingComparisonDemo
|
||||
{
|
||||
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
|
||||
private readonly PathSmoothingComparisonService _comparisonService = new PathSmoothingComparisonService();
|
||||
private readonly SmoothingFigureModelBuilder _figureBuilder = new SmoothingFigureModelBuilder();
|
||||
private readonly SmoothingReportExporter _reportExporter = new SmoothingReportExporter();
|
||||
|
||||
/// <summary>导出八个已验证夹具；不运行 Hybrid A*。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportFixtureReports(
|
||||
string fixturePath,
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
|
||||
IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
|
||||
var results = new List<PathSmoothingComparisonScenarioResult>(requests.Count);
|
||||
for (int index = 0; index < requests.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
results.Add(Export(fixtures[index].Id, requests[index], outputDirectory, cancellationToken));
|
||||
}
|
||||
return results.AsReadOnly();
|
||||
}
|
||||
|
||||
/// <summary>运行四个固定 Hybrid A* 场景，并仅为成功粗路径导出平滑比较报告。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportEndToEndReports(
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
CoarsePathTestScenario[] scenarios =
|
||||
{
|
||||
CoarsePathTestScenario.ExplicitEmpty,
|
||||
CoarsePathTestScenario.RectangleDetour,
|
||||
CoarsePathTestScenario.ManualAndTwoLeg,
|
||||
CoarsePathTestScenario.ReverseGearSwitch,
|
||||
};
|
||||
var planner = new CoarsePathPlanningService();
|
||||
var results = new List<PathSmoothingComparisonScenarioResult>(scenarios.Length);
|
||||
for (int index = 0; index < scenarios.Length; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenarios[index]);
|
||||
CoarsePathPlanningJobResult coarse = planner.Plan(job, cancellationToken);
|
||||
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
||||
{
|
||||
results.Add(PathSmoothingComparisonScenarioResult.CoarsePathFailure(
|
||||
scenarios[index].ToString(), coarse.PlanningResult.Status, coarse.PlanningResult.Diagnostics.TerminationReason));
|
||||
continue;
|
||||
}
|
||||
results.Add(Export(
|
||||
scenarios[index].ToString(),
|
||||
SmoothingScenarioFactory.CreateEndToEndRequest(job, coarse),
|
||||
outputDirectory,
|
||||
cancellationToken));
|
||||
}
|
||||
return results.AsReadOnly();
|
||||
}
|
||||
|
||||
/// <summary>供业务调用示例使用：仅在已成功的粗路径上调用平滑服务。</summary>
|
||||
public PathSmoothingResult SmoothSuccessfulCoarsePath(
|
||||
CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult coarseResult,
|
||||
PathSmoothingConfiguration configuration,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (job == null || coarseResult == null || coarseResult.PlanningResult.Status != PlanningStatus.Success)
|
||||
throw new ArgumentException("只有成功的粗路径能够进入平滑流程。", nameof(coarseResult));
|
||||
return _smoothingService.Smooth(new PathSmoothingRequest(
|
||||
coarseResult.PlanningResult.Path,
|
||||
coarseResult.PlanningResult.Segments,
|
||||
coarseResult.MapResult.Map,
|
||||
job.Vehicle,
|
||||
configuration), cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>业务调用方只可消费成功平滑或显式回退的路径。</summary>
|
||||
public static bool IsConsumable(PathSmoothingStatus status)
|
||||
{
|
||||
return status == PathSmoothingStatus.Complete ||
|
||||
status == PathSmoothingStatus.PartialImprovement ||
|
||||
status == PathSmoothingStatus.NotNeeded ||
|
||||
status == PathSmoothingStatus.Unchanged;
|
||||
}
|
||||
|
||||
private PathSmoothingComparisonScenarioResult Export(
|
||||
string scenarioId,
|
||||
PathSmoothingComparisonRequest request,
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (request == null || request.SmoothingRequest == null || request.SmoothingRequest.CoarsePath.Count == 0)
|
||||
throw new ArgumentException("比较请求必须包含粗路径。", nameof(request));
|
||||
PathSmoothingComparisonResult comparison = _comparisonService.Compare(request, cancellationToken);
|
||||
CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
|
||||
CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
|
||||
SmoothingFigureModel model = _figureBuilder.Build(
|
||||
comparison,
|
||||
request.SmoothingRequest.Map,
|
||||
new Pose2D(first.X, first.Y, first.Heading),
|
||||
new Pose2D(last.X, last.Y, last.Heading),
|
||||
scenarioId,
|
||||
scenarioId);
|
||||
SmoothingReportExportResult report = _reportExporter.Export(new SmoothingReportExportRequest
|
||||
{
|
||||
Model = model,
|
||||
OutputDirectory = System.IO.Path.Combine(outputDirectory, scenarioId),
|
||||
FileStem = "comparison",
|
||||
});
|
||||
return PathSmoothingComparisonScenarioResult.ComparisonComplete(scenarioId, comparison, report);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>批处理为一个场景发布的粗路径状态、平滑比较和报告结果。</summary>
|
||||
public sealed class PathSmoothingComparisonScenarioResult
|
||||
{
|
||||
private PathSmoothingComparisonScenarioResult(
|
||||
string scenarioId,
|
||||
PlanningStatus coarsePathStatus,
|
||||
string diagnostic,
|
||||
PathSmoothingComparisonResult comparison,
|
||||
SmoothingReportExportResult report)
|
||||
{
|
||||
ScenarioId = scenarioId ?? string.Empty;
|
||||
CoarsePathStatus = coarsePathStatus;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
Comparison = comparison;
|
||||
Report = report;
|
||||
}
|
||||
|
||||
public string ScenarioId { get; }
|
||||
public PlanningStatus CoarsePathStatus { get; }
|
||||
public string Diagnostic { get; }
|
||||
public PathSmoothingComparisonResult Comparison { get; }
|
||||
public SmoothingReportExportResult Report { get; }
|
||||
|
||||
internal static PathSmoothingComparisonScenarioResult CoarsePathFailure(
|
||||
string scenarioId,
|
||||
PlanningStatus status,
|
||||
string diagnostic)
|
||||
{
|
||||
return new PathSmoothingComparisonScenarioResult(scenarioId, status, diagnostic, null, null);
|
||||
}
|
||||
|
||||
internal static PathSmoothingComparisonScenarioResult ComparisonComplete(
|
||||
string scenarioId,
|
||||
PathSmoothingComparisonResult comparison,
|
||||
SmoothingReportExportResult report)
|
||||
{
|
||||
return new PathSmoothingComparisonScenarioResult(scenarioId, PlanningStatus.Success, string.Empty, comparison, report);
|
||||
}
|
||||
}
|
||||
@@ -3,7 +3,7 @@ using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
|
||||
+16
-2
@@ -239,6 +239,20 @@ public static class SmoothingScenarioFixtureLoader
|
||||
|
||||
private static void Append(StringBuilder builder, string value) { builder.Append(value ?? string.Empty).Append('|'); }
|
||||
private static void Append(StringBuilder builder, int value) { builder.Append(value.ToString(CultureInfo.InvariantCulture)).Append('|'); }
|
||||
private static void Append(StringBuilder builder, float value) { builder.Append(value.ToString("R", CultureInfo.InvariantCulture)).Append('|'); }
|
||||
private static void Append(StringBuilder builder, double value) { builder.Append(value.ToString("R", CultureInfo.InvariantCulture)).Append('|'); }
|
||||
private static void Append(StringBuilder builder, float value)
|
||||
{
|
||||
AppendBits(builder, BitConverter.GetBytes(value));
|
||||
}
|
||||
|
||||
private static void Append(StringBuilder builder, double value)
|
||||
{
|
||||
AppendBits(builder, BitConverter.GetBytes(value));
|
||||
}
|
||||
|
||||
private static void AppendBits(StringBuilder builder, byte[] bytes)
|
||||
{
|
||||
for (int index = bytes.Length - 1; index >= 0; index--)
|
||||
builder.Append(bytes[index].ToString("x2", CultureInfo.InvariantCulture));
|
||||
builder.Append('|');
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
internal static class CurvatureLimitPolicy
|
||||
{
|
||||
internal static bool TryGetAllowedMaximumVehicleCurvaturePerMeter(
|
||||
VehicleParameters vehicle,
|
||||
double radiusToleranceMeters,
|
||||
out double allowedMaximumCurvaturePerMeter)
|
||||
{
|
||||
allowedMaximumCurvaturePerMeter = 0d;
|
||||
if (!NumericGuard.IsFinite(radiusToleranceMeters) || radiusToleranceMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double nominalMaximumCurvature))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double nominalMinimumRadius = 1d / nominalMaximumCurvature;
|
||||
double toleratedMinimumRadius = nominalMinimumRadius - radiusToleranceMeters;
|
||||
if (!NumericGuard.IsPositiveFinite(toleratedMinimumRadius)) return false;
|
||||
|
||||
allowedMaximumCurvaturePerMeter = 1d / toleratedMinimumRadius;
|
||||
return NumericGuard.IsPositiveFinite(allowedMaximumCurvaturePerMeter);
|
||||
}
|
||||
}
|
||||
@@ -22,15 +22,23 @@ public sealed class SmoothedPathValidator
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车体碰撞检查器的平滑路径验证器。</summary>
|
||||
/// <param name="collisionChecker">以车辆几何中心位姿执行完整车体和扫掠复核的检查器，不能为 <see langword="null"/>。</param>
|
||||
public SmoothedPathValidator(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复核候选平滑路径。每个候选方向段必须保持原始段的端点和换向拓扑；
|
||||
/// 输出中的净空均由本次实际车体检查重新计算，绝不沿用候选声明值。
|
||||
/// </summary>
|
||||
/// <summary>复核候选平滑路径，并以默认零转弯半径容差重新计算所有净空。</summary>
|
||||
/// <param name="candidatePath">待发布候选点序列；位置/弧长单位为 m、航向为 rad、曲率为 1/m。</param>
|
||||
/// <param name="candidateSegments">覆盖候选点的前进/倒车方向段集合。</param>
|
||||
/// <param name="originalPath">预处理原始路径，候选必须保留其端点与换向拓扑。</param>
|
||||
/// <param name="map">已准备的规划地图快照。</param>
|
||||
/// <param name="vehicle">车辆尺寸、安全余量和曲率约束。</param>
|
||||
/// <param name="maximumCollisionCheckStepMeters">相邻点车体扫掠最大中心步长，单位 m。</param>
|
||||
/// <param name="pathWithClearance">成功时为净空由真实复核重算的只读路径；失败时为空集合。</param>
|
||||
/// <param name="minimumClearanceMeters">成功时为完整扩大车体的最小保守净空，单位 m；失败时为 0。</param>
|
||||
/// <param name="reason">失败原因；成功时为空字符串。</param>
|
||||
/// <returns>端点、换向、曲率、点碰撞和扫掠均通过时为 <see langword="true"/>；否则为 <see langword="false"/>，不发布部分路径。</returns>
|
||||
public bool TryValidate(
|
||||
IReadOnlyList<SmoothedPathPoint> candidatePath,
|
||||
IReadOnlyList<SmoothedPathSegment> candidateSegments,
|
||||
@@ -41,6 +49,35 @@ public sealed class SmoothedPathValidator
|
||||
out IReadOnlyList<SmoothedPathPoint> pathWithClearance,
|
||||
out double minimumClearanceMeters,
|
||||
out string reason)
|
||||
{
|
||||
return TryValidate(
|
||||
candidatePath, candidateSegments, originalPath, map, vehicle, maximumCollisionCheckStepMeters, 0d,
|
||||
out pathWithClearance, out minimumClearanceMeters, out reason);
|
||||
}
|
||||
|
||||
/// <summary>以只用于曲率上限的显式转弯半径容差复核候选路径。</summary>
|
||||
/// <param name="candidatePath">待复核候选点序列。</param>
|
||||
/// <param name="candidateSegments">候选方向段集合。</param>
|
||||
/// <param name="originalPath">必须保持端点和换向拓扑的预处理原始路径。</param>
|
||||
/// <param name="map">规划地图快照。</param>
|
||||
/// <param name="vehicle">车辆几何和曲率约束。</param>
|
||||
/// <param name="maximumCollisionCheckStepMeters">扫掠复核最大中心步长，单位 m。</param>
|
||||
/// <param name="curvatureLimitRadiusToleranceMeters">仅曲率限制使用的最小转弯半径容差，单位 m。</param>
|
||||
/// <param name="pathWithClearance">成功时为重新计算净空后的完整只读路径；失败时为空集合。</param>
|
||||
/// <param name="minimumClearanceMeters">成功时最小保守净空，单位 m；失败时为 0。</param>
|
||||
/// <param name="reason">失败诊断；成功时为空字符串。</param>
|
||||
/// <returns>候选满足拓扑、严格弧长、曲率、点碰撞和扫掠约束时为 <see langword="true"/>；否则为 <see langword="false"/>。</returns>
|
||||
public bool TryValidate(
|
||||
IReadOnlyList<SmoothedPathPoint> candidatePath,
|
||||
IReadOnlyList<SmoothedPathSegment> candidateSegments,
|
||||
PreparedPath originalPath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
double maximumCollisionCheckStepMeters,
|
||||
double curvatureLimitRadiusToleranceMeters,
|
||||
out IReadOnlyList<SmoothedPathPoint> pathWithClearance,
|
||||
out double minimumClearanceMeters,
|
||||
out string reason)
|
||||
{
|
||||
pathWithClearance = EmptyPath();
|
||||
minimumClearanceMeters = 0d;
|
||||
@@ -52,7 +89,8 @@ public sealed class SmoothedPathValidator
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvatureMeters))
|
||||
if (!CurvatureLimitPolicy.TryGetAllowedMaximumVehicleCurvaturePerMeter(
|
||||
vehicle, curvatureLimitRadiusToleranceMeters, out double maximumCurvatureMeters))
|
||||
{
|
||||
reason = "车辆曲率约束无效。";
|
||||
return false;
|
||||
|
||||
Reference in New Issue
Block a user