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

MultiWheelC/Movements/MotionPlanExecutor.cs

@@ -2,7 +2,7 @@ using System;
 using System.Collections.Generic;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
-using MDCSToolBox.Commons.Controllers;
+using CommonUsage.Chassis;
 using MultiWheelC.Control.Execution;
 using MultiWheelC.StateEstimation;
 using MultiWheelC.Trajectory;
@@ -81,7 +81,7 @@ namespace MultiWheelC
         public IReadOnlyList<MotionPlanSegment> Segments;
 
         /// <summary>
-        /// 获取或设置所有动作段共享的车辆状态源；为空时使用Detour状态源。
+        /// 获取或设置所有动作段共享的车辆状态源；为空时组合Detour位姿与电机反馈速度。
         /// </summary>
         public IVehicleStateProvider StateProvider;
 
@@ -118,23 +118,26 @@ namespace MultiWheelC
         /// </summary>
         public override IEnumerable<bool> Get()
         {
-            if (Segments == null || Segments.Count == 0)
+            var segments = ValidateAndSnapshotSegments();
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
             {
                 throw new InvalidOperationException(
-                    "组合运动计划至少需要包含一个动作段。");
+                    "当前底盘不是MultiWheelChassis，无法执行组合运动计划。");
             }
 
             var stateProvider =
                 StateProvider ??
-                new DetourVehicleStateProvider();
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
 
             for (var index = 0;
-                 index < Segments.Count;
+                 index < segments.Count;
                  index++)
             {
-                var segment = Segments[index] ??
-                    throw new InvalidOperationException(
-                        $"组合运动计划第{index}段为空。");
+                var segment = segments[index];
 
                 SegmentStarted?.Invoke(index, segment);
 
@@ -186,7 +189,6 @@ namespace MultiWheelC
 
                 if (segment is RotateInPlaceMotionPlanSegment rotate)
                 {
-                    var config = PilotDefinition.Conf;
                     var movement =
                         new MultiWheelRotateInPlace
                         {
@@ -194,25 +196,6 @@ namespace MultiWheelC
                                 (float)AngleMath.RadiansToDegrees(
                                     rotate.TargetYawRadians),
                             StateProvider = stateProvider,
-                            PidparamsRead = () => new PIDParams
-                            {
-                                Kp = config.InPlaceRotateKp,
-                                Ki = config.InPlaceRotateKi,
-                                Kd = config.InPlaceRotateKd,
-                                DeadZone =
-                                    config.InPlaceRotateArriveDeg,
-                                SpeedAccPerSec =
-                                    config.InPlaceRotateAcc,
-                                OutputUpperThreshold =
-                                    config.InPlaceRotateMaxSpeed,
-                                MaxI = config.InPlaceRotateMaxI
-                            },
-                            MinimumAngularSpeedDegreesPerSecond =
-                                config.InPlaceRotateMinimumSpeed,
-                            WheelAlignmentToleranceDegrees =
-                                config.InPlaceRotateWheelAlignDeg,
-                            RotationTimeoutSeconds =
-                                config.InPlaceRotateTimeoutSec,
                             CommandAngularSpeedObserver =
                                 commandDegreesPerSecond =>
                                     RotationCommandObserver?.Invoke(
@@ -242,5 +225,42 @@ namespace MultiWheelC
             // 所有子动作均已完成后，才向外层DriveTask发送组合计划结束信号。
             yield return false;
         }
+
+        /// <summary>
+        /// 在车辆动作开始前验证全部动作段，并创建本次执行使用的稳定快照。
+        /// </summary>
+        private IReadOnlyList<MotionPlanSegment>
+            ValidateAndSnapshotSegments()
+        {
+            if (Segments == null || Segments.Count == 0)
+            {
+                throw new InvalidOperationException(
+                    "组合运动计划至少需要包含一个动作段。");
+            }
+
+            var segments =
+                new MotionPlanSegment[Segments.Count];
+
+            for (var index = 0;
+                 index < Segments.Count;
+                 index++)
+            {
+                var segment = Segments[index] ??
+                    throw new InvalidOperationException(
+                        $"组合运动计划第{index}段为空。");
+
+                if (!(segment is TrackMotionPlanSegment) &&
+                    !(segment is RotateInPlaceMotionPlanSegment))
+                {
+                    throw new NotSupportedException(
+                        "组合运动计划不支持动作段类型：" +
+                        $"{segment.GetType().FullName}。");
+                }
+
+                segments[index] = segment;
+            }
+
+            return segments;
+        }
     }
 }