控制器测试横向跟踪，增加记录

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -16,7 +16,7 @@ namespace MultiWheelC
     /// <summary>
     /// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
     /// </summary>
-    [MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
+    [MovementTest(name = "新版控制器：直线-圆弧-折线组合测试")]
     public sealed class CompositeStopTurnGoTest : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -52,6 +52,13 @@ namespace MultiWheelC
                 return;
             }
 
+            if (!TrajectoryExperimentInput
+                    .TryReadLateralOffsetMeters(
+                        out var lateralOffsetMeters))
+            {
+                return;
+            }
+
             if (!MovementTestPreparation.AreWheelsForward())
             {
                 return;
@@ -65,19 +72,30 @@ namespace MultiWheelC
                 return;
             }
 
-            var stateProvider = new DetourVehicleStateProvider();
-            if (!stateProvider.TryGetState(out var initialState))
+            var detourStateProvider =
+                new DetourVehicleStateProvider();
+            if (!detourStateProvider.TryGetState(
+                    out var initialState))
             {
                 Console.WriteLine(
                     "无法读取组合运动起点位姿：" +
-                    stateProvider.LastFailureReason);
+                    detourStateProvider.LastFailureReason);
                 return;
             }
 
+            var stateProvider =
+                new WheelFeedbackVehicleStateProvider(
+                    detourStateProvider,
+                    chassis);
+
+            var planStartPose =
+                TrajectoryExperimentInput.OffsetPoseLaterally(
+                    initialState.PoseInWorld,
+                    lateralOffsetMeters);
             var firstTrajectory =
                 TestTrajectoryFactory
                     .CreateStraightSmoothLeftTurnStraight(
-                        initialState.PoseInWorld,
+                        planStartPose,
                         StraightLengthMeters,
                         TurnRadiusMeters,
                         AngleMath.DegreesToRadians(
@@ -133,7 +151,9 @@ namespace MultiWheelC
             _recorder = new TrackingExperimentRecorder(
                 controllerName: "NewStanleyPidComposite",
                 trajectoryName:
-                    "SmoothTurnStopRotateStraight",
+                    TrajectoryExperimentInput.BuildTrajectoryName(
+                        "SmoothTurnStopRotateStraight",
+                        lateralOffsetMeters),
                 trialNumber: TrialNumber,
                 referenceStart: ToMillimeterVector(
                     firstTrajectory.StartPoint.PoseInWorld),
@@ -145,7 +165,8 @@ namespace MultiWheelC
                 referenceAccelerationMetersPerSecondSquared:
                     (float)AccelerationMetersPerSecondSquared,
                 referenceDecelerationMetersPerSecondSquared:
-                    (float)DecelerationMetersPerSecondSquared);
+                    (float)DecelerationMetersPerSecondSquared,
+                diagnosticChassis: chassis);
             _recorder.Start();
 
             var controlPointRadiusMeters =
@@ -157,6 +178,7 @@ namespace MultiWheelC
                 StateProvider = stateProvider,
                 ConfigureTrackingMovement = tracking =>
                 {
+                    tracking.StanleyUsesActualSpeed = true;
                     tracking.MaximumCommandSpeedMetersPerSecond =
                         StraightMaximumSpeedMetersPerSecond;
                     tracking
@@ -166,6 +188,7 @@ namespace MultiWheelC
                 SegmentStarted = (index, segment) =>
                 {
                     _recorder?.ClearControlReference();
+                    _recorder?.ClearGcpCommand();
                     _recorder?.UpdateCommand(0f, 0f);
                     Console.WriteLine(
                         $"组合运动开始第{index + 1}段：" +
@@ -174,7 +197,8 @@ namespace MultiWheelC
                 TrackingCycleObserver = (index, controller) =>
                     RecordTrackingCycle(
                         controller,
-                        controlPointRadiusMeters),
+                        controlPointRadiusMeters,
+                        stateProvider),
                 RotationCommandObserver = (index, omega) =>
                     _recorder?.UpdateCommand(
                         0f,
@@ -276,7 +300,8 @@ namespace MultiWheelC
         /// </summary>
         private void RecordTrackingCycle(
             ParkingGeometricController controller,
-            double controlPointRadiusMeters)
+            double controlPointRadiusMeters,
+            WheelFeedbackVehicleStateProvider stateProvider)
         {
             if (controller.LastVehicleState.HasValue)
             {
@@ -284,6 +309,21 @@ namespace MultiWheelC
                     controller.LastVehicleState.Value);
             }
 
+            if (stateProvider.TryGetLatestVelocityDiagnostics(
+                    out var detourBodyVx,
+                    out var detourVelocityValid,
+                    out var rawWheelBodyVx,
+                    out var filteredWheelBodyVx,
+                    out var wheelVelocityValid))
+            {
+                _recorder?.UpdateVelocityDiagnostics(
+                    detourBodyVx,
+                    detourVelocityValid,
+                    rawWheelBodyVx,
+                    filteredWheelBodyVx,
+                    wheelVelocityValid);
+            }
+
             if (!controller.LastCommand.HasValue)
             {
                 return;
@@ -305,6 +345,9 @@ namespace MultiWheelC
             }
 
             var command = controller.LastCommand.Value;
+            _recorder?.UpdateGcpCommand(
+                command.FrontAngleRadians,
+                command.RearAngleRadians);
             var curvaturePerMeter = Math.Tan(
                 command.FrontAngleRadians) /
                 controlPointRadiusMeters;