完善状态估计、车队协调与舵轮辨识日志

MultiWheelC.Tests/FleetControllerTests.cs

@@ -15,13 +15,15 @@ namespace MultiWheelC.Tests
         {
             VerifyInactiveControllerStops();
             VerifyStraightCommand();
+            VerifyFortyFiveDegreeMotionDirection();
+            VerifyNinetyDegreeMotionDirection();
             VerifyInvalidVelocityUsesReferenceSpeed();
             VerifyCompletionStops();
             VerifyExcessiveTrackingErrorFaults();
             VerifyCancelStops();
 
             Console.WriteLine(
-                "FleetController车队中心控制测试通过。共6个场景。");
+                "FleetController车队中心控制测试通过。共8个场景。");
         }
 
         private static void VerifyInactiveControllerStops()
@@ -91,6 +93,56 @@ namespace MultiWheelC.Tests
                 "速度尚未初始化");
         }
 
+        private static void VerifyFortyFiveDegreeMotionDirection()
+        {
+            VerifyMotionDirection(
+                Math.PI / 4.0,
+                "45度运动方向");
+        }
+
+        private static void VerifyNinetyDegreeMotionDirection()
+        {
+            VerifyMotionDirection(
+                Math.PI / 2.0,
+                "90度运动方向");
+        }
+
+        private static void VerifyMotionDirection(
+            double motionDirectionInFleetRadians,
+            string scenario)
+        {
+            var controller = CreateController(
+                motionDirectionInFleetRadians);
+            controller.Start(
+                CreateStraightTrajectory(
+                    motionDirectionInFleetRadians));
+
+            var directionX =
+                Math.Cos(motionDirectionInFleetRadians);
+            var directionY =
+                Math.Sin(motionDirectionInFleetRadians);
+            var result = controller.ComputeCommand(
+                CreateState(
+                    0.5 * directionX,
+                    0.5 * directionY,
+                    0.4 * directionX,
+                    0.4 * directionY,
+                    true),
+                0.02,
+                out var command);
+
+            AssertResult(
+                result,
+                FleetControlCycleResult.CommandGenerated,
+                scenario);
+            AssertTwist(
+                command.TwistAtReferencePoint,
+                0.4 * directionX,
+                0.4 * directionY,
+                0.0,
+                scenario);
+        }
+
         private static void VerifyCompletionStops()
         {
             var controller = CreateController();
@@ -156,18 +208,27 @@ namespace MultiWheelC.Tests
             AssertStop(command, "取消控制");
         }
 
-        private static FleetController CreateController()
+        private static FleetController CreateController(
+            double motionDirectionInFleetRadians = 0.0)
         {
             return new FleetController(
                 new StraightLateralController(),
                 new ReferenceLongitudinalController(),
                 new GcpCommandAllocator(
                     Math.PI / 4.0),
-                virtualControlPointRadiusMeters: 0.5);
+                virtualControlPointRadiusMeters: 0.5,
+                motionDirectionInFleetRadians:
+                    motionDirectionInFleetRadians);
         }
 
-        private static Trajectory2D CreateStraightTrajectory()
+        private static Trajectory2D CreateStraightTrajectory(
+            double motionDirectionInFleetRadians = 0.0)
         {
+            var directionX =
+                Math.Cos(motionDirectionInFleetRadians);
+            var directionY =
+                Math.Sin(motionDirectionInFleetRadians);
+
             return new Trajectory2D(
                 new[]
                 {
@@ -178,7 +239,10 @@ namespace MultiWheelC.Tests
                         0.4),
                     new TrajectoryPoint(
                         1.0,
-                        new Pose2D(1.0, 0.0, 0.0),
+                        new Pose2D(
+                            directionX,
+                            directionY,
+                            0.0),
                         0.0,
                         0.4)
                 });
@@ -189,6 +253,21 @@ namespace MultiWheelC.Tests
             double yMeters,
             double vxMetersPerSecond,
             bool hasValidVelocityEstimate)
+        {
+            return CreateState(
+                xMeters,
+                yMeters,
+                vxMetersPerSecond,
+                0.0,
+                hasValidVelocityEstimate);
+        }
+
+        private static FleetState CreateState(
+            double xMeters,
+            double yMeters,
+            double vxMetersPerSecond,
+            double vyMetersPerSecond,
+            bool hasValidVelocityEstimate)
         {
             return new FleetState(
                 sampleTimestampSeconds: 1.0,
@@ -198,7 +277,7 @@ namespace MultiWheelC.Tests
                     0.0),
                 twistAtFleetOriginInWorld: new Twist2D(
                     vxMetersPerSecond,
-                    0.0,
+                    vyMetersPerSecond,
                     0.0),
                 hasValidVelocityEstimate:
                     hasValidVelocityEstimate);