Merge branch 'master' of http://git.fairylandtech.com/ruifeng.zhou/Tutorial

MultiWheel/MultiWheelC/MovementTests.cs

@@ -523,10 +523,140 @@ public class FleetCrabWalk : MovementDefinition
         return target;
     }
 
+    private static float AverageAngle(float frontTh, float rearTh)
+    {
+        var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
+        return (float)CommonMath.RoundTh(rearTh + diff / 2f);
+    }
+
+    private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
+        out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
+    {
+        var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
+        rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
+        driveSpeed = speed;
+        frontTh = rawFrontTh;
+        rearTh = rawRearTh;
+        reverseEquivalent = false;
+
+        if (rawBaseTh > limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+        else if (rawBaseTh < -limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+
+        frontTh = ClampAbs(frontTh, limit);
+        rearTh = ClampAbs(rearTh, limit);
+    }
+
+    private static float ProbeSpeed(float speed)
+    {
+        return Math.Abs(speed) > 1e-4f ? speed : 1f;
+    }
+
+    private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
+        out float yawSign)
+    {
+        yawSign = 0f;
+        if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
+            return false;
+
+        var radius = Math.Max(1f, Math.Abs(controlRadius));
+        Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
+            normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
+            normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
+        var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
+        if (!intersect)
+            return false;
+
+        // Match MultiWheelChassis.SendMotion: the tangent side is selected by
+        // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
+        var tangentSign = center.Y > 1f ? 1f : -1f;
+        var speedSign = driveSpeed >= 0f ? 1f : -1f;
+        yawSign = speedSign * tangentSign;
+        return true;
+    }
+
+    private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
+    {
+        const float probeDth = 1f;
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
+            ? yawSign
+            : 1f;
+    }
+
+    private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
+        Vector2 pathLeft)
+    {
+        var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
+        var rad = motionTh / 180f * Math.PI;
+        var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
+        if (driveSpeed < 0f)
+            dir = -dir;
+        return Vector2.Dot(dir, pathLeft);
+    }
+
+    private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral, float biasProbe)
+    {
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
+        return -Math.Sign(lateral) * lateralVelocity;
+    }
+
+    private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral)
+    {
+        if (Math.Abs(lateral) <= 1e-3f)
+            return 1f;
+
+        const float probeBias = 1f;
+        var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
+        var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
+        return positiveScore >= negativeScore ? 1f : -1f;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
     public override IEnumerable<bool> Get()
     {
         var self = PilotDefinition.Self;
         var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
+            yield break;
+        }
         _stopping = false;
 
         DLog.Log(
@@ -547,7 +677,7 @@ public class FleetCrabWalk : MovementDefinition
 
         // 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
         DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
-        if (!self.TryGetFleetCenterFromSlam(out var x0, out var y0, out var theta))
+        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
         {
             DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
             Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
@@ -555,6 +685,8 @@ public class FleetCrabWalk : MovementDefinition
         }
         DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
 
+        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
         Vector2 pathStart;
         Vector2 dst;
         float pathLengthMm;
@@ -655,6 +787,28 @@ public class FleetCrabWalk : MovementDefinition
 
         Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
 
+        if (warmReady && !UseAbsolutePath &&
+            self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
+        {
+            x0 = warmX;
+            y0 = warmY;
+            theta = warmTh;
+            pathStart = new Vector2(x0, y0);
+            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+            targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+            phiRad = phi / 180.0 * Math.PI;
+            pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
+            pathLeft = new Vector2(-pathDir.Y, pathDir.X);
+            self.MultiVehicleAutoIdealX = pathStart.X;
+            self.MultiVehicleAutoIdealY = pathStart.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            DLog.Log(
+                $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
+                "FleetCrabDbg");
+        }
+
         var iter = 0;
         var lastLog = DateTime.MinValue;
         var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
@@ -666,18 +820,22 @@ public class FleetCrabWalk : MovementDefinition
         var cmdSpeed = 0f;
         var lastTick = DateTime.Now;
         var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
-        var holdFrontTh = ClampAbs((float)CommonMath.ThDiff(phi, theta), gcpLimit);
-        var holdRearTh = holdFrontTh;
+        var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
+            ? conf.MultiVehicleControlRadius
+            : conf.TestCarSyncDistance / 2f));
+        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
+            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
+            out _, out _);
         var stopReason = "done";
 
         while (!_stopping)
         {
             iter++;
 
-            if (!self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth))
+            if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
             {
                 stopReason = "fleet center invalid";
-                DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg");
+                DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
                 break;
             }
             var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
