备份

Shared/Chassis/MultiWheelChassisAdapter.cs

@@ -11,7 +11,6 @@ namespace MyParking.Shared
     public sealed class MultiWheelChassisAdapter
     {
         #region 辅助内容
-        private const double RadiansToDegrees = 180.0 / Math.PI;
         private const float BiasTolerance = 0.001f;
         private readonly MultiWheelChassis _chassis;
         /// <summary>
@@ -44,15 +43,12 @@ namespace MyParking.Shared
             get => _chassis.SteeringAlignmentSigmaDegrees;
             set
             {
-                if (double.IsNaN(value) ||
-                    double.IsInfinity(value) ||
-                    value <= 0.0 ||
-                    value > float.MaxValue)
-                {
-                    throw new ArgumentOutOfRangeException(
-                        nameof(value),
-                        "Steering alignment sigma must be a positive finite value.");
-                }
+                NumericGuard.EnsureFinitePositive(
+                    value,
+                    nameof(value));
+                EnsureRepresentableAsSingle(
+                    value,
+                    nameof(value));
 
                 _chassis.SteeringAlignmentSigmaDegrees =
                     (float)value;
@@ -74,18 +70,18 @@ namespace MyParking.Shared
         private void EnsureMotionFrameIsActive(
             double motionDirectionRadians)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
 
             var expectedBiasDegrees =
-                (float)(
-                    -FrameTransform2D.NormalizeAngle(
-                        motionDirectionRadians) *
-                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    -AngleMath.NormalizeRadians(
+                        motionDirectionRadians),
+                    nameof(motionDirectionRadians));
             var bias = _chassis.GetOriginBias();
             var angleErrorDegrees =
-                NormalizeDegrees(
+                AngleMath.NormalizeDegrees(
                     bias.Z - expectedBiasDegrees);
 
             if (Math.Abs(bias.X) <= BiasTolerance &&
@@ -102,46 +98,32 @@ namespace MyParking.Shared
                 $"期望Th={expectedBiasDegrees}°。");
         }
 
-        /// <summary>
-        /// 将角度归一化到[-180°,180°]附近。
-        /// </summary>
-        private static float NormalizeDegrees(float degrees)
-        {
-            return (float)(
-                degrees -
-                Math.Round(degrees / 360.0) * 360.0);
-        }
         /// <summary>
         /// 检查底盘命令是否包含无效数值。
         /// </summary>
         private static void ValidateTwist(Twist2D twist)
         {
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 twist.VxMetersPerSecond,
                 nameof(twist.VxMetersPerSecond));
 
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 twist.VyMetersPerSecond,
                 nameof(twist.VyMetersPerSecond));
 
-            ValidateFinite(
-                twist.OmegaRadiansPerSecond,
+            EnsureRepresentableAsSingle(
+                AngleMath.RadiansToDegrees(
+                    twist.OmegaRadiansPerSecond),
                 nameof(twist.OmegaRadiansPerSecond));
         }
         /// <summary>
-        /// 检查数值是否为有限值。
+        /// 检查数值是否为有限值且可安全转换为float。
         /// </summary>
-        private static void ValidateFinite(
+        private static void EnsureRepresentableAsSingle(
             double value,
             string parameterName)
         {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "底盘速度命令不能是NaN或无穷大。");
-            }
+            NumericGuard.EnsureFinite(value, parameterName);
 
             if (value > float.MaxValue ||
                 value < -float.MaxValue)
@@ -152,6 +134,21 @@ namespace MyParking.Shared
             }
         }
 
+        /// <summary>
+        /// 将有限弧度值转换为float可表示的角度值。
+        /// </summary>
+        private static float ConvertRadiansToSingleDegrees(
+            double angleRadians,
+            string parameterName)
+        {
+            var angleDegrees =
+                AngleMath.RadiansToDegrees(angleRadians);
+            EnsureRepresentableAsSingle(
+                angleDegrees,
+                parameterName);
+            return (float)angleDegrees;
+        }
+
         /// <summary>
         /// 获取最近一次底盘运动分解失败原因。
         /// </summary>
@@ -175,15 +172,15 @@ namespace MyParking.Shared
         public void ActivateMotionFrame(
             double motionDirectionRadians)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
 
             var biasDegrees =
-                (float)(
-                    -FrameTransform2D.NormalizeAngle(
-                        motionDirectionRadians) *
-                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    -AngleMath.NormalizeRadians(
+                        motionDirectionRadians),
+                    nameof(motionDirectionRadians));
             var currentBias =
                 _chassis.GetOriginBias();
 
