From 1ac3dbda8da534677e65457f991a9aad194b5971 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Fri, 31 Jul 2026 15:47:33 +0800 Subject: [PATCH] fix: enforce Local G2 total window length --- .../LocalG2/LocalG2SmoothingRegion.cs | 8 +- .../LocalG2/LocalG2WindowPlanner.cs | 228 +++++++++++++++--- ...rify_path_smoothing_local_g2_detection.ps1 | 33 +++ 3 files changed, 236 insertions(+), 33 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs index 710da97..8a99f98 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.Utils; namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; @@ -40,8 +41,11 @@ internal sealed class LocalG2SmoothingRegion IReadOnlyList windowVariants) { if (segmentIndex < 0 || transitions == null || transitions.Count == 0 || - maximumStartArcLengthMeters < 0d || maximumEndArcLengthMeters < maximumStartArcLengthMeters || - windowVariants == null) + !NumericGuard.IsFinite(maximumStartArcLengthMeters) || + !NumericGuard.IsFinite(maximumEndArcLengthMeters) || + maximumStartArcLengthMeters < 0d || + maximumEndArcLengthMeters < maximumStartArcLengthMeters || + windowVariants == null || windowVariants.Count == 0) { throw new ArgumentOutOfRangeException(nameof(transitions)); } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs index b3e9694..26143e2 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing; using MultiWheelC.TrajectoryPlanning.Utils; @@ -48,28 +49,49 @@ internal sealed class LocalG2WindowPlanner { CurvatureTransition first = ordered[cursor]; double segmentLength = segmentLengths[first.SegmentIndex]; - WindowRange merged = MaximumLegalRange(first.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters); var group = new List { first }; + IReadOnlyList variants = + BuildVariants(group, segmentLength, options); + if (variants.Count == 0) + { + reason = "局部 G2 单事件无法生成满足总长度约束的窗口。"; + return false; + } cursor++; - while (cursor < ordered.Count && ordered[cursor].SegmentIndex == first.SegmentIndex) + while (cursor < ordered.Count && + ordered[cursor].SegmentIndex == first.SegmentIndex) { - CurvatureTransition next = ordered[cursor]; - WindowRange nextRange = MaximumLegalRange(next.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters); - if (nextRange.StartArcLengthMeters > merged.EndArcLengthMeters + MergeToleranceMeters) break; - group.Add(next); - merged = new WindowRange( - Math.Min(merged.StartArcLengthMeters, nextRange.StartArcLengthMeters), - Math.Max(merged.EndArcLengthMeters, nextRange.EndArcLengthMeters)); + var tentative = new List(group) + { + ordered[cursor], + }; + IReadOnlyList tentativeVariants = + BuildVariants(tentative, segmentLength, options); + if (tentativeVariants.Count == 0) break; + group = tentative; + variants = tentativeVariants; cursor++; } + double minimumStart = double.PositiveInfinity; + double maximumEnd = double.NegativeInfinity; + for (int variantIndex = 0; variantIndex < variants.Count; variantIndex++) + { + minimumStart = Math.Min( + minimumStart, + variants[variantIndex].StartArcLengthMeters); + maximumEnd = Math.Max( + maximumEnd, + variants[variantIndex].EndArcLengthMeters); + } + planned.Add(new LocalG2SmoothingRegion( first.SegmentIndex, group, - merged.StartArcLengthMeters, - merged.EndArcLengthMeters, - BuildVariants(group, segmentLength, options))); + minimumStart, + maximumEnd, + variants)); } regions = new ReadOnlyCollection(planned); return true; @@ -87,17 +109,18 @@ internal sealed class LocalG2WindowPlanner foreach (double target in BuildTargets(options, segmentLength)) { if (variants.Count >= options.MaximumCandidatesPerRegion) break; - AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options.MaximumCandidatesPerRegion); - AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion); - AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion); + AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options); + AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options); + AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options); } return new ReadOnlyCollection(variants); } private static void AddIfLegal(List variants, double target, double leftRatio, - double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift, int maximumCount) + double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift, + LocalG2OptionsSnapshot options) { - if (variants.Count >= maximumCount) return; + if (variants.Count >= options.MaximumCandidatesPerRegion) return; double left = target * leftRatio; double right = target - left; double availableLeft = anchor; @@ -120,11 +143,20 @@ internal sealed class LocalG2WindowPlanner double end = anchor + right; if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target) return; + double actualLength = end - start; + if (actualLength + MergeToleranceMeters < options.MinimumWindowLengthMeters || + actualLength > options.MaximumWindowLengthMeters + MergeToleranceMeters) + { + return; + } variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right)); } private static IReadOnlyList BuildTargets(LocalG2OptionsSnapshot options, double segmentLength) { + if (segmentLength + MergeToleranceMeters < options.MinimumWindowLengthMeters) + return new ReadOnlyCollection(new List()); + double[] requested = { options.PreferredWindowLengthMeters, @@ -152,14 +184,6 @@ internal sealed class LocalG2WindowPlanner return targets; } - private static WindowRange MaximumLegalRange(double eventArcLength, double segmentLength, double maximumWindowLength) - { - double target = Math.Min(maximumWindowLength, segmentLength); - return new WindowRange( - Math.Max(0d, eventArcLength - target), - Math.Min(segmentLength, eventArcLength + target)); - } - private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary lengths, out string reason) { lengths = new Dictionary(); @@ -198,15 +222,157 @@ internal sealed class LocalG2WindowPlanner private static IReadOnlyList Empty() => new ReadOnlyCollection(new List()); - private readonly struct WindowRange + public static class TestHooks { - internal WindowRange(double startArcLengthMeters, double endArcLengthMeters) + public static WindowPlanningTestSnapshot Execute(string scenario) { - StartArcLengthMeters = startArcLengthMeters; - EndArcLengthMeters = endArcLengthMeters; + if (string.IsNullOrWhiteSpace(scenario)) + throw new ArgumentException("A scenario is required.", nameof(scenario)); + + IReadOnlyList transitions; + double segmentLength; + switch (scenario) + { + case "SeparatedByOneMeter": + transitions = new[] + { + Transition(0.2d, 0), + Transition(1.2d, 1), + }; + segmentLength = 1.4d; + break; + case "Mergeable": + transitions = new[] + { + Transition(0.4d, 0), + Transition(0.7d, 1), + }; + segmentLength = 1.4d; + break; + case "ThreeEventPartition": + transitions = new[] + { + Transition(0.2d, 0), + Transition(0.6d, 1), + Transition(1.2d, 2), + }; + segmentLength = 1.4d; + break; + case "NearBoundary": + transitions = new[] { Transition(0.1d, 0) }; + segmentLength = 1d; + break; + default: + throw new ArgumentOutOfRangeException(nameof(scenario)); + } + + var planner = new LocalG2WindowPlanner(); + if (!planner.TryPlan( + CreatePreparedPath(segmentLength), + transitions, + new LocalG2OptionsSnapshot(new PathSmoothingConfiguration()), + out IReadOnlyList regions, + out string reason)) + { + throw new InvalidOperationException(reason); + } + + double maximumLength = 0d; + bool exactEnvelope = true; + var counts = new List(regions.Count); + var signature = new List(); + for (int regionIndex = 0; regionIndex < regions.Count; regionIndex++) + { + LocalG2SmoothingRegion region = regions[regionIndex]; + counts.Add(region.Transitions.Count.ToString()); + double minimumStart = double.PositiveInfinity; + double maximumEnd = double.NegativeInfinity; + for (int variantIndex = 0; variantIndex < region.WindowVariants.Count; variantIndex++) + { + LocalG2WindowVariant variant = region.WindowVariants[variantIndex]; + maximumLength = Math.Max( + maximumLength, + variant.EndArcLengthMeters - variant.StartArcLengthMeters); + minimumStart = Math.Min(minimumStart, variant.StartArcLengthMeters); + maximumEnd = Math.Max(maximumEnd, variant.EndArcLengthMeters); + signature.Add( + region.SegmentIndex + ":" + + variant.CandidateIndex + ":" + + variant.StartArcLengthMeters.ToString("R") + ":" + + variant.EndArcLengthMeters.ToString("R")); + } + exactEnvelope &= Math.Abs(region.MaximumStartArcLengthMeters - minimumStart) <= 1e-9d; + exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d; + } + + LocalG2WindowVariant first = regions[0].WindowVariants[0]; + return new WindowPlanningTestSnapshot( + regions.Count, + string.Join(",", counts), + maximumLength, + exactEnvelope, + first.LeftWindowLengthMeters, + first.RightWindowLengthMeters, + string.Join("|", signature)); } - internal double StartArcLengthMeters { get; } - internal double EndArcLengthMeters { get; } + public sealed class WindowPlanningTestSnapshot + { + internal WindowPlanningTestSnapshot( + int regionCount, + string transitionCounts, + double maximumWindowLength, + bool exactEnvelope, + double firstLeftLength, + double firstRightLength, + string signature) + { + RegionCount = regionCount; + TransitionCounts = transitionCounts; + MaximumWindowLength = maximumWindowLength; + ExactEnvelope = exactEnvelope; + FirstLeftLength = firstLeftLength; + FirstRightLength = firstRightLength; + Signature = signature; + } + + public int RegionCount { get; } + public string TransitionCounts { get; } + public double MaximumWindowLength { get; } + public bool ExactEnvelope { get; } + public double FirstLeftLength { get; } + public double FirstRightLength { get; } + public string Signature { get; } + } + + private static CurvatureTransition Transition(double arcLength, int index) + { + return new CurvatureTransition( + 0, + index, + index + 1, + arcLength, + arcLength, + 0d, + 0d, + index % 2 == 0 ? 0d : 0.5d, + index % 2 == 0 ? 0.5d : 0d); + } + + private static PreparedPath CreatePreparedPath(double length) + { + var points = new[] + { + new SmoothingPoint2D( + 0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + new SmoothingPoint2D( + length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + }; + return new PreparedPath(new[] + { + new PreparedDirectionSegment( + 0, TravelDirection.Forward, points, false, false), + }); + } } } diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 index 7b82cd9..7b54982 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 @@ -93,4 +93,37 @@ 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.' +$plannerType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2WindowPlanner' +$plannerHooksType = $plannerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic') +Assert-True ($null -ne $plannerHooksType) 'LocalG2WindowPlanner must expose narrow deterministic TestHooks.' +$planScenario = $plannerHooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static') + +$separated = $planScenario.Invoke($null, @('SeparatedByOneMeter')) +Assert-Equal 2 $separated.RegionCount 'Events 1.0 m apart cannot share a 0.80 m total window.' +Assert-Equal '1,1' $separated.TransitionCounts 'Separated events must remain one event per region.' + +$mergeable = $planScenario.Invoke($null, @('Mergeable')) +Assert-Equal 1 $mergeable.RegionCount 'Events covered by one legal total window must merge.' +Assert-Equal '2' $mergeable.TransitionCounts 'The merged region must retain both events.' + +$partition = $planScenario.Invoke($null, @('ThreeEventPartition')) +Assert-Equal 2 $partition.RegionCount 'Three events must split at the first infeasible joint window.' +Assert-Equal '2,1' $partition.TransitionCounts 'Only a feasible consecutive subgroup may merge.' + +$boundary = $planScenario.Invoke($null, @('NearBoundary')) +Assert-True ($boundary.FirstRightLength -gt $boundary.FirstLeftLength) ` + 'A boundary-clamped total window must transfer missing length to the available side.' +Assert-True ($boundary.MaximumWindowLength -le 0.80 + 1e-9) ` + 'No candidate window may exceed 0.80 m total length.' + +foreach ($snapshot in @($separated, $mergeable, $partition, $boundary)) { + Assert-True $snapshot.ExactEnvelope 'Region envelope must equal the extrema of actual legal variants.' + Assert-True ($snapshot.MaximumWindowLength -le 0.80 + 1e-9) ` + 'MaximumWindowLengthMeters is a total, not a per-side length.' +} + +$repeat = $planScenario.Invoke($null, @('ThreeEventPartition')) +Assert-Equal $partition.Signature $repeat.Signature ` + 'Repeated planning must preserve grouping, candidate numbering and variant order.' + Write-Output 'Path smoothing Local G2 detection checks passed.'