Fix fleet rotate synchronization

2026-07-01 10:28:32 +08:00
parent 574987222a
commit 3aa979be8b
3 changed files with 281 additions and 7 deletions
@@ -13,6 +13,7 @@ using ClumsyCore.Pilot;
 using ClumsyCore.Utilities;
 using CommonUsage;
 using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
 using FundamentalLib;
 using FundamentalLib.Utilities;
 using MDCSToolBox.Clumsy.Pilot.MultiWheel;
@@ -99,6 +100,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    private long _multiVehicleAppliedSeq = -1;

    [AsLowerIO(desc = "车号")] public int CarNum = 1;
+    [FieldMember(desc = "多车联动：原地旋转本车舵轮已对齐")]
+    public bool MultiVehicleRotateWheelsReady = true;
+    [FieldMember(desc = "多车联动：原地旋转整队舵轮已对齐")]
+    public bool MultiVehicleRotateFleetReady = true;

    #region 夹臂变量
    [AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
@@ -138,6 +143,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    private bool _multiVehicleMotionFeasible = true;
    private string _multiVehicleMotionInfeasibleReason = "";
    private DateTime _multiVehicleStopLastLog = DateTime.MinValue;
+    private bool _multiVehicleRotateModeActive;
+    private float _multiVehicleRotateDirectionHint = 1f;
+    private string _multiVehicleRotateAlignDetail = "";
+    private DateTime _multiVehicleRotateAlignLastLog = DateTime.MinValue;

    // 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
    private DateTime _mvDbgLastLog = DateTime.MinValue;
@@ -167,6 +176,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    // 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
    private DateTime _mvRemoteInputLastLog = DateTime.MinValue;
    private DateTime _mvRemoteDecisionLastLog = DateTime.MinValue;
+    private DateTime _mvNotifyApplyLastLog = DateTime.MinValue;

    private void LogMultiVehicleRemoteInput(bool isMaster, bool scriptOn, bool manualEnabled, bool autoEnabled,
        int manualMode, float manualVx, float manualVy, float manualVth)
@@ -204,6 +214,118 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        _multiVehicleMotionInfeasibleReason = feasible ? "" : (reason ?? "");
    }

