feat: build EM path speed limits

2026-08-04 09:16:17 +08:00
parent e91dbceb0b
commit 62ea9db8cd
6 changed files with 734 additions and 1 deletions
@@ -0,0 +1,149 @@
+using System;
+using System.Collections.Generic;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class LongitudinalModelChecks
+{
+    public static void Run()
+    {
+        VerifiesFinitePathSIndexedSpeedEnvelope();
+        VerifiesStoppingPrecheckBeforeQpAssembly();
+        VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
+    }
+
+    private static void VerifiesFinitePathSIndexedSpeedEnvelope()
+    {
+        LateralPath directionPath = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(1d, 1d, 0d, 0d),
+        });
+        var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.Goal, EmPlannerConfiguration.CreateDefault(), Array.Empty<double>(), Array.Empty<double>());
+        EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput,
+            out PathSpeedLimit directionEnvelope, out string directionFailureReason);
+        Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
+            directionFailureReason);
+        Verification.NearlyEqual(0.20d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
+            "default direction speed limit");
+
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
+        LateralPath path = CreatePath(new[]
+        {
+            new PathFixture(10d, 0d, 0d, 0d),
+            new PathFixture(20d, 2d, 2d, 4d),
+            new PathFixture(20.5d, 4d, 20d, 0d),
+            new PathFixture(21d, 5d, 0d, 0d),
+        });
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+            out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "speed envelope status: " + failureReason);
+        Verification.NearlyEqual(1d, envelope.DirectionMaximumSpeedMetersPerSecond, "overridden direction speed limit");
+        Verification.NearlyEqual(Math.Sqrt(0.20d / 2d), envelope.LateralAccelerationLimitAt(2d),
+            "curvature lateral-acceleration limit");
+        Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
+        Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
+            double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
+        Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (5d - 4d)), envelope.StoppingLimitAt(4d),
+            "stopping speed limit");
+        Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
+            "combined limit chooses the finite minimum");
+        Verification.NearlyEqual((envelope.MaximumSpeedAt(0d) + envelope.MaximumSpeedAt(2d)) / 2d,
+            envelope.MaximumSpeedAt(1d), "speed envelope interpolates by PathS rather than ReferenceS");
+        Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero");
+    }
+
+    private static void VerifiesStoppingPrecheckBeforeQpAssembly()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        LateralPath shortPath = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(100d, 0.01d, 0d, 0d),
+        });
+        var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
+            EmTerminalType.RollingSafetyStop, configuration, Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+            out string failureReason);
+        Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
+            "jerk/deceleration stopping precheck status");
+        Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
+        Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
+    }
+
+    private static void VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        DirectionSegmentView shortGoal = CreateSegment(0.25d, EmBoundaryType.Goal);
+        EmPlanningStatus status = new PlanningHorizonSelector().Select(shortGoal, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection goalSelection, out string goalFailure);
+
+        Verification.Equal(EmPlanningStatus.Success, status, "goal horizon status: " + goalFailure);
+        Verification.Equal(EmTerminalType.Goal, goalSelection.TerminalType, "goal terminal type");
+        Verification.NearlyEqual(0.25d, goalSelection.TerminalReferenceS, "goal terminal reference S");
+
+        DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.GearSwitchApproach);
+        status = new PlanningHorizonSelector().Select(longSegment, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection rollingSelection, out string rollingFailure);
+        Verification.Equal(EmPlanningStatus.Success, status, "rolling horizon status: " + rollingFailure);
+        Verification.Equal(EmTerminalType.RollingSafetyStop, rollingSelection.TerminalType, "rolling terminal type");
+        Verification.True(rollingSelection.TerminalReferenceS >= 0d && rollingSelection.TerminalReferenceS < 4d,
+            "rolling horizon remains within the current segment");
+    }
+
+    private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
+    {
+        var points = new List<LateralPathPoint>(fixtures.Count);
+        for (int index = 0; index < fixtures.Count; index++)
+        {
+            PathFixture fixture = fixtures[index];
+            points.Add(new LateralPathPoint(fixture.ReferenceS, fixture.PathS, 0d, 0d, 0d, 0d, fixture.PathS, 0d,
+                0d, fixture.Curvature, fixture.Curvature, fixture.CurvatureDerivative));
+        }
+        return new LateralPath(points, true);
+    }
+
+    private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d),
+            Point(length, length),
+        };
+        return new DirectionSegmentView(0, TravelDirection.Forward, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, length, endBoundaryType, length), 0d);
+    }
+
+    private static SmoothedPathPoint Point(double x, double s)
+    {
+        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, false,
+            SmoothedPathPointSource.Anchor);
+    }
+
+    private sealed class PathFixture
+    {
+        public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative)
+        {
+            ReferenceS = referenceS;
+            PathS = pathS;
+            Curvature = curvature;
+            CurvatureDerivative = curvatureDerivative;
+        }
+
+        public double ReferenceS { get; }
+        public double PathS { get; }
+        public double Curvature { get; }
+        public double CurvatureDerivative { get; }
+    }
+}