fix: validate coarse path before smoothing
This commit is contained in:
梁薄云
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -21,16 +21,31 @@ function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function New-EmptyMap {
|
||||
function New-Map([bool]$WithObstacle) {
|
||||
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
||||
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$mapRequest.ResolutionMm = [single]50
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
if ($WithObstacle) {
|
||||
$obstacle = [Activator]::CreateInstance($rectangleType, @([single]900, [single]1100, [single]400, [single]600))
|
||||
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
||||
$obstacles.SetValue($obstacle, 0)
|
||||
$source = [Activator]::CreateInstance($manualSourceType, @('service-safety-obstacle', [long]1, $true, $obstacles))
|
||||
$sources = [Array]::CreateInstance($obstacleSourceType, 1)
|
||||
$sources.SetValue($source, 0)
|
||||
$mapRequest.ObstacleSources = $sources
|
||||
}
|
||||
else {
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
}
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
||||
Assert-True ($null -ne $map) 'Service test must create an explicit empty planning map.'
|
||||
Assert-True ($null -ne $map) 'Service test must create a planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-EmptyMap { return New-Map $false }
|
||||
|
||||
function New-CollidingMap { return New-Map $true }
|
||||
|
||||
function New-Vehicle {
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.20
|
||||
@@ -58,7 +73,8 @@ function New-Configuration($Method = $cubicBSpline) {
|
||||
return $configuration
|
||||
}
|
||||
|
||||
function New-Request([object[]]$Points, $Configuration) {
|
||||
function New-Request([object[]]$Points, $Configuration, $Map = $null) {
|
||||
if ($null -eq $Map) { $Map = New-EmptyMap }
|
||||
$typedPoints = [Array]::CreateInstance($coarsePointType, $Points.Count)
|
||||
for ($index = 0; $index -lt $Points.Count; $index++) {
|
||||
$typedPoints.SetValue($Points[$index], $index)
|
||||
@@ -67,7 +83,7 @@ function New-Request([object[]]$Points, $Configuration) {
|
||||
$segments = [Array]::CreateInstance($coarseSegmentType, 1)
|
||||
$segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(
|
||||
0, $forward, 0, ($Points.Count - 1), $false, $false)), 0)
|
||||
return [Activator]::CreateInstance($requestType, @($typedPoints, $segments, (New-EmptyMap), (New-Vehicle), $Configuration))
|
||||
return [Activator]::CreateInstance($requestType, @($typedPoints, $segments, $Map, (New-Vehicle), $Configuration))
|
||||
}
|
||||
|
||||
function New-StraightRequest($Configuration) {
|
||||
@@ -119,6 +135,10 @@ $directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
|
||||
$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle')
|
||||
$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle')
|
||||
$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource')
|
||||
$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource')
|
||||
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
|
||||
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')
|
||||
|
||||
@@ -185,6 +205,15 @@ Assert-Equal 'InvalidInput' $invalidCoarseResult.Status.ToString() 'A non-finite
|
||||
Assert-NoGeometry $invalidCoarseResult 'Invalid coarse path'
|
||||
Assert-Equal 0 $invalidCoarseResult.Diagnostics.RetryCount 'Invalid coarse input must be rejected before retries.'
|
||||
|
||||
# A finite, structurally valid coarse path may still be unsafe for the requested map and vehicle.
|
||||
# It must be rejected before method selection/retries and may never use fallback to publish the unsafe geometry.
|
||||
$unsafeCoarseResult = Invoke-Smooth (New-Request @(
|
||||
(New-CoarsePoint 0.5 0.5 0.0 $forward),
|
||||
(New-CoarsePoint 1.5 0.5 1.0 $forward)) (New-Configuration) (New-CollidingMap))
|
||||
Assert-Equal 'InvalidInput' $unsafeCoarseResult.Status.ToString() 'A colliding coarse path must be invalid before smoothing starts.'
|
||||
Assert-NoGeometry $unsafeCoarseResult 'Unsafe coarse path'
|
||||
Assert-Equal 0 $unsafeCoarseResult.Diagnostics.RetryCount 'Unsafe coarse geometry must be rejected before retry execution.'
|
||||
|
||||
$cancelledConfiguration = New-Configuration
|
||||
$cancellationSource = [Threading.CancellationTokenSource]::new()
|
||||
$cancellationSource.Cancel()
|
||||
|
||||
Reference in New Issue
Block a user