完善Detour状态估计与轨迹跟踪验证

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -231,7 +231,8 @@ namespace MultiWheelC
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
                         (float)DecelerationMetersPerSecondSquared,
-                    diagnosticChassis: chassis);
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -521,7 +522,7 @@ namespace MultiWheelC
     /// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
     /// </summary>
     [MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
-    public sealed class NewControllerStraightSemicircleStraightTest
+    public class NewControllerStraightSemicircleStraightTest
         : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -579,6 +580,24 @@ namespace MultiWheelC
         /// </summary>
         public double PointSpacingMeters = 0.02;
 
+        /// <summary>
+        /// 获取组合轨迹主运动方向相对车头的夹角，单位为rad。
+        /// </summary>
+        protected virtual double MotionDirectionInBodyRadians =>
+            0.0;
+
+        /// <summary>
+        /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
+        /// </summary>
+        protected virtual bool ReturnWheelsForwardAfterCompletion =>
+            false;
+
+        /// <summary>
+        /// 获取实验记录使用的轨迹基础名称。
+        /// </summary>
+        protected virtual string ExperimentTrajectoryBaseName =>
+            "ProfiledStraightSmoothLeftTurnStraight";
+
         /// <summary>
         /// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
         /// </summary>
@@ -637,7 +656,8 @@ namespace MultiWheelC
                         SemicircleMaximumSpeedMetersPerSecond,
                         AccelerationMetersPerSecondSquared,
                         DecelerationMetersPerSecondSquared,
-                        PointSpacingMeters);
+                        PointSpacingMeters,
+                        MotionDirectionInBodyRadians);
 
             DrawTrajectory(trajectory);
 
@@ -650,7 +670,7 @@ namespace MultiWheelC
                     controllerName: "NewStanleyPid",
                     trajectoryName:
                         TrajectoryExperimentInput.BuildTrajectoryName(
-                            "ProfiledStraightSmoothLeftTurnStraight",
+                            ExperimentTrajectoryBaseName,
                             lateralOffsetMeters),
                     trialNumber: TrialNumber,
                     referenceStart: referenceStart,
@@ -658,11 +678,15 @@ namespace MultiWheelC
                     referenceSpeed:
                         (float)StraightMaximumSpeedMetersPerSecond,
                     sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
                         (float)DecelerationMetersPerSecondSquared,
-                    diagnosticChassis: chassis);
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -673,6 +697,10 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
+                    MotionDirectionInBodyRadians =
+                        MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
@@ -894,4 +922,30 @@ namespace MultiWheelC
                     MillimetersPerMeter));
         }
     }
+
+    /// <summary>
+    /// 将舵轮准备到车体左前45°，跟踪直线—左半圆—直线轨迹，并在停车后恢复车头方向。
+    /// </summary>
+    [MovementTest(name = "新版控制器：45°蟹行直线-左半圆-直线轨迹跟踪")]
+    public sealed class NewControllerCrab45StraightSemicircleStraightTest
+        : NewControllerStraightSemicircleStraightTest
+    {
+        /// <summary>
+        /// 使用车体左前45°作为组合轨迹的固定运动方向。
+        /// </summary>
+        protected override double MotionDirectionInBodyRadians =>
+            Math.PI / 4.0;
+
+        /// <summary>
+        /// 蟹行组合轨迹正常完成后主动将四个舵轮恢复到车头方向。
+        /// </summary>
+        protected override bool ReturnWheelsForwardAfterCompletion =>
+            true;
+
+        /// <summary>
+        /// 将45°蟹行组合实验与普通组合轨迹实验的CSV名称明确区分。
+        /// </summary>
+        protected override string ExperimentTrajectoryBaseName =>
+            "ProfiledCrab45StraightSmoothLeftTurnStraight";
+    }
 }