feat: add EM planner contracts

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/ContractNumeric.cs

@@ -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.");
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmBoundaryType.cs

@@ -0,0 +1,10 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmBoundaryType
+{
+    None,
+    RollingSafetyStop,
+    GearSwitchApproach,
+    GearSwitchDeparture,
+    Goal,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmMotionModel.cs

@@ -0,0 +1,8 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmMotionModel
+{
+    NonholonomicForwardReverse,
+    CrabTranslation,
+    InPlaceRotation,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs

@@ -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
+{
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningResult.cs

@@ -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; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs

@@ -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,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTerminalType.cs

@@ -0,0 +1,8 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmTerminalType
+{
+    RollingSafetyStop,
+    GearSwitch,
+    Goal,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectory.cs

@@ -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; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs

@@ -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; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryPoint.cs

@@ -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; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs

@@ -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; }
+}