From c9cb3a5df361c93f4f09b005844b5f4919937ce9 Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Wed, 1 Jul 2026 13:27:15 +0800 Subject: [PATCH] Use binary multi-vehicle sync transport --- MultiWheel/MultiWheelC/PilotDefinition.cs | 170 +++++++----- .../MultiWheelC/VehicleSyncBinaryCodec.cs | 249 ++++++++++++++++++ 2 files changed, 348 insertions(+), 71 deletions(-) create mode 100644 MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index c0960ae..5fc8996 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -17,7 +17,6 @@ using CommonUsage.Mathematics; using FundamentalLib; using FundamentalLib.Utilities; using MDCSToolBox.Clumsy.Pilot.MultiWheel; -using Newtonsoft.Json; using Vector = ClumsyCore.Utilities.Vector; namespace MultiWheelC; @@ -426,73 +425,32 @@ public class PilotDefinition : MultiWheelPilotDefinition + PicoHttpServer.AddPostByteHandler("/multi-vehicle-register-bin", body => { try { - var infoJson = Uri.UnescapeDataString(query.Info ?? ""); - var info = JsonConvert.DeserializeObject(infoJson) - ?? throw new Exception("VehicleSyncInfo is null"); - lock (FleetLock) - { - MultiVehicleFleet[query.CarNum] = info; - _multiVehicleFleetSeen[query.CarNum] = DateTime.Now; // C: 刷新存活时刻 - } - return JsonConvert.SerializeObject(new { code = 200, message = "ok" }); + var (carNum, info) = VehicleSyncBinaryCodec.DecodeRegister(body); + ApplyVehicleSyncRegister(carNum, info); + return "ok"; } catch (Exception e) { - DLog.Log($"/multi-vehicle-register error: {e.FormatEx()}", "MultiVehicle"); - return JsonConvert.SerializeObject(new { code = 500, message = e.Message }); + DLog.Log($"/multi-vehicle-register-bin error: {e.FormatEx()}", "MultiVehicle"); + throw; } }); - // F: notify 改用 POST + JSON body（取代 GET query 串），避免整队 Fleet 字典撑爆 URL 长度上限； - // 用 Seq 丢弃乱序到达的旧包，避免从车短暂套用过期指令。 - PicoHttpServer.AddPostTextHandler("/multi-vehicle-notify", body => + PicoHttpServer.AddPostByteHandler("/multi-vehicle-notify-bin", body => { try { - var notification = JsonConvert.DeserializeObject(body ?? "") - ?? throw new Exception("notification is null"); - // 乱序丢弃：仅应用序列号大于已应用值的包。Seq==0 视为旧版无序列号始终接受； - // 若 Seq 明显回退(差值>100)，判定为主车重启的新会话，重新接受并对齐序列号。 - if (notification.Seq != 0 && notification.Seq <= _multiVehicleAppliedSeq && - notification.Seq > _multiVehicleAppliedSeq - 100) - return JsonConvert.SerializeObject(new { code = 200, message = "stale" }); - _multiVehicleAppliedSeq = notification.Seq; - - MultiVehicleAligned = notification.Aligned; - lock (FleetLock) - { - MultiVehicleFleet = notification.Fleet ?? new Dictionary(); - // C: notify 内含整队成员，逐一刷新其本地存活时刻。 - var nowSeen = DateTime.Now; - foreach (var key in MultiVehicleFleet.Keys) - _multiVehicleFleetSeen[key] = nowSeen; - } - lock (_multiVehicleNotificationLock) - { - 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" }); + var notification = VehicleSyncBinaryCodec.DecodeNotification(body); + return ApplyVehicleSyncNotification(notification); } catch (Exception e) { - DLog.Log($"/multi-vehicle-notify error: {e.FormatEx()}", "MultiVehicle"); - return JsonConvert.SerializeObject(new { code = 500, message = e.Message }); + DLog.Log($"/multi-vehicle-notify-bin error: {e.FormatEx()}", "MultiVehicle"); + throw; } }); @@ -501,6 +459,53 @@ public class PilotDefinition : MultiWheelPilotDefinition MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start(); } + private void ApplyVehicleSyncRegister(int carNum, VehicleSyncInfo info) + { + lock (FleetLock) + { + MultiVehicleFleet[carNum] = info; + _multiVehicleFleetSeen[carNum] = DateTime.Now; + } + } + + private string ApplyVehicleSyncNotification(VehicleSyncNotification notification) + { + // Keep the same stale-packet semantics as the previous transport. + if (notification.Seq != 0 && notification.Seq <= _multiVehicleAppliedSeq && + notification.Seq > _multiVehicleAppliedSeq - 100) + return "stale"; + _multiVehicleAppliedSeq = notification.Seq; + + MultiVehicleAligned = notification.Aligned; + lock (FleetLock) + { + MultiVehicleFleet = notification.Fleet ?? new Dictionary(); + var nowSeen = DateTime.Now; + foreach (var key in MultiVehicleFleet.Keys) + _multiVehicleFleetSeen[key] = nowSeen; + } + + lock (_multiVehicleNotificationLock) + { + 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 "ok"; + } + private void MultiVehicleLoop(HttpClient hc) { var carLength = CarLength; @@ -1524,29 +1529,52 @@ public class PilotDefinition : MultiWheelPilotDefinition + try { - if (t.IsFaulted) - DLog.Log($"register failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle"); - }, TaskScheduler.Default); + ParseMasterEndpoint(out var masterIp, out var masterPort); + var payload = VehicleSyncBinaryCodec.EncodeRegister(CarNum, info); + var url = $"http://{masterIp}:{masterPort}/multi-vehicle-register-bin"; + _ = PostBinaryAsync(hc, url, payload, "register"); + } + catch (Exception e) + { + DLog.Log($"register binary encode failed: {e.GetBaseException().Message}", "MultiVehicle"); + } } - private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification) + private void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification) { - // F: POST + JSON body，payload 不再受 URL 长度限制；fire-and-forget 但记录失败。 - var notifyJson = JsonConvert.SerializeObject(notification); - var url = $"http://{ip}:{port}/multi-vehicle-notify"; - var content = new StringContent(notifyJson, System.Text.Encoding.UTF8, "application/json"); - _ = hc.PostAsync(url, content).ContinueWith(t => + try { - content.Dispose(); - if (t.IsFaulted) - DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle"); - }, TaskScheduler.Default); + var payload = VehicleSyncBinaryCodec.EncodeNotification(notification); + var url = $"http://{ip}:{port}/multi-vehicle-notify-bin"; + _ = PostBinaryAsync(hc, url, payload, $"notify {ip}:{port}"); + } + catch (Exception e) + { + DLog.Log($"notify {ip}:{port} binary encode failed: {e.GetBaseException().Message}", "MultiVehicle"); + } + } + + private static async Task PostBinaryAsync(HttpClient hc, string url, byte[] payload, string description) + { + try + { + using var content = new ByteArrayContent(payload); + content.Headers.ContentType = + new System.Net.Http.Headers.MediaTypeHeaderValue("application/octet-stream"); + using var response = await hc.PostAsync(url, content).ConfigureAwait(false); + if (response.IsSuccessStatusCode) + return; + + DLog.Log( + $"{description} binary failed: HTTP {(int)response.StatusCode} {response.ReasonPhrase}", + "MultiVehicle"); + } + catch (Exception e) + { + DLog.Log($"{description} binary failed: {e.GetBaseException().Message}", "MultiVehicle"); + } } private void ResolveMultiVehicleSelfEndpoint(out string ip, out int port) diff --git a/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs b/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs new file mode 100644 index 0000000..11ef343 --- /dev/null +++ b/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs @@ -0,0 +1,249 @@ +using System; +using System.Collections.Generic; +using System.IO; +using System.Text; + +namespace MultiWheelC; + +internal static class VehicleSyncBinaryCodec +{ + private const byte Version = 1; + private const byte RegisterType = 1; + private const byte NotificationType = 2; + private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1"); + + public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info) + { + using var stream = new MemoryStream(); + using var writer = new BinaryWriter(stream, Encoding.UTF8); + WriteHeader(writer, RegisterType); + writer.Write(carNum); + WriteInfo(writer, info); + writer.Flush(); + return stream.ToArray(); + } + + public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload) + { + using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload))); + using var reader = new BinaryReader(stream, Encoding.UTF8); + ReadHeader(reader, RegisterType); + var carNum = reader.ReadInt32(); + var info = ReadInfo(reader); + EnsureFullyRead(stream); + return (carNum, info); + } + + public static byte[] EncodeNotification(VehicleSyncNotification notification) + { + using var stream = new MemoryStream(); + using var writer = new BinaryWriter(stream, Encoding.UTF8); + WriteHeader(writer, NotificationType); + + writer.Write(notification.Seq); + writer.Write(BuildNotificationFlags(notification)); + writer.Write(notification.Mode); + writer.Write(notification.FleetStopSourceCar); + writer.Write(notification.CenterX); + writer.Write(notification.CenterY); + writer.Write(notification.CenterTh); + writer.Write(notification.FleetVx); + writer.Write(notification.FleetFrontTh); + writer.Write(notification.FleetRearTh); + writer.Write(notification.FleetOmega); + writer.Write(notification.RequestedFleetOmega); + writer.Write(notification.SyncTh); + writer.Write(notification.SyncDistance); + writer.Write(notification.DeltaDetectCenter); + writer.Write(notification.IdealX); + writer.Write(notification.IdealY); + writer.Write(notification.IdealTh); + WriteString(writer, notification.FleetStopReason); + + var fleet = notification.Fleet ?? new Dictionary(); + if (fleet.Count > ushort.MaxValue) + throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit."); + writer.Write((ushort)fleet.Count); + foreach (var kv in fleet) + { + writer.Write(kv.Key); + WriteInfo(writer, kv.Value); + } + + writer.Flush(); + return stream.ToArray(); + } + + public static VehicleSyncNotification DecodeNotification(byte[] payload) + { + using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload))); + using var reader = new BinaryReader(stream, Encoding.UTF8); + ReadHeader(reader, NotificationType); + + var notification = new VehicleSyncNotification + { + Seq = reader.ReadInt64() + }; + + ApplyNotificationFlags(notification, reader.ReadUInt16()); + notification.Mode = reader.ReadInt32(); + notification.FleetStopSourceCar = reader.ReadInt32(); + notification.CenterX = reader.ReadSingle(); + notification.CenterY = reader.ReadSingle(); + notification.CenterTh = reader.ReadSingle(); + notification.FleetVx = reader.ReadSingle(); + notification.FleetFrontTh = reader.ReadSingle(); + notification.FleetRearTh = reader.ReadSingle(); + notification.FleetOmega = reader.ReadSingle(); + notification.RequestedFleetOmega = reader.ReadSingle(); + notification.SyncTh = reader.ReadSingle(); + notification.SyncDistance = reader.ReadSingle(); + notification.DeltaDetectCenter = reader.ReadSingle(); + notification.IdealX = reader.ReadSingle(); + notification.IdealY = reader.ReadSingle(); + notification.IdealTh = reader.ReadSingle(); + notification.FleetStopReason = ReadString(reader); + + var fleetCount = reader.ReadUInt16(); + notification.Fleet = new Dictionary(fleetCount); + for (var i = 0; i < fleetCount; ++i) + { + var carNum = reader.ReadInt32(); + notification.Fleet[carNum] = ReadInfo(reader); + } + + EnsureFullyRead(stream); + return notification; + } + + private static void WriteHeader(BinaryWriter writer, byte type) + { + writer.Write(Magic); + writer.Write(Version); + writer.Write(type); + writer.Write((ushort)0); + } + + private static void ReadHeader(BinaryReader reader, byte expectedType) + { + for (var i = 0; i < Magic.Length; ++i) + { + if (reader.ReadByte() != Magic[i]) + throw new InvalidDataException("Invalid multi-vehicle sync binary magic."); + } + + var version = reader.ReadByte(); + if (version != Version) + throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}."); + + var type = reader.ReadByte(); + if (type != expectedType) + throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}."); + + var reserved = reader.ReadUInt16(); + if (reserved != 0) + throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field."); + } + + private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info) + { + writer.Write(BuildInfoFlags(info)); + WriteString(writer, info.Ip); + writer.Write(info.Port); + writer.Write(info.X); + writer.Write(info.Y); + writer.Write(info.Th); + writer.Write(info.LayoutX); + writer.Write(info.LayoutY); + writer.Write(info.LayoutTh); + WriteString(writer, info.MotionInfeasibleReason); + WriteString(writer, info.RotateWheelAlignDetail); + } + + private static VehicleSyncInfo ReadInfo(BinaryReader reader) + { + var info = new VehicleSyncInfo(); + ApplyInfoFlags(info, reader.ReadUInt16()); + info.Ip = ReadString(reader); + info.Port = reader.ReadInt32(); + info.X = reader.ReadSingle(); + info.Y = reader.ReadSingle(); + info.Th = reader.ReadSingle(); + info.LayoutX = reader.ReadSingle(); + info.LayoutY = reader.ReadSingle(); + info.LayoutTh = reader.ReadSingle(); + info.MotionInfeasibleReason = ReadString(reader); + info.RotateWheelAlignDetail = ReadString(reader); + return info; + } + + private static ushort BuildInfoFlags(VehicleSyncInfo info) + { + ushort flags = 0; + if (info.Master) flags |= 1 << 0; + if (info.PosAvailable) flags |= 1 << 1; + if (info.Aligned) flags |= 1 << 2; + if (info.DetectOk) flags |= 1 << 3; + if (info.MotionFeasible) flags |= 1 << 4; + if (info.RotateWheelsAligned) flags |= 1 << 5; + return flags; + } + + private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags) + { + info.Master = (flags & (1 << 0)) != 0; + info.PosAvailable = (flags & (1 << 1)) != 0; + info.Aligned = (flags & (1 << 2)) != 0; + info.DetectOk = (flags & (1 << 3)) != 0; + info.MotionFeasible = (flags & (1 << 4)) != 0; + info.RotateWheelsAligned = (flags & (1 << 5)) != 0; + } + + private static ushort BuildNotificationFlags(VehicleSyncNotification notification) + { + ushort flags = 0; + if (notification.PosAvailable) flags |= 1 << 0; + if (notification.Aligned) flags |= 1 << 1; + if (notification.FleetMotionReleased) flags |= 1 << 2; + if (notification.FleetStopActive) flags |= 1 << 3; + if (notification.AutoEnabled) flags |= 1 << 4; + if (notification.ManualEnabled) flags |= 1 << 5; + if (notification.HasIdeal) flags |= 1 << 6; + return flags; + } + + private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags) + { + notification.PosAvailable = (flags & (1 << 0)) != 0; + notification.Aligned = (flags & (1 << 1)) != 0; + notification.FleetMotionReleased = (flags & (1 << 2)) != 0; + notification.FleetStopActive = (flags & (1 << 3)) != 0; + notification.AutoEnabled = (flags & (1 << 4)) != 0; + notification.ManualEnabled = (flags & (1 << 5)) != 0; + notification.HasIdeal = (flags & (1 << 6)) != 0; + } + + private static void WriteString(BinaryWriter writer, string value) + { + var bytes = Encoding.UTF8.GetBytes(value ?? ""); + if (bytes.Length > ushort.MaxValue) + throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit."); + writer.Write((ushort)bytes.Length); + writer.Write(bytes); + } + + private static string ReadString(BinaryReader reader) + { + var length = reader.ReadUInt16(); + var bytes = reader.ReadBytes(length); + if (bytes.Length != length) + throw new EndOfStreamException("Truncated multi-vehicle sync string payload."); + return Encoding.UTF8.GetString(bytes); + } + + private static void EnsureFullyRead(MemoryStream stream) + { + if (stream.Position != stream.Length) + throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet."); + } +}