贯通车队运行链并支持轨迹自动推导β

2026-08-25 17:59:18 +08:00
parent 95c0b19a26
commit 17092c2766
37 changed files with 3632 additions and 37 deletions
@@ -1 +1,141 @@
-// 只定义无线报文的数据结构、版本和字段
+// 四舵轮共同搬运协议
+
+namespace MyParking.Shared
+{
+    /// <summary>定义车队无线协议的公共常量。</summary>
+    public static class FleetProtocol
+    {
+        public const int CurrentVersion = 1;
+        public const int BroadcastVehicleId = 0;
+        public const long NoActivePlanId = 0;
+        public const long NoAppliedCommandSequence = 0;
+    }
+
+    /// <summary>表示成员车本地的车队任务执行阶段。</summary>
+    public enum FleetMemberState
+    {
+        Idle = 0,
+        Preparing = 1,
+        Ready = 2,
+        Active = 3,
+        Faulted = 4
+    }
+
+    /// <summary>表示主车要求成员执行的动作。</summary>
+    public enum FleetCommandKind
+    {
+        PrepareRolling = 1,
+        PrepareSpin = 2,
+        Activate = 3,
+        Motion = 4,
+        Stop = 5
+    }
+
+    /// <summary>成员车周期上报给主车的状态快照，同时承担心跳和命令确认。</summary>
+    public readonly struct FleetMemberReport
+    {
+        public FleetMemberReport(
+            int vehicleId,
+            long planId,
+            long sequenceNumber,
+            double sampleTimestampSeconds,
+            Pose2D poseInCommonWorld,
+            Twist2D twistAtVehicleOriginInCommonWorld,
+            bool isStateAvailable,
+            bool hasValidVelocityEstimate,
+            FleetMemberState state,
+            long lastAppliedCommandSequence,
+            int failureCode = 0)
+        {
+            VehicleId = vehicleId;
+            PlanId = planId;
+            SequenceNumber = sequenceNumber;
+            SampleTimestampSeconds = sampleTimestampSeconds;
+            PoseInCommonWorld = poseInCommonWorld;
+            TwistAtVehicleOriginInCommonWorld =
+                twistAtVehicleOriginInCommonWorld;
+            IsStateAvailable = isStateAvailable;
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+            State = state;
+            LastAppliedCommandSequence =
+                lastAppliedCommandSequence;
+            FailureCode = failureCode;
+        }
+
+        public int VehicleId { get; }
+
+        // 零表示车辆当前不属于活动任务。
+        public long PlanId { get; }
+
+        // 本车上报流中单调递增，用于丢弃乱序旧报文。
+        public long SequenceNumber { get; }
+
+        // 本车单调时钟的采样时刻，通信层负责换算到主车时间轴。
+        public double SampleTimestampSeconds { get; }
+
+        // 位姿必须已经转换到所有成员约定一致的公共世界坐标系。
+        public Pose2D PoseInCommonWorld { get; }
+
+        public Twist2D TwistAtVehicleOriginInCommonWorld { get; }
+
+        public bool IsStateAvailable { get; }
+
+        public bool HasValidVelocityEstimate { get; }
+
+        public FleetMemberState State { get; }
+
+        // 零表示尚未执行任何主车命令。
+        public long LastAppliedCommandSequence { get; }
+
+        // 零表示没有结构化故障码。
+        public int FailureCode { get; }
+    }
+
+    /// <summary>主车向指定成员或全队下发的一条车队任务命令。</summary>
+    public readonly struct FleetCommand
+    {
+        public FleetCommand(
+            long planId,
+            long sequenceNumber,
+            int targetVehicleId,
+            FleetCommandKind kind,
+            double motionDirectionInBodyRadians,
+            Twist2D twistInVehicleBody,
+            double validForSeconds,
+            int reasonCode = 0)
+        {
+            PlanId = planId;
+            SequenceNumber = sequenceNumber;
+            TargetVehicleId = targetVehicleId;
+            Kind = kind;
+            MotionDirectionInBodyRadians =
+                motionDirectionInBodyRadians;
+            TwistInVehicleBody = twistInVehicleBody;
+            ValidForSeconds = validForSeconds;
+            ReasonCode = reasonCode;
+        }
+
+        public long PlanId { get; }
+
+        // 主车命令流中单调递增，成员据此拒绝乱序旧命令。
+        public long SequenceNumber { get; }
+
+        // 零表示广播，正数表示指定成员车。
+        public int TargetVehicleId { get; }
+
+        public FleetCommandKind Kind { get; }
+
+        // 仅PrepareRolling使用，单位rad，车体系X轴到运动X轴逆时针为正。
+        public double MotionDirectionInBodyRadians { get; }
+
+        // 仅Motion使用，采用目标成员车体系。
+        public Twist2D TwistInVehicleBody { get; }
+
+        // 从成员本机收到消息时开始计时，超时后必须停车。
+        public double ValidForSeconds { get; }
+
+        // 零表示没有结构化停止或故障原因。
+        public int ReasonCode { get; }
+    }
+}