From 144a0883b2d23abf0c8ac5096d6843821c497e98 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Sat, 1 Aug 2026 00:05:41 +0800 Subject: [PATCH] fix: deduplicate Local G2 evaluator windows --- .../LocalG2/LocalG2CandidateEvaluator.cs | 20 ++++- ...ify_path_smoothing_local_g2_candidates.ps1 | 74 +++++++++++++++++++ 2 files changed, 92 insertions(+), 2 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs index 1d04d17..50b84d5 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs @@ -16,6 +16,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; internal sealed class LocalG2CandidateEvaluator { private const double CurvatureRangeTolerance = 1e-6d; + private const double WindowPointToleranceMeters = 1e-10d; private static readonly IReadOnlyList EmptyPath = new ReadOnlyCollection(new List()); private static readonly IReadOnlyList EmptySegments = @@ -192,13 +193,28 @@ internal sealed class LocalG2CandidateEvaluator for (int index = 0; index < segment.Points.Count; index++) { SmoothingPoint2D point = segment.Points[index]; - if (point.ArcLength > startArcLength && point.ArcLength < endArcLength) points.Add(point); + if (point.ArcLength > startArcLength && point.ArcLength < endArcLength && + !SamePosition(points[points.Count - 1], point)) + { + points.Add(point); + } } - points.Add(end); + if (SamePosition(points[points.Count - 1], end)) + points[points.Count - 1] = end; + else + points.Add(end); window = new ReadOnlyCollection(points); return true; } + private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right) + { + if (left == null || right == null) return false; + double x = right.X - left.X; + double y = right.Y - left.Y; + return x * x + y * y <= WindowPointToleranceMeters * WindowPointToleranceMeters; + } + private bool TryAnalyzeWindow(PreparedDirectionSegment source, IReadOnlyList points, double spacing, out PathGeometryAnalysis analysis, out string reason) { diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 index 8cbfe73..86caacd 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 @@ -1,6 +1,8 @@ param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll')) $ErrorActionPreference = 'Stop' +$newtonsoft = Join-Path $env:USERPROFILE '.nuget\packages\newtonsoft.json\13.0.4\lib\netstandard2.0\Newtonsoft.Json.dll' +if (Test-Path $newtonsoft) { $null = [Reflection.Assembly]::LoadFrom($newtonsoft) } $assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath)) function Assert-True($Actual, [string]$Message) { @@ -21,6 +23,26 @@ function Get-RequiredType([string]$Name) { return $assembly.GetType($Name, $true) } +function Get-InternalProperty($Instance, [string]$Name) { + return $Instance.GetType().GetProperty( + $Name, + [Reflection.BindingFlags]'Public,NonPublic,Instance').GetValue($Instance) +} + +function Get-InternalMethod($Type, [string]$Name) { + return @($Type.GetMethods([Reflection.BindingFlags]'Public,NonPublic,Instance,Static') | + Where-Object Name -eq $Name)[0] +} + +function New-InternalInstance($Type) { + return [Activator]::CreateInstance( + $Type, + [Reflection.BindingFlags]'Instance,NonPublic,Public', + $null, + @(), + $null) +} + $builderType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateBuilder' $hooksType = $builderType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic') Assert-True ($null -ne $hooksType) 'LocalG2CandidateBuilder must expose narrowly scoped deterministic TestHooks.' @@ -135,4 +157,56 @@ $best = Invoke-EvaluationScenario 'Best' Assert-Equal $smallestDeviation.CandidateIndex $best.CandidateIndex ` 'Among sufficient candidates, minimum deviation must win before extra smoothness.' +# Exercise the real builder → evaluator seam that a hand-built TestHook candidate does not cover. +$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.' +$factoryType = Get-RequiredType ($root + 'Test.SmoothingScenarioFactory') +$requestType = Get-RequiredType ($root + 'PathSmoothingRequest') +$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration') +$methodType = Get-RequiredType ($root + 'SmoothingMethod') +$preprocessorType = Get-RequiredType ($root + 'Processing.PathSmoothingPreprocessor') +$optionsType = Get-RequiredType ($root + 'LocalG2.LocalG2OptionsSnapshot') +$detectorType = Get-RequiredType ($root + 'LocalG2.CurvatureTransitionDetector') +$plannerType = Get-RequiredType ($root + 'LocalG2.LocalG2WindowPlanner') +$fixturePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json' +$fixtures = (Get-InternalMethod $factoryType 'CreateFixtureRequests').Invoke($null, @((Resolve-Path $fixturePath).Path)) +$baseRequest = $fixtures[1].SmoothingRequest # single-turn +$configuration = [Activator]::CreateInstance($configurationType) +$configuration.Method = [Enum]::Parse($methodType, 'LocalG2Quintic') +$singleTurnRequest = [Activator]::CreateInstance($requestType, @( + $baseRequest.CoarsePath, $baseRequest.Segments, $baseRequest.Map, $baseRequest.Vehicle, $configuration)) +$preprocessor = [Activator]::CreateInstance($preprocessorType) +$prepareArgs = [object[]]@($singleTurnRequest, $null, $null) +Assert-True ((Get-InternalMethod $preprocessorType 'TryPrepare').Invoke($preprocessor, $prepareArgs)) ` + 'SingleTurn must prepare before evaluating Local G2 candidates.' +$preparedPath = $prepareArgs[1] +$options = [Activator]::CreateInstance($optionsType, [Reflection.BindingFlags]'Instance,NonPublic,Public', $null, @($configuration), $null) +$detector = New-InternalInstance $detectorType +$detectArgs = [object[]]@($singleTurnRequest, [double]$singleTurnRequest.Vehicle.MaximumCurvaturePerMeter, $options, $null, $null) +Assert-True ((Get-InternalMethod $detectorType 'TryDetect').Invoke($detector, $detectArgs)) ` + 'SingleTurn must detect its Local G2 transition.' +$planner = New-InternalInstance $plannerType +$planArgs = [object[]]@($preparedPath, $detectArgs[3], $options, $null, $null) +Assert-True ((Get-InternalMethod $plannerType 'TryPlan').Invoke($planner, $planArgs)) ` + 'SingleTurn must plan a Local G2 region.' +$region = $null +foreach ($plannedRegion in $planArgs[3]) { $region = $plannedRegion; break } +$preparedSegment = (Get-InternalProperty $preparedPath 'Segments')[(Get-InternalProperty $region 'SegmentIndex')] +$realBuilder = New-InternalInstance $builderType +$realCandidates = (Get-InternalMethod $builderType 'Build').Invoke($realBuilder, @( + $preparedSegment, $region, [double]$configuration.OutputSpacingMeters, $options, [Threading.CancellationToken]::None)) +Assert-True ($realCandidates.Count -gt 0) 'SingleTurn must build real Local G2 candidates.' +$realEvaluator = New-InternalInstance $evaluatorType +$duplicateFailures = @() +foreach ($realCandidate in $realCandidates) { + $evaluation = (Get-InternalMethod $evaluatorType 'Evaluate').Invoke($realEvaluator, @( + $preparedPath, $preparedPath, $region, $realCandidate, $singleTurnRequest, $options, [Threading.CancellationToken]::None)) + $failureReason = Get-InternalProperty $evaluation 'FailureReason' + if ($failureReason -eq 2) { + $candidateIndex = Get-InternalProperty $realCandidate 'CandidateIndex' + $duplicateFailures += $candidateIndex + } +} +Assert-Equal 0 $duplicateFailures.Count ` + 'SingleTurn builder candidates must not fail evaluator window analysis due to duplicate or degenerate points.' + Write-Output 'Path smoothing Local G2 candidate checks passed.'