feat: preserve EM planner segment boundaries

2026-08-03 22:27:00 +08:00
parent b326431d63
commit e492e1610a
8 changed files with 407 additions and 4 deletions
@@ -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<SmoothedPathPoint> 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<SmoothedPathPoint>(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<SmoothedPathPoint>(copy);
+        StartBoundary = startBoundary;
+        EndBoundary = endBoundary;
+        SourceStartArcLength = sourceStartArcLength;
+    }
+
+    public int SegmentIndex { get; }
+
+    public TravelDirection Direction { get; }
+
+    public IReadOnlyList<SmoothedPathPoint> Points { get; }
+
+    public ReferenceBoundary StartBoundary { get; }
+
+    public ReferenceBoundary EndBoundary { get; }
+
+    public double SourceStartArcLength { get; }
+
+    public double LengthMeters { get { return EndBoundary.SegmentLocalS; } }
+}
@@ -0,0 +1,49 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class ReferenceBoundary : IEquatable<ReferenceBoundary>
+{
+    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;
+        }
+    }
+}
@@ -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<SmoothedPathPoint> 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<SmoothedPathPoint>(points.Count);
+        for (int index = 0; index < points.Count; index++) copy.Add(points[index]);
+        Segment = segment;
+        Points = new ReadOnlyCollection<SmoothedPathPoint>(copy);
+        TerminalBoundary = terminalBoundary;
+    }
+
+    public DirectionSegmentView Segment { get; }
+
+    public IReadOnlyList<SmoothedPathPoint> 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<SmoothedPathPoint>();
+        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;
+    }
+}
@@ -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<DirectionSegmentView> 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<DirectionSegmentView>(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<SmoothedPathPoint>(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<DirectionSegmentView>(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);
+    }
+}
@@ -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<SmoothedPathPoint>
+        {
+            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<SmoothedPathSegment>
+        {
+            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<PathSmoothingRegionReport>());
+    }
+
+    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);
+    }
+}
@@ -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)
@@ -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");
+        }
+    }
+}
@@ -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.");
+    }
 }