增加倒车测试

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -94,7 +94,7 @@ namespace MultiWheelC
     /// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
     /// </summary>
     [MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
-    public sealed class NewControllerStraight4mTest
+    public class NewControllerStraight4mTest
         : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -131,6 +131,12 @@ namespace MultiWheelC
         /// </summary>
         public double PointSpacingMeters = 0.02;
 
+        /// <summary>
+        /// 获取实验记录使用的轨迹基础名称，供同一套直线测试流程区分前进和倒车。
+        /// </summary>
+        protected virtual string ExperimentTrajectoryBaseName =>
+            "ProfiledStraight4m";
+
         /// <summary>
         /// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
         /// </summary>
@@ -197,7 +203,7 @@ namespace MultiWheelC
                     controllerName: "NewStanleyPid",
                     trajectoryName:
                         TrajectoryExperimentInput.BuildTrajectoryName(
-                            "ProfiledStraight4m",
+                            ExperimentTrajectoryBaseName,
                             lateralOffsetMeters),
                     trialNumber: TrialNumber,
                     referenceStart: referenceStart,
@@ -336,7 +342,11 @@ namespace MultiWheelC
             if (controller.LastCycleTiming.HasValue)
             {
                 recorder.RecordControlCycleTiming(
-                    controller.LastCycleTiming.Value);
+                    controller.LastCycleTiming.Value,
+                    requestedCommand:
+                        controller.LastRequestedCommand,
+                    sentCommand:
+                        controller.LastCommand);
             }
 
             if (controller.LastVehicleState.HasValue)
@@ -371,8 +381,13 @@ namespace MultiWheelC
                     projection.ReferencePoint.CurvaturePerMeter);
             }
 
+            var requestedCommand =
+                controller.LastRequestedCommand ??
+                controller.LastCommand.Value;
             var command = controller.LastCommand.Value;
             recorder.UpdateGcpCommand(
+                requestedCommand.FrontAngleRadians,
+                requestedCommand.RearAngleRadians,
                 command.FrontAngleRadians,
                 command.RearAngleRadians);
             var curvaturePerMeter = Math.Tan(
@@ -430,6 +445,28 @@ namespace MultiWheelC
 
     }
 
+    /// <summary>
+    /// 从当前Detour位姿开始，沿车体后方执行新版控制器4m直线倒车跟踪并保存实验数据。
+    /// </summary>
+    [MovementTest(name = "新版控制器：4m直线倒车轨迹跟踪")]
+    public sealed class NewControllerReverseStraight4mTest
+        : NewControllerStraight4mTest
+    {
+        /// <summary>
+        /// 使用负参考速度，使轨迹工厂沿车尾方向生成轨迹并触发倒车控制语义。
+        /// </summary>
+        public NewControllerReverseStraight4mTest()
+        {
+            CruiseSpeedMetersPerSecond = -0.40;
+        }
+
+        /// <summary>
+        /// 将倒车实验与前进直线实验的CSV名称明确区分。
+        /// </summary>
+        protected override string ExperimentTrajectoryBaseName =>
+            "ProfiledReverseStraight4m";
+    }
+
     /// <summary>
     /// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
     /// </summary>
@@ -702,7 +739,11 @@ namespace MultiWheelC
             if (controller.LastCycleTiming.HasValue)
             {
                 recorder.RecordControlCycleTiming(
-                    controller.LastCycleTiming.Value);
+                    controller.LastCycleTiming.Value,
+                    requestedCommand:
+                        controller.LastRequestedCommand,
+                    sentCommand:
+                        controller.LastCommand);
             }
 
             if (controller.LastVehicleState.HasValue)
@@ -737,8 +778,13 @@ namespace MultiWheelC
                     projection.ReferencePoint.CurvaturePerMeter);
             }
 
+            var requestedCommand =
+                controller.LastRequestedCommand ??
+                controller.LastCommand.Value;
             var command = controller.LastCommand.Value;
             recorder.UpdateGcpCommand(
+                requestedCommand.FrontAngleRadians,
+                requestedCommand.RearAngleRadians,
                 command.FrontAngleRadians,
                 command.RearAngleRadians);
             var curvaturePerMeter = Math.Tan(