@@ -702,11 +860,20 @@ public class FleetCrabWalk : MovementDefinition
 
             var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
             var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
-            var dthItem = ClampAbs(conf.DthLinearFac * headingErr, conf.DthLinearThreshold);
-            var biasItem = (float)(-Math.Atan(conf.BiasFac * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
-            biasItem = ClampAbs(biasItem, conf.BiasThreshold);
-            var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
-            var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
+            var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
+            var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
+            var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
+                Math.Max(speed, 0.3f)) / Math.PI * 180.0);
+            var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
+            var rawBiasItem = rawBiasMagnitude * biasSign;
+            var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
+            var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
+            var rawDthItem = conf.DthLinearFac * headingErr * yawSplitSign;
+            var dthItem = ClampAbs(rawDthItem, conf.DthLinearThreshold);
+            var rawFrontTh = baseCrabTh + biasItem + dthItem;
+            var rawRearTh = baseCrabTh + biasItem - dthItem;
+            ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
+                out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
             holdFrontTh = frontTh;
             holdRearTh = rearTh;
             var idealAlong = Clamp(along, 0f, pathLengthMm);
@@ -718,7 +885,7 @@ public class FleetCrabWalk : MovementDefinition
             self.MultiVehicleScriptVy = 0;
             self.MultiVehicleScriptVth = 0;
             self.MultiVehicleAutoEnabled = true;
-            self.MultiVehicleAutoVx = speed;
+            self.MultiVehicleAutoVx = driveSpeed;
             self.MultiVehicleAutoFrontTh = frontTh;
             self.MultiVehicleAutoRearTh = rearTh;
             self.MultiVehicleAutoIdealX = ideal.X;
@@ -734,13 +901,24 @@ public class FleetCrabWalk : MovementDefinition
                 int fleetCnt;
                 lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
                 DLog.Log(
-                    $"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
+                    $"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
                     $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
                     $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
-                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
+                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
                     $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
                     $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
                     "FleetCrabDbg");
+                DLog.Log(
+                    $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
+                    $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
+                    $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
+                    $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
+                    $"dthFac:{conf.DthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.DthLinearThreshold:0.00} " +
+                    $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
+                    $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
+                    $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
+                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
+                    "FleetCrabHeadingDbg");
             }
             yield return true;
         }

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -865,6 +865,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega, rotateParams);
 
         var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
+        var inferredFleetCenterValid = false;
+        float inferredFleetCenterX = 0, inferredFleetCenterY = 0, inferredFleetCenterTh = 0;
+        Tuple<float, float, float> supposedPosForDiag = null;
 
         if (isMaster)
         {
@@ -877,7 +880,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             }
 
             if (TryInferFleetCenter(out var cx, out var cy, out var cth))
+            {
+                inferredFleetCenterValid = true;
+                inferredFleetCenterX = cx;
+                inferredFleetCenterY = cy;
+                inferredFleetCenterTh = cth;
                 PublishFleetCenter(cx, cy, cth);
+            }
 
             // D: 自动模式（非手动）下，若控制器给出理想车队中心，则以理想位姿作为各车 layout 目标，
             //    使弧线路径上从车按各自相对曲率中心位置前馈，而非仅靠事后检测/SLAM 纠偏。
@@ -1115,6 +1124,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 var supposedPos = LessMath.Transform2D(
                     Tuple.Create(CenterX, CenterY, CenterTh),
                     Tuple.Create(layoutX, layoutY, layoutTh));
+                supposedPosForDiag = supposedPos;
                 var posBias = LessMath.SolveTransform2D(Tuple.Create(selfX, selfY, selfTh), supposedPos);
                 posBiasX = (float)posBias.Item1;
                 posBiasY = (float)posBias.Item2;
