增加车队轨迹控制核心与轮组自转模式

MultiWheelC.Tests/Program.cs

@@ -1,7 +1,6 @@
 using System;
 using MultiWheelC.Control.Abstractions;
 using MultiWheelC.Control.Lateral;
-using MultiWheelC.StateEstimation;
 using MultiWheelC.Trajectory;
 using MyParking.Shared;
 
@@ -37,6 +36,7 @@ namespace MultiWheelC.Tests
             Console.WriteLine(
                 "Stanley前进/倒车横向符号测试通过。共8个场景。");
             FleetKinematicsTests.Run();
+            FleetControllerTests.Run();
         }
 
         /// <summary>
@@ -140,14 +140,10 @@ namespace MultiWheelC.Tests
             double headingErrorRadians,
             double feedforwardCurvaturePerMeter)
         {
-            var vehicleState = new VehicleState(
-                sampleTimestampSeconds: 0.0,
-                poseInWorld: Pose2D.Identity,
-                twistInWorld: new Twist2D(
-                    signedSpeedMetersPerSecond,
-                    0.0,
-                    0.0),
-                hasValidVelocityEstimate: true);
+            var actualTwistInBody = new Twist2D(
+                signedSpeedMetersPerSecond,
+                0.0,
+                0.0);
             var referencePoint = new TrajectoryPoint(
                 arcLengthMeters: 0.0,
                 poseInWorld: Pose2D.Identity,
@@ -164,7 +160,8 @@ namespace MultiWheelC.Tests
                 remainingDistanceMeters: 1.0);
 
             return new PathTrackingContext(
-                vehicleState,
+                actualTwistInBody,
+                true,
                 projection,
                 signedSpeedMetersPerSecond,
                 feedforwardCurvaturePerMeter,