feat: define full-direction EM planning scope

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs

@@ -1,4 +1,4 @@
-namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
 public sealed partial class EmPlannerConfiguration
 {
@@ -23,6 +23,10 @@ public sealed partial class EmPlannerConfiguration
                 SolverTimeoutSeconds = 0.10d,
                 HandoffLookaheadSeconds = 0.30d,
                 MaximumVehicleStateAgeSeconds = 0.20d,
+                MaximumOptimizationTimeStepSeconds = 0.20d,
+                MaximumOptimizationSpatialStepMeters = 0.10d,
+                MaximumOptimizationKnotCount = 401,
+                MaximumPublishedSampleCount = 5001,
             },
             Corridor = new CorridorConfiguration
             {
@@ -58,8 +62,10 @@ public sealed partial class EmPlannerConfiguration
             },
             Longitudinal = new LongitudinalConfiguration
             {
-                MaximumForwardSpeedMetersPerSecond = 0.20d,
-                MaximumReverseSpeedMetersPerSecond = 0.20d,
+                MaximumForwardSpeedMetersPerSecond = 1d,
+                MaximumReverseSpeedMetersPerSecond = 0.5d,
+                DesiredForwardSpeedMetersPerSecond = 1d,
+                DesiredReverseSpeedMetersPerSecond = 0.5d,
                 MaximumAccelerationMetersPerSecondSquared = 0.20d,
                 MaximumDecelerationMetersPerSecondSquared = 0.30d,
                 MaximumJerkMetersPerSecondCubed = 0.50d,
@@ -91,6 +97,8 @@ public sealed partial class EmPlannerConfiguration
             {
                 SpatialToleranceMeters = 1e-8d,
                 KinematicTolerance = 1e-8d,
+                TerminalPositionToleranceMeters = 0.03d,
+                TerminalYawToleranceRadians = 5d * System.Math.PI / 180d,
             },
         };
     }

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs

@@ -4,6 +4,8 @@ public sealed class LongitudinalConfiguration
 {
     public double MaximumForwardSpeedMetersPerSecond { get; set; }
     public double MaximumReverseSpeedMetersPerSecond { get; set; }
+    public double DesiredForwardSpeedMetersPerSecond { get; set; }
+    public double DesiredReverseSpeedMetersPerSecond { get; set; }
     public double MaximumAccelerationMetersPerSecondSquared { get; set; }
     public double MaximumDecelerationMetersPerSecondSquared { get; set; }
     public double MaximumJerkMetersPerSecondCubed { get; set; }
@@ -19,6 +21,8 @@ public sealed class LongitudinalConfiguration
         {
             MaximumForwardSpeedMetersPerSecond = MaximumForwardSpeedMetersPerSecond,
             MaximumReverseSpeedMetersPerSecond = MaximumReverseSpeedMetersPerSecond,
+            DesiredForwardSpeedMetersPerSecond = DesiredForwardSpeedMetersPerSecond,
+            DesiredReverseSpeedMetersPerSecond = DesiredReverseSpeedMetersPerSecond,
             MaximumAccelerationMetersPerSecondSquared = MaximumAccelerationMetersPerSecondSquared,
             MaximumDecelerationMetersPerSecondSquared = MaximumDecelerationMetersPerSecondSquared,
             MaximumJerkMetersPerSecondCubed = MaximumJerkMetersPerSecondCubed,

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs

@@ -9,6 +9,10 @@ public sealed class SchedulingConfiguration
     public double SolverTimeoutSeconds { get; set; }
     public double HandoffLookaheadSeconds { get; set; }
     public double MaximumVehicleStateAgeSeconds { get; set; }
+    public double MaximumOptimizationTimeStepSeconds { get; set; }
+    public double MaximumOptimizationSpatialStepMeters { get; set; }
+    public int MaximumOptimizationKnotCount { get; set; }
+    public int MaximumPublishedSampleCount { get; set; }
 
     internal SchedulingConfiguration Copy()
     {
@@ -21,6 +25,10 @@ public sealed class SchedulingConfiguration
             SolverTimeoutSeconds = SolverTimeoutSeconds,
             HandoffLookaheadSeconds = HandoffLookaheadSeconds,
             MaximumVehicleStateAgeSeconds = MaximumVehicleStateAgeSeconds,
+            MaximumOptimizationTimeStepSeconds = MaximumOptimizationTimeStepSeconds,
+            MaximumOptimizationSpatialStepMeters = MaximumOptimizationSpatialStepMeters,
+            MaximumOptimizationKnotCount = MaximumOptimizationKnotCount,
+            MaximumPublishedSampleCount = MaximumPublishedSampleCount,
         };
     }
 }

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs

@@ -4,6 +4,8 @@ public sealed class ValidationConfiguration
 {
     public double SpatialToleranceMeters { get; set; }
     public double KinematicTolerance { get; set; }
+    public double TerminalPositionToleranceMeters { get; set; }
+    public double TerminalYawToleranceRadians { get; set; }
 
     internal ValidationConfiguration Copy()
     {
@@ -11,6 +13,8 @@ public sealed class ValidationConfiguration
         {
             SpatialToleranceMeters = SpatialToleranceMeters,
             KinematicTolerance = KinematicTolerance,
+            TerminalPositionToleranceMeters = TerminalPositionToleranceMeters,
+            TerminalYawToleranceRadians = TerminalYawToleranceRadians,
         };
     }
 }