测试后可正常执行

MultiWheelC/Control/Abstractions/PathTrackingContext.cs

@@ -1,6 +1,7 @@
 using System;
 using MultiWheelC.StateEstimation;
 using MultiWheelC.Trajectory;
+using MyParking.Shared;
 
 namespace MultiWheelC.Control.Abstractions
 {
@@ -17,7 +18,8 @@ namespace MultiWheelC.Control.Abstractions
             TrajectoryProjection projection,
             double controlReferenceSpeedMetersPerSecond,
             double feedforwardCurvaturePerMeter,
-            double deltaTimeSeconds)
+            double deltaTimeSeconds,
+            double motionDirectionInBodyRadians = 0.0)
         {
             EnsureFinite(
                 controlReferenceSpeedMetersPerSecond,
@@ -28,6 +30,9 @@ namespace MultiWheelC.Control.Abstractions
             EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
+            EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             VehicleState = vehicleState;
             Projection = projection;
@@ -36,6 +41,9 @@ namespace MultiWheelC.Control.Abstractions
             FeedforwardCurvaturePerMeter =
                 feedforwardCurvaturePerMeter;
             DeltaTimeSeconds = deltaTimeSeconds;
+            MotionDirectionInBodyRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInBodyRadians);
         }
 
         /// <summary>
@@ -59,11 +67,18 @@ namespace MultiWheelC.Control.Abstractions
         public double ControlReferenceSpeedMetersPerSecond { get; }
 
         /// <summary>
-        /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
+        /// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。
         /// </summary>
         public double ActualLongitudinalSpeedMetersPerSecond =>
-            VehicleState.TwistInBody
-                .VxMetersPerSecond;
+            Math.Cos(MotionDirectionInBodyRadians) *
+                VehicleState.TwistInBody.VxMetersPerSecond +
+            Math.Sin(MotionDirectionInBodyRadians) *
+                VehicleState.TwistInBody.VyMetersPerSecond;
+
+        /// <summary>
+        /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
+        /// </summary>
+        public double MotionDirectionInBodyRadians { get; }
 
         /// <summary>
         /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。