@@ -192,7 +189,7 @@ namespace MyParking.Shared
                 Math.Abs(currentBias.Y) <=
                     BiasTolerance &&
                 Math.Abs(
-                    NormalizeDegrees(
+                    AngleMath.NormalizeDegrees(
                         currentBias.Z -
                         biasDegrees)) <=
                     BiasTolerance)
@@ -284,9 +281,9 @@ namespace MyParking.Shared
             var vyMetersPerSecond =
                 (float)command.BodyTwist.VyMetersPerSecond;
             var omegaDegreesPerSecond =
-                (float)(
-                    command.BodyTwist.OmegaRadiansPerSecond *
-                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    command.BodyTwist.OmegaRadiansPerSecond,
+                    nameof(command.BodyTwist.OmegaRadiansPerSecond));
             var success = _chassis.SendXYThSpeed(
                                 vxMetersPerSecond,
                                 vyMetersPerSecond,
@@ -312,13 +309,13 @@ namespace MyParking.Shared
             double steeringRadians,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 steeringRadians,
                 nameof(steeringRadians));
             EnsureMotionFrameIsActive(
@@ -333,9 +330,9 @@ namespace MyParking.Shared
             }
 
             var steeringDegrees =
-                (float)(
-                    steeringRadians *
-                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    steeringRadians,
+                    nameof(steeringRadians));
             var success =
                 _chassis.SendMotion(
                     (float)speedMetersPerSecond,
@@ -360,13 +357,13 @@ namespace MyParking.Shared
             double rearAngleRadians,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 frontAngleRadians,
                 nameof(frontAngleRadians));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 rearAngleRadians,
                 nameof(rearAngleRadians));
             EnsureBodyFrameIsActive();
@@ -383,10 +380,12 @@ namespace MyParking.Shared
 
             var success = _chassis.SendMotion(
                 (float)speedMetersPerSecond,
-                (float)(frontAngleRadians *
-                        RadiansToDegrees),
-                (float)(rearAngleRadians *
-                        RadiansToDegrees),
+                ConvertRadiansToSingleDegrees(
+                    frontAngleRadians,
+                    nameof(frontAngleRadians)),
+                ConvertRadiansToSingleDegrees(
+                    rearAngleRadians,
+                    nameof(rearAngleRadians)),
                 interval);
 
             if (!success)
@@ -422,8 +421,11 @@ namespace MyParking.Shared
             double directionRadians)
         {
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
-                                    RadiansToDegrees);
+            var targetDegrees =
+                ConvertRadiansToSingleDegrees(
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
 #pragma warning disable CS0612, CS0618
             var wheels = _chassis.GetSteerWheels();
@@ -463,23 +465,21 @@ namespace MyParking.Shared
             double directionRadians,
             double toleranceRadians)
         {
-            if (double.IsNaN(toleranceRadians) ||
-        double.IsInfinity(toleranceRadians) ||
-        toleranceRadians < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "舵轮到位容差必须是非负有限值。");
-            }
+            NumericGuard.EnsureFiniteNonNegative(
+                toleranceRadians,
+                nameof(toleranceRadians));
 
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(
-                FrameTransform2D.NormalizeAngle(directionRadians) *
-                180.0 / Math.PI);
+            var targetDegrees =
+                ConvertRadiansToSingleDegrees(
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
-            var toleranceDegrees = (float)(
-                Math.Abs(toleranceRadians) *
-                180.0 / Math.PI);
+            var toleranceDegrees =
+                ConvertRadiansToSingleDegrees(
+                    toleranceRadians,
+                    nameof(toleranceRadians));
 
 #pragma warning disable CS0612, CS0618
             var wheels = _chassis.GetSteerWheels();
@@ -507,16 +507,12 @@ namespace MyParking.Shared
             TimeSpan? interval = null,
             double alignmentToleranceDegrees = 2.0)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFiniteNonNegative(
+                alignmentToleranceDegrees,
+                nameof(alignmentToleranceDegrees));
+            EnsureRepresentableAsSingle(
                 alignmentToleranceDegrees,
                 nameof(alignmentToleranceDegrees));
