增加倒车测试

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -271,7 +271,12 @@ namespace MultiWheelC.Control.Execution
         public TrajectoryProjection? LastProjection { get; private set; }
 
         /// <summary>
-        /// 获取最近一次发送或准备发送的GCP运动命令。
+        /// 获取最近一次由控制器和分配器生成、尚未经过角速度限制的GCP运动命令。
+        /// </summary>
+        public GcpMotionCommand? LastRequestedCommand { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次经过GCP角速度限制后实际发送给底盘的运动命令。
         /// </summary>
         public GcpMotionCommand? LastCommand { get; private set; }
 
@@ -488,6 +493,7 @@ namespace MultiWheelC.Control.Execution
                 var gcpCommand = _gcpAllocator.Allocate(
                     commandSpeedMetersPerSecond,
                     lateralCommand);
+                LastRequestedCommand = gcpCommand;
 
                 controllerComputeMilliseconds =
                     GetElapsedMilliseconds(
@@ -942,6 +948,7 @@ namespace MultiWheelC.Control.Execution
             _commandExecutor.Stop();
             _lateralController.Reset();
             _longitudinalController.Reset();
+            LastRequestedCommand = null;
             LastCommand = null;
             LastFailureReason =
                 "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
@@ -956,6 +963,10 @@ namespace MultiWheelC.Control.Execution
             StopAndResetControllers();
             IsActive = false;
             IsCompleted = true;
+            LastRequestedCommand = new GcpMotionCommand(
+                0.0,
+                0.0,
+                0.0);
             LastCommand = new GcpMotionCommand(
                 0.0,
                 0.0,
@@ -974,6 +985,7 @@ namespace MultiWheelC.Control.Execution
             StopAndResetControllers();
             IsActive = false;
             IsCompleted = false;
+            LastRequestedCommand = null;
             LastCommand = null;
             LastFailureReason = reason;
             LastException = exception;
@@ -1000,6 +1012,7 @@ namespace MultiWheelC.Control.Execution
             _previousStateTimestampSeconds = 0.0;
             LastVehicleState = null;
             LastProjection = null;
+            LastRequestedCommand = null;
             LastCommand = null;
             LastControlReferenceSpeedMetersPerSecond = null;
             LastCurvaturePreviewDistanceMeters = null;