2104 lines
108 KiB
C#
2104 lines
108 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 CommonUsage.Mathematics;
|
||
using FundamentalLib;
|
||
using FundamentalLib.Utilities;
|
||
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
|
||
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;
|
||
|
||
// ===== Clumsy 侧脚本/动作驱动的手动等价输入（不走 Medulla IO，不会被 IO 同步覆盖）=====
|
||
// 手动 IO 字段是 [AsLowerIO]（Medulla→Clumsy，Medulla 每周期回写），Clumsy 侧 MovementTest 写它们会被覆盖。
|
||
// 因此提供这组内部字段，让 Clumsy 侧动作（如 FleetRotateInPlace）能像 FleetRemote 一样驱动车队联动：
|
||
// ScriptEnabled=使能；Mode 0=常规 1=蟹行 2=原地旋转；Vx/Vy/Vth 语义与手动遥控完全一致（m/s、m/s、deg/s）。
|
||
[FieldMember(desc = "多车联动：脚本驱动使能")] public bool MultiVehicleScriptEnabled;
|
||
[FieldMember(desc = "多车联动：脚本驱动模式")] public int MultiVehicleScriptMode;
|
||
[FieldMember(desc = "多车联动：脚本驱动Vx")] public float MultiVehicleScriptVx;
|
||
[FieldMember(desc = "多车联动：脚本驱动Vy")] public float MultiVehicleScriptVy;
|
||
[FieldMember(desc = "多车联动：脚本驱动Vth")] public float MultiVehicleScriptVth;
|
||
|
||
[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;
|
||
|
||
// A: 互斥锁固定为独立 readonly 对象，绝不随 MultiVehicleFleet 字段被整体替换而失效。
|
||
// 所有对 MultiVehicleFleet / _multiVehicleFleetSeen 的读写都必须 lock(FleetLock)。
|
||
public readonly object FleetLock = new();
|
||
|
||
// C: 各 fleet 成员最近一次被 register/notify 刷新的本地时刻（用本地时钟，避免远端时钟偏差）。
|
||
// 主车 Tick 据此剔除掉线成员；编队就绪要求所有成员新鲜。
|
||
private readonly Dictionary<int, DateTime> _multiVehicleFleetSeen = new();
|
||
|
||
[FieldMember(desc = "多车联动：车队姿态x")] public float CenterX;
|
||
[FieldMember(desc = "多车联动：车队姿态y")] public float CenterY;
|
||
[FieldMember(desc = "多车联动：车队姿态th")] public float CenterTh;
|
||
|
||
// G: 车队中心原子快照。三个 float 无法整体原子写，改为整体新建对象后引用赋值（引用赋值原子），
|
||
// 路径控制器线程只读最近一次完整快照，避免读到 x 新 / y,th 旧的撕裂组合。
|
||
public sealed class FleetCenterSnapshot
|
||
{
|
||
public float X, Y, Th;
|
||
public long Tick;
|
||
}
|
||
private volatile FleetCenterSnapshot _fleetCenter = new();
|
||
public FleetCenterSnapshot GetFleetCenterSnapshot() => _fleetCenter;
|
||
private void PublishFleetCenter(float x, float y, float th)
|
||
{
|
||
CenterX = x;
|
||
CenterY = y;
|
||
CenterTh = th;
|
||
_fleetCenter = new FleetCenterSnapshot { X = x, Y = y, Th = th, Tick = DateTime.Now.Ticks };
|
||
}
|
||
|
||
// B: 路径控制器最近一次写入自动速度命令的时刻；超时即视为失效（路径结束/早退/卡顿），清零下发。
|
||
public DateTime MultiVehicleAutoCmdTime = DateTime.MinValue;
|
||
// D: 路径控制器透传的理想车队中心位姿（世界系），由 idealPos/idealAngle 写入。
|
||
public bool MultiVehicleAutoHasIdeal;
|
||
public float MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh;
|
||
|
||
// F: notify 序列号（主车单调递增）与从车已应用的最大序列号（丢弃乱序旧包）。
|
||
private long _multiVehicleNotifySeq;
|
||
private long _multiVehicleAppliedSeq = -1;
|
||
|
||
[AsLowerIO(desc = "车号")] public int CarNum = 1;
|
||
[FieldMember(desc = "多车联动：原地旋转本车舵轮已对齐")]
|
||
public bool MultiVehicleRotateWheelsReady = true;
|
||
[FieldMember(desc = "多车联动：原地旋转整队舵轮已对齐")]
|
||
public bool MultiVehicleRotateFleetReady = true;
|
||
|
||
#region 夹臂变量
|
||
[AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
|
||
|
||
[AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
|
||
|
||
[AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
|
||
|
||
[AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
|
||
|
||
[AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
|
||
#endregion
|
||
|
||
[AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
|
||
[AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
|
||
[AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
|
||
[AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
|
||
[AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
|
||
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
||
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
||
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
||
|
||
[AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
|
||
[AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
|
||
[AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
|
||
[AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
|
||
|
||
[AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
|
||
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
|
||
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
||
|
||
private float _multiVehicleAccumulateTh;
|
||
private DateTime _multiVehicleLastThTime = DateTime.Now;
|
||
private readonly object _multiVehicleNotificationLock = new();
|
||
private DateTime _multiVehicleLastNotifyTime = DateTime.MinValue;
|
||
private bool _multiVehicleSyncInitialized;
|
||
private bool _multiVehicleWasActive;
|
||
private bool _multiVehicleMotionFeasible = true;
|
||
private string _multiVehicleMotionInfeasibleReason = "";
|
||
private DateTime _multiVehicleStopLastLog = DateTime.MinValue;
|
||
private bool _multiVehicleRotateModeActive;
|
||
private float _multiVehicleRotateDirectionHint = 1f;
|
||
private string _multiVehicleRotateAlignDetail = "";
|
||
private DateTime _multiVehicleRotateAlignLastLog = DateTime.MinValue;
|
||
private const float DefaultRotateCompTangentFrac = 0.10f;
|
||
|
||
private struct RotateControlParams
|
||
{
|
||
public float ActiveOmega;
|
||
public float CompXyFac;
|
||
public float CompXyIFac;
|
||
public float CompXyMax;
|
||
public float CompThFac;
|
||
public float CompThIFac;
|
||
public float CompThMax;
|
||
public float CompTangentFrac;
|
||
public float StartWheelAlignDeg;
|
||
public float ActiveWheelAlignDeg;
|
||
}
|
||
|
||
// 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
|
||
private DateTime _mvDbgLastLog = DateTime.MinValue;
|
||
private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist;
|
||
// 原地旋转(mode2)实际叠加的车体系纠偏旋量(mm/s, mm/s, deg/s)，仅用于诊断日志。
|
||
private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
|
||
private DateTime _mvRotateCompLimitLastLog = DateTime.MinValue;
|
||
private bool _mvRotateCenterDriftActive;
|
||
private float _mvRotateCenterStartX, _mvRotateCenterStartY, _mvRotateCenterStartTh, _mvRotateCenterMaxDrift;
|
||
private DateTime _mvRotateCenterLastLog = DateTime.MinValue;
|
||
// 原地旋转纠偏 PI 控制器的积分累加器(mm·s, mm·s, deg·s)与上次计算时刻。
|
||
private float _rotIntegX, _rotIntegY, _rotIntegTh;
|
||
private float _rotOmegaPeak; // 本次旋转过程中观测到的指令角速度峰值(deg/s)，用于纠偏随转速缩放
|
||
private DateTime _rotPiLastTime = DateTime.MinValue;
|
||
|
||
// 原地旋转“两车实际 sim 位姿”诊断采样状态（仅主车，通过 Playground WebAPI 读取）。
|
||
private DateTime _rotPoseLastLog = DateTime.MinValue;
|
||
private DateTime _rotPosePrevTime = DateTime.MinValue;
|
||
private float _rotPosePrevYaw1, _rotPosePrevYaw2;
|
||
private bool _rotPoseEpisode; // 本次旋转片段是否已捕获参考量
|
||
private float _rotPoseMid0X, _rotPoseMid0Y; // 起转瞬间车队几何中心(世界系)，用于度量整体平移漂移
|
||
|
||
// 写盘诊断日志（Clumsy 侧）。改用 DLog 落盘：同一 topic 的日志会归到以 topic 命名的文件夹，
|
||
// 由 DLog 统一管理落点/滚动，无需自行维护文件句柄与 session 清空逻辑。
|
||
private DateTime _mvDiskLastLog = DateTime.MinValue;
|
||
private void FleetDiag(string msg)
|
||
{
|
||
DLog.Log($"car{CarNum} {msg}", "FleetDiagClumsy");
|
||
}
|
||
|
||
// 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
|
||
private DateTime _mvRemoteInputLastLog = DateTime.MinValue;
|
||
private DateTime _mvRemoteDecisionLastLog = DateTime.MinValue;
|
||
private DateTime _mvNotifyApplyLastLog = DateTime.MinValue;
|
||
private DateTime _mvRotateWheelOutputLastLog = 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 void LogRotateWheelOutputs(MultiWheelChassis chassis, string phase, float omega, bool force = false)
|
||
{
|
||
var now = DateTime.Now;
|
||
if (!force && (now - _mvRotateWheelOutputLastLog).TotalMilliseconds < 300) return;
|
||
_mvRotateWheelOutputLastLog = now;
|
||
|
||
#pragma warning disable CS0612, CS0618
|
||
var wheels = chassis.GetSteerWheels();
|
||
#pragma warning restore CS0612, CS0618
|
||
var parts = new List<string>();
|
||
for (var i = 0; i < wheels.Count; i++)
|
||
{
|
||
var wheel = wheels[i];
|
||
if (wheel is DiffSteerWheel diff)
|
||
{
|
||
parts.Add(
|
||
$"w{i}:tgt={diff.GetSendAngle():0.0} read={diff.ReadAngle():0.0} " +
|
||
$"L={diff.GetLeftSendSpeed():0.000} R={diff.GetRightSendSpeed():0.000}");
|
||
}
|
||
else
|
||
{
|
||
parts.Add(
|
||
$"w{i}:tgt={wheel.GetSendAngle():0.0} read={wheel.ReadAngle():0.0} " +
|
||
$"v={wheel.GetSendSpeed():0.000}");
|
||
}
|
||
}
|
||
|
||
DLog.Log(
|
||
$"car{CarNum} phase={phase} omega={omega:0.000} " + string.Join(" | ", parts),
|
||
"MultiVehicleRotateWheelOutput");
|
||
}
|
||
|
||
private void SetMultiVehicleMotionFeasible(bool feasible, string reason = "")
|
||
{
|
||
_multiVehicleMotionFeasible = feasible;
|
||
_multiVehicleMotionInfeasibleReason = feasible ? "" : (reason ?? "");
|
||
}
|
||
|
||
private static float ClampFloat(float value, float min, float max)
|
||
{
|
||
if (value < min) return min;
|
||
if (value > max) return max;
|
||
return value;
|
||
}
|
||
|
||
private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega,
|
||
RotateControlParams rotateParams)
|
||
{
|
||
var rotateActive = active && fleetMode == 2;
|
||
if (!rotateActive)
|
||
{
|
||
_multiVehicleRotateModeActive = false;
|
||
if (!active)
|
||
{
|
||
MultiVehicleRotateWheelsReady = true;
|
||
_multiVehicleRotateAlignDetail = "";
|
||
}
|
||
MultiVehicleRotateFleetReady = true;
|
||
_multiVehicleRotateDirectionHint = 1f;
|
||
return;
|
||
}
|
||
|
||
if (!_multiVehicleRotateModeActive)
|
||
{
|
||
MultiVehicleRotateWheelsReady = false;
|
||
MultiVehicleRotateFleetReady = false;
|
||
_multiVehicleRotateAlignDetail = "rotate mode just entered";
|
||
}
|
||
|
||
_multiVehicleRotateModeActive = true;
|
||
if (Math.Abs(requestedOmega) > rotateParams.ActiveOmega)
|
||
_multiVehicleRotateDirectionHint = Math.Sign(requestedOmega);
|
||
}
|
||
|
||
private bool PrepareMultiVehicleRotateWheels(MultiWheelChassis chassis, float requestedOmega,
|
||
RotateControlParams rotateParams,
|
||
float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f,
|
||
bool rampStop = true)
|
||
{
|
||
var hint = Math.Abs(requestedOmega) > rotateParams.ActiveOmega
|
||
? Math.Sign(requestedOmega)
|
||
: Math.Sign(_multiVehicleRotateDirectionHint);
|
||
if (hint == 0) hint = 1;
|
||
_multiVehicleRotateDirectionHint = hint;
|
||
|
||
// Reuse SendRotateMotion's decomposition and angle-limit checks, but keep wheel speed at zero.
|
||
if (rampStop)
|
||
chassis.RampStop();
|
||
var alignOmega = Math.Abs(requestedOmega) > rotateParams.ActiveOmega
|
||
? requestedOmega
|
||
: 0.01f * hint;
|
||
var motionOk = chassis.SendRotateMotion(alignOmega, TimeSpan.Zero,
|
||
localCompensateX: localCompensateX, localCompensateY: localCompensateY,
|
||
localCompensateTh: localCompensateTh);
|
||
var wheelAligned = TryCheckRotateWheelAlignment(chassis, rotateParams.StartWheelAlignDeg,
|
||
out var alignDetail);
|
||
var aligned = motionOk && wheelAligned;
|
||
if (!motionOk)
|
||
alignDetail = string.IsNullOrWhiteSpace(chassis.LastMotionDecomposeFailureReason)
|
||
? alignDetail
|
||
: chassis.LastMotionDecomposeFailureReason;
|
||
|
||
MultiVehicleRotateWheelsReady = aligned;
|
||
_multiVehicleRotateAlignDetail = alignDetail;
|
||
|
||
var now = DateTime.Now;
|
||
if ((now - _multiVehicleRotateAlignLastLog).TotalMilliseconds >= 200)
|
||
{
|
||
_multiVehicleRotateAlignLastLog = now;
|
||
DLog.Log(
|
||
$"car{CarNum} ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} " +
|
||
$"hint={hint} comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
|
||
$"rampStop={rampStop} startTol={rotateParams.StartWheelAlignDeg:0.0} ok={motionOk} aligned={aligned} detail={alignDetail} " +
|
||
$"chassisReason={chassis.LastMotionDecomposeFailureReason}",
|
||
"MultiVehicleRemoteDbg");
|
||
FleetDiag(
|
||
$"ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} hint={hint} " +
|
||
$"comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
|
||
$"startTol={rotateParams.StartWheelAlignDeg:0.0} ok={motionOk} aligned={aligned} detail={alignDetail}");
|
||
}
|
||
|
||
Hedingben.ToastText(
|
||
aligned ? "车队原地旋转舵轮已对齐" : $"车队原地旋转舵轮预对齐中 {alignDetail}",
|
||
$"MultiVehicle{CarNum}-rotate-align");
|
||
|
||
return motionOk;
|
||
}
|
||
|
||
private static bool TryCheckRotateWheelAlignment(MultiWheelChassis chassis, float toleranceDeg, out string detail)
|
||
{
|
||
try
|
||
{
|
||
#pragma warning disable CS0612, CS0618
|
||
var wheels = chassis.GetSteerWheels();
|
||
#pragma warning restore CS0612, CS0618
|
||
var maxDth = 0f;
|
||
var parts = new List<string>();
|
||
for (var i = 0; i < wheels.Count; i++)
|
||
{
|
||
var sw = wheels[i];
|
||
var dth = Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle()));
|
||
maxDth = Math.Max(maxDth, dth);
|
||
parts.Add($"w{i}:{dth:0.0}");
|
||
}
|
||
|
||
detail = $"maxDth={maxDth:0.0} tol={toleranceDeg:0.0} {string.Join(",", parts)}";
|
||
return maxDth <= toleranceDeg;
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
detail = $"read alignment failed: {e.Message}";
|
||
return chassis.LastRotateAligned;
|
||
}
|
||
}
|
||
|
||
private void LogMultiVehicleStop(string reason, bool force = false)
|
||
{
|
||
var now = DateTime.Now;
|
||
if (!force && (now - _multiVehicleStopLastLog).TotalMilliseconds < 500) return;
|
||
_multiVehicleStopLastLog = now;
|
||
var msg = $"FLEET_STOP car={CarNum} reason={reason}";
|
||
DLog.Log(msg, "MultiVehicleSafety");
|
||
FleetDiag(msg);
|
||
Hedingben.ToastText($"Fleet stop: {reason}", $"MultiVehicle{CarNum}-stop");
|
||
}
|
||
|
||
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.AddPostByteHandler("/multi-vehicle-register-bin", body =>
|
||
{
|
||
try
|
||
{
|
||
var (carNum, info) = VehicleSyncBinaryCodec.DecodeRegister(body);
|
||
ApplyVehicleSyncRegister(carNum, info);
|
||
return "ok";
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
DLog.Log($"/multi-vehicle-register-bin error: {e.FormatEx()}", "MultiVehicle");
|
||
throw;
|
||
}
|
||
});
|
||
|
||
PicoHttpServer.AddPostByteHandler("/multi-vehicle-notify-bin", body =>
|
||
{
|
||
try
|
||
{
|
||
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 "stale";
|
||
_multiVehicleAppliedSeq = notification.Seq;
|
||
|
||
MultiVehicleAligned = notification.Aligned;
|
||
lock (FleetLock)
|
||
{
|
||
MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
|
||
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} useDetourCorr={notification.UseDetourCorrection} " +
|
||
$"rotParam(valid={notification.RotateParamsValid} xyP={notification.RotateCompXyFac:0.###} " +
|
||
$"xyI={notification.RotateCompXyIFac:0.###} xyMax={notification.RotateCompXyMax:0.#} " +
|
||
$"thP={notification.RotateCompThFac:0.###} thI={notification.RotateCompThIFac:0.###} " +
|
||
$"thMax={notification.RotateCompThMax:0.#} frac={notification.RotateCompTangentFrac:0.###} " +
|
||
$"startTol={notification.RotateStartWheelAlignDeg:0.#} activeTol={notification.RotateActiveWheelAlignDeg:0.#})",
|
||
"MultiVehicleRemoteDbg");
|
||
}
|
||
|
||
return "ok";
|
||
}
|
||
|
||
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()
|
||
};
|
||
|
||
DLog.Log("MultiVehicleLoop thread running", "MultiVehicle");
|
||
Console.WriteLine("[MultiVehicle] loop thread running");
|
||
var loopCount = 0L;
|
||
while (true)
|
||
{
|
||
try
|
||
{
|
||
TickMultiVehicle(hc, chassis, contour, sendMotionPainter);
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
DLog.Log($"MultiVehicle loop error: {e.FormatEx()}", "MultiVehicle");
|
||
}
|
||
|
||
// 每 ~5s 打一条心跳，确认循环确实在跑（用于区分“循环没跑”与“写盘失败”）
|
||
if (loopCount++ % Math.Max(1, 5000 / Math.Max(1, Conf.MultiVehicleSyncInterval)) == 0)
|
||
DLog.Log($"MultiVehicleLoop heartbeat #{loopCount}", "MultiVehicle");
|
||
|
||
Thread.Sleep(Conf.MultiVehicleSyncInterval);
|
||
}
|
||
}
|
||
|
||
private void TickMultiVehicle(HttpClient hc, MultiWheelChassis chassis, List<Vector2> contour, Painter sendMotionPainter)
|
||
{
|
||
var isMaster = IsMultiVehicleMaster();
|
||
var autoEnabled = false;
|
||
// 手动等价输入 = Medulla 遥控(IO) 或 Clumsy 脚本/动作驱动（二者择一，脚本优先）。
|
||
var scriptOn = MultiVehicleScriptEnabled;
|
||
var manualEnabled = MultiVehicleManualEnabled || scriptOn;
|
||
// 主车实际生效的手动模式与速度：脚本使能时取脚本字段，否则取 Medulla 手动 IO 字段。
|
||
var manualMode = scriptOn ? MultiVehicleScriptMode : MultiVehicleManualMode;
|
||
var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
|
||
var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
|
||
var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;
|
||
VehicleSyncNotification activeNotification = null;
|
||
|
||
if (isMaster)
|
||
autoEnabled = MultiVehicleAutoEnabled;
|
||
else
|
||
{
|
||
// 从车：自动/手动联动均可由主车广播解锁（无需各自再拨开关）。
|
||
// 仅在 notification 新鲜时认账，主车停发后超时即自动停车，避免用旧指令跑飞。
|
||
lock (_multiVehicleNotificationLock)
|
||
{
|
||
var fresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
|
||
Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
|
||
if (fresh && MultiVehicleNotification != null)
|
||
{
|
||
activeNotification = MultiVehicleNotification;
|
||
autoEnabled = MultiVehicleNotification.AutoEnabled;
|
||
manualEnabled = manualEnabled || MultiVehicleNotification.ManualEnabled;
|
||
}
|
||
}
|
||
}
|
||
var rotateParams = BuildRotateControlParams(isMaster ? null : activeNotification);
|
||
|
||
// 入口诊断（节流 ~300ms）：记录从 Medulla 收到的原始 IO 值与门控判定，
|
||
// 用于确认遥控指令是否真的传到了 Clumsy，以及为何提前 return。
|
||
LogMultiVehicleRemoteInput(isMaster, scriptOn, manualEnabled, autoEnabled,
|
||
manualMode, manualVx, manualVy, manualVth);
|
||
|
||
if ((DateTime.Now - _mvDiskLastLog).TotalMilliseconds >= 300)
|
||
{
|
||
_mvDiskLastLog = DateTime.Now;
|
||
int fleetCnt;
|
||
lock (FleetLock) 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} " +
|
||
$"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
|
||
$"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
|
||
$"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
|
||
$"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} " +
|
||
$"useDetour={Conf.MultiVehicleSyncUseDetour} manualDetour={Conf.MultiVehicleManualUseDetourCorrection}");
|
||
}
|
||
|
||
if (!manualEnabled && !autoEnabled)
|
||
{
|
||
if (_multiVehicleWasActive)
|
||
{
|
||
chassis.RampStop();
|
||
SetMultiVehicleMotionFeasible(true);
|
||
LogMultiVehicleStop("fleet control disabled; ramp stop previous fleet command", true);
|
||
}
|
||
_multiVehicleWasActive = false;
|
||
UpdateMultiVehicleRotateModeState(0, false, 0, rotateParams);
|
||
ResetMultiVehicleRotateCenterDrift();
|
||
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)
|
||
{
|
||
MultiVehicleFleet.Clear();
|
||
_multiVehicleFleetSeen.Clear();
|
||
}
|
||
MultiVehicleAutoEnabled = false;
|
||
MultiVehicleAutoHasIdeal = false;
|
||
_multiVehicleAppliedSeq = -1;
|
||
_multiVehicleAccumulateTh = 0f;
|
||
_multiVehicleLastThTime = DateTime.Now;
|
||
|
||
if (!isMaster)
|
||
FireAndForgetRegister(hc, BuildSelfInfo(false, false, 0, 0, 0, 0, 0, 0, false));
|
||
|
||
return;
|
||
}
|
||
_multiVehicleWasActive = true;
|
||
|
||
// 自动模式整队姿态依赖 Detour 全局定位（与 MultiVehicleSyncUseDetour 无关）：要求编队至少一台车有定位。
|
||
if (isMaster && autoEnabled && !manualEnabled && !FleetHasPosAvailable())
|
||
{
|
||
MultiVehicleAutoEnabled = false;
|
||
chassis.RampStop();
|
||
SetMultiVehicleMotionFeasible(true);
|
||
LogMultiVehicleStop("auto mode requires at least one Detour-positioned fleet member", true);
|
||
Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
|
||
return;
|
||
}
|
||
|
||
// 两类用途解耦（关键语义）：
|
||
// - useDetourCorrection (= MultiVehicleSyncUseDetour)：是否用 SLAM 位姿做"车队内姿态纠正"(POS 补偿)。
|
||
// false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
|
||
// - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
|
||
// SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
|
||
// 手动外部遥控默认不读 Detour，避免 getCartLocation 阻塞拖慢 2 腿检测；脚本自动原地旋转
|
||
// (FleetRotateInPlace) 已经依赖 Detour 判停，因此显式打开 POS 纠偏以保持旋转中心。
|
||
var autoMode = autoEnabled && !manualEnabled;
|
||
var scriptRotateDetourCorrection = isMaster && scriptOn && manualMode == 2 && Conf.FleetRotateUseDetourHeading;
|
||
var notificationDetourCorrection = !isMaster && activeNotification != null &&
|
||
activeNotification.UseDetourCorrection;
|
||
var manualDetourCorrection = manualEnabled &&
|
||
(Conf.MultiVehicleManualUseDetourCorrection ||
|
||
scriptRotateDetourCorrection ||
|
||
notificationDetourCorrection);
|
||
var useDetourCorrection = Conf.MultiVehicleSyncUseDetour && (!manualEnabled || manualDetourCorrection);
|
||
var slamRead = useDetourCorrection || (isMaster && autoMode);
|
||
float selfX = 0, selfY = 0, selfTh = 0;
|
||
if (slamRead)
|
||
{
|
||
var carPos = DetourInterface.getCartLocation();
|
||
selfX = (float)carPos.x;
|
||
selfY = (float)carPos.y;
|
||
selfTh = (float)carPos.th;
|
||
}
|
||
// fleetPosValid：整队全局姿态是否已知。主车=自身读到 SLAM；从车=主车广播标志（下方覆盖）。
|
||
var fleetPosValid = slamRead;
|
||
|
||
var syncTh = Conf.TestCarSyncTh;
|
||
var syncDistance = Conf.TestCarSyncDistance;
|
||
var deltaDetectCenter = Conf.DeltaDetectCenter;
|
||
float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0, fleetOmega = 0;
|
||
var fleetMode = 0; // 0=常规 1=蟹行 2=原地旋转
|
||
var autoCommandTimedOut = false;
|
||
var notificationStopActive = false;
|
||
var notificationStopReason = "";
|
||
var notificationStopSourceCar = 0;
|
||
var notificationRequestedFleetOmega = 0f;
|
||
var notificationMotionReleased = true;
|
||
float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
|
||
float crabSteerLimit = 0;
|
||
var crabReverseEquivalent = false;
|
||
|
||
if (isMaster)
|
||
{
|
||
if (manualEnabled)
|
||
{
|
||
syncTh = 0;
|
||
fleetMode = manualMode;
|
||
if (fleetMode == 2)
|
||
{
|
||
// 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
|
||
fleetOmega = manualVth;
|
||
fleetVx = 0;
|
||
fleetFrontTh = 0;
|
||
fleetRearTh = 0;
|
||
_multiVehicleAccumulateTh = 0f;
|
||
_multiVehicleLastThTime = DateTime.Now;
|
||
}
|
||
else if (fleetMode == 1)
|
||
{
|
||
// 手动蟹行：Vx 只表示线速度，Vy 表示方向摇杆比例(-1..1)，由 VyFac 映射为舵角。
|
||
var speed = manualVx * Conf.ManualCarSyncVxFac;
|
||
var crabRatio = Math.Max(-90f, Math.Min(90f, manualVy));
|
||
var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
|
||
crabInputVx = speed;
|
||
crabInputVy = crabRatio;
|
||
crabRawAngle = crabAngle;
|
||
crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));
|
||
|
||
if (crabAngle > crabSteerLimit) { crabAngle -= 180f; speed = -speed; crabReverseEquivalent = true; }
|
||
else if (crabAngle < -crabSteerLimit) { crabAngle += 180f; speed = -speed; crabReverseEquivalent = true; }
|
||
fleetVx = speed;
|
||
fleetFrontTh = crabAngle;
|
||
fleetRearTh = crabAngle;
|
||
_multiVehicleAccumulateTh = 0f;
|
||
_multiVehicleLastThTime = DateTime.Now;
|
||
}
|
||
else
|
||
{
|
||
fleetVx = manualVx * Conf.ManualCarSyncVxFac;
|
||
var targetTh = manualVth * 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
|
||
{
|
||
// B: 自动模式速度命令新鲜度门控。路径结束/回调早退/卡顿后，控制器不再刷新 AutoCmdTime，
|
||
// 超时即视为失效：清零下发速度与 idealPos，关闭 AutoEnabled，避免车队按末速度滑行。
|
||
var autoTimeoutMs = Conf.MultiVehicleAutoCmdTimeoutMs > 0
|
||
? Conf.MultiVehicleAutoCmdTimeoutMs
|
||
: Math.Max(200, Conf.MultiVehicleSyncInterval * 4);
|
||
var autoFresh = (DateTime.Now - MultiVehicleAutoCmdTime).TotalMilliseconds < autoTimeoutMs;
|
||
if (autoFresh)
|
||
{
|
||
fleetVx = MultiVehicleAutoVx;
|
||
fleetFrontTh = MultiVehicleAutoFrontTh;
|
||
fleetRearTh = MultiVehicleAutoRearTh;
|
||
}
|
||
else
|
||
{
|
||
autoCommandTimedOut = true;
|
||
fleetVx = fleetFrontTh = fleetRearTh = 0;
|
||
MultiVehicleAutoVx = MultiVehicleAutoFrontTh = MultiVehicleAutoRearTh = 0;
|
||
MultiVehicleAutoHasIdeal = false;
|
||
MultiVehicleAutoEnabled = false;
|
||
Hedingben.ToastText("自动速度命令超时，已停车（路径结束/控制器停发）", "MultiVehicle-auto-timeout");
|
||
}
|
||
}
|
||
}
|
||
else if (MultiVehicleNotification != null)
|
||
{
|
||
VehicleSyncNotification notification;
|
||
lock (_multiVehicleNotificationLock)
|
||
notification = MultiVehicleNotification;
|
||
|
||
syncTh = notification.SyncTh;
|
||
syncDistance = notification.SyncDistance;
|
||
deltaDetectCenter = notification.DeltaDetectCenter;
|
||
PublishFleetCenter(notification.CenterX, notification.CenterY, notification.CenterTh);
|
||
// 从车整队姿态来自主车广播：以广播标志作为 fleetPosValid（自身 slamRead 仅决定是否做本车纠偏）。
|
||
fleetPosValid = notification.PosAvailable;
|
||
MultiVehicleAutoEnabled = notification.AutoEnabled;
|
||
fleetVx = notification.FleetVx;
|
||
fleetFrontTh = notification.FleetFrontTh;
|
||
fleetRearTh = notification.FleetRearTh;
|
||
fleetMode = notification.Mode;
|
||
fleetOmega = notification.FleetOmega;
|
||
notificationRequestedFleetOmega = notification.RequestedFleetOmega;
|
||
notificationMotionReleased = notification.FleetMotionReleased;
|
||
notificationStopActive = notification.FleetStopActive;
|
||
notificationStopReason = notification.FleetStopReason ?? "";
|
||
notificationStopSourceCar = notification.FleetStopSourceCar;
|
||
// D: 从车采用主车广播的理想车队中心（弧线时由 idealPos/idealAngle 而来）做前馈目标。
|
||
MultiVehicleAutoHasIdeal = notification.HasIdeal;
|
||
MultiVehicleAutoIdealX = notification.IdealX;
|
||
MultiVehicleAutoIdealY = notification.IdealY;
|
||
MultiVehicleAutoIdealTh = notification.IdealTh;
|
||
}
|
||
|
||
var requestedFleetOmega = !isMaster && fleetMode == 2
|
||
? ((Math.Abs(notificationRequestedFleetOmega) > 1e-6f || !notificationMotionReleased)
|
||
? notificationRequestedFleetOmega
|
||
: fleetOmega)
|
||
: fleetOmega;
|
||
UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega, rotateParams);
|
||
|
||
var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
|
||
var inferredFleetCenterValid = false;
|
||
float inferredFleetCenterX = 0, inferredFleetCenterY = 0, inferredFleetCenterTh = 0;
|
||
Tuple<float, float, float> supposedPosForDiag = null;
|
||
|
||
if (isMaster)
|
||
{
|
||
lock (FleetLock)
|
||
{
|
||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
|
||
layoutX, layoutY, layoutTh, true);
|
||
_multiVehicleFleetSeen[CarNum] = DateTime.Now; // 主车自身恒新鲜
|
||
PruneStaleFleetMembers(); // C: 剔除掉线从车
|
||
}
|
||
|
||
if (TryInferFleetCenter(out var cx, out var cy, out var cth))
|
||
{
|
||
inferredFleetCenterValid = true;
|
||
inferredFleetCenterX = cx;
|
||
inferredFleetCenterY = cy;
|
||
inferredFleetCenterTh = cth;
|
||
PublishFleetCenter(cx, cy, cth);
|
||
}
|
||
|
||
// D: 自动模式（非手动）下，若控制器给出理想车队中心，则以理想位姿作为各车 layout 目标，
|
||
// 使弧线路径上从车按各自相对曲率中心位置前馈，而非仅靠事后检测/SLAM 纠偏。
|
||
if (!manualEnabled && MultiVehicleAutoEnabled &&
|
||
Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal)
|
||
PublishFleetCenter(MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh);
|
||
}
|
||
|
||
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) && slamRead)
|
||
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 (FleetLock)
|
||
{
|
||
MultiVehicleAligned = MultiVehicleFleet.Count == Conf.MultiVehicleFleetNum &&
|
||
MultiVehicleFleet.Values.All(v => v.Aligned);
|
||
}
|
||
|
||
var fleetReady = false;
|
||
var fleetCount = 0;
|
||
lock (FleetLock)
|
||
{
|
||
fleetCount = MultiVehicleFleet.Count;
|
||
fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
|
||
}
|
||
|
||
// 安全门：开启互识别时，本车或任一其它车检测不到邻车则整队停车（速度置零）。
|
||
// ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
|
||
var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
|
||
bool othersDetectOk;
|
||
string otherDetectLostCars;
|
||
List<KeyValuePair<int, VehicleSyncInfo>> motionInfeasibleMembers;
|
||
List<KeyValuePair<int, VehicleSyncInfo>> rotateUnalignedMembers;
|
||
lock (FleetLock)
|
||
{
|
||
othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
|
||
otherDetectLostCars = string.Join(",", MultiVehicleFleet
|
||
.Where(kv => kv.Key != CarNum && !kv.Value.DetectOk)
|
||
.Select(kv => kv.Key.ToString()));
|
||
motionInfeasibleMembers = MultiVehicleFleet
|
||
.Where(kv => kv.Key != CarNum && !kv.Value.MotionFeasible)
|
||
.ToList();
|
||
rotateUnalignedMembers = MultiVehicleFleet
|
||
.Where(kv => !kv.Value.RotateWheelsAligned)
|
||
.ToList();
|
||
}
|
||
if (!MultiVehicleRotateWheelsReady && rotateUnalignedMembers.All(kv => kv.Key != CarNum))
|
||
{
|
||
rotateUnalignedMembers.Add(new KeyValuePair<int, VehicleSyncInfo>(CarNum, new VehicleSyncInfo
|
||
{
|
||
RotateWheelsAligned = false,
|
||
RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
|
||
}));
|
||
}
|
||
var rotateUnalignedCars = string.Join(",", rotateUnalignedMembers.Select(kv => kv.Key.ToString()));
|
||
var rotateFleetWheelsAligned = fleetMode != 2 || (fleetReady && rotateUnalignedMembers.Count == 0);
|
||
MultiVehicleRotateFleetReady = rotateFleetWheelsAligned;
|
||
|
||
var fleetStopActive = false;
|
||
var fleetStopReason = "";
|
||
var fleetStopSourceCar = 0;
|
||
void AddFleetStop(string reason, int sourceCar = 0)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(reason)) return;
|
||
if (!fleetStopActive)
|
||
{
|
||
fleetStopReason = reason;
|
||
fleetStopSourceCar = sourceCar;
|
||
}
|
||
else
|
||
{
|
||
fleetStopReason += " | " + reason;
|
||
if (fleetStopSourceCar == 0) fleetStopSourceCar = sourceCar;
|
||
}
|
||
fleetStopActive = true;
|
||
}
|
||
|
||
if (!fleetReady)
|
||
AddFleetStop($"member not ready/stale fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
|
||
if (autoCommandTimedOut)
|
||
AddFleetStop("auto command timeout");
|
||
if (Conf.MultiVehicleUseDetect && !ownDetectOk)
|
||
AddFleetStop("own two-leg detection lost", CarNum);
|
||
if (Conf.MultiVehicleUseDetect && !othersDetectOk)
|
||
AddFleetStop($"other two-leg detection lost cars=[{otherDetectLostCars}]");
|
||
if (notificationStopActive)
|
||
AddFleetStop($"master stop from car{notificationStopSourceCar}: {notificationStopReason}",
|
||
notificationStopSourceCar);
|
||
if (!_multiVehicleMotionFeasible)
|
||
AddFleetStop($"car{CarNum} motion infeasible: {_multiVehicleMotionInfeasibleReason}", CarNum);
|
||
foreach (var kv in motionInfeasibleMembers)
|
||
AddFleetStop($"car{kv.Key} motion infeasible: {kv.Value.MotionInfeasibleReason}", kv.Key);
|
||
|
||
var canMove = !fleetStopActive;
|
||
|
||
// H: 自动模式（非手动）必须有有效车队中心——主车由 SLAM 反推、从车依赖主车广播 fleetPosValid。
|
||
// 自动模式整队姿态始终依赖 Detour（与 MultiVehicleSyncUseDetour 无关）：定位全程丢失时强制停车。
|
||
// （用 Detour 时若定位丢失，getCartLocation() 已先行阻塞，此处再兜底要求有效车队中心。）
|
||
// 手动模式不受限（允许仅靠互识别/遥控行驶）。
|
||
if (canMove && autoMode && Conf.MultiVehicleAutoRequireFleetCenter)
|
||
{
|
||
var fleetCenterValid = fleetPosValid && (!isMaster || TryInferFleetCenter(out _, out _, out _));
|
||
if (!fleetCenterValid)
|
||
{
|
||
AddFleetStop("auto fleet center invalid/localization lost", CarNum);
|
||
canMove = false;
|
||
Hedingben.ToastText("自动模式无有效车队中心（定位丢失），已停车", $"MultiVehicle{CarNum}-autostop");
|
||
}
|
||
}
|
||
|
||
if (!canMove)
|
||
{
|
||
fleetVx = 0;
|
||
fleetFrontTh = 0;
|
||
fleetRearTh = 0;
|
||
fleetOmega = 0;
|
||
Hedingben.ToastText(
|
||
$"车队停车：{(!ownDetectOk ? "本车" : "其它车")}2腿检测丢失（速度已置零）",
|
||
$"MultiVehicle{CarNum}-stop");
|
||
}
|
||
else
|
||
{
|
||
Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
|
||
}
|
||
|
||
var rotateHoldForAlignment = false;
|
||
if (fleetMode == 2 && fleetReady && !fleetStopActive && !notificationMotionReleased)
|
||
{
|
||
rotateHoldForAlignment = true;
|
||
fleetOmega = 0;
|
||
Hedingben.ToastText("车队原地旋转等待主车释放速度", $"MultiVehicle{CarNum}-rotate-align");
|
||
LogMultiVehicleRemoteDecision(
|
||
$"ROTATE_HOLD_RELEASE master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
|
||
$"requestedOmega={requestedFleetOmega:0.000}", true);
|
||
}
|
||
else if (fleetMode == 2 && fleetReady && !fleetStopActive && !rotateFleetWheelsAligned)
|
||
{
|
||
rotateHoldForAlignment = true;
|
||
fleetOmega = 0;
|
||
Hedingben.ToastText($"车队原地旋转舵轮预对齐中 car=[{rotateUnalignedCars}]", $"MultiVehicle{CarNum}-rotate-align");
|
||
LogMultiVehicleRemoteDecision(
|
||
$"ROTATE_HOLD_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
|
||
$"requestedOmega={requestedFleetOmega:0.000} unalignedCars=[{rotateUnalignedCars}]", true);
|
||
}
|
||
|
||
if (fleetStopActive)
|
||
LogMultiVehicleStop(fleetStopReason);
|
||
|
||
VisualizeFleet(contour, layoutX, layoutY, layoutTh);
|
||
|
||
// 补偿量提到块外，便于诊断日志统一记录三类来源（检测/SLAM）的贡献。
|
||
float xDetectCompensate = 0, yDetectCompensate = 0, thDetectCompensate = 0;
|
||
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 (fleetStopActive)
|
||
{
|
||
chassis.RampStop();
|
||
SetMultiVehicleMotionFeasible(true);
|
||
if (fleetMode == 2)
|
||
{
|
||
MultiVehicleRotateWheelsReady = false;
|
||
MultiVehicleRotateFleetReady = false;
|
||
_multiVehicleRotateAlignDetail = $"fleet stop: {fleetStopReason}";
|
||
}
|
||
LogMultiVehicleRemoteDecision(
|
||
$"RAMP_STOP master={isMaster} manual={manualEnabled} auto={autoEnabled} ready={fleetReady} " +
|
||
$"reason={fleetStopReason}", true);
|
||
}
|
||
|
||
if (fleetReady && !fleetStopActive)
|
||
{
|
||
chassis.SetOriginBias(layoutX, layoutY, layoutTh);
|
||
// E: 统一控制点半径，取 syncDistance/2，与编队几何一致。
|
||
var controlRadius = syncDistance / 2f;
|
||
chassis.ControlPointRadius = controlRadius;
|
||
// #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
|
||
chassis.RotateCompTangentFrac = rotateParams.CompTangentFrac;
|
||
|
||
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);
|
||
}
|
||
|
||
// 车队内姿态纠正(POS 补偿)：仅当 useDetourCorrection 开启且本车读到全局定位时施加。
|
||
// 这是"定位参与车队内姿态纠正"的唯一开关点——关闭它不影响上面的整队姿态计算/安全门。
|
||
if (canMove && useDetourCorrection && slamRead && TryInferFleetCenter(out _, out _, out _))
|
||
{
|
||
var supposedPos = LessMath.Transform2D(
|
||
Tuple.Create(CenterX, CenterY, CenterTh),
|
||
Tuple.Create(layoutX, layoutY, layoutTh));
|
||
supposedPosForDiag = supposedPos;
|
||
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();
|
||
if (fleetMode == 2)
|
||
{
|
||
// 原地旋转：底盘原点已偏置到车队中心，绕队中心旋转，并叠加 PI 闭环纠偏维持队形。
|
||
// detect/pos 偏差(dx,dy,dth) 本身就是"本车参考点应在车体系移动到的位置/应转的角"，作为误差喂给 PI。
|
||
// 纯 P 对抗恒定横向滑移有稳态残差，积分项消除之。检测丢失(canMove=false)时不纠偏并清空积分。
|
||
float rotCompVx = 0, rotCompVy = 0, rotCompOmega = 0;
|
||
var now = DateTime.Now;
|
||
// dt 限幅，避免首帧/卡顿导致积分突跳
|
||
var dt = _rotPiLastTime == DateTime.MinValue ? 0f
|
||
: (float)Math.Min(0.2, Math.Max(0, (now - _rotPiLastTime).TotalSeconds));
|
||
_rotPiLastTime = now;
|
||
|
||
// 仅在"被指令旋转"时才纠偏：松开摇杆(fleetOmega≈0)时绝不再下发补偿，
|
||
// 否则积分残留会持续驱动车辆平移/旋转，表现为"松杆后轮子来回打、自转停不下来"。
|
||
var rotating = Math.Abs(fleetOmega) > rotateParams.ActiveOmega;
|
||
if (canMove && rotating)
|
||
{
|
||
// #3 抗饱和：上一拍舵轮未对齐(gate=0、车没真正转动)时冻结积分，避免卡死时积分越积越大。
|
||
var allowInteg = chassis.LastRotateAligned &&
|
||
MultiVehicleRotateWheelsReady &&
|
||
MultiVehicleRotateFleetReady &&
|
||
!rotateHoldForAlignment;
|
||
var errX = detectDx + posBiasX; // mm，车体系：本车纵向(前+)应移动量
|
||
var errY = detectDy + posBiasY; // mm，车体系：本车横向(左+)应移动量
|
||
var errTh = detectDth + posBiasTh; // deg，本车应转角
|
||
rotCompVx = RotatePiTerm(errX, ref _rotIntegX, Conf.SingleCarSyncPrecisionXy,
|
||
rotateParams.CompXyFac, rotateParams.CompXyIFac,
|
||
rotateParams.CompXyMax, dt, allowInteg);
|
||
rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
|
||
rotateParams.CompXyFac, rotateParams.CompXyIFac,
|
||
rotateParams.CompXyMax, dt, allowInteg);
|
||
rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
|
||
rotateParams.CompThFac, rotateParams.CompThIFac,
|
||
rotateParams.CompThMax, dt, allowInteg);
|
||
|
||
// #1 纠偏随旋转指令缩放：comp ×= |fleetOmega| / 本次峰值。
|
||
// 加速+匀速段峰值≈当前 → 系数≈1（全力纠偏，不削弱）；减速段当前<峰值 → 系数随转速同步下降。
|
||
// 关键：旋转切向也∝转速，故"纠偏:切向"比例全程恒定=匀速段比例（远<1），既杜绝末段轮子乱打方向，
|
||
// 又不像按切向钳位那样在匀速段就削弱纠偏。
|
||
var absOmega = Math.Abs(fleetOmega);
|
||
_rotOmegaPeak = Math.Max(_rotOmegaPeak, absOmega);
|
||
if (_rotOmegaPeak > 1e-3f)
|
||
{
|
||
var compScale = Math.Min(1f, absOmega / _rotOmegaPeak);
|
||
rotCompVx *= compScale;
|
||
rotCompVy *= compScale;
|
||
rotCompOmega *= compScale;
|
||
}
|
||
LimitRotateCompensation(ref rotCompVx, ref rotCompVy, ref rotCompOmega,
|
||
absOmega, controlRadius, rotateParams.CompTangentFrac);
|
||
}
|
||
else
|
||
{
|
||
// 检测丢失或未指令旋转：清零补偿并复位积分/计时/峰值，停止时不再有残留驱动。
|
||
_rotIntegX = _rotIntegY = _rotIntegTh = 0;
|
||
_rotPiLastTime = DateTime.MinValue;
|
||
_rotOmegaPeak = 0;
|
||
}
|
||
|
||
_mvRotCompVx = rotCompVx;
|
||
_mvRotCompVy = rotCompVy;
|
||
_mvRotCompOmega = rotCompOmega;
|
||
|
||
bool motionOk;
|
||
if (rotateHoldForAlignment)
|
||
{
|
||
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams,
|
||
rotCompVx, rotCompVy, rotCompOmega);
|
||
}
|
||
else if (rotating)
|
||
{
|
||
// Preparation calls SendRotateMotion with zero delta; after release it would starve the speed ramp.
|
||
if (!MultiVehicleRotateWheelsReady)
|
||
{
|
||
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams,
|
||
rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
|
||
rotateHoldForAlignment = true;
|
||
fleetOmega = 0;
|
||
chassis.RampStop();
|
||
if (!motionOk || !MultiVehicleRotateWheelsReady)
|
||
{
|
||
LogMultiVehicleRemoteDecision(
|
||
$"ROTATE_HOLD_LOCAL_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
|
||
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
|
||
}
|
||
else
|
||
{
|
||
LogMultiVehicleRemoteDecision(
|
||
$"ROTATE_HOLD_LOCAL_READY master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
|
||
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
motionOk = chassis.SendRotateMotion(fleetOmega,
|
||
localCompensateX: rotCompVx, localCompensateY: rotCompVy,
|
||
localCompensateTh: rotCompOmega);
|
||
var activeWheelAlignDeg = rotateParams.ActiveWheelAlignDeg;
|
||
var activeAligned = TryCheckRotateWheelAlignment(chassis,
|
||
activeWheelAlignDeg, out _multiVehicleRotateAlignDetail);
|
||
if (!motionOk)
|
||
MultiVehicleRotateWheelsReady = false;
|
||
if (motionOk && !activeAligned)
|
||
{
|
||
MultiVehicleRotateWheelsReady = false;
|
||
MultiVehicleRotateFleetReady = false;
|
||
rotateHoldForAlignment = true;
|
||
fleetOmega = 0;
|
||
_rotIntegX = _rotIntegY = _rotIntegTh = 0;
|
||
_rotPiLastTime = DateTime.MinValue;
|
||
_rotOmegaPeak = 0;
|
||
rotCompVx = rotCompVy = rotCompOmega = 0;
|
||
_mvRotCompVx = _mvRotCompVy = _mvRotCompOmega = 0;
|
||
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams);
|
||
LogMultiVehicleRemoteDecision(
|
||
$"ROTATE_ACTIVE_REHOLD master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
|
||
$"requestedOmega={requestedFleetOmega:0.000} activeTol={activeWheelAlignDeg:0.0} " +
|
||
$"align={_multiVehicleRotateAlignDetail}", true);
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
motionOk = chassis.SendRotateMotion(fleetOmega,
|
||
localCompensateX: rotCompVx, localCompensateY: rotCompVy,
|
||
localCompensateTh: rotCompOmega);
|
||
MultiVehicleRotateWheelsReady = motionOk &&
|
||
TryCheckRotateWheelAlignment(chassis, rotateParams.StartWheelAlignDeg,
|
||
out _multiVehicleRotateAlignDetail);
|
||
}
|
||
SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
|
||
if (!motionOk)
|
||
{
|
||
AddFleetStop($"car{CarNum} chassis rotate infeasible: {chassis.LastMotionDecomposeFailureReason}",
|
||
CarNum);
|
||
canMove = false;
|
||
LogMultiVehicleStop(fleetStopReason, true);
|
||
}
|
||
LogRotateWheelOutputs(chassis, rotateHoldForAlignment ? "hold-align" : (rotating ? "rotate" : "idle"),
|
||
fleetOmega);
|
||
if (fleetPosValid)
|
||
LogMultiVehicleRotateCenterDrift(isMaster, manualEnabled, autoEnabled, fleetReady, canMove, rotating,
|
||
CenterX, CenterY, CenterTh, requestedFleetOmega, fleetOmega,
|
||
rotCompVx, rotCompVy, rotCompOmega);
|
||
else
|
||
ResetMultiVehicleRotateCenterDrift();
|
||
LogMultiVehicleRemoteDecision(
|
||
$"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
|
||
$"holdAlign={rotateHoldForAlignment} wheelReady={MultiVehicleRotateWheelsReady} fleetReady={MultiVehicleRotateFleetReady} " +
|
||
$"mode={fleetMode} omegaReq={requestedFleetOmega:0.000} omega={fleetOmega:0.000} " +
|
||
$"comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) align={_multiVehicleRotateAlignDetail} " +
|
||
$"rotParam(xyP={rotateParams.CompXyFac:0.###} xyI={rotateParams.CompXyIFac:0.###} " +
|
||
$"xyMax={rotateParams.CompXyMax:0.#} thP={rotateParams.CompThFac:0.###} " +
|
||
$"thI={rotateParams.CompThIFac:0.###} thMax={rotateParams.CompThMax:0.#} " +
|
||
$"frac={rotateParams.CompTangentFrac:0.###} activeOmega={rotateParams.ActiveOmega:0.###} " +
|
||
$"startTol={rotateParams.StartWheelAlignDeg:0.#} activeTol={rotateParams.ActiveWheelAlignDeg:0.#}) " +
|
||
$"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
|
||
|
||
// 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
|
||
if (isMaster && Conf.MultiVehicleRotatePoseWebApiDiagEnabled)
|
||
LogRotatePoseSample();
|
||
}
|
||
else
|
||
{
|
||
_mvRotCompVx = 0;
|
||
_mvRotCompVy = 0;
|
||
_mvRotCompOmega = 0;
|
||
MultiVehicleRotateFleetReady = true;
|
||
// Reset rotate-only PI state outside rotate mode.
|
||
_rotIntegX = _rotIntegY = _rotIntegTh = 0;
|
||
_rotPiLastTime = DateTime.MinValue;
|
||
_rotPoseEpisode = false;
|
||
_rotPosePrevTime = DateTime.MinValue;
|
||
ResetMultiVehicleRotateCenterDrift();
|
||
// 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
|
||
var motionOk = chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
|
||
localCompensateX: xDetectCompensate + xPosCompensate,
|
||
localCompensateY: yDetectCompensate + yPosCompensate,
|
||
localCompensateTh: thDetectCompensate + thPosCompensate);
|
||
if (motionOk)
|
||
MultiVehicleRotateWheelsReady = TryCheckRotateWheelAlignment(chassis,
|
||
Math.Max(0.1f, Conf.FleetCrabStartWheelAlignDeg), out _multiVehicleRotateAlignDetail);
|
||
else
|
||
{
|
||
MultiVehicleRotateWheelsReady = false;
|
||
_multiVehicleRotateAlignDetail = chassis.LastMotionDecomposeFailureReason;
|
||
}
|
||
MultiVehicleRotateFleetReady = true;
|
||
SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
|
||
if (!motionOk)
|
||
{
|
||
AddFleetStop($"car{CarNum} chassis motion infeasible: {chassis.LastMotionDecomposeFailureReason}",
|
||
CarNum);
|
||
canMove = false;
|
||
LogMultiVehicleStop(fleetStopReason, true);
|
||
}
|
||
LogMultiVehicleRemoteDecision(
|
||
$"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
|
||
$"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}) " +
|
||
$"wheelReady={MultiVehicleRotateWheelsReady} align={_multiVehicleRotateAlignDetail} " +
|
||
$"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
|
||
}
|
||
}
|
||
|
||
// === 诊断日志（节流 ~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 actualCenterThForDiag = inferredFleetCenterValid ? inferredFleetCenterTh : CenterTh;
|
||
var idealHeadingErr = (float)CommonMath.ThDiff(MultiVehicleAutoIdealTh, actualCenterThForDiag);
|
||
var idealHeadingErrReverse = (float)CommonMath.ThDiff(actualCenterThForDiag, MultiVehicleAutoIdealTh);
|
||
var frontRearDiff = (float)CommonMath.ThDiff(fleetFrontTh, fleetRearTh);
|
||
var commandHeadingSplit = frontRearDiff / 2f;
|
||
var commandBaseTh = (float)CommonMath.RoundTh(fleetRearTh + commandHeadingSplit);
|
||
var supposedPosText = supposedPosForDiag == null
|
||
? "N/A"
|
||
: $"({supposedPosForDiag.Item1:0},{supposedPosForDiag.Item2:0},{supposedPosForDiag.Item3:0.0})";
|
||
var crabDbg = isMaster && manualEnabled && fleetMode == 1
|
||
? $"| CRAB speed:{crabInputVx:F3} steerRatio:{crabInputVy:F3} raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
|
||
: "";
|
||
|
||
var dbg =
|
||
$"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} slam:{slamRead} corr:{useDetourCorrection} fleetPos:{fleetPosValid} " +
|
||
$"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} stop:{fleetStopActive} stopReason:{fleetStopReason} useDetect:{Conf.MultiVehicleUseDetect} " +
|
||
$"| 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} outBias({Conf.TwoLegOutputBiasX:F0},{Conf.TwoLegOutputBiasY:F0}) " +
|
||
$"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
|
||
$"fac({Conf.MultiVehicleDetectBiasXFac:F2},{Conf.MultiVehicleDetectBiasYFac:F2},{Conf.MultiVehicleDetectBiasThFac:F2}) " +
|
||
$"lim({Conf.MultiVehicleDetectBiasXThreshold:F1},{Conf.MultiVehicleDetectBiasYThreshold:F1},{Conf.MultiVehicleDetectBiasThThreshold:F1}) " +
|
||
$"| 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} " +
|
||
$"fac({Conf.MultiVehiclePosBiasXFac:F2},{Conf.MultiVehiclePosBiasYFac:F2},{Conf.MultiVehiclePosBiasThFac:F2}) " +
|
||
$"lim({Conf.MultiVehiclePosBiasXThreshold:F1},{Conf.MultiVehiclePosBiasYThreshold:F1},{Conf.MultiVehiclePosBiasThThreshold:F1}) " +
|
||
$"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
|
||
$"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " +
|
||
$"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " +
|
||
$"| ROT_ALIGN hold:{rotateHoldForAlignment} wheelReady:{MultiVehicleRotateWheelsReady} fleetReady:{MultiVehicleRotateFleetReady} " +
|
||
$"unaligned:[{rotateUnalignedCars}] reqOmega:{requestedFleetOmega:F1} detail:{_multiVehicleRotateAlignDetail}";
|
||
|
||
DLog.Log(dbg, "MultiVehicleDbg");
|
||
FleetDiag(dbg);
|
||
|
||
if (autoMode || autoEnabled || MultiVehicleAutoEnabled)
|
||
DLog.Log(
|
||
$"APPLY car{CarNum} master:{isMaster} autoMode:{autoMode} manual:{manualEnabled} " +
|
||
$"cmd(vx:{fleetVx:0.000},f:{fleetFrontTh:0.00},r:{fleetRearTh:0.00},base:{commandBaseTh:0.00},split:{commandHeadingSplit:0.00},f-r:{frontRearDiff:0.00}) " +
|
||
$"ideal(has:{MultiVehicleAutoHasIdeal},x:{MultiVehicleAutoIdealX:0},y:{MultiVehicleAutoIdealY:0},th:{MultiVehicleAutoIdealTh:0.00}) " +
|
||
$"centerPublished({CenterX:0},{CenterY:0},{CenterTh:0.00}) " +
|
||
$"centerInferred(valid:{inferredFleetCenterValid},x:{inferredFleetCenterX:0},y:{inferredFleetCenterY:0},th:{inferredFleetCenterTh:0.00}) " +
|
||
$"idealHeadingErr(target-actual):{idealHeadingErr:0.00} reverse(actual-target):{idealHeadingErrReverse:0.00} " +
|
||
$"layout({layoutX:0},{layoutY:0},{layoutTh:0.00}) self({selfX:0},{selfY:0},{selfTh:0.00}) supposed:{supposedPosText} " +
|
||
$"detectDth:{detectDth:0.00}->comp:{thDetectCompensate:0.00} " +
|
||
$"posBiasTh:{posBiasTh:0.00}->comp:{thPosCompensate:0.00} " +
|
||
$"totalLocalComp(x:{cx:0.0},y:{cy:0.0},th:{cth:0.00}) " +
|
||
$"useDetourCorr:{useDetourCorrection} slam:{slamRead} fleetPos:{fleetPosValid} ready:{fleetReady}",
|
||
"FleetCrabHeadingDbg");
|
||
|
||
// 屏幕分两行显示，便于直接观察（无需开启 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} stop{fleetStopActive}",
|
||
$"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 (FleetLock)
|
||
{
|
||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
|
||
layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
|
||
}
|
||
|
||
if (fleetReady || fleetStopActive)
|
||
{
|
||
VehicleSyncNotification notification;
|
||
lock (FleetLock)
|
||
{
|
||
notification = new VehicleSyncNotification
|
||
{
|
||
// 广播"整队姿态有效"：主车读到全局定位即为真，从车据此放行自动模式安全门。
|
||
PosAvailable = slamRead,
|
||
CenterX = CenterX,
|
||
CenterY = CenterY,
|
||
CenterTh = CenterTh,
|
||
Aligned = MultiVehicleAligned,
|
||
Fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet),
|
||
FleetVx = fleetStopActive ? 0 : fleetVx,
|
||
FleetFrontTh = fleetStopActive ? 0 : fleetFrontTh,
|
||
FleetRearTh = fleetStopActive ? 0 : fleetRearTh,
|
||
Mode = fleetMode,
|
||
FleetOmega = fleetStopActive ? 0 : fleetOmega,
|
||
RequestedFleetOmega = fleetMode == 2 && !fleetStopActive ? requestedFleetOmega : 0,
|
||
FleetMotionReleased = !fleetStopActive && !(fleetMode == 2 && rotateHoldForAlignment),
|
||
FleetStopActive = fleetStopActive,
|
||
FleetStopReason = fleetStopReason,
|
||
FleetStopSourceCar = fleetStopSourceCar,
|
||
AutoEnabled = MultiVehicleAutoEnabled,
|
||
// 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
|
||
ManualEnabled = manualEnabled,
|
||
UseDetourCorrection = useDetourCorrection,
|
||
SyncTh = syncTh,
|
||
SyncDistance = syncDistance,
|
||
DeltaDetectCenter = deltaDetectCenter,
|
||
// F: 单调递增序列号（从 1 起），从车据此丢弃乱序旧包。
|
||
Seq = ++_multiVehicleNotifySeq,
|
||
// D: 透传理想车队中心（仅自动模式且控制器给出时有效）。
|
||
HasIdeal = !manualEnabled && MultiVehicleAutoEnabled &&
|
||
Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal,
|
||
IdealX = MultiVehicleAutoIdealX,
|
||
IdealY = MultiVehicleAutoIdealY,
|
||
IdealTh = MultiVehicleAutoIdealTh
|
||
};
|
||
FillRotateNotificationParams(notification, rotateParams);
|
||
}
|
||
|
||
LogMultiVehicleRemoteDecision(
|
||
$"NOTIFY_SEND 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} useDetourCorr={notification.UseDetourCorrection} " +
|
||
$"fleetCnt={notification.Fleet.Count}/{Conf.MultiVehicleFleetNum} " +
|
||
$"rotParam(valid={notification.RotateParamsValid} xyP={notification.RotateCompXyFac:0.###} " +
|
||
$"xyI={notification.RotateCompXyIFac:0.###} xyMax={notification.RotateCompXyMax:0.#} " +
|
||
$"thP={notification.RotateCompThFac:0.###} thI={notification.RotateCompThIFac:0.###} " +
|
||
$"thMax={notification.RotateCompThMax:0.#} frac={notification.RotateCompTangentFrac:0.###} " +
|
||
$"startTol={notification.RotateStartWheelAlignDeg:0.#} activeTol={notification.RotateActiveWheelAlignDeg:0.#})");
|
||
|
||
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, slamRead, selfX, selfY, selfTh,
|
||
layoutX, layoutY, layoutTh, selfAligned, ownDetectOk));
|
||
}
|
||
}
|
||
|
||
private bool IsMultiVehicleMaster() => Conf.MultiVehicleMasterEndpoint == "/";
|
||
|
||
/// <summary>
|
||
/// C: 剔除超过 TTL 未刷新（register/notify）的 fleet 成员。调用方须已持有 FleetLock。
|
||
/// 从车崩溃/断网后其条目过期被删除，使 fleetReady(数量==总数) 同时蕴含"全部成员在线且新鲜"，
|
||
/// 主车不再基于过期 layout/DetectOk 继续 SendMotion+notify。
|
||
/// </summary>
|
||
private void PruneStaleFleetMembers()
|
||
{
|
||
var ttlMs = Conf.MultiVehicleMemberTtlMs > 0
|
||
? Conf.MultiVehicleMemberTtlMs
|
||
: Math.Max(500, Conf.MultiVehicleSyncInterval * 6);
|
||
var now = DateTime.Now;
|
||
var stale = MultiVehicleFleet.Keys
|
||
.Where(k => k != CarNum &&
|
||
(!_multiVehicleFleetSeen.TryGetValue(k, out var seen) ||
|
||
(now - seen).TotalMilliseconds > ttlMs))
|
||
.ToList();
|
||
foreach (var k in stale)
|
||
{
|
||
MultiVehicleFleet.Remove(k);
|
||
_multiVehicleFleetSeen.Remove(k);
|
||
Hedingben.ToastText($"剔除掉线成员 car{k}（{ttlMs:F0}ms 未刷新）", $"MultiVehicle{CarNum}-prune");
|
||
}
|
||
}
|
||
|
||
private bool FleetHasPosAvailable()
|
||
{
|
||
lock (FleetLock)
|
||
return MultiVehicleFleet.Values.Any(v => v.PosAvailable);
|
||
}
|
||
|
||
// 基于 SLAM 世界坐标的最近邻车间距（mm）；无可用定位邻车时返回 NaN。
|
||
private float TryGetSlamSpacing(float selfX, float selfY)
|
||
{
|
||
List<VehicleSyncInfo> others;
|
||
lock (FleetLock)
|
||
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 (FleetLock)
|
||
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;
|
||
}
|
||
|
||
public bool TryGetFleetCenterFromMembers(out float centerX, out float centerY, out float centerTh)
|
||
{
|
||
return TryInferFleetCenter(out centerX, out centerY, out centerTh);
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 由本车（通常为主车）当前 Detour SLAM 位姿反推车队中心位姿（世界系）。
|
||
/// 复用 GetLayoutPose + InferFleetCenterFromCar 的同款 SE(2) 反推（center = carWorld ∘ layout⁻¹），
|
||
/// 保证与自动模式车队中心计算一致。供动作（如 FleetCrabWalk）取路径起点用；无有效定位时返回 false。
|
||
/// </summary>
|
||
public bool TryGetFleetCenterFromSlam(out float centerX, out float centerY, out float centerTh)
|
||
{
|
||
centerX = centerY = centerTh = 0;
|
||
var carPos = DetourInterface.getCartLocation();
|
||
return TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||
out centerX, out centerY, out centerTh);
|
||
}
|
||
|
||
public bool TryGetFleetCenterFromPose(float carX, float carY, float carTh,
|
||
out float centerX, out float centerY, out float centerTh)
|
||
{
|
||
centerX = centerY = centerTh = 0;
|
||
var (layoutX, layoutY, layoutTh) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
|
||
var self = new VehicleSyncInfo
|
||
{
|
||
X = carX,
|
||
Y = carY,
|
||
Th = carTh,
|
||
LayoutX = layoutX,
|
||
LayoutY = layoutY,
|
||
LayoutTh = layoutTh
|
||
};
|
||
var (cx, cy, cth) = InferFleetCenterFromCar(self);
|
||
centerX = cx;
|
||
centerY = cy;
|
||
centerTh = cth;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 预热：用主车 SLAM 位姿，立即(1) 对外发布有效"车队中心快照"，(2) 把主车自身以 posAvailable=true
|
||
/// 写入 fleet 表。供 FleetCrabWalk 等动作在启动控制器 Track() 前调用，解决两个启动期问题：
|
||
/// - 控制器首帧通过 MultiVehicleGetFleetPos 读到 (0,0,0) → 误判已到终点、立即结束；
|
||
/// - 自动联动循环的安全门 FleetHasPosAvailable（PilotDefinition.cs ~402）在冷启动(无车上报定位)时
|
||
/// 会把 AutoEnabled 关掉、循环无法发布真实中心。播种主车 posAvailable 条目即可越过该门，使循环正常接管。
|
||
/// 需在主车上调用；getCartLocation() 无定位时会阻塞（与自动模式一致）。
|
||
/// </summary>
|
||
public bool PrimeMasterAutoFromSlam()
|
||
{
|
||
var carPos = DetourInterface.getCartLocation();
|
||
var (lx, ly, lth) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
|
||
var info = BuildSelfInfo(true, true, (float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||
lx, ly, lth, false);
|
||
var (cx, cy, cth) = InferFleetCenterFromCar(info);
|
||
PublishFleetCenter(cx, cy, cth);
|
||
lock (FleetLock)
|
||
{
|
||
MultiVehicleFleet[CarNum] = info;
|
||
_multiVehicleFleetSeen[CarNum] = DateTime.Now;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
public long BeginFleetMotionWarmup()
|
||
{
|
||
MultiVehicleRotateWheelsReady = false;
|
||
MultiVehicleRotateFleetReady = false;
|
||
_multiVehicleRotateAlignDetail = "fleet motion warmup pending";
|
||
return Interlocked.Read(ref _multiVehicleNotifySeq);
|
||
}
|
||
|
||
public bool IsFleetMotionWarmupReady(DateTime warmStartTime, long notificationSeqBaseline,
|
||
float syncTh, float syncDistance, out string detail)
|
||
{
|
||
var pending = new List<string>();
|
||
var requiredCount = Math.Max(1, Conf.MultiVehicleFleetNum);
|
||
lock (FleetLock)
|
||
{
|
||
if (MultiVehicleFleet.Count != requiredCount)
|
||
pending.Add($"fleetCnt={MultiVehicleFleet.Count}/{requiredCount}");
|
||
|
||
foreach (var kv in MultiVehicleFleet.OrderBy(k => k.Key))
|
||
{
|
||
var carNum = kv.Key;
|
||
var info = kv.Value;
|
||
if (!_multiVehicleFleetSeen.TryGetValue(carNum, out var seen) || seen < warmStartTime)
|
||
pending.Add($"car{carNum}:notFresh");
|
||
|
||
var (layoutX, layoutY, layoutTh) = GetLayoutPoseForCar(carNum, syncTh, syncDistance);
|
||
if (Math.Abs(info.LayoutX - layoutX) > 1f ||
|
||
Math.Abs(info.LayoutY - layoutY) > 1f ||
|
||
Math.Abs(CommonMath.ThDiff(info.LayoutTh, layoutTh)) > 1f)
|
||
pending.Add(
|
||
$"car{carNum}:layout=({info.LayoutX:0},{info.LayoutY:0},{info.LayoutTh:0.0})");
|
||
|
||
if (!info.MotionFeasible)
|
||
pending.Add($"car{carNum}:motion={info.MotionInfeasibleReason}");
|
||
if (!info.RotateWheelsAligned)
|
||
pending.Add($"car{carNum}:wheel={info.RotateWheelAlignDetail}");
|
||
if (carNum != CarNum && info.AppliedNotificationSeq <= notificationSeqBaseline)
|
||
pending.Add($"car{carNum}:seq={info.AppliedNotificationSeq}<={notificationSeqBaseline}");
|
||
}
|
||
}
|
||
|
||
detail = pending.Count == 0
|
||
? $"ready seqBase={notificationSeqBaseline}"
|
||
: string.Join("; ", pending);
|
||
return pending.Count == 0;
|
||
}
|
||
|
||
private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
|
||
=> GetLayoutPoseForCar(CarNum, syncTh, syncDistance);
|
||
|
||
private static (float, float, float) GetLayoutPoseForCar(int carNum, float syncTh, float syncDistance)
|
||
{
|
||
// Two-car layout: members are mirrored around the fleet center; car2 faces 180 deg away.
|
||
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)
|
||
{
|
||
ResolveMultiVehicleSelfEndpoint(out var selfIp, out var selfPort);
|
||
|
||
return new VehicleSyncInfo
|
||
{
|
||
Master = master,
|
||
Ip = selfIp,
|
||
Port = selfPort,
|
||
PosAvailable = posAvailable,
|
||
X = x,
|
||
Y = y,
|
||
Th = th,
|
||
LayoutX = layoutX,
|
||
LayoutY = layoutY,
|
||
LayoutTh = layoutTh,
|
||
Aligned = aligned,
|
||
DetectOk = detectOk,
|
||
MotionFeasible = _multiVehicleMotionFeasible,
|
||
MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason,
|
||
RotateWheelsAligned = MultiVehicleRotateWheelsReady,
|
||
RotateWheelAlignDetail = _multiVehicleRotateAlignDetail,
|
||
AppliedNotificationSeq = _multiVehicleAppliedSeq
|
||
};
|
||
}
|
||
|
||
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 (FleetLock)
|
||
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)
|
||
{
|
||
try
|
||
{
|
||
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 void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
|
||
{
|
||
try
|
||
{
|
||
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)
|
||
{
|
||
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";
|
||
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);
|
||
|
||
private RotateControlParams BuildRotateControlParams(VehicleSyncNotification notification)
|
||
{
|
||
var parameters = new RotateControlParams
|
||
{
|
||
ActiveOmega = Conf.MultiVehicleRotateActiveOmega,
|
||
CompXyFac = Conf.MultiVehicleRotateCompXyFac,
|
||
CompXyIFac = Conf.MultiVehicleRotateCompXyIFac,
|
||
CompXyMax = Conf.MultiVehicleRotateCompXyMax,
|
||
CompThFac = Conf.MultiVehicleRotateCompThFac,
|
||
CompThIFac = Conf.MultiVehicleRotateCompThIFac,
|
||
CompThMax = Conf.MultiVehicleRotateCompThMax,
|
||
CompTangentFrac = NormalizeRotateCompTangentFrac(Conf.MultiVehicleRotateCompTangentFrac),
|
||
StartWheelAlignDeg = Conf.InPlaceRotateWheelAlignDeg,
|
||
ActiveWheelAlignDeg = NormalizeRotateWheelAlignDeg(Conf.InPlaceRotateActiveWheelAlignDeg,
|
||
Conf.InPlaceRotateWheelAlignDeg)
|
||
};
|
||
|
||
if (notification == null || !notification.RotateParamsValid)
|
||
return parameters;
|
||
|
||
parameters.ActiveOmega = notification.RotateActiveOmega;
|
||
parameters.CompXyFac = notification.RotateCompXyFac;
|
||
parameters.CompXyIFac = notification.RotateCompXyIFac;
|
||
parameters.CompXyMax = notification.RotateCompXyMax;
|
||
parameters.CompThFac = notification.RotateCompThFac;
|
||
parameters.CompThIFac = notification.RotateCompThIFac;
|
||
parameters.CompThMax = notification.RotateCompThMax;
|
||
parameters.CompTangentFrac = NormalizeRotateCompTangentFrac(notification.RotateCompTangentFrac);
|
||
parameters.StartWheelAlignDeg = NormalizeRotateWheelAlignDeg(notification.RotateStartWheelAlignDeg,
|
||
Conf.InPlaceRotateWheelAlignDeg);
|
||
parameters.ActiveWheelAlignDeg = NormalizeRotateWheelAlignDeg(notification.RotateActiveWheelAlignDeg,
|
||
parameters.StartWheelAlignDeg);
|
||
return parameters;
|
||
}
|
||
|
||
private static void FillRotateNotificationParams(VehicleSyncNotification notification, RotateControlParams parameters)
|
||
{
|
||
notification.RotateParamsValid = true;
|
||
notification.RotateActiveOmega = parameters.ActiveOmega;
|
||
notification.RotateCompXyFac = parameters.CompXyFac;
|
||
notification.RotateCompXyIFac = parameters.CompXyIFac;
|
||
notification.RotateCompXyMax = parameters.CompXyMax;
|
||
notification.RotateCompThFac = parameters.CompThFac;
|
||
notification.RotateCompThIFac = parameters.CompThIFac;
|
||
notification.RotateCompThMax = parameters.CompThMax;
|
||
notification.RotateCompTangentFrac = parameters.CompTangentFrac;
|
||
notification.RotateStartWheelAlignDeg = parameters.StartWheelAlignDeg;
|
||
notification.RotateActiveWheelAlignDeg = parameters.ActiveWheelAlignDeg;
|
||
}
|
||
|
||
private static float NormalizeRotateWheelAlignDeg(float value, float fallback)
|
||
{
|
||
if (float.IsNaN(value) || float.IsInfinity(value) || value <= 0)
|
||
return fallback;
|
||
return value;
|
||
}
|
||
|
||
private static float NormalizeRotateCompTangentFrac(float frac)
|
||
{
|
||
if (float.IsNaN(frac) || float.IsInfinity(frac) || frac < 0)
|
||
return DefaultRotateCompTangentFrac;
|
||
return frac;
|
||
}
|
||
|
||
private void LimitRotateCompensation(ref float compVx, ref float compVy, ref float compOmega,
|
||
float absOmega, float controlRadius, float tangentFrac)
|
||
{
|
||
var frac = NormalizeRotateCompTangentFrac(tangentFrac);
|
||
if (frac < 0 || absOmega <= 1e-6f) return;
|
||
|
||
var rawVx = compVx;
|
||
var rawVy = compVy;
|
||
var rawOmega = compOmega;
|
||
var tangentMmps = absOmega / 180f * (float)Math.PI * Math.Max(1f, Math.Abs(controlRadius));
|
||
var xyLimit = tangentMmps * frac;
|
||
var xyMag = (float)Math.Sqrt(compVx * compVx + compVy * compVy);
|
||
if (xyMag > xyLimit && xyMag > 1e-6f)
|
||
{
|
||
var scale = xyLimit / xyMag;
|
||
compVx *= scale;
|
||
compVy *= scale;
|
||
}
|
||
|
||
var omegaLimit = absOmega * frac;
|
||
compOmega = ClampBias(compOmega, omegaLimit);
|
||
|
||
var changed = Math.Abs(rawVx - compVx) > 1e-3f ||
|
||
Math.Abs(rawVy - compVy) > 1e-3f ||
|
||
Math.Abs(rawOmega - compOmega) > 1e-3f;
|
||
var now = DateTime.Now;
|
||
if (changed && (now - _mvRotateCompLimitLastLog).TotalMilliseconds >= 200)
|
||
{
|
||
_mvRotateCompLimitLastLog = now;
|
||
DLog.Log(
|
||
$"car{CarNum} ROTATE_COMP_LIMIT frac={frac:0.00} omega={absOmega:0.000} radius={controlRadius:0} " +
|
||
$"tan={tangentMmps:0.0} xyLimit={xyLimit:0.0} omLimit={omegaLimit:0.000} " +
|
||
$"raw=({rawVx:0.0},{rawVy:0.0},{rawOmega:0.000}) " +
|
||
$"limited=({compVx:0.0},{compVy:0.0},{compOmega:0.000})",
|
||
"MultiVehicleRemoteDbg");
|
||
}
|
||
}
|
||
|
||
private void ResetMultiVehicleRotateCenterDrift()
|
||
{
|
||
_mvRotateCenterDriftActive = false;
|
||
_mvRotateCenterMaxDrift = 0;
|
||
_mvRotateCenterLastLog = DateTime.MinValue;
|
||
}
|
||
|
||
private void LogMultiVehicleRotateCenterDrift(bool isMaster, bool manualEnabled, bool autoEnabled,
|
||
bool fleetReady, bool canMove, bool rotating, float centerX, float centerY, float centerTh,
|
||
float requestedOmega, float fleetOmega, float compVx, float compVy, float compOmega)
|
||
{
|
||
if (!_mvRotateCenterDriftActive)
|
||
{
|
||
_mvRotateCenterDriftActive = true;
|
||
_mvRotateCenterStartX = centerX;
|
||
_mvRotateCenterStartY = centerY;
|
||
_mvRotateCenterStartTh = centerTh;
|
||
_mvRotateCenterMaxDrift = 0;
|
||
_mvRotateCenterLastLog = DateTime.MinValue;
|
||
}
|
||
|
||
var dx = centerX - _mvRotateCenterStartX;
|
||
var dy = centerY - _mvRotateCenterStartY;
|
||
var drift = (float)Math.Sqrt(dx * dx + dy * dy);
|
||
_mvRotateCenterMaxDrift = Math.Max(_mvRotateCenterMaxDrift, drift);
|
||
var dth = (float)CommonMath.ThDiff(centerTh, _mvRotateCenterStartTh);
|
||
var now = DateTime.Now;
|
||
if ((now - _mvRotateCenterLastLog).TotalMilliseconds < 250)
|
||
return;
|
||
|
||
_mvRotateCenterLastLog = now;
|
||
DLog.Log(
|
||
$"car{CarNum} CTRL master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
|
||
$"ready={fleetReady} canMove={canMove} rotating={rotating} " +
|
||
$"center=({centerX:0.0},{centerY:0.0},{centerTh:0.00}) " +
|
||
$"start=({_mvRotateCenterStartX:0.0},{_mvRotateCenterStartY:0.0},{_mvRotateCenterStartTh:0.00}) " +
|
||
$"drift=({dx:0.0},{dy:0.0}) dist={drift:0.0} max={_mvRotateCenterMaxDrift:0.0} dth={dth:0.00} " +
|
||
$"omegaReq={requestedOmega:0.000} omega={fleetOmega:0.000} comp=({compVx:0.0},{compVy:0.0},{compOmega:0.000})",
|
||
"FleetRotateCenterDbg");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 原地旋转纠偏单轴 PI 控制器。err 为车体系偏差(mm 或 deg)，输出为修正速度(mm/s 或 deg/s)，
|
||
/// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。
|
||
/// </summary>
|
||
private static float RotatePiTerm(float err, ref float integ, float deadband,
|
||
float pFac, float iFac, float max, float dt, bool allowIntegrate = true)
|
||
{
|
||
// 死区内：不再累加误差，仅输出已积累的积分项(维持对恒定扰动的稳态补偿)。
|
||
if (Math.Abs(err) < deadband)
|
||
return ClampBias(integ * iFac, max);
|
||
|
||
// 条件积分(抗 windup)：仅当 (a) 允许积分(舵轮已对齐、车在真正转动) 且
|
||
// (b) 总输出未在同向饱和 时才累加误差。
|
||
// allowIntegrate=false：上一拍舵轮未对齐(gate=0、车没动)，此时积分误差是纯 windup，
|
||
// 会让纠偏越积越大、舵轮更对不齐 → 原地卡死；故冻结积分(保留已有值，仅输出 P+已积分)。
|
||
// 同向饱和停积分：起步大误差让 P 顶满 max 时继续积分会顶满积分器，误差反向后泄放慢、造成过冲回拉。
|
||
var unclamped = err * pFac + integ * iFac;
|
||
var saturatedSameSign = Math.Abs(unclamped) >= max && Math.Sign(unclamped) == Math.Sign(err);
|
||
if (allowIntegrate && !saturatedSameSign)
|
||
integ += err * dt;
|
||
|
||
var iTerm = integ * iFac;
|
||
// 抗积分饱和：把积分贡献限制在 ±max，并反算回写积分器，避免 windup。
|
||
if (iFac > 1e-9f)
|
||
{
|
||
var iLimit = max / iFac;
|
||
if (integ > iLimit) integ = iLimit;
|
||
else if (integ < -iLimit) integ = -iLimit;
|
||
iTerm = integ * iFac;
|
||
}
|
||
|
||
return ClampBias(err * pFac + iTerm, max);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 主车专用：通过 Playground WebAPI 读取本车与邻车的真实 sim 位姿，落 RotatePoseDbg 日志。
|
||
/// 量化"开环横向滑移"来源：
|
||
/// - w1/w2/dw：两车实际偏航角速率(deg/s)。dw 持续非零 ⇒ 主从转速不一致(相对旋转)。
|
||
/// - rel_in1(x,y,dyaw)：邻车在本车体系下的真实相对位姿；dyaw=实际相对朝向−180°。
|
||
/// dyaw≈0 但 y 持续漂 ⇒ 纯横向平移滑移(非角度滞后，印证 B 无用)。
|
||
/// - midDrift：车队几何中心(世界系)相对起转瞬间的位移 ⇒ 整体平移漂移量。
|
||
/// 节流 ~200ms；WebAPI 异常时静默跳过，不影响控制环。
|
||
/// </summary>
|
||
private void LogRotatePoseSample()
|
||
{
|
||
if (!Conf.MultiVehicleRotatePoseWebApiDiagEnabled) return;
|
||
|
||
var now = DateTime.Now;
|
||
if ((now - _rotPoseLastLog).TotalMilliseconds < 200) return;
|
||
_rotPoseLastLog = now;
|
||
|
||
var neighbor = Conf.PlaygroundNeighborRobotName;
|
||
if (string.IsNullOrWhiteSpace(neighbor) || neighbor == Conf.PlaygroundRobotName) return;
|
||
|
||
try
|
||
{
|
||
var p1 = PlaygroundWebApi.GetPose(Conf.PlaygroundWebApiUrl, Conf.PlaygroundRobotName);
|
||
var p2 = PlaygroundWebApi.GetPose(Conf.PlaygroundWebApiUrl, neighbor);
|
||
|
||
// 邻车在本车(car1)体系下的相对位姿：rel = R(-yaw1)·(p2-p1)
|
||
var ddx = p2.X - p1.X;
|
||
var ddy = p2.Y - p1.Y;
|
||
var th1 = p1.YawDeg * (float)Math.PI / 180f;
|
||
var c = (float)Math.Cos(th1);
|
||
var s = (float)Math.Sin(th1);
|
||
var relX = ddx * c + ddy * s;
|
||
var relY = -ddx * s + ddy * c;
|
||
var relYaw = (float)LessMath.ThDiff(p2.YawDeg - p1.YawDeg, 180f); // 实际相对朝向偏离 180° 的量
|
||
var dist = (float)Math.Sqrt(ddx * ddx + ddy * ddy);
|
||
|
||
var midX = (p1.X + p2.X) / 2f;
|
||
var midY = (p1.Y + p2.Y) / 2f;
|
||
if (!_rotPoseEpisode)
|
||
{
|
||
_rotPoseEpisode = true;
|
||
_rotPoseMid0X = midX;
|
||
_rotPoseMid0Y = midY;
|
||
}
|
||
var midDx = midX - _rotPoseMid0X;
|
||
var midDy = midY - _rotPoseMid0Y;
|
||
|
||
// 实际偏航角速率（数值微分）
|
||
float w1 = 0, w2 = 0, dw = 0;
|
||
if (_rotPosePrevTime != DateTime.MinValue)
|
||
{
|
||
var dt = (float)(now - _rotPosePrevTime).TotalSeconds;
|
||
if (dt > 1e-3)
|
||
{
|
||
w1 = (float)LessMath.ThDiff(p1.YawDeg, _rotPosePrevYaw1) / dt;
|
||
w2 = (float)LessMath.ThDiff(p2.YawDeg, _rotPosePrevYaw2) / dt;
|
||
dw = w1 - w2;
|
||
}
|
||
}
|
||
_rotPosePrevTime = now;
|
||
_rotPosePrevYaw1 = p1.YawDeg;
|
||
_rotPosePrevYaw2 = p2.YawDeg;
|
||
|
||
DLog.Log(
|
||
$"car1({p1.X:F0},{p1.Y:F0},{p1.YawDeg:F2}) car2({p2.X:F0},{p2.Y:F0},{p2.YawDeg:F2}) " +
|
||
$"| rel_in1(x:{relX:F0} y:{relY:F0} dyaw:{relYaw:F2}) dist:{dist:F0} " +
|
||
$"| mid({midX:F0},{midY:F0}) midDrift(dx:{midDx:F0} dy:{midDy:F0} |d|:{Math.Sqrt(midDx * midDx + midDy * midDy):F0}) " +
|
||
$"| w1:{w1:F2} w2:{w2:F2} dw:{dw:F2}",
|
||
"RotatePoseDbg");
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
// 诊断用途，WebAPI 不通时静默（仅偶发提示），不打断控制环。
|
||
Hedingben.ToastText($"RotatePose WebAPI err: {e.Message}", $"MultiVehicle{CarNum}-rotpose");
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
}
|