泛化轨迹工厂参数并新增组合运动计划执行器与复合测试

MultiWheelC/Experiments/ClampTests.cs

@@ -1,90 +1,90 @@
 
 
-using System;
+// using System;
-using ClumsyCore;
+// using ClumsyCore;
-using ClumsyCore.Pilot;
+// using ClumsyCore.Pilot;
-using FundamentalLib;
+// using FundamentalLib;
-using MDCSToolBox.Clumsy.Movements;
+// using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
+// using MDCSToolBox.Clumsy.Pilot;
 
-namespace MultiWheelC
+// namespace MultiWheelC
-{
+// {
-    public abstract class ClampMovementTestBase : MovementTest
+//     public abstract class ClampMovementTestBase : MovementTest
-    {
+//     {
-        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
+//         public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
 
-        private DriveTask _task;
+//         private DriveTask _task;
-        protected abstract bool Close { get; }
+//         protected abstract bool Close { get; }
 
-        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
+//         // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
-        public override void Test()
+//         public override void Test()
-        {
+//         {
-            var leftTarget = Close
+//             var leftTarget = Close
-                ? PilotDefinition.Self.LeftArmUpperPos
+//                 ? PilotDefinition.Self.LeftArmUpperPos
-                : PilotDefinition.Self.LeftArmLowerPos;
+//                 : PilotDefinition.Self.LeftArmLowerPos;
-            var rightTarget = Close
+//             var rightTarget = Close
-                ? PilotDefinition.Self.RightArmUpperPos
+//                 ? PilotDefinition.Self.RightArmUpperPos
-                : PilotDefinition.Self.RightArmLowerPos;
+//                 : PilotDefinition.Self.RightArmLowerPos;
 
-            if (float.IsNaN(leftTarget) ||
+//             if (float.IsNaN(leftTarget) ||
-                float.IsInfinity(leftTarget) ||
+//                 float.IsInfinity(leftTarget) ||
-                float.IsNaN(rightTarget) ||
+//                 float.IsNaN(rightTarget) ||
-                float.IsInfinity(rightTarget))
+//                 float.IsInfinity(rightTarget))
-            {
+//             {
-                Console.WriteLine(
+//                 Console.WriteLine(
-                    "夹臂目标位置无效，取消夹臂运动测试。");
+//                     "夹臂目标位置无效，取消夹臂运动测试。");
-                return;
+//                 return;
-            }
+//             }
 
-            // 防止重复点击时上一项夹臂任务仍在运行。
+//             // 防止重复点击时上一项夹臂任务仍在运行。
-            TestStop();
+//             TestStop();
-            Console.WriteLine(
+//             Console.WriteLine(
-                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
+//                 $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
-                $"左目标={leftTarget}，右目标={rightTarget}");
+//                 $"左目标={leftTarget}，右目标={rightTarget}");
 
-            var task = new DriveTask(
+//             var task = new DriveTask(
-                new ClampToTarget
+//                 new ClampToTarget
-                {
+//                 {
-                    LeftClampTarget = leftTarget,
+//                     LeftClampTarget = leftTarget,
-                    RightClampTarget = rightTarget,
+//                     RightClampTarget = rightTarget,
-                    TimeoutSeconds = TimeoutSeconds
+//                     TimeoutSeconds = TimeoutSeconds
-                }.Get());
+//                 }.Get());
-            _task = task;
+//             _task = task;
 
-            try
+//             try
-            {
+//             {
-                task.Wait();
+//                 task.Wait();
-            }
+//             }
-            finally
+//             finally
-            {
+//             {
-                PilotDefinition.Self.SpeedLeftArm = 0f;
+//                 PilotDefinition.Self.SpeedLeftArm = 0f;
-                PilotDefinition.Self.SpeedRightArm = 0f;
+//                 PilotDefinition.Self.SpeedRightArm = 0f;
-                if (ReferenceEquals(_task, task))
+//                 if (ReferenceEquals(_task, task))
-                    _task = null;
+//                     _task = null;
-            }
+//             }
-        }
+//         }
 
-        // 停止夹臂任务并立即清零左右夹臂下发速度。
+//         // 停止夹臂任务并立即清零左右夹臂下发速度。
-        public override void TestStop()
+//         public override void TestStop()
-        {
+//         {
-            _task?.Stop();
+//             _task?.Stop();
-            _task = null;
+//             _task = null;
-            PilotDefinition.Self.SpeedLeftArm = 0f;
+//             PilotDefinition.Self.SpeedLeftArm = 0f;
-            PilotDefinition.Self.SpeedRightArm = 0f;
+//             PilotDefinition.Self.SpeedRightArm = 0f;
-        }
+//         }
-    }
+//     }
 
-    [MovementTest(name = "夹臂关闭测试")]
+//     [MovementTest(name = "夹臂关闭测试")]
-    public sealed class TestClampCloseMovement
+//     public sealed class TestClampCloseMovement
-        : ClampMovementTestBase
+//         : ClampMovementTestBase
-    {
+//     {
-        protected override bool Close => false;
+//         protected override bool Close => false;
-    }
+//     }
 
-    [MovementTest(name = "夹臂启动测试")]
+//     [MovementTest(name = "夹臂启动测试")]
-    public sealed class TestClampOpenMovement
+//     public sealed class TestClampOpenMovement
-        : ClampMovementTestBase
+//         : ClampMovementTestBase
-    {
+//     {
-        protected override bool Close => true;
+//         protected override bool Close => true;
-    }
+//     }
-}
+// }

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -0,0 +1,332 @@
+using System;
+using System.Drawing;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
+    /// </summary>
+    [MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
+    public sealed class CompositeStopTurnGoTest : MovementTest
+    {
+        private const float MillimetersPerMeter = 1000f;
+
+        private readonly Painter _painter =
+            UI.GetPainter("CompositeStopTurnGoTest");
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        public int TrialNumber = 1; // 重复实验编号。
+        public double StraightLengthMeters = 2.0; // 圆弧前后直线长度，单位m。
+        public double TurnRadiusMeters = 2.0; // 平滑左转名义半径，单位m。
+        public double TurnAngleDegrees = 90.0; // 含过渡段在内的总左转角度。
+        public double CurvatureTransitionLengthMeters = 0.80; // 单侧过渡长度，单位m。
+        public double InPlaceLeftTurnDegrees = 90.0; // 停车后的原地左转角度。
+        public double FinalStraightLengthMeters = 4.5; // 自转后的直线长度，单位m。
+        public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
+        public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
+        public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
+        public double DecelerationMetersPerSecondSquared = 0.12; // 参考减速度。
+        public double PointSpacingMeters = 0.02; // 离散轨迹点间距。
+
+        /// <summary>
+        /// 从当前Detour位姿构造完整计划并依次执行连续跟踪、原地自转和最终直线。
+        /// </summary>
+        public override void Test()
+        {
+            if (_task != null)
+            {
+                Console.WriteLine(
+                    "曲线-停车自转-直线组合测试已经在运行。");
+                return;
+            }
+
+            if (!MovementTestPreparation.AreWheelsForward())
+            {
+                return;
+            }
+
+            var chassis = PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行组合运动测试。");
+                return;
+            }
+
+            var stateProvider = new DetourVehicleStateProvider();
+            if (!stateProvider.TryGetState(out var initialState))
+            {
+                Console.WriteLine(
+                    "无法读取组合运动起点位姿：" +
+                    stateProvider.LastFailureReason);
+                return;
+            }
+
+            var firstTrajectory =
+                TestTrajectoryFactory
+                    .CreateStraightSmoothLeftTurnStraight(
+                        initialState.PoseInWorld,
+                        StraightLengthMeters,
+                        TurnRadiusMeters,
+                        AngleMath.DegreesToRadians(
+                            TurnAngleDegrees),
+                        CurvatureTransitionLengthMeters,
+                        StraightMaximumSpeedMetersPerSecond,
+                        CurveMaximumSpeedMetersPerSecond,
+                        AccelerationMetersPerSecondSquared,
+                        DecelerationMetersPerSecondSquared,
+                        PointSpacingMeters);
+
+            var firstStopPose =
+                firstTrajectory.EndPoint.PoseInWorld;
+            var finalStraightYawRadians =
+                AngleMath.NormalizeRadians(
+                    firstStopPose.YawRadians +
+                    AngleMath.DegreesToRadians(
+                        InPlaceLeftTurnDegrees));
+            var finalStraightStartPose =
+                new Pose2D(
+                    firstStopPose.XMeters,
+                    firstStopPose.YMeters,
+                    finalStraightYawRadians);
+            var finalTrajectory =
+                TestTrajectoryFactory.CreateStraight(
+                    finalStraightStartPose,
+                    FinalStraightLengthMeters,
+                    StraightMaximumSpeedMetersPerSecond,
+                    AccelerationMetersPerSecondSquared,
+                    DecelerationMetersPerSecondSquared,
+                    PointSpacingMeters);
+
+            DrawPlan(
+                firstTrajectory,
+                finalTrajectory,
+                firstStopPose);
+
+            var plan = new MotionPlanSegment[]
+            {
+                new TrackMotionPlanSegment(firstTrajectory)
+                {
+                    // 中间停车点允许后续原地转向和末段跟踪继续收敛位置误差。
+                    FinishDistanceMeters = 0.05,
+                    FinishSpeedMetersPerSecond = 0.03,
+                    FinishHeadingToleranceRadians =
+                        AngleMath.DegreesToRadians(3.0)
+                },
+                new RotateInPlaceMotionPlanSegment(
+                    finalStraightYawRadians),
+                new TrackMotionPlanSegment(finalTrajectory)
+            };
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "NewStanleyPidComposite",
+                trajectoryName:
+                    "SmoothTurnStopRotateStraight",
+                trialNumber: TrialNumber,
+                referenceStart: ToMillimeterVector(
+                    firstTrajectory.StartPoint.PoseInWorld),
+                referenceEnd: ToMillimeterVector(
+                    finalTrajectory.EndPoint.PoseInWorld),
+                referenceSpeed:
+                    (float)StraightMaximumSpeedMetersPerSecond,
+                sampleIntervalMs: 50,
+                referenceAccelerationMetersPerSecondSquared:
+                    (float)AccelerationMetersPerSecondSquared,
+                referenceDecelerationMetersPerSecondSquared:
+                    (float)DecelerationMetersPerSecondSquared);
+            _recorder.Start();
+
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius /
+                MillimetersPerMeter;
+            var movement = new MotionPlanExecutor
+            {
+                Segments = plan,
+                StateProvider = stateProvider,
+                ConfigureTrackingMovement = tracking =>
+                {
+                    tracking.MaximumCommandSpeedMetersPerSecond =
+                        StraightMaximumSpeedMetersPerSecond;
+                    tracking
+                        .LongitudinalSpeedErrorDeadbandMetersPerSecond =
+                        0.025;
+                },
+                SegmentStarted = (index, segment) =>
+                {
+                    _recorder?.ClearControlReference();
+                    _recorder?.UpdateCommand(0f, 0f);
+                    Console.WriteLine(
+                        $"组合运动开始第{index + 1}段：" +
+                        segment.GetType().Name);
+                },
+                TrackingCycleObserver = (index, controller) =>
+                    RecordTrackingCycle(
+                        controller,
+                        controlPointRadiusMeters),
+                RotationCommandObserver = (index, omega) =>
+                    _recorder?.UpdateCommand(
+                        0f,
+                        (float)omega)
+            };
+
+            try
+            {
+                _task = new DriveTask(movement.Get());
+                _task.Wait();
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+                _painter?.Clear();
+            }
+        }
+
+        /// <summary>
+        /// 停止组合运动、保存已有实验数据并清除计划轨迹。
+        /// </summary>
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+            _task = null;
+            _recorder = null;
+            _painter?.Clear();
+        }
+
+        /// <summary>
+        /// 将两段轨迹和中间原地自转位置绘制到Clumsy界面。
+        /// </summary>
+        private void DrawPlan(
+            Trajectory2D firstTrajectory,
+            Trajectory2D finalTrajectory,
+            Pose2D rotationPoseInWorld)
+        {
+            _painter.Clear();
+            DrawTrajectory(
+                firstTrajectory,
+                Color.DeepSkyBlue);
+            DrawTrajectory(
+                finalTrajectory,
+                Color.Gold);
+
+            var rotationPoint =
+                ToMillimeterVector(rotationPoseInWorld);
+            _painter.DrawDot(
+                Color.Magenta,
+                rotationPoint.X,
+                rotationPoint.Y,
+                10f);
+        }
+
+        /// <summary>
+        /// 绘制一段离散世界坐标系轨迹。
+        /// </summary>
+        private void DrawTrajectory(
+            Trajectory2D trajectory,
+            Color color)
+        {
+            for (var index = 0;
+                 index < trajectory.Count;
+                 index++)
+            {
+                var point = ToMillimeterVector(
+                    trajectory[index].PoseInWorld);
+                _painter.DrawDot(
+                    color,
+                    point.X,
+                    point.Y,
+                    3f);
+
+                if (index == 0)
+                {
+                    continue;
+                }
+
+                var previousPoint = ToMillimeterVector(
+                    trajectory[index - 1].PoseInWorld);
+                _painter.DrawLine(
+                    color,
+                    previousPoint.X,
+                    previousPoint.Y,
+                    point.X,
+                    point.Y,
+                    width: 2);
+            }
+        }
+
+        /// <summary>
+        /// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
+        /// </summary>
+        private void RecordTrackingCycle(
+            ParkingGeometricController controller,
+            double controlPointRadiusMeters)
+        {
+            if (controller.LastVehicleState.HasValue)
+            {
+                _recorder?.UpdateProcessedState(
+                    controller.LastVehicleState.Value);
+            }
+
+            if (!controller.LastCommand.HasValue)
+            {
+                return;
+            }
+
+            if (controller.LastProjection.HasValue &&
+                controller.LastReferenceSpeedMetersPerSecond.HasValue)
+            {
+                var projection =
+                    controller.LastProjection.Value;
+                _recorder?.UpdateControlReference(
+                    projection.ArcLengthMeters,
+                    controller
+                        .LastReferenceSpeedMetersPerSecond.Value,
+                    projection.LateralErrorMeters,
+                    projection.HeadingErrorRadians,
+                    projection.DistanceToTrajectoryMeters,
+                    projection.RemainingDistanceMeters);
+            }
+
+            var command = controller.LastCommand.Value;
+            var curvaturePerMeter = Math.Tan(
+                command.FrontAngleRadians) /
+                controlPointRadiusMeters;
+            var angularSpeedRadiansPerSecond =
+                command.SpeedMetersPerSecond *
+                curvaturePerMeter;
+            _recorder?.UpdateCommand(
+                (float)command.SpeedMetersPerSecond,
+                (float)angularSpeedRadiansPerSecond);
+        }
+
+        /// <summary>
+        /// 将米制世界位姿转换为Clumsy绘图和记录使用的毫米坐标。
+        /// </summary>
+        private static Vector2 ToMillimeterVector(
+            Pose2D poseInWorld)
+        {
+            return new Vector2(
+                (float)(poseInWorld.XMeters *
+                    MillimetersPerMeter),
+                (float)(poseInWorld.YMeters *
+                    MillimetersPerMeter));
+        }
+    }
+}

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -21,10 +21,33 @@ namespace MultiWheelC
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.20,
             double pointSpacingMeters = 0.02)
