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

fix: unify path smoothing geometry metrics

Browse Source
This commit is contained in:
梁薄云
2026-07-30 16:39:08 +08:00
parent d7ceb761b7
commit d6b34f88ce
16 changed files with 259 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/verify_path_smoothing_validation.ps1
+12 -5
View File
@@ -45,10 +45,11 @@ function New-Map([bool]$WithObstacle) {
}
function New-SmoothedPoint([double]$X, [double]$Y, [double]$ArcLength, $Direction,
[double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0) {
[double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0,
[double]$VehicleCurvatureDerivative = 0.0) {
return [Activator]::CreateInstance($smoothedPointType, @(
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
$VehicleCurvature, $VehicleCurvature, $Clearance, $IsGearSwitch, $anchor))
$VehicleCurvature, $VehicleCurvature, $VehicleCurvatureDerivative, $Clearance, $IsGearSwitch, $anchor))
}
function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$IsGearSwitch = $false) {
@@ -57,10 +58,10 @@ function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$Is
}
function New-OneSegmentCase([double]$X0, [double]$Y0, [double]$X1, [double]$Y1, [double]$VehicleCurvature = 0.0,
[double]$StartArcLength = 0.0) {
[double]$StartArcLength = 0.0, [double]$VehicleCurvatureDerivative = 0.0) {
$candidatePath = [Array]::CreateInstance($smoothedPointType, 2)
$candidatePath.SetValue((New-SmoothedPoint $X0 $Y0 $StartArcLength $forward), 0)
$candidatePath.SetValue((New-SmoothedPoint $X1 $Y1 ($StartArcLength + 1.0) $forward $VehicleCurvature), 1)
$candidatePath.SetValue((New-SmoothedPoint $X0 $Y0 $StartArcLength $forward 0.0 $false 999.0 $VehicleCurvatureDerivative), 0)
$candidatePath.SetValue((New-SmoothedPoint $X1 $Y1 ($StartArcLength + 1.0) $forward $VehicleCurvature $false 999.0 $VehicleCurvatureDerivative), 1)
$candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 1)
$candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
$preparedPoints = [Array]::CreateInstance($smoothingPointType, 2)
@@ -151,6 +152,12 @@ Assert-True $valid.Accepted ('A valid straight candidate must pass. Reason=' + $
Assert-True ($null -ne $valid.Path) 'A valid candidate must return clearance-recomputed points.'
Assert-True ($valid.Path[0].BodyClearance -lt 999.0) 'Validated output must replace an overclaimed candidate clearance.'
Assert-True ($valid.MinimumClearance -ge 0.0) 'A valid candidate must report non-negative conservative clearance.'
$derivativePreserved = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 0.0 0.0 0.25) $obstacleMap
Assert-True $derivativePreserved.Accepted ('A valid derivative-bearing candidate must pass. Reason=' + $derivativePreserved.Reason)
Assert-Near 0.25 $derivativePreserved.Path[0].VehicleCurvatureDerivative 0.000000001 `
'Clearance recomputation must preserve the first point curvature derivative.'
Assert-Near 0.25 $derivativePreserved.Path[1].VehicleCurvatureDerivative 0.000000001 `
'Clearance recomputation must preserve the final point curvature derivative.'
$pointCollision = Invoke-Validation (New-OneSegmentCase 2.0 2.0 2.5 2.0) $obstacleMap
Assert-False $pointCollision.Accepted 'A smoothing candidate that touches an obstacle must be rejected.'