新增倒车以及项目结构优化

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -59,11 +59,6 @@ namespace MultiWheelC
                 return;
             }
 
-            if (!MovementTestPreparation.AreWheelsForward())
-            {
-                return;
-            }
-
             var chassis = PilotDefinition.Chassis as MultiWheelChassis;
             if (chassis == null)
             {
@@ -72,22 +67,18 @@ namespace MultiWheelC
                 return;
             }
 
-            var detourStateProvider =
-                new DetourVehicleStateProvider();
-            if (!detourStateProvider.TryGetState(
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(
                     out var initialState))
             {
                 Console.WriteLine(
-                    "无法读取组合运动起点位姿：" +
-                    detourStateProvider.LastFailureReason);
+                    "无法读取组合运动起点状态：" +
+                    stateProvider.LastFailureReason);
                 return;
             }
 
-            var stateProvider =
-                new WheelFeedbackVehicleStateProvider(
-                    detourStateProvider,
-                    chassis);
-
             var planStartPose =
                 TrajectoryExperimentInput.OffsetPoseLaterally(
                     initialState.PoseInWorld,
@@ -176,15 +167,6 @@ namespace MultiWheelC
             {
                 Segments = plan,
                 StateProvider = stateProvider,
-                ConfigureTrackingMovement = tracking =>
-                {
-                    tracking.StanleyUsesActualSpeed = true;
-                    tracking.MaximumCommandSpeedMetersPerSecond =
-                        StraightMaximumSpeedMetersPerSecond;
-                    tracking
-                        .LongitudinalSpeedErrorDeadbandMetersPerSecond =
-                        0.025;
-                },
                 SegmentStarted = (index, segment) =>
                 {
                     _recorder?.ClearControlReference();
@@ -330,14 +312,14 @@ namespace MultiWheelC
             }
 
             if (controller.LastProjection.HasValue &&
-                controller.LastReferenceSpeedMetersPerSecond.HasValue)
+                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
             {
                 var projection =
                     controller.LastProjection.Value;
                 _recorder?.UpdateControlReference(
                     projection.ArcLengthMeters,
                     controller
-                        .LastReferenceSpeedMetersPerSecond.Value,
+                        .LastControlReferenceSpeedMetersPerSecond.Value,
                     projection.LateralErrorMeters,
                     projection.HeadingErrorRadians,
                     projection.DistanceToTrajectoryMeters,