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
@@ -11,6 +11,12 @@
    <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
  </ItemGroup>

+  <ItemGroup>
+    <Compile Remove="tests\EMPlannerVerificationHost\**\*.cs" />
+    <Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
+             Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
+  </ItemGroup>
+
  <ItemGroup>
    <Reference Include="CommonUsage">
      <HintPath>ref\CommonUsage.dll</HintPath>
@@ -0,0 +1,12 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class ContractNumeric
+{
+    public static void RequireFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "A finite value is required.");
+    }
+}
@@ -0,0 +1,10 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmBoundaryType
+{
+    None,
+    RollingSafetyStop,
+    GearSwitchApproach,
+    GearSwitchDeparture,
+    Goal,
+}
@@ -0,0 +1,8 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmMotionModel
+{
+    NonholonomicForwardReverse,
+    CrabTranslation,
+    InPlaceRotation,
+}
@@ -0,0 +1,58 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlanningRequest
+{
+    public EmPlanningRequest(
+        PathSmoothingResult referencePath,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        VehicleMotionState vehicleState,
+        EmPlannerConfiguration configuration,
+        int segmentIndex,
+        EmTrajectory previousTrajectory,
+        DateTimeOffset requestedAtUtc,
+        DateTimeOffset effectiveAtUtc,
+        string outputTrajectoryId,
+        string referencePathId,
+        string previousTrajectoryId,
+        EmMotionModel motionModel)
+    {
+        ReferencePath = referencePath;
+        Map = map;
+        Vehicle = vehicle;
+        VehicleState = vehicleState;
+        Configuration = configuration;
+        SegmentIndex = segmentIndex;
+        PreviousTrajectory = previousTrajectory;
+        RequestedAtUtc = requestedAtUtc;
+        EffectiveAtUtc = effectiveAtUtc;
+        OutputTrajectoryId = outputTrajectoryId;
+        ReferencePathId = referencePathId;
+        PreviousTrajectoryId = previousTrajectoryId;
+        MotionModel = motionModel;
+    }
+
+    public PathSmoothingResult ReferencePath { get; }
+    public PlanningGridMap Map { get; }
+    public VehicleParameters Vehicle { get; }
+    public VehicleMotionState VehicleState { get; }
+    public EmPlannerConfiguration Configuration { get; }
+    public int SegmentIndex { get; }
+    public EmTrajectory PreviousTrajectory { get; }
+    public DateTimeOffset RequestedAtUtc { get; }
+    public DateTimeOffset EffectiveAtUtc { get; }
+    public string OutputTrajectoryId { get; }
+    public string ReferencePathId { get; }
+    public string PreviousTrajectoryId { get; }
+    public EmMotionModel MotionModel { get; }
+}
+
+// The request owns this DTO contract; Task 2 adds its request-bound settings.
+public sealed partial class EmPlannerConfiguration
+{
+}
@@ -0,0 +1,28 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlanningResult
+{
+    public EmPlanningResult(EmPlanningStatus status, EmTrajectory trajectory, string failureReason)
+    {
+        if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
+            throw new ArgumentOutOfRangeException(nameof(status));
+
+        bool isSuccess = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+        if (isSuccess && trajectory == null)
+            throw new ArgumentException("Successful results require a trajectory.", nameof(trajectory));
+        if (!isSuccess && trajectory != null)
+            throw new ArgumentException("Only successful results may contain a trajectory.", nameof(trajectory));
+
+        Status = status;
+        Trajectory = trajectory;
+        FailureReason = failureReason ?? string.Empty;
+    }
+
+    public EmPlanningStatus Status { get; }
+
+    public EmTrajectory Trajectory { get; }
+
+    public string FailureReason { get; }
+}
@@ -0,0 +1,23 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmPlanningStatus
+{
+    Success,
+    SuccessWithFallback,
+    InvalidInput,
+    UnsupportedMotionMode,
+    StaleVehicleState,
+    StateDirectionMismatch,
+    InvalidReferencePath,
+    ProjectionFailed,
+    CorridorInfeasible,
+    LateralInfeasible,
+    LongitudinalInfeasible,
+    StoppingDistanceInsufficient,
+    SolverUnavailable,
+    SolverTimedOut,
+    Cancelled,
+    ValidationFailed,
+    Superseded,
+    Failed,
+}
@@ -0,0 +1,8 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmTerminalType
+{
+    RollingSafetyStop,
+    GearSwitch,
+    Goal,
+}
@@ -0,0 +1,33 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmTrajectory
+{
+    public EmTrajectory(EmTrajectoryMetadata metadata, IReadOnlyList<EmTrajectoryPoint> points)
+    {
+        if (metadata == null)
+            throw new ArgumentNullException(nameof(metadata));
+        if (points == null)
+            throw new ArgumentNullException(nameof(points));
+        if (points.Count == 0)
+            throw new ArgumentException("A published trajectory requires at least one point.", nameof(points));
+
+        var copy = new List<EmTrajectoryPoint>(points.Count);
+        for (int index = 0; index < points.Count; index++)
+        {
+            if (points[index] == null)
+                throw new ArgumentException("Trajectory points cannot contain null values.", nameof(points));
+            copy.Add(points[index]);
+        }
+
+        Metadata = metadata;
+        Points = new ReadOnlyCollection<EmTrajectoryPoint>(copy);
+    }
+
+    public EmTrajectoryMetadata Metadata { get; }
+
+    public IReadOnlyList<EmTrajectoryPoint> Points { get; }
+}
@@ -0,0 +1,57 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmTrajectoryMetadata
+{
+    public EmTrajectoryMetadata(
+        string trajectoryId,
+        DateTimeOffset generatedAtUtc,
+        DateTimeOffset effectiveAtUtc,
+        long mapSnapshotId,
+        string referencePathId,
+        long vehicleStateSequenceId,
+        string previousTrajectoryId,
+        int segmentIndex,
+        TravelDirection direction,
+        EmTerminalType terminalType)
+    {
+        if (string.IsNullOrWhiteSpace(trajectoryId))
+            throw new ArgumentException("A trajectory ID is required.", nameof(trajectoryId));
+        if (mapSnapshotId < 0)
+            throw new ArgumentOutOfRangeException(nameof(mapSnapshotId));
+        if (string.IsNullOrWhiteSpace(referencePathId))
+            throw new ArgumentException("A reference path ID is required.", nameof(referencePathId));
+        if (vehicleStateSequenceId < 0)
+            throw new ArgumentOutOfRangeException(nameof(vehicleStateSequenceId));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+            throw new ArgumentOutOfRangeException(nameof(terminalType));
+
+        TrajectoryId = trajectoryId;
+        GeneratedAtUtc = generatedAtUtc;
+        EffectiveAtUtc = effectiveAtUtc;
+        MapSnapshotId = mapSnapshotId;
+        ReferencePathId = referencePathId;
+        VehicleStateSequenceId = vehicleStateSequenceId;
+        PreviousTrajectoryId = previousTrajectoryId ?? string.Empty;
+        SegmentIndex = segmentIndex;
+        Direction = direction;
+        TerminalType = terminalType;
+    }
+
+    public string TrajectoryId { get; }
+    public DateTimeOffset GeneratedAtUtc { get; }
+    public DateTimeOffset EffectiveAtUtc { get; }
+    public long MapSnapshotId { get; }
+    public string ReferencePathId { get; }
+    public long VehicleStateSequenceId { get; }
+    public string PreviousTrajectoryId { get; }
+    public int SegmentIndex { get; }
+    public TravelDirection Direction { get; }
+    public EmTerminalType TerminalType { get; }
+}
@@ -0,0 +1,84 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmTrajectoryPoint
+{
+    public EmTrajectoryPoint(
+        double x,
+        double y,
+        double yaw,
+        double signedLongitudinalVelocity,
+        double timeFromStart,
+        double vehicleCurvature,
+        int segmentIndex,
+        double segmentLocalS,
+        double pathS,
+        TravelDirection direction,
+        EmBoundaryType boundaryType,
+        double longitudinalAcceleration,
+        double longitudinalJerk)
+    {
+        ContractNumeric.RequireFinite(x, nameof(x));
+        ContractNumeric.RequireFinite(y, nameof(y));
+        ContractNumeric.RequireFinite(yaw, nameof(yaw));
+        ContractNumeric.RequireFinite(signedLongitudinalVelocity, nameof(signedLongitudinalVelocity));
+        ContractNumeric.RequireFinite(timeFromStart, nameof(timeFromStart));
+        ContractNumeric.RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        ContractNumeric.RequireFinite(segmentLocalS, nameof(segmentLocalS));
+        ContractNumeric.RequireFinite(pathS, nameof(pathS));
+        ContractNumeric.RequireFinite(longitudinalAcceleration, nameof(longitudinalAcceleration));
+        ContractNumeric.RequireFinite(longitudinalJerk, nameof(longitudinalJerk));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (timeFromStart < 0d)
+            throw new ArgumentOutOfRangeException(nameof(timeFromStart));
+        if (segmentLocalS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(segmentLocalS));
+        if (pathS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType))
+            throw new ArgumentOutOfRangeException(nameof(boundaryType));
+
+        X = x;
+        Y = y;
+        Yaw = yaw;
+        SignedLongitudinalVelocity = signedLongitudinalVelocity;
+        Speed = Math.Abs(signedLongitudinalVelocity);
+        VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
+        VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
+        YawRate = signedLongitudinalVelocity * vehicleCurvature;
+        TimeFromStart = timeFromStart;
+        VehicleCurvature = vehicleCurvature;
+        SegmentIndex = segmentIndex;
+        SegmentLocalS = segmentLocalS;
+        PathS = pathS;
+        Direction = direction;
+        BoundaryType = boundaryType;
+        LongitudinalAcceleration = longitudinalAcceleration;
+        LongitudinalJerk = longitudinalJerk;
+    }
+
+    public double X { get; }
+    public double Y { get; }
+    public double Yaw { get; }
+    public double SignedLongitudinalVelocity { get; }
+    public double Speed { get; }
+    public double VelocityX { get; }
+    public double VelocityY { get; }
+    public double YawRate { get; }
+    public double TimeFromStart { get; }
+    public double VehicleCurvature { get; }
+    public int SegmentIndex { get; }
+    public double SegmentLocalS { get; }
+    public double PathS { get; }
+    public TravelDirection Direction { get; }
+    public EmBoundaryType BoundaryType { get; }
+
+    internal double LongitudinalAcceleration { get; }
+
+    internal double LongitudinalJerk { get; }
+}
@@ -0,0 +1,42 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class VehicleMotionState
+{
+    public VehicleMotionState(
+        Pose2D pose,
+        double signedLongitudinalSpeedMetersPerSecond,
+        double? longitudinalAccelerationMetersPerSecondSquared,
+        DateTimeOffset capturedAtUtc,
+        long sequenceId)
+    {
+        if (pose == null)
+            throw new ArgumentNullException(nameof(pose));
+        ContractNumeric.RequireFinite(pose.X, nameof(pose));
+        ContractNumeric.RequireFinite(pose.Y, nameof(pose));
+        ContractNumeric.RequireFinite(pose.Heading, nameof(pose));
+        ContractNumeric.RequireFinite(signedLongitudinalSpeedMetersPerSecond, nameof(signedLongitudinalSpeedMetersPerSecond));
+        if (longitudinalAccelerationMetersPerSecondSquared.HasValue)
+            ContractNumeric.RequireFinite(longitudinalAccelerationMetersPerSecondSquared.Value, nameof(longitudinalAccelerationMetersPerSecondSquared));
+        if (sequenceId < 0)
+            throw new ArgumentOutOfRangeException(nameof(sequenceId));
+
+        Pose = pose;
+        SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond;
+        LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared;
+        CapturedAtUtc = capturedAtUtc;
+        SequenceId = sequenceId;
+    }
+
+    public Pose2D Pose { get; }
+
+    public double SignedLongitudinalSpeedMetersPerSecond { get; }
+
+    public double? LongitudinalAccelerationMetersPerSecondSquared { get; }
+
+    public DateTimeOffset CapturedAtUtc { get; }
+
+    public long SequenceId { get; }
+}
@@ -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 + ".");
+    }
+}