diff --git a/MultiWheel/MultiWheelC/MovementTests.TwoLegDetect.cs b/MultiWheel/MultiWheelC/MovementTests.TwoLegDetect.cs index effb259..c0a4f57 100644 --- a/MultiWheel/MultiWheelC/MovementTests.TwoLegDetect.cs +++ b/MultiWheel/MultiWheelC/MovementTests.TwoLegDetect.cs @@ -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); } /// 在猜测中心周围构造一个矩形 ROI,过滤掉框外点云,降低误识别。 diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index 1ec705d..7e50f7b 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -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; diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index 9170199..0e6e01a 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -144,6 +144,39 @@ public class PilotDefinition : MultiWheelPilotDefinition= 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= 300) { _mvDiskLastLog = DateTime.Now; @@ -391,6 +427,9 @@ public class PilotDefinition : MultiWheelPilotDefinition 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 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";