重构横向控制器输出前后GCP目标转角并简化命令分配器

MultiWheelC/Movements/MotionPlanExecutor.cs

@@ -173,7 +173,12 @@ namespace MultiWheelC
 
                     foreach (var keepRunning in movement.Get())
                     {
-                        yield return keepRunning;
+                        if (!keepRunning)
+                        {
+                            break;
+                        }
+
+                        yield return true;
                     }
 
                     continue;
@@ -219,7 +224,12 @@ namespace MultiWheelC
 
                     foreach (var keepRunning in movement.Get())
                     {
-                        yield return keepRunning;
+                        if (!keepRunning)
+                        {
+                            break;
+                        }
+
+                        yield return true;
                     }
 
                     continue;
@@ -228,6 +238,9 @@ namespace MultiWheelC
                 throw new NotSupportedException(
                     $"组合运动计划不支持动作段类型：{segment.GetType().FullName}。");
             }
+
+            // 所有子动作均已完成后，才向外层DriveTask发送组合计划结束信号。
+            yield return false;
         }
     }
 }

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -55,6 +55,18 @@ namespace MultiWheelC
         /// </summary>
         public bool StanleyUsesActualSpeed = true;
 
+        /// <summary>
+        /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
         /// <summary>
         /// 纵向速度外环比例增益。
         /// </summary>
@@ -163,7 +175,9 @@ namespace MultiWheelC
                     StanleyCrossTrackGainPerSecond,
                     StanleyHeadingErrorGain,
                     StanleyMinimumSpeedMetersPerSecond,
-                    StanleyUsesActualSpeed);
+                    StanleyUsesActualSpeed,
+                    MaximumCrossTrackCorrectionRadians,
+                    MaximumHeadingCorrectionRadians);
             var longitudinalController =
                 new PidLongitudinalController(
                     LongitudinalKp,
@@ -173,8 +187,7 @@ namespace MultiWheelC
                     MaximumCommandSpeedMetersPerSecond,
                     LongitudinalSpeedErrorDeadbandMetersPerSecond);
             var gcpAllocator =
-                new AckermannGcpAllocator(
-                    controlPointRadiusMeters,
+                new GcpCommandAllocator(
                     MaximumGcpAngleRadians);
             var commandExecutor =
                 new GcpCommandExecutor(