Fix fleet in-place rotation coordination
- Gate rotate-mode compensation PI by active fleet omega and reset the integrator/timer when idle, so releasing the joystick stops the cars cleanly instead of drifting/self-rotating from a wound-up integral. - Add conditional integration anti-windup in RotatePiTerm to reduce the startup overshoot after the initial relative-pose spike. - Raise translational P gain for faster startup correction. - Superimpose body-frame compensation onto swerve rotation and add MultiVehicleDbg / RotateDbg / RotatePoseDbg diagnostics. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -64,30 +64,25 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
// 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
|
||||
private DateTime _mvDbgLastLog = DateTime.MinValue;
|
||||
private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist;
|
||||
// 原地旋转(mode2)实际叠加的车体系纠偏旋量(mm/s, mm/s, deg/s)，仅用于诊断日志。
|
||||
private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
|
||||
// 原地旋转纠偏 PI 控制器的积分累加器(mm·s, mm·s, deg·s)与上次计算时刻。
|
||||
private float _rotIntegX, _rotIntegY, _rotIntegTh;
|
||||
private DateTime _rotPiLastTime = DateTime.MinValue;
|
||||
|
||||
// 写盘诊断日志（Clumsy 侧）。文件落在 Clumsy 工作目录，复现后离线分析。
|
||||
// 每次进程启动清空一次（_fleetDiagInit），同一次运行内追加，避免跨多次运行累积。
|
||||
// 原地旋转“两车实际 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 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
|
||||
{
|
||||
// 诊断日志失败不影响主流程
|
||||
}
|
||||
DLog.Log($"car{CarNum} {msg}", "FleetDiagClumsy");
|
||||
}
|
||||
|
||||
// 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
|
||||
@@ -222,6 +217,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
}
|
||||
});
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
@@ -246,6 +243,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
Clear = () => sendMotionPainter.Clear()
|
||||
};
|
||||
|
||||
DLog.Log("MultiVehicleLoop thread running", "MultiVehicle");
|
||||
Console.WriteLine("[MultiVehicle] loop thread running");
|
||||
var loopCount = 0L;
|
||||
while (true)
|
||||
{
|
||||
try
|
||||
@@ -257,6 +257,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -338,7 +342,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
var syncTh = Conf.TestCarSyncTh;
|
||||
var syncDistance = Conf.TestCarSyncDistance;
|
||||
var deltaDetectCenter = Conf.DeltaDetectCenter;
|
||||
float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0;
|
||||
float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0, fleetOmega = 0;
|
||||
var fleetMode = 0; // 0=常规 1=蟹行 2=原地旋转
|
||||
|
||||
CenterX = CenterY = CenterTh = 0;
|
||||
|
||||
@@ -347,16 +352,46 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
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;
|
||||
fleetMode = MultiVehicleManualMode;
|
||||
if (fleetMode == 2)
|
||||
{
|
||||
// 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
|
||||
fleetOmega = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
|
||||
fleetVx = 0;
|
||||
fleetFrontTh = 0;
|
||||
fleetRearTh = 0;
|
||||
_multiVehicleAccumulateTh = 0f;
|
||||
_multiVehicleLastThTime = DateTime.Now;
|
||||
}
|
||||
else if (fleetMode == 1)
|
||||
{
|
||||
// 蟹行：把 (前后向Vx, 横向Vy) 合成速度矢量，四轮同向打到该方向（前后舵轮角相同）。
|
||||
// 舵轮角限制在 ±90°，超出则取反向并令速度取负，避免出现 180° 这类不可达转角。
|
||||
var vx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
|
||||
var vy = MultiVehicleManualVy * Conf.ManualCarSyncVyFac;
|
||||
var speed = (float)Math.Sqrt(vx * vx + vy * vy);
|
||||
var crabAngle = (float)(Math.Atan2(vy, vx) * 180.0 / Math.PI);
|
||||
if (crabAngle > 90f) { crabAngle -= 180f; speed = -speed; }
|
||||
else if (crabAngle < -90f) { crabAngle += 180f; speed = -speed; }
|
||||
fleetVx = speed;
|
||||
fleetFrontTh = crabAngle;
|
||||
fleetRearTh = crabAngle;
|
||||
_multiVehicleAccumulateTh = 0f;
|
||||
_multiVehicleLastThTime = DateTime.Now;
|
||||
}
|
||||
else
|
||||
{
|
||||
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
|
||||
{
|
||||
@@ -382,6 +417,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
fleetVx = notification.FleetVx;
|
||||
fleetFrontTh = notification.FleetFrontTh;
|
||||
fleetRearTh = notification.FleetRearTh;
|
||||
fleetMode = notification.Mode;
|
||||
fleetOmega = notification.FleetOmega;
|
||||
}
|
||||
|
||||
var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
|
||||
@@ -451,6 +488,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
fleetVx = 0;
|
||||
fleetFrontTh = 0;
|
||||
fleetRearTh = 0;
|
||||
fleetOmega = 0;
|
||||
Hedingben.ToastText(
|
||||
$"车队停车：{(!ownDetectOk ? "本车" : "其它车")}2腿检测丢失（速度已置零）",
|
||||
$"MultiVehicle{CarNum}-stop");
|
||||
@@ -516,10 +554,70 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
}
|
||||
|
||||
sendMotionPainter.Clear();
|
||||
chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510,
|
||||
localCompensateX: xDetectCompensate + xPosCompensate,
|
||||
localCompensateY: yDetectCompensate + yPosCompensate,
|
||||
localCompensateTh: thDetectCompensate + thPosCompensate);
|
||||
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) > Conf.MultiVehicleRotateActiveOmega;
|
||||
if (canMove && rotating)
|
||||
{
|
||||
var errX = detectDx + posBiasX; // mm，车体系：本车纵向(前+)应移动量
|
||||
var errY = detectDy + posBiasY; // mm，车体系：本车横向(左+)应移动量
|
||||
var errTh = detectDth + posBiasTh; // deg，本车应转角
|
||||
rotCompVx = RotatePiTerm(errX, ref _rotIntegX, Conf.SingleCarSyncPrecisionXy,
|
||||
Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
|
||||
Conf.MultiVehicleRotateCompXyMax, dt);
|
||||
rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
|
||||
Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
|
||||
Conf.MultiVehicleRotateCompXyMax, dt);
|
||||
rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
|
||||
Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac,
|
||||
Conf.MultiVehicleRotateCompThMax, dt);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 检测丢失或未指令旋转：清零补偿并复位积分/计时，停止时不再有残留驱动。
|
||||
_rotIntegX = _rotIntegY = _rotIntegTh = 0;
|
||||
_rotPiLastTime = DateTime.MinValue;
|
||||
}
|
||||
|
||||
_mvRotCompVx = rotCompVx;
|
||||
_mvRotCompVy = rotCompVy;
|
||||
_mvRotCompOmega = rotCompOmega;
|
||||
|
||||
chassis.SendRotateMotion(fleetOmega,
|
||||
localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);
|
||||
|
||||
// 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
|
||||
if (isMaster)
|
||||
LogRotatePoseSample();
|
||||
}
|
||||
else
|
||||
{
|
||||
_mvRotCompVx = 0;
|
||||
_mvRotCompVy = 0;
|
||||
_mvRotCompOmega = 0;
|
||||
// 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
|
||||
_rotIntegX = _rotIntegY = _rotIntegTh = 0;
|
||||
_rotPiLastTime = DateTime.MinValue;
|
||||
_rotPoseEpisode = false;
|
||||
_rotPosePrevTime = DateTime.MinValue;
|
||||
// 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
|
||||
chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510,
|
||||
localCompensateX: xDetectCompensate + xPosCompensate,
|
||||
localCompensateY: yDetectCompensate + yPosCompensate,
|
||||
localCompensateTh: thDetectCompensate + thPosCompensate);
|
||||
}
|
||||
}
|
||||
|
||||
// === 诊断日志（节流 ~300ms）===
|
||||
@@ -544,7 +642,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
$"| 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}";
|
||||
$"| 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})";
|
||||
|
||||
DLog.Log(dbg, "MultiVehicleDbg");
|
||||
FleetDiag(dbg);
|
||||
@@ -584,6 +683,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
FleetVx = fleetVx,
|
||||
FleetFrontTh = fleetFrontTh,
|
||||
FleetRearTh = fleetRearTh,
|
||||
Mode = fleetMode,
|
||||
FleetOmega = fleetOmega,
|
||||
AutoEnabled = MultiVehicleAutoEnabled,
|
||||
ManualEnabled = MultiVehicleManualEnabled,
|
||||
SyncTh = syncTh,
|
||||
@@ -769,5 +870,112 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
private static float ClampBias(float value, float threshold)
|
||||
=> Math.Sign(value) * Math.Min(Math.Abs(value), threshold);
|
||||
|
||||
/// <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)
|
||||
{
|
||||
// 死区内：不再累加误差，仅输出已积累的积分项(维持对恒定扰动的稳态补偿)。
|
||||
if (Math.Abs(err) < deadband)
|
||||
return ClampBias(integ * iFac, max);
|
||||
|
||||
// 条件积分(抗 windup)：仅当总输出未在同向饱和时才累加误差。
|
||||
// 起步阶段大误差会让 P 项接近/超过 max，此时继续积分会顶满积分器，
|
||||
// 误差反向后需很久才能泄放，造成纠偏"过冲再回拉"。此处饱和即停积分。
|
||||
var unclamped = err * pFac + integ * iFac;
|
||||
var saturatedSameSign = Math.Abs(unclamped) >= max && Math.Sign(unclamped) == Math.Sign(err);
|
||||
if (!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()
|
||||
{
|
||||
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
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user