fix: harden path smoothing contracts

This commit is contained in:
梁薄云
2026-07-28 17:06:58 +08:00
parent ebf7d7d3d4
commit 5a471705b5
3 changed files with 106 additions and 13 deletions
@@ -19,8 +19,8 @@ public sealed class PathSmoothingRequest
CoarsePath = CopyReadOnly(coarsePath);
Segments = CopyReadOnly(segments);
Map = map;
Vehicle = vehicle;
Configuration = configuration;
Vehicle = CopyVehicle(vehicle);
Configuration = CopyConfiguration(configuration);
}
/// <summary>原始粗路径的不可变快照。</summary>
@@ -48,4 +48,38 @@ public sealed class PathSmoothingRequest
}
return new ReadOnlyCollection<T>(copy);
}
private static VehicleParameters CopyVehicle(VehicleParameters source)
{
if (source == null) return null;
return new VehicleParameters
{
LengthMeters = source.LengthMeters,
WidthMeters = source.WidthMeters,
SafetyMarginMeters = source.SafetyMarginMeters,
MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
};
}
private static PathSmoothingConfiguration CopyConfiguration(PathSmoothingConfiguration source)
{
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,
};
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;
return copy;
}
}
@@ -53,7 +53,7 @@ public sealed class PathSmoothingResult
IReadOnlyList<SmoothedPathSegment> segments,
PathSmoothingDiagnostics diagnostics)
{
ValidatePublishedPath(path, segments);
ValidatePublishedResult(method, path, segments, diagnostics);
return new PathSmoothingResult(
PathSmoothingStatus.Success,
method,
@@ -69,7 +69,7 @@ public sealed class PathSmoothingResult
IReadOnlyList<SmoothedPathSegment> segments,
PathSmoothingDiagnostics diagnostics)
{
ValidatePublishedPath(path, segments);
ValidatePublishedResult(attemptedMethod, path, segments, diagnostics);
return new PathSmoothingResult(
PathSmoothingStatus.FallbackToCoarsePath,
attemptedMethod,
@@ -88,14 +88,20 @@ public sealed class PathSmoothingResult
return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics);
}
private static void ValidatePublishedPath(
private static void ValidatePublishedResult(
SmoothingMethod method,
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments)
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)
throw new ArgumentException("Published smoothing results require non-empty segments.", nameof(segments));
if (diagnostics == null || diagnostics.Metrics == null || !diagnostics.Metrics.IsFeasible)
throw new ArgumentException("Published smoothing results require feasible diagnostics.", nameof(diagnostics));
}
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)