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

fix: preserve raw path smoothing baselines

Browse Source
This commit is contained in:
梁薄云
2026-07-30 08:42:14 +08:00
parent 09953ab1f1
commit 20215defdd
5 changed files with 197 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs
+8 -33
View File
@@ -13,7 +13,6 @@ public sealed class PathSmoothingComparisonService
{
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
/// <summary>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
@@ -137,31 +136,23 @@ public sealed class PathSmoothingComparisonService
if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason))
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
if (!_analyzer.TryAnalyze(preparedPath.Segments, configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
if (!_validator.TryValidate(
analysis.Path,
analysis.Segments,
if (!RawPathBaselineBuilder.TryCreate(
smoothingRequest,
preparedPath,
smoothingRequest.Map,
smoothingRequest.Vehicle,
_validator,
configuration.MaximumCollisionCheckStepMeters,
out IReadOnlyList<SmoothedPathPoint> safePath,
out double minimumClearanceMeters,
out RawPathBaseline rawPath,
out reason))
{
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
}
PathQualityMetrics metrics = CreateMetrics(analysis, minimumClearanceMeters);
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, safePath, analysis.Segments);
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, rawPath.Path, rawPath.Segments);
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
PathSmoothingStatus.Success,
metrics,
rawPath.Metrics,
digest,
string.Empty,
safePath,
analysis.Segments);
rawPath.Path,
rawPath.Segments);
}
private static PathSmoothingComparisonEntry FailedBaseline(
@@ -199,22 +190,6 @@ public sealed class PathSmoothingComparisonService
configuration);
}
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
{
return new PathQualityMetrics(
true,
analysis.PathLengthMeters,
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
analysis.RootMeanSquareVehicleCurvaturePerMeter,
analysis.TotalAbsoluteCurvatureVariationPerMeter,
analysis.CurvatureVariationEnergy,
minimumClearanceMeters,
0d,
0d,
0d,
0d);
}
private static PathQualityMetrics NormalizeMetrics(
PathQualityMetrics candidate,
PathQualityMetrics raw)
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs
+11 -106
View File
@@ -16,7 +16,6 @@ public sealed class PathSmoothingService
{
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother();
@@ -37,15 +36,11 @@ public sealed class PathSmoothingService
if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
if (!TryAnalyzeAndValidate(
if (!RawPathBaselineBuilder.TryCreate(
request,
preparedPath,
preparedPath.Segments,
configuration,
false,
cancellationToken,
out _,
out _,
_validator,
configuration.MaximumCollisionCheckStepMeters,
out _,
out reason))
{
@@ -136,64 +131,20 @@ public sealed class PathSmoothingService
if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false;
cancellationToken.ThrowIfCancellationRequested();
return TryAnalyzeAndValidate(
request,
revalidatedPath,
ToFallbackSegments(revalidatedPath),
configuration,
true,
cancellationToken,
out fallbackPath,
out fallbackSegments,
out fallbackMetrics,
out reason);
}
private bool TryAnalyzeAndValidate(
PathSmoothingRequest request,
PreparedPath originalPath,
IReadOnlyList<PreparedDirectionSegment> candidateSegments,
PathSmoothingConfiguration configuration,
bool useFallbackSource,
CancellationToken cancellationToken,
out IReadOnlyList<SmoothedPathPoint> safePath,
out IReadOnlyList<SmoothedPathSegment> safeSegments,
out PathQualityMetrics metrics,
out string reason)
{
safePath = null;
safeSegments = null;
metrics = null;
reason = string.Empty;
if (!_analyzer.TryAnalyze(
candidateSegments,
configuration.OutputSpacingMeters,
out PathGeometryAnalysis analysis,
out reason))
{
return false;
}
IReadOnlyList<SmoothedPathPoint> pathForValidation = useFallbackSource
? ToFallbackPoints(analysis.Path)
: analysis.Path;
cancellationToken.ThrowIfCancellationRequested();
if (!_validator.TryValidate(
pathForValidation,
analysis.Segments,
originalPath,
request.Map,
request.Vehicle,
if (!RawPathBaselineBuilder.TryCreate(
request,
revalidatedPath,
_validator,
configuration.MaximumCollisionCheckStepMeters,
out safePath,
out double minimumClearanceMeters,
out RawPathBaseline rawPath,
out reason))
{
return false;
}
safeSegments = analysis.Segments;
metrics = CreateMetrics(analysis, minimumClearanceMeters);
fallbackPath = ToFallbackPoints(rawPath.Path);
fallbackSegments = rawPath.Segments;
fallbackMetrics = rawPath.Metrics;
return true;
}
@@ -268,36 +219,6 @@ public sealed class PathSmoothingService
return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI;
}
private static IReadOnlyList<PreparedDirectionSegment> ToFallbackSegments(PreparedPath path)
{
var segments = new List<PreparedDirectionSegment>(path.Segments.Count);
for (int segmentIndex = 0; segmentIndex < path.Segments.Count; segmentIndex++)
{
PreparedDirectionSegment segment = path.Segments[segmentIndex];
var points = new List<SmoothingPoint2D>(segment.Points.Count);
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
{
SmoothingPoint2D point = segment.Points[pointIndex];
points.Add(new SmoothingPoint2D(
point.X,
point.Y,
point.ArcLength,
point.Heading,
point.UnwrappedHeading,
point.BodyClearance,
point.IsGearSwitchPoint,
SmoothedPathPointSource.CoarsePathFallback));
}
segments.Add(new PreparedDirectionSegment(
segment.SegmentIndex,
segment.Direction,
points,
segment.StartsAtGearSwitch,
segment.EndsAtGearSwitch));
}
return segments;
}
private static IReadOnlyList<SmoothedPathPoint> ToFallbackPoints(IReadOnlyList<SmoothedPathPoint> path)
{
var points = new List<SmoothedPathPoint>(path.Count);
@@ -320,22 +241,6 @@ public sealed class PathSmoothingService
return points;
}
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
{
return new PathQualityMetrics(
true,
analysis.PathLengthMeters,
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
analysis.RootMeanSquareVehicleCurvaturePerMeter,
analysis.TotalAbsoluteCurvatureVariationPerMeter,
analysis.CurvatureVariationEnergy,
minimumClearanceMeters,
0d,
0d,
0d,
0d);
}
private static int GetRetryCount(IReadOnlyList<double> attemptedStrengths)
{
return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1;
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs
+3 -10
View File
@@ -144,21 +144,14 @@ public sealed class PathGeometryAnalyzer
for (int index = 0; index < count; index++)
{
double travelHeading;
if (count == 1)
if (index == 0 || index == count - 1)
{
// Coarse-path endpoints encode the vehicle pose at the exact integration anchor.
// A chord across a finite integration step is not that pose's heading.
travelHeading = segment.Direction == TravelDirection.Forward
? samples[index].Heading
: samples[index].Heading - Math.PI;
}
else if (index == 0)
{
travelHeading = Math.Atan2(samples[1].Y - samples[0].Y, samples[1].X - samples[0].X);
}
else if (index == count - 1)
{
travelHeading = Math.Atan2(samples[index].Y - samples[index - 1].Y,
samples[index].X - samples[index - 1].X);
}
else
{
travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y,
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs
+166
View File
@@ -0,0 +1,166 @@
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
/// <summary>将已验证的粗路径转换为不改变任何锚点位姿或车辆曲率的安全比较基线。</summary>
internal static class RawPathBaselineBuilder
{
private const double MinimumArcDeltaMeters = 1e-12d;
internal static bool TryCreate(
PathSmoothingRequest request,
PreparedPath preparedPath,
SmoothedPathValidator validator,
double maximumCollisionCheckStepMeters,
out RawPathBaseline baseline,
out string reason)
{
baseline = null;
reason = string.Empty;
if (request == null || preparedPath == null || validator == null)
{
reason = "原始粗路径基线缺少请求、预处理路径或安全验证器。";
return false;
}
IReadOnlyList<SmoothedPathPoint> candidatePath = CreatePoints(request.CoarsePath);
IReadOnlyList<SmoothedPathSegment> candidateSegments = CreateSegments(request.Segments);
if (!validator.TryValidate(
candidatePath,
candidateSegments,
preparedPath,
request.Map,
request.Vehicle,
maximumCollisionCheckStepMeters,
out IReadOnlyList<SmoothedPathPoint> safePath,
out double minimumClearanceMeters,
out reason))
{
return false;
}
baseline = new RawPathBaseline(safePath, candidateSegments, CreateMetrics(safePath, candidateSegments, minimumClearanceMeters));
return true;
}
private static IReadOnlyList<SmoothedPathPoint> CreatePoints(IReadOnlyList<CoarsePathPoint> coarsePath)
{
var points = new List<SmoothedPathPoint>(coarsePath == null ? 0 : coarsePath.Count);
if (coarsePath != null)
{
for (int index = 0; index < coarsePath.Count; index++)
{
CoarsePathPoint point = coarsePath[index];
points.Add(new SmoothedPathPoint(
point.X,
point.Y,
point.Heading,
point.UnwrappedHeading,
point.ArcLength,
point.Direction,
point.VehicleCurvature,
point.VehicleCurvature,
point.BodyClearance,
point.IsGearSwitchPoint,
point.IsGearSwitchPoint ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
}
}
return new ReadOnlyCollection<SmoothedPathPoint>(points);
}
private static IReadOnlyList<SmoothedPathSegment> CreateSegments(IReadOnlyList<PathSegment> coarseSegments)
{
var segments = new List<SmoothedPathSegment>(coarseSegments == null ? 0 : coarseSegments.Count);
if (coarseSegments != null)
{
for (int index = 0; index < coarseSegments.Count; index++)
{
PathSegment segment = coarseSegments[index];
segments.Add(new SmoothedPathSegment(
segment.SegmentIndex,
segment.Direction,
segment.StartIndex,
segment.EndIndex,
segment.StartsAtGearSwitch,
segment.EndsAtGearSwitch));
}
}
return new ReadOnlyCollection<SmoothedPathSegment>(segments);
}
private static PathQualityMetrics CreateMetrics(
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments,
double minimumClearanceMeters)
{
double maximumAbsoluteVehicleCurvature = 0d;
double curvatureSquareSum = 0d;
int curvatureSampleCount = 0;
double totalAbsoluteCurvatureVariation = 0d;
double curvatureVariationEnergy = 0d;
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
{
SmoothedPathSegment segment = segments[segmentIndex];
SmoothedPathPoint previous = null;
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
{
SmoothedPathPoint current = path[pointIndex];
double curvature = current.VehicleCurvature;
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(curvature));
curvatureSquareSum += curvature * curvature;
curvatureSampleCount++;
if (previous != null)
{
double arcDelta = current.ArcLength - previous.ArcLength;
if (arcDelta > MinimumArcDeltaMeters)
{
double curvatureDelta = curvature - previous.VehicleCurvature;
totalAbsoluteCurvatureVariation += Math.Abs(curvatureDelta);
curvatureVariationEnergy += curvatureDelta * curvatureDelta / arcDelta;
}
}
previous = current;
}
}
double rootMeanSquareVehicleCurvature = curvatureSampleCount == 0
? 0d
: Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
double pathLengthMeters = path.Count == 0 ? 0d : path[path.Count - 1].ArcLength;
return new PathQualityMetrics(
true,
pathLengthMeters,
maximumAbsoluteVehicleCurvature,
rootMeanSquareVehicleCurvature,
totalAbsoluteCurvatureVariation,
curvatureVariationEnergy,
minimumClearanceMeters,
0d,
0d,
0d,
0d);
}
}
/// <summary>已通过完整车体复核的原始粗路径及其比较指标。</summary>
internal sealed class RawPathBaseline
{
internal RawPathBaseline(
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments,
PathQualityMetrics metrics)
{
Path = path;
Segments = segments;
Metrics = metrics;
}
internal IReadOnlyList<SmoothedPathPoint> Path { get; }
internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
internal PathQualityMetrics Metrics { get; }
}