增加车队布局与刚体速度分配基础

2026-08-20 17:43:02 +08:00
parent 0d5539e595
commit fea2265e2d
30 changed files with 404 additions and 121 deletions
@@ -1 +1,76 @@
-// 把车队整体速度分解为每辆车的局部速度
+using System;
+using System.Collections.Generic;
+
+namespace MyParking.Shared
+{
+    // 纯数学地把车队参考点速度分解成各成员车体系中的刚体速度。
+    public static class FleetKinematics
+    {
+        public static IReadOnlyList<FleetMemberCommand> Decompose(
+            FleetLayout layout,
+            FleetMotionCommand command)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            EnsureCommandIsFinite(command);
+
+            var commands =
+                new FleetMemberCommand[layout.VehicleCount];
+            var referencePoint = command.ReferencePointInFleet;
+            var referenceTwist = command.TwistAtReferencePoint;
+
+            for (var index = 0;
+                 index < layout.VehicleCount;
+                 index++)
+            {
+                var vehicle = layout.Vehicles[index];
+                var offsetXMeters =
+                    vehicle.PoseInFleet.XMeters -
+                    referencePoint.XMeters;
+                var offsetYMeters =
+                    vehicle.PoseInFleet.YMeters -
+                    referencePoint.YMeters;
+
+                var twistAtVehicleInFleet = new Twist2D(
+                    referenceTwist.VxMetersPerSecond -
+                    referenceTwist.OmegaRadiansPerSecond *
+                    offsetYMeters,
+                    referenceTwist.VyMetersPerSecond +
+                    referenceTwist.OmegaRadiansPerSecond *
+                    offsetXMeters,
+                    referenceTwist.OmegaRadiansPerSecond);
+
+                var fleetPoseInVehicle =
+                    FrameTransform2D.Inverse(
+                        vehicle.PoseInFleet);
+                var twistInVehicleBody =
+                    FrameTransform2D.TransformTwistAtSamePoint(
+                        fleetPoseInVehicle,
+                        twistAtVehicleInFleet);
+
+                commands[index] = new FleetMemberCommand(
+                    vehicle.VehicleId,
+                    twistInVehicleBody);
+            }
+
+            return Array.AsReadOnly(commands);
+        }
+
+        private static void EnsureCommandIsFinite(
+            FleetMotionCommand command)
+        {
+            NumericGuard.EnsureFinite(
+                command.ReferencePointInFleet.XMeters,
+                nameof(command.ReferencePointInFleet));
+            NumericGuard.EnsureFinite(
+                command.ReferencePointInFleet.YMeters,
+                nameof(command.ReferencePointInFleet));
+            NumericGuard.EnsureFinite(
+                command.TwistAtReferencePoint,
+                nameof(command.TwistAtReferencePoint));
+        }
+    }
+}
@@ -0,0 +1,76 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MyParking.Shared
+{
+    // 编队固定布局快照，车队坐标系原点就是编队参考中心。
+    public sealed class FleetLayout
+    {
+        private readonly ReadOnlyCollection<VehicleLayout> _vehicles;
+
+        public FleetLayout(IReadOnlyList<VehicleLayout> vehicles)
+        {
+            if (vehicles == null)
+            {
+                throw new ArgumentNullException(nameof(vehicles));
+            }
+
+            if (vehicles.Count == 0)
+            {
+                throw new ArgumentException(
+                    "编队布局至少需要包含一辆车。",
+                    nameof(vehicles));
+            }
+
+            var snapshot = new VehicleLayout[vehicles.Count];
+            var vehicleIds = new HashSet<int>();
+
+            for (var index = 0; index < vehicles.Count; index++)
+            {
+                var vehicle = vehicles[index];
+                if (vehicle.VehicleId <= 0)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(vehicles),
+                        $"第{index}辆车的编号必须大于零。");
+                }
+
+                if (!vehicleIds.Add(vehicle.VehicleId))
+                {
+                    throw new ArgumentException(
+                        $"编队布局包含重复车号{vehicle.VehicleId}。",
+                        nameof(vehicles));
+                }
+
+                NumericGuard.EnsureFinite(
+                    vehicle.PoseInFleet,
+                    $"{nameof(vehicles)}[{index}].{nameof(VehicleLayout.PoseInFleet)}");
+                snapshot[index] = vehicle;
+            }
+
+            _vehicles = Array.AsReadOnly(snapshot);
+        }
+
+        public IReadOnlyList<VehicleLayout> Vehicles => _vehicles;
+
+        public int VehicleCount => _vehicles.Count;
+
+        public bool TryGetVehicle(
+            int vehicleId,
+            out VehicleLayout vehicle)
+        {
+            for (var index = 0; index < _vehicles.Count; index++)
+            {
+                if (_vehicles[index].VehicleId == vehicleId)
+                {
+                    vehicle = _vehicles[index];
+                    return true;
+                }
+            }
+
+            vehicle = default;
+            return false;
+        }
+    }
+}
@@ -0,0 +1,67 @@
+namespace MyParking.Shared
+{
+    // 单车车体系在车队坐标系中的固定位姿。
+    public readonly struct VehicleLayout
+    {
+        public VehicleLayout(int vehicleId, Pose2D poseInFleet)
+        {
+            VehicleId = vehicleId;
+            PoseInFleet = poseInFleet;
+        }
+
+        public int VehicleId { get; }
+
+        public Pose2D PoseInFleet { get; }
+    }
+
+    // 车队参考点及该点在车队坐标系中表达的刚体速度。
+    public readonly struct FleetMotionCommand
+    {
+        public FleetMotionCommand(
+            Point2D referencePointInFleet,
+            Twist2D twistAtReferencePoint)
+        {
+            ReferencePointInFleet = referencePointInFleet;
+            TwistAtReferencePoint = twistAtReferencePoint;
+        }
+
+        public Point2D ReferencePointInFleet { get; }
+
+        public Twist2D TwistAtReferencePoint { get; }
+
+        public static FleetMotionCommand RotateAround(
+            Point2D rotationCenterInFleet,
+            double omegaRadiansPerSecond)
+        {
+            return new FleetMotionCommand(
+                rotationCenterInFleet,
+                new Twist2D(
+                    0.0,
+                    0.0,
+                    omegaRadiansPerSecond));
+        }
+
+        public static FleetMotionCommand Stop()
+        {
+            return new FleetMotionCommand(
+                Point2D.Zero,
+                Twist2D.Zero);
+        }
+    }
+
+    // 分配给指定车辆、在该车车体系中表达的刚体速度。
+    public readonly struct FleetMemberCommand
+    {
+        public FleetMemberCommand(
+            int vehicleId,
+            Twist2D twistInVehicleBody)
+        {
+            VehicleId = vehicleId;
+            TwistInVehicleBody = twistInVehicleBody;
+        }
+
+        public int VehicleId { get; }
+
+        public Twist2D TwistInVehicleBody { get; }
+    }
+}
@@ -0,0 +1 @@
+// 只定义无线报文的数据结构、版本和字段
@@ -1,15 +1,7 @@
-// 纯数据层：只描述坐标、速度和命令
-// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
-// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
-// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
-// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
-
-// 共享的二维运动模型，不包含车辆路由或底盘执行策略。
+// 使用SI单位；车体系X向前、Y向左、逆时针为正，XxxInYyy表示Xxx在Yyy坐标系中。
 namespace MyParking.Shared
 {
-    /// <summary>
-    /// 二维坐标点，X、Y单位均为米。
-    /// </summary>
+    // 二维坐标点，单位为m。
    public readonly struct Point2D
    {
        public Point2D(double xMeters, double yMeters)
@@ -25,11 +17,7 @@ namespace MyParking.Shared
        public static Point2D Zero => new Point2D(0.0, 0.0);
    }

-    /// <summary>
-    /// 二维局部坐标系在父坐标系中的位姿。
-    /// 位置单位为米，朝向单位为弧度，逆时针为正。
-    /// 具体父子关系由变量名称说明，例如RadarPoseInBody。
-    /// </summary>
+    // 二维位姿，位置单位为m，航向单位为rad。
    public readonly struct Pose2D
    {
        public Pose2D(
@@ -55,11 +43,7 @@ namespace MyParking.Shared
            new Pose2D(0.0, 0.0, 0.0);
    }

-    /// <summary>
-    /// 二维刚体速度。
-    /// 线速度单位为m/s，角速度单位为rad/s。
-    /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
-    /// </summary>
+    // 二维刚体速度，线速度单位为m/s，角速度单位为rad/s。
    public readonly struct Twist2D
    {
        public Twist2D(
@@ -81,74 +65,4 @@ namespace MyParking.Shared
        public static Twist2D Zero =>
            new Twist2D(0.0, 0.0, 0.0);
    }
-
-
-    /// <summary>
-    /// 单辆车的车体坐标系在车队坐标系中的位姿。
-    /// </summary>
-    public readonly struct VehicleLayout
-    {
-        public VehicleLayout(
-            int vehicleId,
-            Pose2D poseInFleet)
-        {
-            VehicleId = vehicleId;
-            PoseInFleet = poseInFleet;
-        }
-
-        public int VehicleId { get; }
-
-        public Pose2D PoseInFleet { get; }
-    }
-
-    /// <summary>
-    /// 车队整体运动命令，速度分量均在车队坐标系中表达。
-    /// </summary>
-    public readonly struct FleetMotionCommand
-    {
-        public FleetMotionCommand(
-            Point2D referencePointInFleet,
-            Twist2D twistAtReferencePoint)
-        {
-            ReferencePointInFleet = referencePointInFleet;
-            TwistAtReferencePoint = twistAtReferencePoint;
-        }
-
-        /// <summary>
-        /// 速度命令对应的参考点，也可作为自定义旋转中心。
-        /// </summary>
-        public Point2D ReferencePointInFleet { get; }
-
-        /// <summary>
-        /// 参考点处的车队速度。
-        /// </summary>
-        public Twist2D TwistAtReferencePoint { get; }
-
-        /// <summary>
-        /// 创建绕指定中心原地旋转的车队命令。
-        /// </summary>
-        public static FleetMotionCommand RotateAround(
-            Point2D rotationCenterInFleet,
-            double omegaRadiansPerSecond)
-        {
-            return new FleetMotionCommand(
-                rotationCenterInFleet,
-                new Twist2D(
-                    0.0,
-                    0.0,
-                    omegaRadiansPerSecond));
-        }
-
-        /// <summary>
-        /// 创建车队停止命令。
-        /// </summary>
-        public static FleetMotionCommand Stop()
-        {
-            return new FleetMotionCommand(
-                Point2D.Zero,
-                Twist2D.Zero);
-        }
-    }
-
-
 }
@@ -2,23 +2,17 @@ using System;

 namespace MyParking.Shared
 {
-    /// <summary>
-    /// 统一检查跨层数值和二维位姿是否由满足基本范围要求的有限值组成。
-    /// </summary>
+    // 统一拒绝跨层数据中的NaN、无穷大和不符合正负范围的数值。
    public static class NumericGuard
    {
-        /// <summary>
-        /// 判断指定浮点数是否既不是NaN也不是无穷大。
-        /// </summary>
+        // 判断浮点数是否为有限值。
        public static bool IsFinite(double value)
        {
            return !double.IsNaN(value) &&
                   !double.IsInfinity(value);
        }

-        /// <summary>
-        /// 确保指定浮点数不是NaN或无穷大。
-        /// </summary>
+        // 拒绝NaN和无穷大。
        public static void EnsureFinite(
            double value,
            string parameterName)
@@ -31,9 +25,7 @@ namespace MyParking.Shared
            }
        }

-        /// <summary>
-        /// 确保指定浮点数是非负有限值。
-        /// </summary>
+        // 拒绝负数、NaN和无穷大。
        public static void EnsureFiniteNonNegative(
            double value,
            string parameterName)
@@ -48,9 +40,7 @@ namespace MyParking.Shared
            }
        }

-        /// <summary>
-        /// 确保指定浮点数是正有限值。
-        /// </summary>
+        // 拒绝零、负数、NaN和无穷大。
        public static void EnsureFinitePositive(
            double value,
            string parameterName)
@@ -65,9 +55,7 @@ namespace MyParking.Shared
            }
        }

-        /// <summary>
-        /// 确保二维位姿的位置和航向均为有限值。
-        /// </summary>
+        // 同时检查二维位姿的位置和航向。
        public static void EnsureFinite(
            Pose2D pose,
            string parameterName)
@@ -82,9 +70,7 @@ namespace MyParking.Shared
            }
        }

-        /// <summary>
-        /// 确保二维刚体速度的两个线速度分量和角速度均为有限值。
-        /// </summary>
+        // 同时检查二维刚体速度的两个线速度分量和角速度。
        public static void EnsureFinite(
            Twist2D twist,
            string parameterName)
				`@@ -0,0 +1 @@`
				`// 只定义无线报文的数据结构、版本和字段`