From e492e1610a4039dd4a959a6bf0cc3e261b23e845 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Mon, 3 Aug 2026 22:27:00 +0800 Subject: [PATCH] feat: preserve EM planner segment boundaries --- .../Segmentation/DirectionSegmentView.cs | 64 +++++++++++ .../Segmentation/ReferenceBoundary.cs | 49 ++++++++ .../Segmentation/ReferenceHorizonSlicer.cs | 105 ++++++++++++++++++ .../Segmentation/ReferencePathSegmenter.cs | 84 ++++++++++++++ .../EmFixtureFactory.cs | 41 +++++++ .../EMPlannerVerificationHost/Program.cs | 16 ++- .../SegmentationChecks.cs | 46 ++++++++ .../EMPlannerVerificationHost/Verification.cs | 6 + 8 files changed, 407 insertions(+), 4 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/DirectionSegmentView.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceBoundary.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceHorizonSlicer.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferencePathSegmenter.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/SegmentationChecks.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/DirectionSegmentView.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/DirectionSegmentView.cs new file mode 100644 index 0000000..a9bbe5b --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/DirectionSegmentView.cs @@ -0,0 +1,64 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class DirectionSegmentView +{ + public DirectionSegmentView( + int segmentIndex, + TravelDirection direction, + IReadOnlyList points, + ReferenceBoundary startBoundary, + ReferenceBoundary endBoundary, + double sourceStartArcLength) + { + if (segmentIndex < 0) + throw new ArgumentOutOfRangeException(nameof(segmentIndex)); + if (points == null || points.Count == 0) + throw new ArgumentException("A direction segment requires points.", nameof(points)); + if (startBoundary == null || endBoundary == null) + throw new ArgumentNullException(startBoundary == null ? nameof(startBoundary) : nameof(endBoundary)); + if (startBoundary.SegmentIndex != segmentIndex || endBoundary.SegmentIndex != segmentIndex) + throw new ArgumentException("Boundary segment identity must match the segment."); + if (points[0] == null || Math.Abs(points[0].ArcLength) > 1e-12d) + throw new ArgumentException("A direction segment must begin at local S zero.", nameof(points)); + + var copy = new List(points.Count); + double previousS = -1d; + for (int index = 0; index < points.Count; index++) + { + SmoothedPathPoint point = points[index]; + if (point == null || point.Direction != direction || point.ArcLength < 0d || point.ArcLength < previousS) + throw new ArgumentException("Direction segment points are invalid.", nameof(points)); + copy.Add(point); + previousS = point.ArcLength; + } + if (Math.Abs(endBoundary.SegmentLocalS - previousS) > 1e-12d) + throw new ArgumentException("End boundary must match the final local S.", nameof(endBoundary)); + + SegmentIndex = segmentIndex; + Direction = direction; + Points = new ReadOnlyCollection(copy); + StartBoundary = startBoundary; + EndBoundary = endBoundary; + SourceStartArcLength = sourceStartArcLength; + } + + public int SegmentIndex { get; } + + public TravelDirection Direction { get; } + + public IReadOnlyList Points { get; } + + public ReferenceBoundary StartBoundary { get; } + + public ReferenceBoundary EndBoundary { get; } + + public double SourceStartArcLength { get; } + + public double LengthMeters { get { return EndBoundary.SegmentLocalS; } } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceBoundary.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceBoundary.cs new file mode 100644 index 0000000..12f4268 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceBoundary.cs @@ -0,0 +1,49 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class ReferenceBoundary : IEquatable +{ + public ReferenceBoundary(int segmentIndex, double segmentLocalS, EmBoundaryType boundaryType, double sourceArcLength) + { + if (segmentIndex < 0) + throw new ArgumentOutOfRangeException(nameof(segmentIndex)); + if (double.IsNaN(segmentLocalS) || double.IsInfinity(segmentLocalS) || segmentLocalS < 0d) + throw new ArgumentOutOfRangeException(nameof(segmentLocalS)); + if (double.IsNaN(sourceArcLength) || double.IsInfinity(sourceArcLength) || sourceArcLength < 0d) + throw new ArgumentOutOfRangeException(nameof(sourceArcLength)); + if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType)) + throw new ArgumentOutOfRangeException(nameof(boundaryType)); + + SegmentIndex = segmentIndex; + SegmentLocalS = segmentLocalS; + BoundaryType = boundaryType; + SourceArcLength = sourceArcLength; + } + + public int SegmentIndex { get; } + + public double SegmentLocalS { get; } + + public EmBoundaryType BoundaryType { get; } + + public double SourceArcLength { get; } + + public bool Equals(ReferenceBoundary other) + { + return other != null && SegmentIndex == other.SegmentIndex && SegmentLocalS.Equals(other.SegmentLocalS) && + BoundaryType == other.BoundaryType; + } + + public override bool Equals(object obj) { return Equals(obj as ReferenceBoundary); } + + public override int GetHashCode() + { + unchecked + { + int hash = SegmentIndex; + hash = (hash * 397) ^ SegmentLocalS.GetHashCode(); + return (hash * 397) ^ (int)BoundaryType; + } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceHorizonSlicer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceHorizonSlicer.cs new file mode 100644 index 0000000..cc55cc9 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceHorizonSlicer.cs @@ -0,0 +1,105 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class ReferenceHorizonSlice +{ + public ReferenceHorizonSlice(DirectionSegmentView segment, IReadOnlyList points, + ReferenceBoundary terminalBoundary) + { + if (segment == null) + throw new ArgumentNullException(nameof(segment)); + if (points == null || points.Count == 0) + throw new ArgumentException("A horizon slice requires points.", nameof(points)); + if (terminalBoundary == null || terminalBoundary.SegmentIndex != segment.SegmentIndex) + throw new ArgumentException("A matching terminal boundary is required.", nameof(terminalBoundary)); + + var copy = new List(points.Count); + for (int index = 0; index < points.Count; index++) copy.Add(points[index]); + Segment = segment; + Points = new ReadOnlyCollection(copy); + TerminalBoundary = terminalBoundary; + } + + public DirectionSegmentView Segment { get; } + + public IReadOnlyList Points { get; } + + public ReferenceBoundary TerminalBoundary { get; } +} + +public static class ReferenceHorizonSlicer +{ + private const double Epsilon = 1e-12d; + + public static ReferenceHorizonSlice Slice(DirectionSegmentView segment, double requestedEndSegmentLocalS) + { + if (segment == null) + throw new ArgumentNullException(nameof(segment)); + if (double.IsNaN(requestedEndSegmentLocalS) || double.IsInfinity(requestedEndSegmentLocalS) || requestedEndSegmentLocalS < 0d) + throw new ArgumentOutOfRangeException(nameof(requestedEndSegmentLocalS)); + + double terminalS = Math.Min(requestedEndSegmentLocalS, segment.LengthMeters); + var points = new List(); + for (int index = 0; index < segment.Points.Count; index++) + { + SmoothedPathPoint point = segment.Points[index]; + if (point.ArcLength < terminalS - Epsilon) + points.Add(point); + } + + points.Add(GetExactTerminalPoint(segment, terminalS)); + ReferenceBoundary terminal = terminalS >= segment.LengthMeters - Epsilon + ? segment.EndBoundary + : new ReferenceBoundary(segment.SegmentIndex, terminalS, EmBoundaryType.RollingSafetyStop, + segment.SourceStartArcLength + terminalS); + return new ReferenceHorizonSlice(segment, points, terminal); + } + + private static SmoothedPathPoint GetExactTerminalPoint(DirectionSegmentView segment, double terminalS) + { + for (int index = 0; index < segment.Points.Count; index++) + { + SmoothedPathPoint point = segment.Points[index]; + if (Math.Abs(point.ArcLength - terminalS) <= Epsilon) + return point; + if (point.ArcLength > terminalS) + { + SmoothedPathPoint previous = segment.Points[index - 1]; + return Interpolate(previous, point, terminalS); + } + } + return segment.Points[segment.Points.Count - 1]; + } + + private static SmoothedPathPoint Interpolate(SmoothedPathPoint lower, SmoothedPathPoint upper, double localS) + { + double interval = upper.ArcLength - lower.ArcLength; + if (interval <= 0d) + throw new ArgumentException("Reference points do not bracket a positive interval."); + double fraction = (localS - lower.ArcLength) / interval; + double unwrappedHeading = lower.UnwrappedHeading + (upper.UnwrappedHeading - lower.UnwrappedHeading) * fraction; + return new SmoothedPathPoint( + Interpolate(lower.X, upper.X, fraction), + Interpolate(lower.Y, upper.Y, fraction), + AngleMath.NormalizeRadians(unwrappedHeading), + unwrappedHeading, + localS, + lower.Direction, + Interpolate(lower.GeometricCurvature, upper.GeometricCurvature, fraction), + Interpolate(lower.VehicleCurvature, upper.VehicleCurvature, fraction), + Interpolate(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction), + Interpolate(lower.BodyClearance, upper.BodyClearance, fraction), + false, + SmoothedPathPointSource.Interpolated); + } + + private static double Interpolate(double lower, double upper, double fraction) + { + return lower + (upper - lower) * fraction; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferencePathSegmenter.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferencePathSegmenter.cs new file mode 100644 index 0000000..7fcc3a5 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferencePathSegmenter.cs @@ -0,0 +1,84 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public static class ReferencePathSegmenter +{ + public static IReadOnlyList Create(PathSmoothingResult referencePath) + { + if (referencePath == null || referencePath.Path == null || referencePath.Segments == null || + referencePath.Path.Count == 0 || referencePath.Segments.Count == 0) + throw new ArgumentException("A published reference path with direction segments is required.", nameof(referencePath)); + + var result = new List(referencePath.Segments.Count); + int expectedStart = 0; + for (int segmentListIndex = 0; segmentListIndex < referencePath.Segments.Count; segmentListIndex++) + { + SmoothedPathSegment sourceSegment = referencePath.Segments[segmentListIndex]; + if (sourceSegment == null || sourceSegment.SegmentIndex != segmentListIndex || + sourceSegment.StartIndex != expectedStart || sourceSegment.StartIndex < 0 || + sourceSegment.EndIndex < sourceSegment.StartIndex || sourceSegment.EndIndex >= referencePath.Path.Count) + throw new ArgumentException("Reference path segment indexing is invalid.", nameof(referencePath)); + + SmoothedPathPoint firstSourcePoint = referencePath.Path[sourceSegment.StartIndex]; + if (firstSourcePoint == null || firstSourcePoint.Direction != sourceSegment.Direction) + throw new ArgumentException("Reference path segment start is invalid.", nameof(referencePath)); + + double sourceStartS = firstSourcePoint.ArcLength; + var rebasedPoints = new List(sourceSegment.EndIndex - sourceSegment.StartIndex + 1); + double previousLocalS = -1d; + for (int pathIndex = sourceSegment.StartIndex; pathIndex <= sourceSegment.EndIndex; pathIndex++) + { + SmoothedPathPoint sourcePoint = referencePath.Path[pathIndex]; + if (sourcePoint == null || sourcePoint.Direction != sourceSegment.Direction || + double.IsNaN(sourcePoint.ArcLength) || double.IsInfinity(sourcePoint.ArcLength)) + throw new ArgumentException("Reference path point is invalid.", nameof(referencePath)); + + double localS = sourcePoint.ArcLength - sourceStartS; + if (localS < 0d || (pathIndex > sourceSegment.StartIndex && localS <= previousLocalS)) + throw new ArgumentException("Reference path segment arc length must strictly increase.", nameof(referencePath)); + rebasedPoints.Add(CloneAtLocalS(sourcePoint, localS)); + previousLocalS = localS; + } + + EmBoundaryType startType = sourceSegment.StartsAtGearSwitch + ? EmBoundaryType.GearSwitchDeparture + : EmBoundaryType.None; + EmBoundaryType endType = sourceSegment.EndsAtGearSwitch + ? EmBoundaryType.GearSwitchApproach + : segmentListIndex == referencePath.Segments.Count - 1 + ? EmBoundaryType.Goal + : EmBoundaryType.None; + var startBoundary = new ReferenceBoundary(sourceSegment.SegmentIndex, 0d, startType, sourceStartS); + var endBoundary = new ReferenceBoundary(sourceSegment.SegmentIndex, previousLocalS, endType, + referencePath.Path[sourceSegment.EndIndex].ArcLength); + result.Add(new DirectionSegmentView(sourceSegment.SegmentIndex, sourceSegment.Direction, rebasedPoints, + startBoundary, endBoundary, sourceStartS)); + expectedStart = sourceSegment.EndIndex + 1; + } + + if (expectedStart != referencePath.Path.Count) + throw new ArgumentException("Reference path segments do not cover the full path.", nameof(referencePath)); + return new ReadOnlyCollection(result); + } + + internal static SmoothedPathPoint CloneAtLocalS(SmoothedPathPoint source, double localS) + { + return new SmoothedPathPoint( + source.X, + source.Y, + source.Heading, + source.UnwrappedHeading, + localS, + source.Direction, + source.GeometricCurvature, + source.VehicleCurvature, + source.VehicleCurvatureDerivative, + source.BodyClearance, + source.IsGearSwitchPoint, + source.Source); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs new file mode 100644 index 0000000..64b9d3f --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs @@ -0,0 +1,41 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; + +namespace EMPlannerVerificationHost; + +internal static class EmFixtureFactory +{ + public static PathSmoothingResult CreateGearPairReferencePath() + { + var points = new List + { + Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor), + Point(1d, 1d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor), + Point(2d, 2d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor), + Point(2d, 2d, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch), + Point(1d, 3d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor), + Point(0d, 4d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor), + }; + var segments = new List + { + new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true), + new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false), + }; + var metrics = new PathQualityMetrics(true, 4d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d); + return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments, + new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List()); + } + + private static SmoothedPathPoint Point( + double x, + double arcLength, + TravelDirection direction, + bool isGearSwitchPoint, + SmoothedPathPointSource source) + { + return new SmoothedPathPoint(x, 0d, 0d, 0d, arcLength, direction, 0d, 0d, 0d, 1d, + isGearSwitchPoint, source); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 6a165e9..a769551 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -6,16 +6,24 @@ internal static class Program { private static int Main(string[] args) { - if (args.Length != 1 || args[0] != "foundation") + if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation")) { - Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation"); + Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation"); return 2; } try { - MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run(); - Console.WriteLine("PASS foundation"); + if (args[0] == "foundation") + { + MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run(); + Console.WriteLine("PASS foundation"); + } + else + { + MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run(); + Console.WriteLine("PASS segmentation"); + } return 0; } catch (Exception exception) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/SegmentationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/SegmentationChecks.cs new file mode 100644 index 0000000..bc62cce --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/SegmentationChecks.cs @@ -0,0 +1,46 @@ +using MultiWheelC.TrajectoryPlanning.EMPlanner; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal static class SegmentationChecks +{ + public static void Run() + { + var referencePath = EMPlannerVerificationHost.EmFixtureFactory.CreateGearPairReferencePath(); + var segments = ReferencePathSegmenter.Create(referencePath); + + EMPlannerVerificationHost.Verification.Equal(2, segments.Count, "segment count"); + EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, + segments[0].EndBoundary.BoundaryType, "forward end boundary"); + EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchDeparture, + segments[1].StartBoundary.BoundaryType, "reverse start boundary"); + EMPlannerVerificationHost.Verification.True(!segments[0].EndBoundary.Equals(segments[1].StartBoundary), + "gear-pair boundary identities differ"); + EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[2].ArcLength, "forward source arc length"); + EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[3].ArcLength, "reverse source arc length"); + EMPlannerVerificationHost.Verification.NearlyEqual(0d, segments[1].Points[0].ArcLength, "rebased reverse start"); + + ReferenceHorizonSlice rolling = ReferenceHorizonSlicer.Slice(segments[0], 1.95d); + EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.RollingSafetyStop, + rolling.TerminalBoundary.BoundaryType, "rolling terminal type"); + EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.TerminalBoundary.SegmentLocalS, + "rolling terminal local s"); + EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].ArcLength, + "rolling anchor path s"); + EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].X, + "rolling anchor x"); + + ReferenceHorizonSlice gearSwitch = ReferenceHorizonSlicer.Slice(segments[0], 2.05d); + EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, + gearSwitch.TerminalBoundary.BoundaryType, "gear terminal type"); + EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.TerminalBoundary.SegmentLocalS, + "gear terminal local s"); + EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.Points[gearSwitch.Points.Count - 1].ArcLength, + "gear anchor path s"); + for (int index = 0; index < gearSwitch.Points.Count; index++) + { + EMPlannerVerificationHost.Verification.Equal(segments[0].Direction, gearSwitch.Points[index].Direction, + "horizon point direction"); + } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs index 2acb888..4d0b1e0 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs @@ -15,4 +15,10 @@ internal static class Verification if (Math.Abs(expected - actual) > 1e-12d) throw new InvalidOperationException(name + " expected " + expected + " but was " + actual + "."); } + + public static void True(bool condition, string name) + { + if (!condition) + throw new InvalidOperationException(name + " was false."); + } }