+        {
+            return CreateStraight(
+                startPoseInWorld,
+                StraightLengthMeters,
+                cruiseSpeedMetersPerSecond,
+                accelerationMetersPerSecondSquared,
+                decelerationMetersPerSecondSquared,
+                pointSpacingMeters);
+        }
+
+        /// <summary>
+        /// 从给定车体中心位姿沿当前航向生成指定长度并在终点停车的直线轨迹。
+        /// </summary>
+        public static Trajectory2D CreateStraight(
+            Pose2D startPoseInWorld,
+            double lengthMeters,
+            double cruiseSpeedMetersPerSecond = 0.30,
+            double accelerationMetersPerSecondSquared = 0.20,
+            double decelerationMetersPerSecondSquared = 0.20,
+            double pointSpacingMeters = 0.02)
         {
             EnsureFinitePose(
                 startPoseInWorld,
                 nameof(startPoseInWorld));
+            EnsureFinitePositive(
+                lengthMeters,
+                nameof(lengthMeters));
             EnsureFinitePositive(
                 cruiseSpeedMetersPerSecond,
                 nameof(cruiseSpeedMetersPerSecond));
@@ -38,7 +61,7 @@ namespace MultiWheelC
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
 
-            if (pointSpacingMeters > StraightLengthMeters)
+            if (pointSpacingMeters > lengthMeters)
             {
                 throw new ArgumentOutOfRangeException(
                     nameof(pointSpacingMeters),
@@ -46,7 +69,7 @@ namespace MultiWheelC
             }
 
             var segmentCount = (int)Math.Ceiling(
-                StraightLengthMeters /
+                lengthMeters /
                 pointSpacingMeters);
             var points = new List<TrajectoryPoint>(
                 segmentCount + 1);
@@ -59,13 +82,13 @@ namespace MultiWheelC
                  index <= segmentCount;
                  index++)
             {
-                // 均分后最后一个点严格落在4m终点，避免浮点累加越界。
+                // 均分后最后一个点严格落在指定终点，避免浮点累加越界。
                 var arcLengthMeters =
-                    StraightLengthMeters *
+                    lengthMeters *
                     index /
                     segmentCount;
                 var remainingDistanceMeters =
-                    StraightLengthMeters -
+                    lengthMeters -
                     arcLengthMeters;
                 var referenceSpeedMetersPerSecond =
                     CalculateReferenceSpeed(
@@ -105,6 +128,34 @@ namespace MultiWheelC
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.12,
             double pointSpacingMeters = 0.02)
+        {
+            return CreateStraightSmoothLeftTurnStraight(
+                startPoseInWorld,
+                straightLengthMeters,
+                turnRadiusMeters,
+                Math.PI,
+                curvatureTransitionLengthMeters,
+                straightMaximumSpeedMetersPerSecond,
+                semicircleMaximumSpeedMetersPerSecond,
+                accelerationMetersPerSecondSquared,
+                decelerationMetersPerSecondSquared,
+                pointSpacingMeters);
+        }
+
+        /// <summary>
+        /// 生成“直线、平滑左转、直线”轨迹，并使总转向角严格等于指定角度。
+        /// </summary>
+        public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
+            Pose2D startPoseInWorld,
+            double straightLengthMeters,
+            double turnRadiusMeters,
+            double turnAngleRadians,
+            double curvatureTransitionLengthMeters,
+            double straightMaximumSpeedMetersPerSecond,
+            double turnMaximumSpeedMetersPerSecond,
+            double accelerationMetersPerSecondSquared,
+            double decelerationMetersPerSecondSquared,
+            double pointSpacingMeters)
         {
             EnsureFinitePose(
                 startPoseInWorld,
@@ -115,6 +166,9 @@ namespace MultiWheelC
             EnsureFinitePositive(
                 turnRadiusMeters,
                 nameof(turnRadiusMeters));
+            EnsureFinitePositive(
+                turnAngleRadians,
+                nameof(turnAngleRadians));
             EnsureFinitePositive(
                 curvatureTransitionLengthMeters,
                 nameof(curvatureTransitionLengthMeters));
@@ -122,8 +176,8 @@ namespace MultiWheelC
                 straightMaximumSpeedMetersPerSecond,
                 nameof(straightMaximumSpeedMetersPerSecond));
             EnsureFinitePositive(
-                semicircleMaximumSpeedMetersPerSecond,
+                turnMaximumSpeedMetersPerSecond,
-                nameof(semicircleMaximumSpeedMetersPerSecond));
+                nameof(turnMaximumSpeedMetersPerSecond));
             EnsureFinitePositive(
                 accelerationMetersPerSecondSquared,
                 nameof(accelerationMetersPerSecondSquared));
@@ -134,21 +188,28 @@ namespace MultiWheelC
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
 
-            var originalSemicircleLengthMeters =
+            if (turnAngleRadians > 2.0 * Math.PI)
-                Math.PI * turnRadiusMeters;
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(turnAngleRadians),
+                    "单段平滑左转角度不能大于2π。");
+            }
+
+            var nominalTurnArcLengthMeters =
+                turnAngleRadians * turnRadiusMeters;
             var constantCurvatureLengthMeters =