+    private static float ClampFloat(float value, float min, float max)
+    {
+        if (value < min) return min;
+        if (value > max) return max;
+        return value;
+    }
+
+    private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega)
+    {
+        var rotateActive = active && fleetMode == 2;
+        if (!rotateActive)
+        {
+            _multiVehicleRotateModeActive = false;
+            MultiVehicleRotateWheelsReady = true;
+            MultiVehicleRotateFleetReady = true;
+            _multiVehicleRotateDirectionHint = 1f;
+            _multiVehicleRotateAlignDetail = "";
+            return;
+        }
+
+        if (!_multiVehicleRotateModeActive)
+        {
+            MultiVehicleRotateWheelsReady = false;
+            MultiVehicleRotateFleetReady = false;
+            _multiVehicleRotateAlignDetail = "rotate mode just entered";
+        }
+
+        _multiVehicleRotateModeActive = true;
+        if (Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega)
+            _multiVehicleRotateDirectionHint = Math.Sign(requestedOmega);
+    }
+
+    private bool PrepareMultiVehicleRotateWheels(MultiWheelChassis chassis, float requestedOmega,
+        float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f,
+        bool rampStop = true)
+    {
+        var hint = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega
+            ? Math.Sign(requestedOmega)
+            : Math.Sign(_multiVehicleRotateDirectionHint);
+        if (hint == 0) hint = 1;
+        _multiVehicleRotateDirectionHint = hint;
+
+        // Reuse SendRotateMotion's decomposition and angle-limit checks, but keep wheel speed at zero.
+        if (rampStop)
+            chassis.RampStop();
+        var alignOmega = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega
+            ? requestedOmega
+            : 0.01f * hint;
+        var motionOk = chassis.SendRotateMotion(alignOmega, TimeSpan.Zero,
+            localCompensateX: localCompensateX, localCompensateY: localCompensateY,
+            localCompensateTh: localCompensateTh);
+        var wheelAligned = TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
+            out var alignDetail);
+        var aligned = motionOk && wheelAligned;
+        if (!motionOk)
+            alignDetail = string.IsNullOrWhiteSpace(chassis.LastMotionDecomposeFailureReason)
+                ? alignDetail
+                : chassis.LastMotionDecomposeFailureReason;
+
+        MultiVehicleRotateWheelsReady = aligned;
+        _multiVehicleRotateAlignDetail = alignDetail;
+
+        var now = DateTime.Now;
+        if ((now - _multiVehicleRotateAlignLastLog).TotalMilliseconds >= 200)
+        {
+            _multiVehicleRotateAlignLastLog = now;
+            DLog.Log(
+                $"car{CarNum} ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} " +
+                $"hint={hint} comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
+                $"rampStop={rampStop} ok={motionOk} aligned={aligned} detail={alignDetail} " +
+                $"chassisReason={chassis.LastMotionDecomposeFailureReason}",
+                "MultiVehicleRemoteDbg");
+            FleetDiag(
+                $"ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} hint={hint} " +
+                $"comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
+                $"ok={motionOk} aligned={aligned} detail={alignDetail}");
+        }
+
+        Hedingben.ToastText(
+            aligned ? "车队原地旋转舵轮已对齐" : $"车队原地旋转舵轮预对齐中 {alignDetail}",
+            $"MultiVehicle{CarNum}-rotate-align");
+
+        return motionOk;
+    }
+
+    private static bool TryCheckRotateWheelAlignment(MultiWheelChassis chassis, float toleranceDeg, out string detail)
+    {
+        try
+        {
+#pragma warning disable CS0612, CS0618
+            var wheels = chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+            var maxDth = 0f;
+            var parts = new List<string>();
+            for (var i = 0; i < wheels.Count; i++)
+            {
+                var sw = wheels[i];
+                var dth = Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle()));
+                maxDth = Math.Max(maxDth, dth);
+                parts.Add($"w{i}:{dth:0.0}");
+            }
+
+            detail = $"maxDth={maxDth:0.0} tol={toleranceDeg:0.0} {string.Join(",", parts)}";
+            return maxDth <= toleranceDeg;
+        }
+        catch (Exception e)
+        {
+            detail = $"read alignment failed: {e.Message}";
+            return chassis.LastRotateAligned;
+        }
+    }
+
    private void LogMultiVehicleStop(string reason, bool force = false)
    {
        var now = DateTime.Now;
@@ -354,6 +476,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                    MultiVehicleNotification = notification;
                    _multiVehicleLastNotifyTime = DateTime.Now;
                }
+                var applyNow = DateTime.Now;
+                if ((applyNow - _mvNotifyApplyLastLog).TotalMilliseconds >= 200)
+                {
+                    _mvNotifyApplyLastLog = applyNow;
+                    DLog.Log(
+                        $"car{CarNum} NOTIFY_APPLY seq={notification.Seq} mode={notification.Mode} " +
+                        $"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " +
+                        $"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " +
+                        $"reason={notification.FleetStopReason}",
+                        "MultiVehicleRemoteDbg");
+                }
                return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
            }
            catch (Exception e)
@@ -472,6 +605,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                LogMultiVehicleStop("fleet control disabled; ramp stop previous fleet command", true);
            }
            _multiVehicleWasActive = false;
+            UpdateMultiVehicleRotateModeState(0, false, 0);
            LogMultiVehicleRemoteDecision(
                $"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
                $"rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
@@ -535,6 +669,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        var notificationStopActive = false;
        var notificationStopReason = "";
        var notificationStopSourceCar = 0;
+        var notificationRequestedFleetOmega = 0f;
+        var notificationMotionReleased = true;
        float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
        float crabSteerLimit = 0;
        var crabReverseEquivalent = false;
@@ -548,7 +684,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                if (fleetMode == 2)
                {
                    // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
-                    fleetOmega = manualVth * Conf.ManualCarSyncVthFac;
+                    var rotateInput = scriptOn ? manualVth : ClampFloat(manualVth, -1f, 1f);
+                    fleetOmega = scriptOn ? rotateInput : rotateInput * Conf.ManualCarSyncVthFac;
                    fleetVx = 0;
                    fleetFrontTh = 0;
                    fleetRearTh = 0;
@@ -631,6 +768,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            fleetRearTh = notification.FleetRearTh;
            fleetMode = notification.Mode;
            fleetOmega = notification.FleetOmega;
+            notificationRequestedFleetOmega = notification.RequestedFleetOmega;
+            notificationMotionReleased = notification.FleetMotionReleased;
            notificationStopActive = notification.FleetStopActive;
            notificationStopReason = notification.FleetStopReason ?? "";
            notificationStopSourceCar = notification.FleetStopSourceCar;
@@ -641,6 +780,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            MultiVehicleAutoIdealTh = notification.IdealTh;
        }

+        var requestedFleetOmega = !isMaster && fleetMode == 2
+            ? ((Math.Abs(notificationRequestedFleetOmega) > 1e-6f || !notificationMotionReleased)
+                ? notificationRequestedFleetOmega
+                : fleetOmega)
+            : fleetOmega;
+        UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega);
+
        var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);

        if (isMaster)