@@ -1340,6 +1350,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             var cx = xDetectCompensate + xPosCompensate;
             var cy = yDetectCompensate + yPosCompensate;
             var cth = thDetectCompensate + thPosCompensate;
+            var actualCenterThForDiag = inferredFleetCenterValid ? inferredFleetCenterTh : CenterTh;
+            var idealHeadingErr = (float)CommonMath.ThDiff(MultiVehicleAutoIdealTh, actualCenterThForDiag);
+            var idealHeadingErrReverse = (float)CommonMath.ThDiff(actualCenterThForDiag, MultiVehicleAutoIdealTh);
+            var frontRearDiff = (float)CommonMath.ThDiff(fleetFrontTh, fleetRearTh);
+            var commandHeadingSplit = frontRearDiff / 2f;
+            var commandBaseTh = (float)CommonMath.RoundTh(fleetRearTh + commandHeadingSplit);
+            var supposedPosText = supposedPosForDiag == null
+                ? "N/A"
+                : $"({supposedPosForDiag.Item1:0},{supposedPosForDiag.Item2:0},{supposedPosForDiag.Item3:0.0})";
             var crabDbg = isMaster && manualEnabled && fleetMode == 1
                 ? $"| CRAB speed:{crabInputVx:F3} steerRatio:{crabInputVy:F3} raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
                 : "";
@@ -1367,6 +1386,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             DLog.Log(dbg, "MultiVehicleDbg");
             FleetDiag(dbg);
 
+            if (autoMode || autoEnabled || MultiVehicleAutoEnabled)
+                DLog.Log(
+                    $"APPLY car{CarNum} master:{isMaster} autoMode:{autoMode} manual:{manualEnabled} " +
+                    $"cmd(vx:{fleetVx:0.000},f:{fleetFrontTh:0.00},r:{fleetRearTh:0.00},base:{commandBaseTh:0.00},split:{commandHeadingSplit:0.00},f-r:{frontRearDiff:0.00}) " +
+                    $"ideal(has:{MultiVehicleAutoHasIdeal},x:{MultiVehicleAutoIdealX:0},y:{MultiVehicleAutoIdealY:0},th:{MultiVehicleAutoIdealTh:0.00}) " +
+                    $"centerPublished({CenterX:0},{CenterY:0},{CenterTh:0.00}) " +
+                    $"centerInferred(valid:{inferredFleetCenterValid},x:{inferredFleetCenterX:0},y:{inferredFleetCenterY:0},th:{inferredFleetCenterTh:0.00}) " +
+                    $"idealHeadingErr(target-actual):{idealHeadingErr:0.00} reverse(actual-target):{idealHeadingErrReverse:0.00} " +
+                    $"layout({layoutX:0},{layoutY:0},{layoutTh:0.00}) self({selfX:0},{selfY:0},{selfTh:0.00}) supposed:{supposedPosText} " +
+                    $"detectDth:{detectDth:0.00}->comp:{thDetectCompensate:0.00} " +
+                    $"posBiasTh:{posBiasTh:0.00}->comp:{thPosCompensate:0.00} " +
+                    $"totalLocalComp(x:{cx:0.0},y:{cy:0.0},th:{cth:0.00}) " +
+                    $"useDetourCorr:{useDetourCorrection} slam:{slamRead} fleetPos:{fleetPosValid} ready:{fleetReady}",
+                    "FleetCrabHeadingDbg");
+
             // 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储）
             Hedingben.ToastText(
                 $"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " +
@@ -1521,6 +1555,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         return true;
     }
 
+    public bool TryGetFleetCenterFromMembers(out float centerX, out float centerY, out float centerTh)
+    {
+        return TryInferFleetCenter(out centerX, out centerY, out centerTh);
+    }
+
     private static (float, float, float) InferFleetCenterFromCar(VehicleSyncInfo car)
     {
         // 由 carWorld = Transform2D(center, layout) 反推 center = carWorld ∘ layout⁻¹。