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

MultiWheelC/Experiments/RotationTests.cs

@@ -7,10 +7,12 @@ using System.Threading;
 using ClumsyCore;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.Movements;
 using MDCSToolBox.Clumsy.Pilot;
 using MyParking.Shared;
+using MultiWheelC.StateEstimation;
 
 namespace MultiWheelC
 {
@@ -66,6 +68,26 @@ namespace MultiWheelC
                 return;
             }
 
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
+                return;
+            }
+
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(out _))
+            {
+                Console.WriteLine(
+                    "无法读取原地旋转起点状态：" +
+                    stateProvider.LastFailureReason);
+                return;
+            }
+
             var rotationCenter =
                 new Vector2((float)location.x, (float)location.y);
             var targetWorldAngle =
@@ -98,7 +120,9 @@ namespace MultiWheelC
                 referenceSpeed: 0f,
                 referenceAngularSpeed:
                     (float)AngleMath.DegreesToRadians(
-                        config.InPlaceRotateMaxSpeed));
+                        config.InPlaceRotateMaxSpeed),
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
             _recorder.Start();
 
             try
@@ -108,6 +132,8 @@ namespace MultiWheelC
                     {
                         // MultiWheelRotateInPlace接收世界坐标系绝对航向。
                         AngleTarget = targetWorldAngle,
+                        Chassis = chassis,
+                        StateProvider = stateProvider,
                         CommandAngularSpeedObserver =
                             commandAngularSpeed =>
                                 _recorder?.UpdateCommand(
@@ -175,6 +201,26 @@ namespace MultiWheelC
                 return;
             }
 
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
+                return;
+            }
+
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(out _))
+            {
+                Console.WriteLine(
+                    "无法读取原地旋转起点状态：" +
+                    stateProvider.LastFailureReason);
+                return;
+            }
+
             if (Math.Abs(relativeAngleDegrees) < 1e-3f)
             {
                 Console.WriteLine("旋转角度不能为0，测试已经取消。");