@@ -716,6 +862,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        bool othersDetectOk;
        string otherDetectLostCars;
        List<KeyValuePair<int, VehicleSyncInfo>> motionInfeasibleMembers;
+        List<KeyValuePair<int, VehicleSyncInfo>> rotateUnalignedMembers;
        lock (FleetLock)
        {
            othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
@@ -725,7 +872,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            motionInfeasibleMembers = MultiVehicleFleet
                .Where(kv => kv.Key != CarNum && !kv.Value.MotionFeasible)
                .ToList();
+            rotateUnalignedMembers = MultiVehicleFleet
+                .Where(kv => !kv.Value.RotateWheelsAligned)
+                .ToList();
        }
+        if (!MultiVehicleRotateWheelsReady && rotateUnalignedMembers.All(kv => kv.Key != CarNum))
+        {
+            rotateUnalignedMembers.Add(new KeyValuePair<int, VehicleSyncInfo>(CarNum, new VehicleSyncInfo
+            {
+                RotateWheelsAligned = false,
+                RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
+            }));
+        }
+        var rotateUnalignedCars = string.Join(",", rotateUnalignedMembers.Select(kv => kv.Key.ToString()));
+        var rotateFleetWheelsAligned = fleetMode != 2 || (fleetReady && rotateUnalignedMembers.Count == 0);
+        MultiVehicleRotateFleetReady = rotateFleetWheelsAligned;

        var fleetStopActive = false;
        var fleetStopReason = "";
@@ -794,6 +955,26 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
        }

