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

MultiWheelC/Control/Allocation/GcpCommandAllocator.cs

@@ -1,6 +1,7 @@
 using System;
 using MultiWheelC.Control.Abstractions;
-
+using MyParking.Shared;
+// 限制目标角度的最大绝对值，例如不能超过60°。
 namespace MultiWheelC.Control.Allocation
 {
     /// <summary>
@@ -13,7 +14,7 @@ namespace MultiWheelC.Control.Allocation
         /// </summary>
         public GcpCommandAllocator(double maximumGcpAngleRadians)
         {
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 maximumGcpAngleRadians,
                 nameof(maximumGcpAngleRadians));
 
@@ -39,7 +40,7 @@ namespace MultiWheelC.Control.Allocation
             double speedMetersPerSecond,
             LateralControlCommand lateralCommand)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
 
@@ -68,37 +69,5 @@ namespace MultiWheelC.Control.Allocation
                 Math.Min(maximumAbsoluteValue, value));
         }
 
-        /// <summary>
-        /// 检查参数是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "GCP分配参数必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查参数或命令是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "GCP分配参数和命令必须是有限值。");
-            }
-        }
     }
 }