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,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);
+    }
+}