diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs index 8cb2bb6..671f3ca 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs @@ -26,6 +26,10 @@ internal sealed class CurvatureTransition throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters)); } + double curvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter); + if (!NumericGuard.IsFinite(curvatureJumpPerMeter)) + throw new ArgumentOutOfRangeException(nameof(rightVehicleCurvaturePerMeter)); + SegmentIndex = segmentIndex; LeftCoarsePathIndex = leftCoarsePathIndex; RightCoarsePathIndex = rightCoarsePathIndex; @@ -35,7 +39,7 @@ internal sealed class CurvatureTransition VehicleHeadingRadians = vehicleHeadingRadians; LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter; RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter; - CurvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter); + CurvatureJumpPerMeter = curvatureJumpPerMeter; } internal int SegmentIndex { get; } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs index b2a10ef..54d95ba 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs @@ -46,6 +46,11 @@ internal sealed class CurvatureTransitionDetector CoarsePathPoint left = request.CoarsePath[leftIndex]; CoarsePathPoint right = request.CoarsePath[leftIndex + 1]; double delta = right.VehicleCurvature - left.VehicleCurvature; + if (!NumericGuard.IsFinite(delta)) + { + reason = "局部 G2 曲率事件跳变溢出。"; + return false; + } if (Math.Abs(delta) >= threshold) { detected.Add(new CurvatureTransition( diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 index d35eff4..7b82cd9 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 @@ -7,6 +7,10 @@ function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } } +function Assert-False($Actual, [string]$Message) { + if ($Actual) { throw $Message } +} + function Assert-Equal($Expected, $Actual, [string]$Message) { if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" } } @@ -55,4 +59,38 @@ Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be cl Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) ` 'Unavailable left length must be shifted to the right.' +# Finite endpoint curvatures may still overflow during subtraction; detection must fail rather than publish infinity. +$coarsePathPointType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.CoarsePathPoint' +$pathSegmentType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.PathSegment' +$directionType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.TravelDirection' +$coarseSourceType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.CoarsePathPointSource' +$requestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.PathSmoothingRequest' +$configurationType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.PathSmoothingConfiguration' +$optionsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2OptionsSnapshot' +$forward = [Enum]::Parse($directionType, 'Forward') +$motionPrimitive = [Enum]::Parse($coarseSourceType, 'MotionPrimitive') +$overflowPoints = [Array]::CreateInstance($coarsePathPointType, 2) +$overflowPoints.SetValue([Activator]::CreateInstance($coarsePathPointType, @( + [double]0.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0, $forward, + -[double]::MaxValue, [double]1.0, $false, $motionPrimitive)), 0) +$overflowPoints.SetValue([Activator]::CreateInstance($coarsePathPointType, @( + [double]0.1, [double]0.0, [double]0.0, [double]0.0, [double]0.1, $forward, + [double]::MaxValue, [double]1.0, $false, $motionPrimitive)), 1) +$overflowSegments = [Array]::CreateInstance($pathSegmentType, 1) +$overflowSegments.SetValue([Activator]::CreateInstance($pathSegmentType, @(0, $forward, 0, 1, $false, $false)), 0) +$configuration = [Activator]::CreateInstance($configurationType) +$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null) +Assert-True ($null -ne $optionsConstructor) 'Local G2 options must be constructible for detector reflection tests.' +$options = $optionsConstructor.Invoke(@($configuration)) +$overflowRequest = [Activator]::CreateInstance($requestType, @($overflowPoints, $overflowSegments, $null, $null, $configuration)) +$detector = [Activator]::CreateInstance($detectorType, $true) +$tryDetect = $detectorType.GetMethod('TryDetect', [Reflection.BindingFlags]'Instance,NonPublic') +Assert-True ($null -ne $tryDetect) 'CurvatureTransitionDetector must retain its internal TryDetect contract.' +$overflowArguments = [object[]]@($overflowRequest, [double]0.8333, $options, $null, $null) +$overflowAccepted = $tryDetect.Invoke($detector, $overflowArguments) +Assert-False $overflowAccepted 'Curvature subtraction overflow must reject detection.' +Assert-True (-not [string]::IsNullOrWhiteSpace([string]$overflowArguments[4])) ` + 'Rejected overflow detection must provide a stable reason.' +Assert-Equal 0 $overflowArguments[3].Count 'Rejected overflow detection must not publish a curvature event.' + Write-Output 'Path smoothing Local G2 detection checks passed.'