测试后可正常执行

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -42,6 +42,16 @@ namespace MultiWheelC
         /// </summary>
         public Action<ParkingGeometricController> CycleObserver;
 
+        /// <summary>
+        /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；0表示车头方向。
+        /// </summary>
+        public double MotionDirectionInBodyRadians;
+
+        /// <summary>
+        /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
+        /// </summary>
+        public bool ReturnWheelsForwardAfterCompletion;
+
         /// <summary>
         /// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
         /// </summary>
@@ -270,7 +280,11 @@ namespace MultiWheelC
             }
 
             var wheelPreparation =
-                new PrepareWheelsForward();
+                new PrepareWheelsForward
+                {
+                    DirectionRadians =
+                        MotionDirectionInBodyRadians
+                };
             foreach (var keepRunning in wheelPreparation.Get())
             {
                 if (!keepRunning)
@@ -284,15 +298,15 @@ namespace MultiWheelC
             if (!wheelPreparation.Completed)
             {
                 throw new InvalidOperationException(
-                    "轨迹跟踪开始前舵轮未能稳定回到车头方向。");
+                    "轨迹跟踪开始前舵轮未能稳定到达目标运动方向。");
             }
 
             var adapter = new MultiWheelChassisAdapter(
                 chassis,
                 PilotDefinition.Self.CarNum);
 
-            // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
-            adapter.ResetToBodyFrame();
+            adapter.ActivateMotionFrame(
+                MotionDirectionInBodyRadians);
 
             var stateProvider =
                 StateProvider ??
@@ -333,7 +347,8 @@ namespace MultiWheelC
             var commandExecutor =
                 new GcpCommandExecutor(
                     adapter,
-                    maximumGcpAngleRateRadiansPerSecond);
+                    maximumGcpAngleRateRadiansPerSecond,
+                    MotionDirectionInBodyRadians);
 
             Controller = new ParkingGeometricController(
                 stateProvider,
@@ -350,7 +365,8 @@ namespace MultiWheelC
                 terminalApproachGainPerSecond,
                 maximumTerminalApproachSpeedMetersPerSecond,
                 stanleyCurvaturePreviewSeconds,
-                stanleyMaximumCurvaturePreviewMeters);
+                stanleyMaximumCurvaturePreviewMeters,
+                MotionDirectionInBodyRadians);
 
             var clock = Stopwatch.StartNew();
             var previousCycleSeconds =
@@ -425,6 +441,28 @@ namespace MultiWheelC
                 Controller.Cancel();
             }
 
+            if (ReturnWheelsForwardAfterCompletion)
+            {
+                var forwardPreparation =
+                    new PrepareWheelsForward();
+                foreach (var keepRunning in
+                         forwardPreparation.Get())
+                {
+                    if (!keepRunning)
+                    {
+                        break;
+                    }
+
+                    yield return true;
+                }
+
+                if (!forwardPreparation.Completed)
+                {
+                    throw new InvalidOperationException(
+                        "轨迹完成后舵轮未能稳定回到车头方向。");
+                }
+            }
+
             yield return false;
         }
 
@@ -440,6 +478,10 @@ namespace MultiWheelC
                     "新版轨迹跟踪动作没有设置Trajectory。");
             }
 
+            NumericGuard.EnsureFinite(
+                MotionDirectionInBodyRadians,
+                nameof(MotionDirectionInBodyRadians));
+
             if (double.IsNaN(executionTimeoutSeconds) ||
                 double.IsInfinity(executionTimeoutSeconds) ||
                 executionTimeoutSeconds <= 0.0)