ruifeng.zhouandCursor
235c6c70ac
Restructure into DiffWheel/MultiWheel folders, add DiffWheelC and MultiWheelC with MovementTests, FleetRemote multi-vehicle manual sync, deploy templates, and documentation. Update dependency paths to D:\MDCS\Release and mirror motor feedback in MotorRoutine for simulation. Co-authored-by: Cursor <cursoragent@cursor.com>
774 lines
36 KiB
C#
774 lines
36 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Drawing;
|
||
using System.Linq;
|
||
using System.Net.Http;
|
||
using System.Numerics;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
using ClumsyCore;
|
||
using ClumsyCore.DTools;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Pilot;
|
||
using ClumsyCore.Utilities;
|
||
using CommonUsage;
|
||
using CommonUsage.Chassis;
|
||
using FundamentalLib;
|
||
using FundamentalLib.Utilities;
|
||
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
|
||
using Newtonsoft.Json;
|
||
using Vector = ClumsyCore.Utilities.Vector;
|
||
|
||
namespace MultiWheelC;
|
||
|
||
public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefinition>
|
||
{
|
||
public new float CarLength = 1472f;
|
||
public new float CarWidth = 948f;
|
||
|
||
public override void StandardInit()
|
||
{
|
||
InitMultiVehicleCoordination();
|
||
}
|
||
|
||
#region MultiVehicleCoordination (从 MDCSToolBox 内联回 Tutorial)
|
||
|
||
[AsLowerIO(desc = "启用（手动）多车联动")] public bool MultiVehicleManualEnabled;
|
||
[AsLowerIO(desc = "（手动）多车联动模式")] public int MultiVehicleManualMode;
|
||
[FieldMember(desc = "多车联动已同步")] public bool MultiVehicleAligned;
|
||
|
||
[AsLowerIO(desc = "多车联动：遥控器Vx")] public float MultiVehicleManualVx;
|
||
[AsLowerIO(desc = "多车联动：遥控器Vy")] public float MultiVehicleManualVy;
|
||
[AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth;
|
||
|
||
[AsUpperIO(desc = "多车联动：自动驾驶", timeOutReset = true)] public bool MultiVehicleAutoEnabled;
|
||
[FieldMember(desc = "多车联动：自动驾驶Vx")] public float MultiVehicleAutoVx;
|
||
[FieldMember(desc = "多车联动：自动驾驶FrontTh")] public float MultiVehicleAutoFrontTh;
|
||
[FieldMember(desc = "多车联动：自动驾驶RearTh")] public float MultiVehicleAutoRearTh;
|
||
|
||
public Dictionary<int, VehicleSyncInfo> MultiVehicleFleet = new();
|
||
public VehicleSyncNotification MultiVehicleNotification;
|
||
|
||
[FieldMember(desc = "多车联动：车队姿态x")] public float CenterX;
|
||
[FieldMember(desc = "多车联动：车队姿态y")] public float CenterY;
|
||
[FieldMember(desc = "多车联动：车队姿态th")] public float CenterTh;
|
||
|
||
[AsLowerIO(desc = "车号")] public int CarNum = 1;
|
||
|
||
private float _multiVehicleAccumulateTh;
|
||
private DateTime _multiVehicleLastThTime = DateTime.Now;
|
||
private readonly object _multiVehicleNotificationLock = new();
|
||
private DateTime _multiVehicleLastNotifyTime = DateTime.MinValue;
|
||
private bool _multiVehicleSyncInitialized;
|
||
|
||
// 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
|
||
private DateTime _mvDbgLastLog = DateTime.MinValue;
|
||
private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist;
|
||
|
||
// 写盘诊断日志（Clumsy 侧）。文件落在 Clumsy 工作目录，复现后离线分析。
|
||
// 每次进程启动清空一次（_fleetDiagInit），同一次运行内追加，避免跨多次运行累积。
|
||
private DateTime _mvDiskLastLog = DateTime.MinValue;
|
||
private static bool _fleetDiagInit;
|
||
private void FleetDiag(string msg)
|
||
{
|
||
try
|
||
{
|
||
const string path = "fleet_diag_clumsy.log";
|
||
var line = $"{DateTime.Now:HH:mm:ss.fff} car{CarNum} {msg}{Environment.NewLine}";
|
||
if (!_fleetDiagInit)
|
||
{
|
||
_fleetDiagInit = true;
|
||
System.IO.File.WriteAllText(path,
|
||
$"=== session start {DateTime.Now:yyyy-MM-dd HH:mm:ss} ==={Environment.NewLine}" + line);
|
||
}
|
||
else
|
||
System.IO.File.AppendAllText(path, line);
|
||
}
|
||
catch
|
||
{
|
||
// 诊断日志失败不影响主流程
|
||
}
|
||
}
|
||
|
||
// 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
|
||
private readonly object _neighborDetectLock = new();
|
||
private readonly List<(DateTime Time, Vector2 Src, Vector2 Dst)> _neighborDetects = new();
|
||
|
||
// 互识别：上一帧检测中心，用作下一帧猜测（闭环跟踪），与「多舵轮-2腿检测」MovementTest 一致
|
||
private Vector2 _neighborGuess;
|
||
private bool _neighborGuessValid;
|
||
private DateTime _neighborGuessTime = DateTime.MinValue;
|
||
|
||
/// <summary>
|
||
/// 互识别钩子：用单线雷达识别邻车的两腿/两轮，换算成本车体坐标系下相对“理应对齐参考点”的偏差。
|
||
/// 全对齐时返回 (true, 0, 0, 0)；未检测到时返回 (false, ...)。
|
||
/// detectDistance = 编队车间距 - 检测中心偏移（即邻车检测中心到本车原点的标称距离，恒为正值，方向由 TwoLegGuessX 符号决定）。
|
||
/// </summary>
|
||
private (bool Valid, float Dx, float Dy, float Dth, float NeighborDist) DetectNeighborBias(float detectDistance)
|
||
{
|
||
// 检测猜测点与「多舵轮-2腿检测」MovementTest 完全一致：首帧 / 跟丢回落都用 Conf.TwoLegGuessX
|
||
// （方向与距离都由它一处决定），跟到后用上一帧中心闭环跟踪。
|
||
// detectDistance 只用于计算编队对齐偏差/间距，不参与“去哪里找邻车”，避免猜测点偏离导致 ROI 框漏掉真实邻车。
|
||
var nominal = new Vector2(Conf.TwoLegGuessX, 0f);
|
||
|
||
var now = DateTime.Now;
|
||
// 闭环跟踪：1s 内检到过就用上一帧中心作猜测，使绿色 ROI 框收敛到真实邻车（与 MovementTest 行为一致）；
|
||
// 跟丢超时则回落到编队标称位置，避免框漂走。
|
||
var guess = (_neighborGuessValid && (now - _neighborGuessTime).TotalSeconds < 1) ? _neighborGuess : nominal;
|
||
var seg = TwoLegDetect.Detect(Conf.TwoLegLidarName, guess.X, TwoLegDetect.SetFilters(guess.X, guess.Y));
|
||
|
||
Vector2 avgSrc, avgDst;
|
||
lock (_neighborDetectLock)
|
||
{
|
||
if (seg != null)
|
||
{
|
||
_neighborDetects.Add((now, seg.Src, seg.Dst));
|
||
_neighborGuess = (seg.Src + seg.Dst) / 2f;
|
||
_neighborGuessValid = true;
|
||
_neighborGuessTime = now;
|
||
}
|
||
_neighborDetects.RemoveAll(r => (now - r.Time).TotalSeconds > 1);
|
||
|
||
var cnt = _neighborDetects.Count;
|
||
if (cnt == 0)
|
||
{
|
||
_neighborGuessValid = false;
|
||
Hedingben.ToastText($"邻车未检到 (guess x:{guess.X:F0} y:{guess.Y:F0})", $"MultiVehicle{CarNum}-detect");
|
||
return (false, 0f, 0f, 0f, 0f);
|
||
}
|
||
if (cnt > 10) _neighborDetects.RemoveRange(0, cnt - 10);
|
||
|
||
avgSrc = new Vector2(_neighborDetects.Average(r => r.Src.X), _neighborDetects.Average(r => r.Src.Y));
|
||
avgDst = new Vector2(_neighborDetects.Average(r => r.Dst.X), _neighborDetects.Average(r => r.Dst.Y));
|
||
}
|
||
|
||
// 两腿连线中点为邻车检测中心；连线的垂线方向即邻车朝向
|
||
var dirVec = avgDst - avgSrc;
|
||
var center = (avgSrc + avgDst) / 2f;
|
||
var dir = (float)LessMath.RoundTh(Math.Atan2(dirVec.Y, dirVec.X) / Math.PI * 180 + 90);
|
||
|
||
// 从检测中心沿邻车朝向外推 detectDistance，得到“本车理应所在的参考点”（车体系）
|
||
var target = LessMath.Transform2D(center, dir, detectDistance, 0);
|
||
|
||
var dx = target.X;
|
||
var dy = target.Y;
|
||
var dth = (float)LessMath.ThDiff(dir, 0);
|
||
var neighborDist = center.Length();
|
||
|
||
_mvLastDetCenterX = center.X;
|
||
_mvLastDetCenterY = center.Y;
|
||
_mvLastDetDir = dir;
|
||
_mvLastDetDist = neighborDist;
|
||
|
||
var painter = UI.GetPainter("MultiVehicleDetectBias", false);
|
||
painter.Clear();
|
||
painter.DrawLine(Color.DeepPink, center, target, width: 2);
|
||
painter.DrawText(Color.Yellow, $"dx:{dx:F0} dy:{dy:F0} dth:{dth:F2}", center.X, center.Y);
|
||
|
||
Hedingben.ToastText(
|
||
$"[联动检测] lidar:{Conf.TwoLegLidarName} guess x:{guess.X:F0} y:{guess.Y:F0} | " +
|
||
$"中心 x:{center.X:F0} y:{center.Y:F0} dir:{dir:F1} dist:{neighborDist:F0} | 偏差 dx:{dx:F0} dy:{dy:F0} dth:{dth:F2}",
|
||
$"MultiVehicle{CarNum}-detect");
|
||
|
||
return (true, dx, dy, dth, neighborDist);
|
||
}
|
||
|
||
private void InitMultiVehicleCoordination()
|
||
{
|
||
if (_multiVehicleSyncInitialized) return;
|
||
_multiVehicleSyncInitialized = true;
|
||
|
||
var hc = new HttpClient { Timeout = TimeSpan.FromSeconds(2) };
|
||
|
||
PicoHttpServer.AddGetHandler("/multi-vehicle-register", new { CarNum = 0, Info = "" }, query =>
|
||
{
|
||
try
|
||
{
|
||
var infoJson = Uri.UnescapeDataString(query.Info ?? "");
|
||
var info = JsonConvert.DeserializeObject<VehicleSyncInfo>(infoJson)
|
||
?? throw new Exception("VehicleSyncInfo is null");
|
||
lock (MultiVehicleFleet)
|
||
MultiVehicleFleet[query.CarNum] = info;
|
||
return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
DLog.Log($"/multi-vehicle-register error: {e.FormatEx()}", "MultiVehicle");
|
||
return JsonConvert.SerializeObject(new { code = 500, message = e.Message });
|
||
}
|
||
});
|
||
|
||
PicoHttpServer.AddGetHandler("/multi-vehicle-notify", new { Notification = "" }, query =>
|
||
{
|
||
try
|
||
{
|
||
var notifyJson = Uri.UnescapeDataString(query.Notification ?? "");
|
||
var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(notifyJson)
|
||
?? throw new Exception("notification is null");
|
||
MultiVehicleAligned = notification.Aligned;
|
||
lock (MultiVehicleFleet)
|
||
MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
|
||
lock (_multiVehicleNotificationLock)
|
||
{
|
||
MultiVehicleNotification = notification;
|
||
_multiVehicleLastNotifyTime = DateTime.Now;
|
||
}
|
||
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 });
|
||
}
|
||
});
|
||
|
||
new Thread(() => MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start();
|
||
}
|
||
|
||
private void MultiVehicleLoop(HttpClient hc)
|
||
{
|
||
var carLength = CarLength;
|
||
var carWidth = CarWidth;
|
||
var contour = new List<Vector2>
|
||
{
|
||
new(carLength / 2f, carWidth / 2f),
|
||
new(-carLength / 2f, carWidth / 2f),
|
||
new(-carLength / 2f, -carWidth / 2f),
|
||
new(carLength / 2f, -carWidth / 2f),
|
||
};
|
||
var chassis = (MultiWheelChassis)Chassis;
|
||
var sendMotionPainter = UI.GetPainter("MultiWheelChassis-SendMotionVis", false);
|
||
chassis.SendMotionVisualizer = new Visualizer
|
||
{
|
||
LineAction = (color, src, dst, startArrow, endArrow, width) =>
|
||
sendMotionPainter.DrawLine(color, src, dst, startArrow, endArrow, width),
|
||
TextAction = (color, str, pos) => sendMotionPainter.DrawText(color, str, pos),
|
||
Clear = () => sendMotionPainter.Clear()
|
||
};
|
||
|
||
while (true)
|
||
{
|
||
try
|
||
{
|
||
TickMultiVehicle(hc, chassis, contour, sendMotionPainter);
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
DLog.Log($"MultiVehicle loop error: {e.FormatEx()}", "MultiVehicle");
|
||
}
|
||
|
||
Thread.Sleep(Conf.MultiVehicleSyncInterval);
|
||
}
|
||
}
|
||
|
||
private void TickMultiVehicle(HttpClient hc, MultiWheelChassis chassis, List<Vector2> contour, Painter sendMotionPainter)
|
||
{
|
||
var isMaster = IsMultiVehicleMaster();
|
||
var autoEnabled = false;
|
||
var manualEnabled = MultiVehicleManualEnabled;
|
||
|
||
if (isMaster)
|
||
autoEnabled = MultiVehicleAutoEnabled;
|
||
else
|
||
{
|
||
// 从车：自动/手动联动均可由主车广播解锁（无需各自再拨开关）。
|
||
// 仅在 notification 新鲜时认账，主车停发后超时即自动停车，避免用旧指令跑飞。
|
||
lock (_multiVehicleNotificationLock)
|
||
{
|
||
var fresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
|
||
Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
|
||
if (fresh && MultiVehicleNotification != null)
|
||
{
|
||
autoEnabled = MultiVehicleNotification.AutoEnabled;
|
||
manualEnabled = manualEnabled || MultiVehicleNotification.ManualEnabled;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 入口诊断（节流 ~300ms）：记录从 Medulla 收到的原始 IO 值与门控判定，
|
||
// 用于确认遥控指令是否真的传到了 Clumsy，以及为何提前 return。
|
||
if ((DateTime.Now - _mvDiskLastLog).TotalMilliseconds >= 300)
|
||
{
|
||
_mvDiskLastLog = DateTime.Now;
|
||
int fleetCnt;
|
||
lock (MultiVehicleFleet) fleetCnt = MultiVehicleFleet.Count;
|
||
var notifFresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
|
||
Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
|
||
FleetDiag(
|
||
$"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " +
|
||
$"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
|
||
$"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
|
||
$"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
|
||
$"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} pos={Conf.PosAvailable}");
|
||
}
|
||
|
||
if (!manualEnabled && !autoEnabled)
|
||
{
|
||
UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
|
||
sendMotionPainter.Clear();
|
||
lock (MultiVehicleFleet)
|
||
MultiVehicleFleet.Clear();
|
||
MultiVehicleAutoEnabled = false;
|
||
_multiVehicleAccumulateTh = 0f;
|
||
_multiVehicleLastThTime = DateTime.Now;
|
||
|
||
if (!isMaster)
|
||
FireAndForgetRegister(hc, BuildSelfInfo(false, false, 0, 0, 0, 0, 0, 0, false));
|
||
|
||
return;
|
||
}
|
||
|
||
if (isMaster && autoEnabled && !MultiVehicleManualEnabled && !FleetHasPosAvailable())
|
||
{
|
||
MultiVehicleAutoEnabled = false;
|
||
Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
|
||
return;
|
||
}
|
||
|
||
var posAvailable = Conf.PosAvailable;
|
||
float selfX = 0, selfY = 0, selfTh = 0;
|
||
if (posAvailable)
|
||
{
|
||
var carPos = DetourInterface.getCartLocation();
|
||
selfX = (float)carPos.x;
|
||
selfY = (float)carPos.y;
|
||
selfTh = (float)carPos.th;
|
||
}
|
||
|
||
var syncTh = Conf.TestCarSyncTh;
|
||
var syncDistance = Conf.TestCarSyncDistance;
|
||
var deltaDetectCenter = Conf.DeltaDetectCenter;
|
||
float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0;
|
||
|
||
CenterX = CenterY = CenterTh = 0;
|
||
|
||
if (isMaster)
|
||
{
|
||
if (MultiVehicleManualEnabled)
|
||
{
|
||
syncTh = 0;
|
||
fleetVx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
|
||
var targetTh = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
|
||
var now = DateTime.Now;
|
||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
|
||
_multiVehicleLastThTime = now;
|
||
var sign = Math.Sign(targetTh - _multiVehicleAccumulateTh);
|
||
_multiVehicleAccumulateTh += sign * Math.Min(Math.Abs(targetTh - _multiVehicleAccumulateTh),
|
||
Conf.SyncThAccPerSec * dt);
|
||
fleetFrontTh = _multiVehicleAccumulateTh;
|
||
fleetRearTh = -fleetFrontTh;
|
||
}
|
||
else
|
||
{
|
||
fleetVx = MultiVehicleAutoVx;
|
||
fleetFrontTh = MultiVehicleAutoFrontTh;
|
||
fleetRearTh = MultiVehicleAutoRearTh;
|
||
}
|
||
}
|
||
else if (MultiVehicleNotification != null)
|
||
{
|
||
VehicleSyncNotification notification;
|
||
lock (_multiVehicleNotificationLock)
|
||
notification = MultiVehicleNotification;
|
||
|
||
syncTh = notification.SyncTh;
|
||
syncDistance = notification.SyncDistance;
|
||
deltaDetectCenter = notification.DeltaDetectCenter;
|
||
CenterX = notification.CenterX;
|
||
CenterY = notification.CenterY;
|
||
CenterTh = notification.CenterTh;
|
||
posAvailable = notification.PosAvailable;
|
||
MultiVehicleAutoEnabled = notification.AutoEnabled;
|
||
fleetVx = notification.FleetVx;
|
||
fleetFrontTh = notification.FleetFrontTh;
|
||
fleetRearTh = notification.FleetRearTh;
|
||
}
|
||
|
||
var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
|
||
|
||
if (isMaster)
|
||
{
|
||
lock (MultiVehicleFleet)
|
||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
|
||
layoutX, layoutY, layoutTh, true);
|
||
|
||
if (TryInferFleetCenter(out var cx, out var cy, out var cth))
|
||
{
|
||
CenterX = cx;
|
||
CenterY = cy;
|
||
CenterTh = cth;
|
||
}
|
||
}
|
||
|
||
var selfAligned = !Conf.MultiVehicleUseDetect;
|
||
var detectValid = false;
|
||
float detectDx = 0, detectDy = 0, detectDth = 0;
|
||
var currentSpacing = float.NaN;
|
||
if (Conf.MultiVehicleUseDetect)
|
||
{
|
||
var (valid, dx, dy, dth, neighborDist) = DetectNeighborBias(syncDistance - deltaDetectCenter);
|
||
detectValid = valid;
|
||
if (valid)
|
||
{
|
||
detectDx = dx;
|
||
detectDy = dy;
|
||
detectDth = dth;
|
||
// 检测中心到本车原点距离 + 检测中心偏移 = 两车参考点当前间距
|
||
currentSpacing = neighborDist + deltaDetectCenter;
|
||
selfAligned = Math.Abs(dx) < Conf.SingleCarSyncPrecisionXy &&
|
||
Math.Abs(dy) < Conf.SingleCarSyncPrecisionXy &&
|
||
Math.Abs(dth) < Conf.SingleCarSyncPrecisionTh;
|
||
}
|
||
else selfAligned = false;
|
||
}
|
||
|
||
// 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（两台车都需有定位）
|
||
if (float.IsNaN(currentSpacing) && posAvailable)
|
||
currentSpacing = TryGetSlamSpacing(selfX, selfY);
|
||
|
||
if (float.IsNaN(currentSpacing))
|
||
Hedingben.ToastText($"间距 当前:N/A 目标:{syncDistance:F0}mm", $"MultiVehicle{CarNum}-spacing");
|
||
else
|
||
Hedingben.ToastText(
|
||
$"间距 当前:{currentSpacing:F0}mm 目标:{syncDistance:F0}mm 差:{currentSpacing - syncDistance:F0}mm",
|
||
$"MultiVehicle{CarNum}-spacing");
|
||
|
||
lock (MultiVehicleFleet)
|
||
{
|
||
MultiVehicleAligned = MultiVehicleFleet.Count == Conf.MultiVehicleFleetNum &&
|
||
MultiVehicleFleet.Values.All(v => v.Aligned);
|
||
}
|
||
|
||
// 安全门：开启互识别时，本车或任一其它车检测不到邻车则整队停车（速度置零）。
|
||
// ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
|
||
var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
|
||
bool othersDetectOk;
|
||
lock (MultiVehicleFleet)
|
||
othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
|
||
var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk);
|
||
if (!canMove)
|
||
{
|
||
fleetVx = 0;
|
||
fleetFrontTh = 0;
|
||
fleetRearTh = 0;
|
||
Hedingben.ToastText(
|
||
$"车队停车：{(!ownDetectOk ? "本车" : "其它车")}2腿检测丢失（速度已置零）",
|
||
$"MultiVehicle{CarNum}-stop");
|
||
}
|
||
else
|
||
{
|
||
Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
|
||
}
|
||
|
||
VisualizeFleet(contour, layoutX, layoutY, layoutTh);
|
||
|
||
var fleetReady = false;
|
||
var fleetCount = 0;
|
||
lock (MultiVehicleFleet)
|
||
{
|
||
fleetCount = MultiVehicleFleet.Count;
|
||
fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
|
||
}
|
||
|
||
// 补偿量提到块外，便于诊断日志统一记录三类来源（检测/SLAM）的贡献。
|
||
float xDetectCompensate = 0, yDetectCompensate = 0, thDetectCompensate = 0;
|
||
float xPosCompensate = 0, yPosCompensate = 0, thPosCompensate = 0;
|
||
float posBiasX = 0, posBiasY = 0, posBiasTh = 0;
|
||
|
||
if (fleetReady)
|
||
{
|
||
chassis.SetOriginBias(layoutX, layoutY, layoutTh);
|
||
chassis.ControlPointRadius = syncDistance / 2f;
|
||
|
||
if (canMove && Conf.MultiVehicleUseDetect)
|
||
{
|
||
xDetectCompensate = Math.Abs(detectDx) > Conf.SingleCarSyncPrecisionXy
|
||
? detectDx * Conf.MultiVehicleDetectBiasXFac : 0;
|
||
yDetectCompensate = Math.Abs(detectDy) > Conf.SingleCarSyncPrecisionXy
|
||
? detectDy * Conf.MultiVehicleDetectBiasYFac : 0;
|
||
thDetectCompensate = Math.Abs(detectDth) > Conf.SingleCarSyncPrecisionTh
|
||
? detectDth * Conf.MultiVehicleDetectBiasThFac : 0;
|
||
|
||
xDetectCompensate = ClampBias(xDetectCompensate, Conf.MultiVehicleDetectBiasXThreshold);
|
||
yDetectCompensate = ClampBias(yDetectCompensate, Conf.MultiVehicleDetectBiasYThreshold);
|
||
thDetectCompensate = ClampBias(thDetectCompensate, Conf.MultiVehicleDetectBiasThThreshold);
|
||
}
|
||
|
||
if (canMove && posAvailable && TryInferFleetCenter(out _, out _, out _))
|
||
{
|
||
var supposedPos = LessMath.Transform2D(
|
||
Tuple.Create(CenterX, CenterY, CenterTh),
|
||
Tuple.Create(layoutX, layoutY, layoutTh));
|
||
var posBias = LessMath.SolveTransform2D(Tuple.Create(selfX, selfY, selfTh), supposedPos);
|
||
posBiasX = (float)posBias.Item1;
|
||
posBiasY = (float)posBias.Item2;
|
||
posBiasTh = (float)posBias.Item3;
|
||
|
||
xPosCompensate = Math.Abs(posBias.Item1) > Conf.SingleCarSyncPrecisionXy
|
||
? ClampBias(posBias.Item1 * Conf.MultiVehiclePosBiasXFac, Conf.MultiVehiclePosBiasXThreshold)
|
||
: 0;
|
||
yPosCompensate = Math.Abs(posBias.Item2) > Conf.SingleCarSyncPrecisionXy
|
||
? ClampBias(posBias.Item2 * Conf.MultiVehiclePosBiasYFac, Conf.MultiVehiclePosBiasYThreshold)
|
||
: 0;
|
||
thPosCompensate = Math.Abs(posBias.Item3) > Conf.SingleCarSyncPrecisionTh
|
||
? ClampBias(posBias.Item3 * Conf.MultiVehiclePosBiasThFac, Conf.MultiVehiclePosBiasThThreshold)
|
||
: 0;
|
||
}
|
||
|
||
sendMotionPainter.Clear();
|
||
chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510,
|
||
localCompensateX: xDetectCompensate + xPosCompensate,
|
||
localCompensateY: yDetectCompensate + yPosCompensate,
|
||
localCompensateTh: thDetectCompensate + thPosCompensate);
|
||
}
|
||
|
||
// === 诊断日志（节流 ~300ms）===
|
||
// 用于定位“剧烈运动”来源：BASE=遥控基础速度；DETECT=2腿检测补偿；POS=SLAM编队补偿；SEND=最终下发。
|
||
// 若 BASE≈0 但 SEND 持续非零，说明是补偿在驱动；再看 DETECT/POS 哪一路的 comp 持续非零即定位到来源。
|
||
// 同时落 DLog(tag MultiVehicleDbg) 与屏幕 ToastText(tag MultiVehicle{CarNum}-dbg)，后者无需开启磁盘转储即可直接观察。
|
||
if ((DateTime.Now - _mvDbgLastLog).TotalMilliseconds >= 300)
|
||
{
|
||
_mvDbgLastLog = DateTime.Now;
|
||
var spacingStr = float.IsNaN(currentSpacing) ? "NaN" : currentSpacing.ToString("F0");
|
||
var cx = xDetectCompensate + xPosCompensate;
|
||
var cy = yDetectCompensate + yPosCompensate;
|
||
var cth = thDetectCompensate + thPosCompensate;
|
||
|
||
var dbg =
|
||
$"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} pos:{posAvailable} " +
|
||
$"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} useDetect:{Conf.MultiVehicleUseDetect} " +
|
||
$"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " +
|
||
$"| 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} " +
|
||
$"-> 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} " +
|
||
$"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
|
||
$"| SEND vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2}";
|
||
|
||
DLog.Log(dbg, "MultiVehicleDbg");
|
||
FleetDiag(dbg);
|
||
|
||
// 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储）
|
||
Hedingben.ToastText(
|
||
$"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " +
|
||
$"| ready{fleetReady} canMove{canMove}",
|
||
$"MultiVehicle{CarNum}-dbg");
|
||
Hedingben.ToastText(
|
||
$"DET v{detectValid} dx{detectDx:F0} dy{detectDy:F0} dth{detectDth:F1} cmp({xDetectCompensate:F0},{yDetectCompensate:F0},{thDetectCompensate:F1}) " +
|
||
$"| POS bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) cmp({xPosCompensate:F0},{yPosCompensate:F0},{thPosCompensate:F1})",
|
||
$"MultiVehicle{CarNum}-dbg2");
|
||
}
|
||
|
||
if (isMaster)
|
||
{
|
||
lock (MultiVehicleFleet)
|
||
{
|
||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
|
||
layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
|
||
}
|
||
|
||
if (fleetReady)
|
||
{
|
||
VehicleSyncNotification notification;
|
||
lock (MultiVehicleFleet)
|
||
{
|
||
notification = new VehicleSyncNotification
|
||
{
|
||
PosAvailable = posAvailable,
|
||
CenterX = CenterX,
|
||
CenterY = CenterY,
|
||
CenterTh = CenterTh,
|
||
Aligned = MultiVehicleAligned,
|
||
Fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet),
|
||
FleetVx = fleetVx,
|
||
FleetFrontTh = fleetFrontTh,
|
||
FleetRearTh = fleetRearTh,
|
||
AutoEnabled = MultiVehicleAutoEnabled,
|
||
ManualEnabled = MultiVehicleManualEnabled,
|
||
SyncTh = syncTh,
|
||
SyncDistance = syncDistance,
|
||
DeltaDetectCenter = deltaDetectCenter
|
||
};
|
||
}
|
||
|
||
foreach (var kv in notification.Fleet)
|
||
{
|
||
var ip = kv.Value.Ip;
|
||
var port = kv.Value.Port > 0 ? kv.Value.Port : WebAPI.port;
|
||
if (string.IsNullOrWhiteSpace(ip) || kv.Key == CarNum)
|
||
continue;
|
||
FireAndForgetNotify(hc, ip, port, notification);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
FireAndForgetRegister(hc, BuildSelfInfo(false, posAvailable, selfX, selfY, selfTh,
|
||
layoutX, layoutY, layoutTh, selfAligned, ownDetectOk));
|
||
}
|
||
}
|
||
|
||
private bool IsMultiVehicleMaster() => Conf.MultiVehicleMasterEndpoint == "/";
|
||
|
||
private bool FleetHasPosAvailable()
|
||
{
|
||
lock (MultiVehicleFleet)
|
||
return MultiVehicleFleet.Values.Any(v => v.PosAvailable);
|
||
}
|
||
|
||
// 基于 SLAM 世界坐标的最近邻车间距（mm）；无可用定位邻车时返回 NaN。
|
||
private float TryGetSlamSpacing(float selfX, float selfY)
|
||
{
|
||
List<VehicleSyncInfo> others;
|
||
lock (MultiVehicleFleet)
|
||
others = MultiVehicleFleet
|
||
.Where(kv => kv.Key != CarNum && kv.Value.PosAvailable)
|
||
.Select(kv => kv.Value).ToList();
|
||
|
||
if (others.Count == 0) return float.NaN;
|
||
return others.Min(o => (float)Math.Sqrt((o.X - selfX) * (o.X - selfX) + (o.Y - selfY) * (o.Y - selfY)));
|
||
}
|
||
|
||
private bool TryInferFleetCenter(out float centerX, out float centerY, out float centerTh)
|
||
{
|
||
centerX = centerY = centerTh = 0;
|
||
List<VehicleSyncInfo> positioned;
|
||
lock (MultiVehicleFleet)
|
||
positioned = MultiVehicleFleet.Values.Where(v => v.PosAvailable).ToList();
|
||
|
||
if (positioned.Count == 0) return false;
|
||
|
||
var centers = positioned.Select(InferFleetCenterFromCar).ToList();
|
||
centerX = centers.Average(c => c.Item1);
|
||
centerY = centers.Average(c => c.Item2);
|
||
var avgSin = centers.Average(c => Math.Sin(c.Item3 * Math.PI / 180.0));
|
||
var avgCos = centers.Average(c => Math.Cos(c.Item3 * Math.PI / 180.0));
|
||
centerTh = (float)(Math.Atan2(avgSin, avgCos) * 180.0 / Math.PI);
|
||
return true;
|
||
}
|
||
|
||
private static (float, float, float) InferFleetCenterFromCar(VehicleSyncInfo car)
|
||
{
|
||
// 由 carWorld = Transform2D(center, layout) 反推 center = carWorld ∘ layout⁻¹。
|
||
// 注意 layout⁻¹ 是真正的 SE(2) 逆，而非逐分量取负 (-layout)：
|
||
// 当 layoutTh≠0（如从车 180°）时，逐分量取负会把平移方向算错，导致车队中心偏移 → 位姿补偿跑飞。
|
||
var layout = Tuple.Create((double)car.LayoutX, (double)car.LayoutY, (double)car.LayoutTh);
|
||
var layoutInv = LessMath.SolveTransform2D(layout, Tuple.Create(0.0, 0.0, 0.0));
|
||
var center = LessMath.Transform2D(
|
||
Tuple.Create((double)car.X, (double)car.Y, (double)car.Th),
|
||
layoutInv);
|
||
return ((float)center.Item1, (float)center.Item2, (float)center.Item3);
|
||
}
|
||
|
||
private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
|
||
{
|
||
// 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
|
||
// 车体相对车队中心沿编队方向 ±syncDistance/2 对称分布，从车额外朝向翻转 180°。
|
||
var rad = syncTh / 180f * Math.PI;
|
||
var sign = CarNum == 1 ? 1f : -1f;
|
||
var xx = (float)(Math.Cos(rad) * syncDistance / 2 * sign);
|
||
var yy = (float)(Math.Sin(rad) * syncDistance / 2 * sign);
|
||
return (xx, yy, syncTh + (CarNum == 1 ? 0 : 180));
|
||
}
|
||
|
||
private VehicleSyncInfo BuildSelfInfo(bool master, bool posAvailable, float x, float y, float th,
|
||
float layoutX, float layoutY, float layoutTh, bool aligned, bool detectOk = true)
|
||
{
|
||
return new VehicleSyncInfo
|
||
{
|
||
Master = master,
|
||
Ip = Conf.SimpleIp,
|
||
Port = WebAPI.port,
|
||
PosAvailable = posAvailable,
|
||
X = x,
|
||
Y = y,
|
||
Th = th,
|
||
LayoutX = layoutX,
|
||
LayoutY = layoutY,
|
||
LayoutTh = layoutTh,
|
||
Aligned = aligned,
|
||
DetectOk = detectOk
|
||
};
|
||
}
|
||
|
||
private void VisualizeFleet(List<Vector2> contour, float egoLayoutX, float egoLayoutY, float egoLayoutTh)
|
||
{
|
||
// 画在本车车体系：global=false，渲染时由 ClumsyLite 叠加本车真实位姿。
|
||
// 每个成员按“相对本车 layout 的位姿”绘制（memberInEgo = egoLayout⁻¹ ∘ memberLayout），
|
||
// 这样手动模式无 SLAM 定位也能正确显示编队相对关系；避免之前用车队系 layout 坐标叠加本车位姿造成的整体平移。
|
||
var fleetPainter = UI.GetPainter("MultiVehicleFleet-vis", false);
|
||
Dictionary<int, VehicleSyncInfo> fleet;
|
||
lock (MultiVehicleFleet)
|
||
fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet);
|
||
|
||
var egoLayout = Tuple.Create((double)egoLayoutX, (double)egoLayoutY, (double)egoLayoutTh);
|
||
|
||
Tuple<double, double, double> MemberInEgo(float mx, float my, float mth)
|
||
=> LessMath.SolveTransform2D(egoLayout, Tuple.Create((double)mx, (double)my, (double)mth));
|
||
|
||
void DrawContour(Color color, Tuple<double, double, double> rel)
|
||
{
|
||
var transformed = contour
|
||
.Select(pp => LessMath.Transform2D((float)rel.Item1, (float)rel.Item2, (float)rel.Item3, pp))
|
||
.ToList();
|
||
for (var i = 0; i < 4; ++i)
|
||
fleetPainter.DrawLine(color, transformed[i], transformed[(i + 1) % 4]);
|
||
// 对角线便于辨认朝向
|
||
fleetPainter.DrawLine(color, transformed[0], transformed[2]);
|
||
fleetPainter.DrawLine(color, transformed[1], transformed[3]);
|
||
}
|
||
|
||
fleetPainter.Clear();
|
||
foreach (var kvp in fleet)
|
||
{
|
||
var color = kvp.Value.Master ? Color.Red : Color.Orange;
|
||
var rel = MemberInEgo(kvp.Value.LayoutX, kvp.Value.LayoutY, kvp.Value.LayoutTh);
|
||
DrawContour(color, rel);
|
||
fleetPainter.DrawText(color, $"{kvp.Key}", new Vector((float)rel.Item1, (float)rel.Item2));
|
||
}
|
||
}
|
||
|
||
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 =>
|
||
{
|
||
if (t.IsFaulted)
|
||
DLog.Log($"register failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
|
||
}, TaskScheduler.Default);
|
||
}
|
||
|
||
private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
|
||
{
|
||
var notifyJson = JsonConvert.SerializeObject(notification);
|
||
var url = $"http://{ip}:{port}/multi-vehicle-notify?Notification={Uri.EscapeDataString(notifyJson)}";
|
||
_ = hc.GetStringAsync(url).ContinueWith(t =>
|
||
{
|
||
if (t.IsFaulted)
|
||
DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
|
||
}, TaskScheduler.Default);
|
||
}
|
||
|
||
private void ParseMasterEndpoint(out string ip, out int port)
|
||
{
|
||
ip = "127.0.0.1";
|
||
port = 8008;
|
||
var endpoint = Conf.MultiVehicleMasterEndpoint ?? "/";
|
||
if (endpoint == "/") return;
|
||
var parts = endpoint.Split(':');
|
||
if (parts.Length >= 1 && !string.IsNullOrWhiteSpace(parts[0]))
|
||
ip = parts[0];
|
||
if (parts.Length >= 2 && int.TryParse(parts[1], out var p))
|
||
port = p;
|
||
}
|
||
|
||
private static float ClampBias(float value, float threshold)
|
||
=> Math.Sign(value) * Math.Min(Math.Abs(value), threshold);
|
||
|
||
#endregion
|
||
}
|