-                originalSemicircleLengthMeters -
+                nominalTurnArcLengthMeters -
                 curvatureTransitionLengthMeters;
 
             if (constantCurvatureLengthMeters <= 0.0)
             {
                 throw new ArgumentOutOfRangeException(
                     nameof(curvatureTransitionLengthMeters),
-                    "曲率过渡段长度必须小于半径对应的原始半圆弧长。");
+                    "曲率过渡段长度必须小于指定转角对应的圆弧长度。");
             }
 
             // 两段平滑过渡的平均曲率均为最大曲率的一半；
-            // 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于π。
+            // 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于指定转角。
             var turnLengthMeters =
                 2.0 * curvatureTransitionLengthMeters +
                 constantCurvatureLengthMeters;
@@ -184,7 +245,7 @@ namespace MultiWheelC
                         turnStartArcLengthMeters &&
                     arcLengthMeters <=
                         turnEndArcLengthMeters
-                        ? semicircleMaximumSpeedMetersPerSecond
+                        ? turnMaximumSpeedMetersPerSecond
                         : straightMaximumSpeedMetersPerSecond;
             }
 
@@ -341,7 +402,7 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 计算180°左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
+        /// 计算平滑左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
         /// </summary>
         private static double CalculateSmoothTurnCurvature(
             double distanceInTurnMeters,

MultiWheelC/Movements/MotionPlanExecutor.cs

@@ -0,0 +1,233 @@
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using MDCSToolBox.Commons.Controllers;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 表示组合运动计划中由一种控制方式完整执行的单个动作段。
+    /// </summary>
+    public abstract class MotionPlanSegment
+    {
+    }
+
+    /// <summary>
+    /// 表示使用新版几何控制器跟踪一条连续二维轨迹的动作段。
+    /// </summary>
+    public sealed class TrackMotionPlanSegment : MotionPlanSegment
+    {
+        public TrackMotionPlanSegment(Trajectory2D trajectory)
+        {
+            Trajectory = trajectory ??
+                throw new ArgumentNullException(nameof(trajectory));
+        }
+
+        public Trajectory2D Trajectory { get; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的终点距离容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishDistanceMeters { get; set; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的停车速度容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishSpeedMetersPerSecond { get; set; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的终点航向容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishHeadingToleranceRadians { get; set; }
+    }
+
+    /// <summary>
+    /// 表示车辆停车后原地旋转到指定世界航向的动作段。
+    /// </summary>
+    public sealed class RotateInPlaceMotionPlanSegment
+        : MotionPlanSegment
+    {
+        public RotateInPlaceMotionPlanSegment(
+            double targetYawRadians)
+        {
+            if (double.IsNaN(targetYawRadians) ||
+                double.IsInfinity(targetYawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(targetYawRadians),
+                    "原地自转目标航向必须是有限值。");
+            }
+
+            TargetYawRadians =
+                AngleMath.NormalizeRadians(targetYawRadians);
+        }
+
+        public double TargetYawRadians { get; }
+    }
+
+    /// <summary>
+    /// 顺序执行连续轨迹和原地自转动作，并在动作段边界完成停车与控制器切换。
+    /// </summary>
+    public sealed class MotionPlanExecutor : MovementDefinition
+    {
+        /// <summary>
+        /// 获取或设置一次性提交并按顺序执行的组合运动计划。
+        /// </summary>
+        public IReadOnlyList<MotionPlanSegment> Segments;
+
+        /// <summary>
+        /// 获取或设置所有动作段共享的车辆状态源；为空时使用Detour状态源。
+        /// </summary>
+        public IVehicleStateProvider StateProvider;
+
+        /// <summary>
+        /// 获取或设置创建每段轨迹动作后应用参数的回调。
+        /// </summary>
+        public Action<TrajectoryTrackingMovement>
+            ConfigureTrackingMovement;
+
+        /// <summary>
+        /// 获取或设置创建每段原地自转动作后应用参数的回调。
+        /// </summary>
+        public Action<MultiWheelRotateInPlace>
+            ConfigureRotationMovement;
+
+        /// <summary>
+        /// 获取或设置动作段开始前的通知，参数依次为索引和动作段。
+        /// </summary>
+        public Action<int, MotionPlanSegment> SegmentStarted;
+
+        /// <summary>
+        /// 获取或设置轨迹段每个有效控制周期后的诊断通知。
+        /// </summary>
+        public Action<int, ParkingGeometricController>
+            TrackingCycleObserver;
+
+        /// <summary>
+        /// 获取或设置自转段角速度命令通知，角速度单位为rad/s。
+        /// </summary>
+        public Action<int, double> RotationCommandObserver;
+
+        /// <summary>
+        /// 按计划顺序执行各动作段，任一动作失败时停止后续动作。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            if (Segments == null || Segments.Count == 0)
+            {
+                throw new InvalidOperationException(
+                    "组合运动计划至少需要包含一个动作段。");
+            }
+
+            var stateProvider =
+                StateProvider ??
+                new DetourVehicleStateProvider();
+
+            for (var index = 0;
+                 index < Segments.Count;
+                 index++)
+            {
+                var segment = Segments[index] ??
+                    throw new InvalidOperationException(
+                        $"组合运动计划第{index}段为空。");
+
+                SegmentStarted?.Invoke(index, segment);
+
+                if (segment is TrackMotionPlanSegment track)
+                {
+                    var movement =
+                        new TrajectoryTrackingMovement
+                        {
+                            Trajectory = track.Trajectory,
+                            StateProvider = stateProvider,
+                            CycleObserver = controller =>
+                                TrackingCycleObserver?.Invoke(
+                                    index,
+                                    controller)
+                        };
+                    ConfigureTrackingMovement?.Invoke(movement);
+
+                    // 单段参数后应用，确保中间连接段可以覆盖组合动作的公共配置。
+                    if (track.FinishDistanceMeters.HasValue)
+                    {
+                        movement.FinishDistanceMeters =
+                            track.FinishDistanceMeters.Value;
+                    }
+
+                    if (track.FinishSpeedMetersPerSecond.HasValue)
+                    {
+                        movement.FinishSpeedMetersPerSecond =
+                            track.FinishSpeedMetersPerSecond.Value;
+                    }
+
+                    if (track.FinishHeadingToleranceRadians.HasValue)
+                    {
+                        movement.FinishHeadingToleranceRadians =
+                            track.FinishHeadingToleranceRadians.Value;
+                    }
+
+                    foreach (var keepRunning in movement.Get())
+                    {
+                        yield return keepRunning;
+                    }
+
+                    continue;
+                }
+
+                if (segment is RotateInPlaceMotionPlanSegment rotate)
+                {
+                    var config = PilotDefinition.Conf;
+                    var movement =
+                        new MultiWheelRotateInPlace
+                        {
+                            AngleTarget =
+                                (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(
+                                        index,
+                                        AngleMath.DegreesToRadians(
+                                            commandDegreesPerSecond))
+                        };
+                    ConfigureRotationMovement?.Invoke(movement);
+
+                    foreach (var keepRunning in movement.Get())
+                    {
+                        yield return keepRunning;
+                    }
+
+                    continue;
+                }
+
+                throw new NotSupportedException(
+                    $"组合运动计划不支持动作段类型：{segment.GetType().FullName}。");
+            }
+        }
+    }
+}

data_process/新版控制器轨迹测试处理/混合测试/第一次/记录.txt

@@ -0,0 +1,10 @@
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.19°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 180
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.19°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 244
+   at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 140
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+

data_process/新版控制器轨迹测试处理/直线-圆弧-直线测试/第一组-失败/记录.txt

@@ -0,0 +1,19 @@
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.328m，航向误差=8.10°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraightSemicircleStraightTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 449
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.328m，航向误差=8.10°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+最主要的问题：曲率不连续
+目前轨迹几何是：
+直线：曲率 0
+→ 瞬间进入半径2m圆弧：曲率 0.5 1/m
+→ 瞬间离开圆弧：曲率重新变成0
+位置和航向是连续的，因此轨迹不会断开；但是曲率不连续。
+
+主要问题是“直线与圆弧的曲率瞬间跳变”，而你的 GCP 转角又被限制为每秒最多变化 10°，车辆无法瞬间进入或退出半径2m的圆弧。随后 Stanley 反馈不断补偿，形成明显振荡；速度偏高又进一步放大了问题。

data_process/新版控制器轨迹测试处理/直线测试/第一组4m测试/记录.txt

@@ -0,0 +1,18 @@
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.063m，航向误差=3.91°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.063m，航向误差=3.91°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+当前 Stanley 参数与车辆转向动态不匹配，形成了低频蛇形振荡。
+数据很有代表性：
+横向误差 RMSE：约 12.3 mm
+航向误差 RMSE：约 1.98°
+航向误差范围：约 -3.24°～+4.82°
+角速度命令共明显换向约5次，不是 Detour 高频噪声
+Stanley 目标角与经过 GCP 限速后实际发送角的平均差仅约 0.26°
+所以确实是“中心贴线，但车身左右摆”。

data_process/新版控制器轨迹测试处理/直线测试/第三组4m测试/记录.txt

@@ -0,0 +1,15 @@
+误差主要集中在轨迹 s=0.7～2.3m：
+该区间：
+横向RMSE约22.4mm
+航向RMSE约1.91°
+
+s>2.5m以后：
+横向RMSE约4.58mm
+航向RMSE约0.57°
+也就是说，第三组不是持续蛇形，而是在中途发生了一次：
+车辆向一侧偏移
+→ 控制器给出修正
+→ 实际角速度响应滞后
+→ 修正稍微过头
+→ 随后重新稳定
+第三组终点成功、后半段精度很好，说明控制器没有结构性错误。纵向 PID 也暂时不用调整。

data_process/新版控制器轨迹测试处理/直线测试/第二组4m测试/记录.txt

@@ -0,0 +1,22 @@
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.048m，航向误差=0.08°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.048m，航向误差=0.08°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+第二组在终点前的情况是：
+距离终点约97mm：
+参考速度 0.197m/s
+命令速度 0.135m/s
+实际速度约0.320m/s
+
+距离终点约12mm：
+参考速度 0.053m/s
+命令速度已经为0
+实际速度仍约0.286m/s
+说明控制器已经要求停车，但车辆和 M 层速度斜坡来不及完全降速，最终越过终点约48 mm。
+这不是 Stanley 横向控制问题，而是参考速度规划的减速距离不够。

data_process/新版控制器轨迹测试处理/直线测试/第五组4m测试3次/记录.txt

@@ -0,0 +1,15 @@
+第一次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.034m，航向误差=0.43°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.034m，航向误差=0.43°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+第二次：
+
+第三次：
+

data_process/新版控制器轨迹测试处理/直线测试/第四组4m测试3次/记录.txt

@@ -0,0 +1,57 @@
+第一次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.045m，航向误差=0.51°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.045m，航向误差=0.51°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+第二次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.032m，航向误差=0.48°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.032m，航向误差=0.48°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+第三次：
+
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.07°。, stack:
+   at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+   at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+   at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.<MainPanelHandler>b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+  *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.07°。, stack:
+   at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 237
+   at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+次数	横向 RMSE	航向 RMSE	速度 RMSE	终点误差
+第1次	19.70 mm	1.42°	0.0492 m/s	45.2 mm
+第2次	6.08 mm	0.45°	0.0515 m/s	31.7 mm
+第3次	8.66 mm	0.72°	0.0540 m/s	40.7 mm
+
+第二、三次横向效果不错；第一次数值较差。
+第一次变差的主要原因
+第一次在：
+t ≈ 9.74s
+s ≈ 2.22m
+出现了一次约 33.8mm 的 Detour 横向位置突变。突变后的位置持续保持在新的坐标基准上，不是单帧尖峰。
+控制器随后正常纠偏，但由于车辆转向响应存在滞后，形成了一次明显摆动：
+Detour横向位置突然变化
+→ Stanley认为车辆偏离约30mm
+→ 给出较大转向修正
+→ 实际角速度滞后
+→ 航向和横向误差产生一次超调
+因此第一次不是 Stanley 自己无缘无故发散，而是定位突变触发了欠阻尼响应。
+
+
+