+        var rotateHoldForAlignment = false;
+        if (fleetMode == 2 && fleetReady && !fleetStopActive && !notificationMotionReleased)
+        {
+            rotateHoldForAlignment = true;
+            fleetOmega = 0;
+            Hedingben.ToastText("车队原地旋转等待主车释放速度", $"MultiVehicle{CarNum}-rotate-align");
+            LogMultiVehicleRemoteDecision(
+                $"ROTATE_HOLD_RELEASE master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                $"requestedOmega={requestedFleetOmega:0.000}", true);
+        }
+        else if (fleetMode == 2 && fleetReady && !fleetStopActive && !rotateFleetWheelsAligned)
+        {
+            rotateHoldForAlignment = true;
+            fleetOmega = 0;
+            Hedingben.ToastText($"车队原地旋转舵轮预对齐中 car=[{rotateUnalignedCars}]", $"MultiVehicle{CarNum}-rotate-align");
+            LogMultiVehicleRemoteDecision(
+                $"ROTATE_HOLD_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                $"requestedOmega={requestedFleetOmega:0.000} unalignedCars=[{rotateUnalignedCars}]", true);
+        }
+
        if (fleetStopActive)
            LogMultiVehicleStop(fleetStopReason);

@@ -814,6 +995,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        {
            chassis.RampStop();
            SetMultiVehicleMotionFeasible(true);
+            if (fleetMode == 2)
+            {
+                MultiVehicleRotateWheelsReady = false;
+                MultiVehicleRotateFleetReady = false;
+                _multiVehicleRotateAlignDetail = $"fleet stop: {fleetStopReason}";
+            }
            LogMultiVehicleRemoteDecision(
                $"RAMP_STOP master={isMaster} manual={manualEnabled} auto={autoEnabled} ready={fleetReady} " +
                $"reason={fleetStopReason}", true);
@@ -926,8 +1113,45 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                _mvRotCompVy = rotCompVy;
                _mvRotCompOmega = rotCompOmega;

-                var motionOk = chassis.SendRotateMotion(fleetOmega,
-                    localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);
+                bool motionOk;
+                if (rotateHoldForAlignment)
+                {
+                    motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
+                        rotCompVx, rotCompVy, rotCompOmega);
+                }
+                else if (rotating)
+                {
+                    motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
+                        rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
+                    if (motionOk && MultiVehicleRotateWheelsReady)
+                    {
+                        motionOk = chassis.SendRotateMotion(fleetOmega,
+                            localCompensateX: rotCompVx, localCompensateY: rotCompVy,
+                            localCompensateTh: rotCompOmega);
+                        MultiVehicleRotateWheelsReady = motionOk &&
+                                                        TryCheckRotateWheelAlignment(chassis,
+                                                            Conf.InPlaceRotateWheelAlignDeg,
+                                                            out _multiVehicleRotateAlignDetail);
+                    }
+                    else
+                    {
+                        rotateHoldForAlignment = true;
+                        fleetOmega = 0;
+                        chassis.RampStop();
+                        LogMultiVehicleRemoteDecision(
+                            $"ROTATE_HOLD_LOCAL_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                            $"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
+                    }
+                }
+                else
+                {
+                    motionOk = chassis.SendRotateMotion(fleetOmega,
+                        localCompensateX: rotCompVx, localCompensateY: rotCompVy,
+                        localCompensateTh: rotCompOmega);
+                    MultiVehicleRotateWheelsReady = motionOk &&
+                                                    TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
+                                                        out _multiVehicleRotateAlignDetail);
+                }
                SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
                if (!motionOk)
                {
@@ -938,7 +1162,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                }
                LogMultiVehicleRemoteDecision(
                    $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
-                    $"mode={fleetMode} omega={fleetOmega:0.000} comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) " +
+                    $"holdAlign={rotateHoldForAlignment} wheelReady={MultiVehicleRotateWheelsReady} fleetReady={MultiVehicleRotateFleetReady} " +
+                    $"mode={fleetMode} omegaReq={requestedFleetOmega:0.000} omega={fleetOmega:0.000} " +
+                    $"comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) align={_multiVehicleRotateAlignDetail} " +
                    $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");

                // 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
@@ -950,6 +1176,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                _mvRotCompVx = 0;
                _mvRotCompVy = 0;
                _mvRotCompOmega = 0;
+                MultiVehicleRotateWheelsReady = true;
+                MultiVehicleRotateFleetReady = true;
+                _multiVehicleRotateAlignDetail = "";
                // 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
                _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                _rotPiLastTime = DateTime.MinValue;
@@ -1003,7 +1232,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " +
                $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
                $"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " +
-                $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2})";
+                $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " +
+                $"| ROT_ALIGN hold:{rotateHoldForAlignment} wheelReady:{MultiVehicleRotateWheelsReady} fleetReady:{MultiVehicleRotateFleetReady} " +
+                $"unaligned:[{rotateUnalignedCars}] reqOmega:{requestedFleetOmega:F1} detail:{_multiVehicleRotateAlignDetail}";

            DLog.Log(dbg, "MultiVehicleDbg");
            FleetDiag(dbg);
@@ -1046,6 +1277,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                        FleetRearTh = fleetStopActive ? 0 : fleetRearTh,
                        Mode = fleetMode,
                        FleetOmega = fleetStopActive ? 0 : fleetOmega,
+                        RequestedFleetOmega = fleetMode == 2 && !fleetStopActive ? requestedFleetOmega : 0,
+                        FleetMotionReleased = !fleetStopActive && !(fleetMode == 2 && rotateHoldForAlignment),
                        FleetStopActive = fleetStopActive,
                        FleetStopReason = fleetStopReason,
                        FleetStopSourceCar = fleetStopSourceCar,
@@ -1066,6 +1299,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                    };
                }

+                LogMultiVehicleRemoteDecision(
+                    $"NOTIFY_SEND seq={notification.Seq} mode={notification.Mode} " +
+                    $"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " +
+                    $"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " +
+                    $"reason={notification.FleetStopReason} fleetCnt={notification.Fleet.Count}/{Conf.MultiVehicleFleetNum}");
+
                foreach (var kv in notification.Fleet)
                {
                    var ip = kv.Value.Ip;
@@ -1240,7 +1479,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            Aligned = aligned,
            DetectOk = detectOk,
            MotionFeasible = _multiVehicleMotionFeasible,
-            MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason
+            MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason,
+            RotateWheelsAligned = MultiVehicleRotateWheelsReady,
+            RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
        };
    }