增加车队轨迹控制核心与轮组自转模式

MultiWheelC/Control/Abstractions/PathTrackingContext.cs

@@ -1,12 +1,11 @@
 using System;
-using MultiWheelC.StateEstimation;
 using MultiWheelC.Trajectory;
 using MyParking.Shared;
 
 namespace MultiWheelC.Control.Abstractions
 {
     /// <summary>
-    /// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。
+    /// 保存一次轨迹跟踪控制周期使用的刚体速度、轨迹投影和真实时间间隔。
     /// </summary>
     public readonly struct PathTrackingContext
     {
@@ -14,7 +13,8 @@ namespace MultiWheelC.Control.Abstractions
         /// 创建横向和纵向控制器共享的只读控制输入快照。
         /// </summary>
         public PathTrackingContext(
-            VehicleState vehicleState,
+            Twist2D actualTwistInBody,
+            bool hasValidVelocityEstimate,
             TrajectoryProjection projection,
             double controlReferenceSpeedMetersPerSecond,
             double feedforwardCurvaturePerMeter,
@@ -33,8 +33,13 @@ namespace MultiWheelC.Control.Abstractions
             EnsureFinite(
                 motionDirectionInBodyRadians,
                 nameof(motionDirectionInBodyRadians));
+            NumericGuard.EnsureFinite(
+                actualTwistInBody,
+                nameof(actualTwistInBody));
 
-            VehicleState = vehicleState;
+            ActualTwistInBody = actualTwistInBody;
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
             Projection = projection;
             ControlReferenceSpeedMetersPerSecond =
                 controlReferenceSpeedMetersPerSecond;
@@ -47,9 +52,9 @@ namespace MultiWheelC.Control.Abstractions
         }
 
         /// <summary>
-        /// 获取本周期经过校验的实际车辆位姿和速度状态。
+        /// 获取受控刚体坐标系下的实际速度。
         /// </summary>
-        public VehicleState VehicleState { get; }
+        public Twist2D ActualTwistInBody { get; }
 
         /// <summary>
         /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
@@ -71,9 +76,9 @@ namespace MultiWheelC.Control.Abstractions
         /// </summary>
         public double ActualLongitudinalSpeedMetersPerSecond =>
             Math.Cos(MotionDirectionInBodyRadians) *
-                VehicleState.TwistInBody.VxMetersPerSecond +
+                ActualTwistInBody.VxMetersPerSecond +
             Math.Sin(MotionDirectionInBodyRadians) *
-                VehicleState.TwistInBody.VyMetersPerSecond;
+                ActualTwistInBody.VyMetersPerSecond;
 
         /// <summary>
         /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
@@ -111,10 +116,9 @@ namespace MultiWheelC.Control.Abstractions
             Projection.RemainingDistanceMeters;
 
         /// <summary>
-        /// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。
+        /// 获取本周期实际速度估计是否可供闭环控制使用。
         /// </summary>
-        public bool HasValidVelocityEstimate =>
-            VehicleState.HasValidVelocityEstimate;
+        public bool HasValidVelocityEstimate { get; }
 
         /// <summary>
         /// 检查控制周期是否为正有限值。