测试后可正常执行

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -86,6 +86,7 @@ namespace MultiWheelC.Control.Execution
         private readonly ILongitudinalController _longitudinalController;
         private readonly GcpCommandAllocator _gcpAllocator;
         private readonly GcpCommandExecutor _commandExecutor;
+        private readonly double _motionDirectionInBodyRadians;
 
         private Trajectory2D _trajectory;
         private double _terminalTravelDirection = 1.0;
@@ -112,7 +113,8 @@ namespace MultiWheelC.Control.Execution
             double terminalApproachGainPerSecond = 0.8,
             double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
             double curvaturePreviewSeconds = 0.20,
-            double maximumCurvaturePreviewMeters = 0.12)
+            double maximumCurvaturePreviewMeters = 0.12,
+            double motionDirectionInBodyRadians = 0.0)
         {
             _stateProvider = stateProvider ??
                 throw new ArgumentNullException(
@@ -160,6 +162,9 @@ namespace MultiWheelC.Control.Execution
             EnsureFiniteNonNegative(
                 maximumCurvaturePreviewMeters,
                 nameof(maximumCurvaturePreviewMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (terminalApproachDistanceMeters <=
                 finishDistanceMeters)
@@ -187,6 +192,9 @@ namespace MultiWheelC.Control.Execution
             CurvaturePreviewSeconds = curvaturePreviewSeconds;
             MaximumCurvaturePreviewMeters =
                 maximumCurvaturePreviewMeters;
+            _motionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -484,7 +492,8 @@ namespace MultiWheelC.Control.Execution
                     projection,
                     controlReferenceSpeedMetersPerSecond,
                     feedforwardCurvaturePerMeter,
-                    deltaTimeSeconds);
+                    deltaTimeSeconds,
+                    _motionDirectionInBodyRadians);
                 var lateralCommand =
                     _lateralController.Compute(context);
                 var commandSpeedMetersPerSecond =
@@ -617,9 +626,8 @@ namespace MultiWheelC.Control.Execution
 
             var previewSpeedMetersPerSecond =
                 vehicleState.HasValidVelocityEstimate
-                    ? Math.Abs(
-                        vehicleState.TwistInBody
-                            .VxMetersPerSecond)
+                    ? CalculateActualLongitudinalSpeedMetersPerSecond(
+                        vehicleState)
                     : Math.Abs(
                         controlReferenceSpeedMetersPerSecond);
 
@@ -931,13 +939,16 @@ namespace MultiWheelC.Control.Execution
         }
 
         /// <summary>
-        /// 计算车体坐标系实际纵向速度的绝对值，单位为m/s。
+        /// 计算车辆沿当前运动坐标系X轴实际速度的绝对值，单位为m/s。
         /// </summary>
-        private static double CalculateActualLongitudinalSpeedMetersPerSecond(
+        private double CalculateActualLongitudinalSpeedMetersPerSecond(
             VehicleState vehicleState)
         {
             return Math.Abs(
-                vehicleState.TwistInBody.VxMetersPerSecond);
+                Math.Cos(_motionDirectionInBodyRadians) *
+                    vehicleState.TwistInBody.VxMetersPerSecond +
+                Math.Sin(_motionDirectionInBodyRadians) *
+                    vehicleState.TwistInBody.VyMetersPerSecond);
         }
 
         /// <summary>