First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use binary multi-vehicle sync transport

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-01 13:27:15 +08:00
parent 3aa979be8b
commit c9cb3a5df3
2 changed files with 348 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/PilotDefinition.cs
+79 -51
View File
@@ -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,56 +425,72 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
var hc = new HttpClient { Timeout = TimeSpan.FromSeconds(2) };
PicoHttpServer.AddGetHandler("/multi-vehicle-register", new { CarNum = 0, Info = "" }, query =>
PicoHttpServer.AddPostByteHandler("/multi-vehicle-register-bin", body =>
{
try
{
var infoJson = Uri.UnescapeDataString(query.Info ?? "");
var info = JsonConvert.DeserializeObject<VehicleSyncInfo>(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<VehicleSyncNotification>(body ?? "")
?? throw new Exception("notification is null");
// 乱序丢弃：仅应用序列号大于已应用值的包。Seq==0 视为旧版无序列号始终接受；
// 若 Seq 明显回退(差值>100)，判定为主车重启的新会话，重新接受并对齐序列号。
var notification = VehicleSyncBinaryCodec.DecodeNotification(body);
return ApplyVehicleSyncNotification(notification);
}
catch (Exception e)
{
DLog.Log($"/multi-vehicle-notify-bin error: {e.FormatEx()}", "MultiVehicle");
throw;
}
});
DLog.Log("MultiVehicle coordination initialized, starting loop thread", "MultiVehicle");
Console.WriteLine("[MultiVehicle] coordination initialized, starting loop thread");
new Thread(() => 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 JsonConvert.SerializeObject(new { code = 200, message = "stale" });
return "stale";
_multiVehicleAppliedSeq = notification.Seq;
MultiVehicleAligned = notification.Aligned;
lock (FleetLock)
{
MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
// 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)
{
@@ -487,18 +502,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
$"reason={notification.FleetStopReason}",
"MultiVehicleRemoteDbg");
}
return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
}
catch (Exception e)
{
DLog.Log($"/multi-vehicle-notify error: {e.FormatEx()}", "MultiVehicle");
return JsonConvert.SerializeObject(new { code = 500, message = e.Message });
}
});
DLog.Log("MultiVehicle coordination initialized, starting loop thread", "MultiVehicle");
Console.WriteLine("[MultiVehicle] coordination initialized, starting loop thread");
new Thread(() => MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start();
return "ok";
}
private void MultiVehicleLoop(HttpClient hc)
@@ -1524,29 +1529,52 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
private void FireAndForgetRegister(HttpClient hc, VehicleSyncInfo info)
{
ParseMasterEndpoint(out var masterIp, out var masterPort);
var infoJson = JsonConvert.SerializeObject(info);
var url =
$"http://{masterIp}:{masterPort}/multi-vehicle-register?CarNum={CarNum}&Info={Uri.EscapeDataString(infoJson)}";
_ = hc.GetStringAsync(url).ContinueWith(t =>
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)
MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs
+249
View File
@@ -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<int, VehicleSyncInfo>();
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<int, VehicleSyncInfo>(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.");
}
}