update

2026-06-30 16:12:57 +08:00
parent 14b967f889
commit 306be14a41
3 changed files with 96 additions and 4 deletions
@@ -86,12 +86,25 @@ public class TwoLegDetect : MovementTest
        };
 #pragma warning restore CS0612, CS0618

-        return detector.DetectWithGuess(
+        var result = detector.DetectWithGuess(
            lidarName,
            new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
            guessCoordinateSystem: CoordinateSystem.Car2D,
            outCoordinateSystem: CoordinateSystem.Car2D,
            filters);
+        return ApplyOutputBias(result);
+    }
+
+    private static LineSegment ApplyOutputBias(LineSegment result)
+    {
+        if (result == null) return null;
+
+        var conf = PilotDefinition.Conf;
+        if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
+            return result;
+
+        var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
+        return new LineSegment(result.Src + bias, result.Dst + bias);
    }

    /// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
@@ -28,6 +28,8 @@ public class PilotConfig : MultiWheelPilotConfig
    [FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
    [FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";

+    [FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
+
    [JsonProperty("MultiVehicleMasterIp")]
    private string LegacyMasterIpSetter
    {
@@ -204,6 +206,12 @@ public class PilotConfig : MultiWheelPilotConfig
    [FieldMember(desc = "2腿检测：中心X偏移(mm)")]
    public float TwoLegCenterChangeX = 0f;

+    [FieldMember(desc = "2腿检测：输出X补偿(mm)")]
+    public float TwoLegOutputBiasX = 0f;
+
+    [FieldMember(desc = "2腿检测：输出Y补偿(mm)")]
+    public float TwoLegOutputBiasY = 0f;
+
    [FieldMember(desc = "2腿检测：ROI滤波框长(mm)")]
    public float TwoLegFilterLength = 1800f;

@@ -144,6 +144,39 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    }

    // 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
+    private DateTime _mvRemoteInputLastLog = DateTime.MinValue;
+    private DateTime _mvRemoteDecisionLastLog = DateTime.MinValue;
+
+    private void LogMultiVehicleRemoteInput(bool isMaster, bool scriptOn, bool manualEnabled, bool autoEnabled,
+        int manualMode, float manualVx, float manualVy, float manualVth)
+    {
+        var now = DateTime.Now;
+        if ((now - _mvRemoteInputLastLog).TotalMilliseconds < 200) return;
+        _mvRemoteInputLastLog = now;
+
+        int fleetCnt;
+        lock (FleetLock) fleetCnt = MultiVehicleFleet.Count;
+        var notifAgeMs = _multiVehicleLastNotifyTime == DateTime.MinValue
+            ? -1
+            : (now - _multiVehicleLastNotifyTime).TotalMilliseconds;
+        var notifFresh = notifAgeMs >= 0 && notifAgeMs < Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
+
+        DLog.Log(
+            $"INPUT master={isMaster} endpoint={Conf.MultiVehicleMasterEndpoint} selfEndpoint={Conf.MultiVehicleSelfEndpoint} car={CarNum} " +
+            $"rawEn={MultiVehicleManualEnabled} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000} " +
+            $"scriptOn={scriptOn} effEn={manualEnabled} effMode={manualMode} effVx={manualVx:0.000} effVy={manualVy:0.000} effVth={manualVth:0.000} " +
+            $"auto={autoEnabled} notifFresh={notifFresh} notifAgeMs={notifAgeMs:0} fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum}",
+            "MultiVehicleRemoteDbg");
+    }
+
+    private void LogMultiVehicleRemoteDecision(string msg, bool force = false)
+    {
+        var now = DateTime.Now;
+        if (!force && (now - _mvRemoteDecisionLastLog).TotalMilliseconds < 200) return;
+        _mvRemoteDecisionLastLog = now;
+        DLog.Log($"car{CarNum} {msg}", "MultiVehicleRemoteDbg");
+    }
+
    private readonly object _neighborDetectLock = new();
    private readonly List<(DateTime Time, Vector2 Src, Vector2 Dst)> _neighborDetects = new();

@@ -373,6 +406,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

        // 入口诊断（节流 ~300ms）：记录从 Medulla 收到的原始 IO 值与门控判定，
        // 用于确认遥控指令是否真的传到了 Clumsy，以及为何提前 return。
+        LogMultiVehicleRemoteInput(isMaster, scriptOn, manualEnabled, autoEnabled,
+            manualMode, manualVx, manualVy, manualVth);
+
        if ((DateTime.Now - _mvDiskLastLog).TotalMilliseconds >= 300)
        {
            _mvDiskLastLog = DateTime.Now;
@@ -391,6 +427,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

        if (!manualEnabled && !autoEnabled)
        {
+            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}");
            UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
            sendMotionPainter.Clear();
            lock (FleetLock)
@@ -661,6 +700,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        float xPosCompensate = 0, yPosCompensate = 0, thPosCompensate = 0;
        float posBiasX = 0, posBiasY = 0, posBiasTh = 0;

+        if (!fleetReady)
+            LogMultiVehicleRemoteDecision(
+                $"NO_SEND_NOT_READY master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
+                $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum} canMove={canMove} mode={fleetMode} " +
+                $"cmdVx={fleetVx:0.000} cmdFTh={fleetFrontTh:0.00} cmdRTh={fleetRearTh:0.00} omega={fleetOmega:0.000}");
+
        if (fleetReady)
        {
            chassis.SetOriginBias(layoutX, layoutY, layoutTh);
@@ -770,6 +815,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

                chassis.SendRotateMotion(fleetOmega,
                    localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);
+                LogMultiVehicleRemoteDecision(
+                    $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} " +
+                    $"mode={fleetMode} omega={fleetOmega:0.000} comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) " +
+                    $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");

                // 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
                if (isMaster)
@@ -790,6 +839,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                    localCompensateX: xDetectCompensate + xPosCompensate,
                    localCompensateY: yDetectCompensate + yPosCompensate,
                    localCompensateTh: thDetectCompensate + thPosCompensate);
+                LogMultiVehicleRemoteDecision(
+                    $"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} " +
+                    $"mode={fleetMode} vx={fleetVx:0.000} fTh={fleetFrontTh:0.00} rTh={fleetRearTh:0.00} " +
+                    $"comp=({xDetectCompensate + xPosCompensate:0.0},{yDetectCompensate + yPosCompensate:0.0},{thDetectCompensate + thPosCompensate:0.000}) " +
+                    $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
            }
        }

@@ -814,7 +868,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                $"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " +
                crabDbg +
                $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
-                $"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} " +
+                $"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} outBias({Conf.TwoLegOutputBiasX:F0},{Conf.TwoLegOutputBiasY:F0}) " +
                $"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
                $"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " +
                $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " +
@@ -1037,11 +1091,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    private VehicleSyncInfo BuildSelfInfo(bool master, bool posAvailable, float x, float y, float th,
        float layoutX, float layoutY, float layoutTh, bool aligned, bool detectOk = true)
    {
+        ResolveMultiVehicleSelfEndpoint(out var selfIp, out var selfPort);
+
        return new VehicleSyncInfo
        {
            Master = master,
-            Ip = Conf.SimpleIp,
-            Port = WebAPI.port,
+            Ip = selfIp,
+            Port = selfPort,
            PosAvailable = posAvailable,
            X = x,
            Y = y,
@@ -1118,6 +1174,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        }, TaskScheduler.Default);
    }

+    private void ResolveMultiVehicleSelfEndpoint(out string ip, out int port)
+    {
+        ip = "127.0.0.1";
+        port = WebAPI.port > 0 ? WebAPI.port : 8008;
+
+        var endpoint = Conf.MultiVehicleSelfEndpoint;
+        if (string.IsNullOrWhiteSpace(endpoint)) return;
+
+        var parts = endpoint.Trim().Split(':');
+        if (parts.Length >= 1 && !string.IsNullOrWhiteSpace(parts[0]))
+            ip = parts[0].Trim();
+        if (parts.Length >= 2 && int.TryParse(parts[1], out var p) && p > 0)
+            port = p;
+    }
+
    private void ParseMasterEndpoint(out string ip, out int port)
    {
        ip = "127.0.0.1";