feat: add EM planner contracts

2026-08-03 22:14:37 +08:00
parent 882f80bca0
commit e12eb31205
16 changed files with 477 additions and 0 deletions
@@ -0,0 +1,12 @@
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net10.0-windows</TargetFramework>
+    <ImplicitUsings>disable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\..\ClumsyPilot.csproj"
+                      AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
+  </ItemGroup>
+</Project>
@@ -0,0 +1,51 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class FoundationChecks
+{
+    public static void Run()
+    {
+        var reverseState = new VehicleMotionState(
+            new Pose2D(1.5d, -2d, 0.25d),
+            -0.15d,
+            -0.03d,
+            new DateTimeOffset(2026, 8, 3, 0, 0, 0, TimeSpan.Zero),
+            17L);
+
+        EMPlannerVerificationHost.Verification.NearlyEqual(-0.15d, reverseState.SignedLongitudinalSpeedMetersPerSecond,
+            "reverse signed speed");
+
+        var point = new EmTrajectoryPoint(
+            1.25d,
+            -0.75d,
+            0.5d,
+            -0.12d,
+            0.4d,
+            -0.2d,
+            3,
+            0.6d,
+            0.8d,
+            TravelDirection.Reverse,
+            EmBoundaryType.GearSwitchApproach,
+            -0.04d,
+            0.03d);
+
+        EMPlannerVerificationHost.Verification.NearlyEqual(1.25d, point.X, "point x");
+        EMPlannerVerificationHost.Verification.NearlyEqual(-0.75d, point.Y, "point y");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, point.Yaw, "point yaw");
+        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d, point.SignedLongitudinalVelocity, "point signed speed");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.4d, point.TimeFromStart, "point time");
+        EMPlannerVerificationHost.Verification.NearlyEqual(-0.2d, point.VehicleCurvature, "point curvature");
+        EMPlannerVerificationHost.Verification.Equal(3, point.SegmentIndex, "point segment index");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.6d, point.SegmentLocalS, "point segment local s");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.8d, point.PathS, "point path s");
+        EMPlannerVerificationHost.Verification.Equal(TravelDirection.Reverse, point.Direction, "point direction");
+        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, point.BoundaryType, "point boundary type");
+        EMPlannerVerificationHost.Verification.NearlyEqual(0.12d, point.Speed, "point derived speed");
+        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Cos(0.5d), point.VelocityX, "point derived velocity x");
+        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y");
+        EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate");
+    }
+}
@@ -0,0 +1,27 @@
+using System;
+
+namespace EMPlannerVerificationHost;
+
+internal static class Program
+{
+    private static int Main(string[] args)
+    {
+        if (args.Length != 1 || args[0] != "foundation")
+        {
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation");
+            return 2;
+        }
+
+        try
+        {
+            MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
+            Console.WriteLine("PASS foundation");
+            return 0;
+        }
+        catch (Exception exception)
+        {
+            Console.Error.WriteLine(exception);
+            return 1;
+        }
+    }
+}
@@ -0,0 +1,18 @@
+using System;
+
+namespace EMPlannerVerificationHost;
+
+internal static class Verification
+{
+    public static void Equal<T>(T expected, T actual, string name)
+    {
+        if (!Equals(expected, actual))
+            throw new InvalidOperationException(name + " expected " + expected + " but was " + actual + ".");
+    }
+
+    public static void NearlyEqual(double expected, double actual, string name)
+    {
+        if (Math.Abs(expected - actual) > 1e-12d)
+            throw new InvalidOperationException(name + " expected " + expected + " but was " + actual + ".");
+    }
+}