-
-            if (alignmentToleranceDegrees < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(alignmentToleranceDegrees),
-                    "自转舵轮到位容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
 
@@ -540,23 +536,18 @@ namespace MyParking.Shared
             double toleranceRadians =
                 2.0 * Math.PI / 180.0)
         {
-            if (double.IsNaN(toleranceRadians) ||
-                double.IsInfinity(toleranceRadians) ||
-                toleranceRadians < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "自转状态交接容差必须是非负有限值。");
-            }
+            NumericGuard.EnsureFiniteNonNegative(
+                toleranceRadians,
+                nameof(toleranceRadians));
 
             EnsureBodyFrameIsActive();
 
             var success =
                 _chassis
                     .AdoptPreparedRotateWheelsForXYTh(
-                        (float)(
-                            toleranceRadians *
-                            RadiansToDegrees));
+                        ConvertRadiansToSingleDegrees(
+                            toleranceRadians,
+                            nameof(toleranceRadians)));
 
             if (!success)
             {

Shared/Mathematics/FrameTransform2D.cs

@@ -9,27 +9,6 @@ namespace MyParking.Shared
     /// </summary>
     public static class FrameTransform2D
     {
-        /// <summary>
-        /// 将角度归一化到[-π, π)范围。
-        /// </summary>
-        public static double NormalizeAngle(double angleRadians)
-        {
-            return AngleMath.NormalizeRadians(angleRadians);
-        }
-
-        /// <summary>
-        /// 计算从current到target的最短角度差。
-        /// 返回正值表示逆时针旋转。
-        /// </summary>
-        public static double ShortestAngleDifference(
-            double targetRadians,
-            double currentRadians)
-        {
-            return AngleMath.ShortestDifferenceRadians(
-                targetRadians,
-                currentRadians);
-        }
-
         /// <summary>
         /// 将源坐标系中的点变换到目标坐标系。
         /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
@@ -106,7 +85,7 @@ namespace MyParking.Shared
             return new Pose2D(
                 childPositionInParent.XMeters,
                 childPositionInParent.YMeters,
-                NormalizeAngle(
+                AngleMath.NormalizeRadians(
                     parentFromMiddle.YawRadians +
                     middleFromChild.YawRadians));
         }
@@ -127,7 +106,7 @@ namespace MyParking.Shared
                 sin * childPoseInParent.XMeters -
                 cos * childPoseInParent.YMeters,
 
-                NormalizeAngle(
+                AngleMath.NormalizeRadians(
                     -childPoseInParent.YawRadians));
         }
 

Shared/Models/ChassisCommand.cs

@@ -81,36 +81,6 @@ namespace MyParking.Shared
             new Twist2D(0.0, 0.0, 0.0);
     }
 
-    /// <summary>
-    /// 发送给单辆车的车体坐标系速度命令。
-    /// </summary>
-    public readonly struct ChassisCommand
-    {
-        public ChassisCommand(
-            int vehicleId,
-            Twist2D bodyTwist)
-        {
-            VehicleId = vehicleId;
-            BodyTwist = bodyTwist;
-        }
-
-        public int VehicleId { get; }
-
-        /// <summary>
-        /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
-        /// </summary>
-        public Twist2D BodyTwist { get; }
-
-        /// <summary>
-        /// 创建指定车辆的停止命令。
-        /// </summary>
-        public static ChassisCommand Stop(int vehicleId)
-        {
-            return new ChassisCommand(
-                vehicleId,
-                Twist2D.Zero);
-        }
-    }
 
     /// <summary>
     /// 单辆车的车体坐标系在车队坐标系中的位姿。

Shared/Validation/NumericGuard.cs

@@ -7,6 +7,15 @@ namespace MyParking.Shared
     /// </summary>
     public static class NumericGuard
     {
+        /// <summary>
+        /// 判断指定浮点数是否既不是NaN也不是无穷大。
+        /// </summary>
+        public static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+
         /// <summary>
         /// 确保指定浮点数不是NaN或无穷大。
         /// </summary>
@@ -14,8 +23,7 @@ namespace MyParking.Shared
             double value,
             string parameterName)
         {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
+            if (!IsFinite(value))
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
@@ -64,17 +72,31 @@ namespace MyParking.Shared
             Pose2D pose,
             string parameterName)
         {
-            if (double.IsNaN(pose.XMeters) ||
-                double.IsInfinity(pose.XMeters) ||
-                double.IsNaN(pose.YMeters) ||
-                double.IsInfinity(pose.YMeters) ||
-                double.IsNaN(pose.YawRadians) ||
-                double.IsInfinity(pose.YawRadians))
+            if (!IsFinite(pose.XMeters) ||
+                !IsFinite(pose.YMeters) ||
+                !IsFinite(pose.YawRadians))
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
                     "二维位姿必须由有限值组成。");
             }
         }
+
+        /// <summary>
+        /// 确保二维刚体速度的两个线速度分量和角速度均为有限值。
+        /// </summary>
+        public static void EnsureFinite(
+            Twist2D twist,
+            string parameterName)
+        {
+            if (!IsFinite(twist.VxMetersPerSecond) ||
+                !IsFinite(twist.VyMetersPerSecond) ||
+                !IsFinite(twist.OmegaRadiansPerSecond))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "二维刚体速度必须由有限值组成。");
+            }
+        }
     }
 }