feat: detect local curvature transition regions

This commit is contained in:
梁薄云
2026-07-30 16:52:01 +08:00
parent a267a6210e
commit 0fc64f7693
6 changed files with 700 additions and 0 deletions
@@ -0,0 +1,58 @@
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
$ErrorActionPreference = 'Stop'
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
function Assert-True($Actual, [string]$Message) {
if (-not $Actual) { throw $Message }
}
function Assert-Equal($Expected, $Actual, [string]$Message) {
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
}
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
}
}
function Get-RequiredType([string]$Name) {
return $assembly.GetType($Name, $true)
}
$detectorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.CurvatureTransitionDetector'
$hooksType = $detectorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
Assert-True ($null -ne $hooksType) 'CurvatureTransitionDetector must expose its narrowly scoped nested TestHooks helper.'
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
Assert-True ($null -ne $executeMethod) 'TestHooks must expose deterministic scenario execution for reflection tests.'
function Invoke-Scenario([string]$Scenario) {
return $executeMethod.Invoke($null, @($Scenario))
}
# Same direction: 0 -> 0.4167 is detected once.
$singleTransition = Invoke-Scenario 'SingleTransition'
Assert-Equal 1 $singleTransition.TransitionCount 'One primitive curvature jump must be detected.'
Assert-Near 0.4167 $singleTransition.MaximumJump 0.0001 'The jump magnitude must be retained.'
# Same pose at a forward/reverse boundary: no event crosses the stop.
$gearSwitch = Invoke-Scenario 'GearSwitch'
Assert-Equal 0 $gearSwitch.TransitionCount 'A stopped gear switch must not be a smoothing event.'
# A 0.01 1/m numerical change is below max(0.001, 5% of 0.8333).
$noise = Invoke-Scenario 'Noise'
Assert-Equal 0 $noise.TransitionCount 'Sub-threshold curvature noise must be ignored.'
# Two 0.50 m windows whose ranges overlap are merged.
$overlap = Invoke-Scenario 'Overlap'
Assert-Equal 1 $overlap.RegionCount 'Overlapping windows must form one joint region.'
Assert-Equal 2 $overlap.TransitionCountInFirstRegion 'The merged region must retain both events.'
# Near a segment start, the window becomes asymmetric without crossing the hard boundary.
$nearStart = Invoke-Scenario 'NearStart'
Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be clamped to the segment.'
Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) `
'Unavailable left length must be shifted to the right.'
Write-Output 'Path smoothing Local G2 detection checks passed.'