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

fix: validate coarse path before smoothing

Browse Source
This commit is contained in:
梁薄云
2026-07-29 14:50:26 +08:00
parent d1df995b51
commit a693cbf323
2 changed files with 87 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs
+53 -8
View File
@@ -37,6 +37,21 @@ public sealed class PathSmoothingService
if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
if (!TryAnalyzeAndValidate(
request,
preparedPath,
preparedPath.Segments,
configuration,
false,
cancellationToken,
out _,
out _,
out _,
out reason))
{
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
}
cancellationToken.ThrowIfCancellationRequested();
var input = new SmoothingAlgorithmInput(
preparedPath,
@@ -121,8 +136,37 @@ 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(
ToFallbackSegments(revalidatedPath),
candidateSegments,
configuration.OutputSpacingMeters,
out PathGeometryAnalysis analysis,
out reason))
@@ -130,25 +174,26 @@ public sealed class PathSmoothingService
return false;
}
IReadOnlyList<SmoothedPathPoint> fallbackSourcePath = ToFallbackPoints(analysis.Path);
IReadOnlyList<SmoothedPathPoint> pathForValidation = useFallbackSource
? ToFallbackPoints(analysis.Path)
: analysis.Path;
cancellationToken.ThrowIfCancellationRequested();
if (!_validator.TryValidate(
fallbackSourcePath,
pathForValidation,
analysis.Segments,
revalidatedPath,
originalPath,
request.Map,
request.Vehicle,
configuration.MaximumCollisionCheckStepMeters,
out IReadOnlyList<SmoothedPathPoint> safePath,
out safePath,
out double minimumClearanceMeters,
out reason))
{
return false;
}
fallbackPath = safePath;
fallbackSegments = analysis.Segments;
fallbackMetrics = CreateMetrics(analysis, minimumClearanceMeters);
safeSegments = analysis.Segments;
metrics = CreateMetrics(analysis, minimumClearanceMeters);
return true;
}