Update multi-vehicle sync and crab walk docs
This commit is contained in:
@@ -3,6 +3,7 @@ using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
@@ -13,6 +14,8 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
private DateTime _sendMotionDbgLast = DateTime.MinValue;
|
||||
|
||||
public override MultiWheelGeometricController Get()
|
||||
{
|
||||
return new MultiWheelGeometricController
|
||||
@@ -45,25 +48,52 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
|
||||
|
||||
MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
|
||||
{
|
||||
PilotDefinition.Self.MultiVehicleAutoEnabled = true;
|
||||
lock (PilotDefinition.Self.MultiVehicleFleet)
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
// A: 用固定锁对象（不再锁会被替换的字段引用）。
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock)
|
||||
fleetCnt = self.MultiVehicleFleet.Count;
|
||||
|
||||
// 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
|
||||
if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
|
||||
{
|
||||
if (PilotDefinition.Self.MultiVehicleFleet.Count != PilotDefinition.Conf.MultiVehicleFleetNum)
|
||||
return;
|
||||
_sendMotionDbgLast = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
|
||||
$"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
|
||||
$"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
|
||||
$"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
PilotDefinition.Self.MultiVehicleAutoVx = speed;
|
||||
PilotDefinition.Self.MultiVehicleAutoFrontTh = frontTh;
|
||||
PilotDefinition.Self.MultiVehicleAutoRearTh = rearTh;
|
||||
if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
|
||||
return;
|
||||
|
||||
self.MultiVehicleAutoVx = speed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
// D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealAngle;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
// B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
},
|
||||
|
||||
MultiVehicleGetFleetPos = () => new Location
|
||||
// G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
|
||||
MultiVehicleGetFleetPos = () =>
|
||||
{
|
||||
x = PilotDefinition.Self.CenterX,
|
||||
y = PilotDefinition.Self.CenterY,
|
||||
th = PilotDefinition.Self.CenterTh,
|
||||
l_step = 1,
|
||||
tick = DateTime.Now.Ticks
|
||||
var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
|
||||
return new Location
|
||||
{
|
||||
x = snap.X,
|
||||
y = snap.Y,
|
||||
th = snap.Th,
|
||||
l_step = 1,
|
||||
tick = DateTime.Now.Ticks
|
||||
};
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
@@ -7,8 +7,10 @@ using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
@@ -124,6 +126,473 @@ public class MultiRotateMovementTest : MovementTest
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 车队联动-原地旋转动作 =====
|
||||
// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
|
||||
// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
|
||||
// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
|
||||
// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
|
||||
// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
|
||||
//
|
||||
// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
|
||||
// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
|
||||
// 无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
|
||||
public class FleetRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
|
||||
public float Omega = 15f;
|
||||
|
||||
/// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
|
||||
public float TargetDeltaDeg = 90f;
|
||||
|
||||
/// <summary>到位角度精度(deg)。</summary>
|
||||
public float ArriveDeg = 1.5f;
|
||||
|
||||
/// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
|
||||
public float SlowDeg = 25f;
|
||||
|
||||
/// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
|
||||
public float MinOmega = 3f;
|
||||
|
||||
/// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，<=0 关闭缓启动（阶跃起步）。</summary>
|
||||
public float AccelDegPerSec2 = 20f;
|
||||
|
||||
/// <summary>
|
||||
/// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
|
||||
/// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
|
||||
/// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
|
||||
/// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
|
||||
/// </summary>
|
||||
public bool UseDetourHeading = true;
|
||||
|
||||
// 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。
|
||||
|
||||
/// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
|
||||
public float SettleSec = 0.5f;
|
||||
|
||||
private void ClearScript()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop() => ClearScript();
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var dir = Math.Sign(TargetDeltaDeg);
|
||||
if (dir == 0) dir = 1;
|
||||
var maxOmega = Math.Abs(Omega);
|
||||
var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
|
||||
var slowDeg = Math.Max(1e-3f, SlowDeg); // 减速区宽度
|
||||
var accel = AccelDegPerSec2; // 缓启动角加速度，仅作用于起步，<=0 关闭
|
||||
var targetMag = Math.Abs(TargetDeltaDeg);
|
||||
|
||||
var hasPos = UseDetourHeading;
|
||||
var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
|
||||
var startTh = prevTh;
|
||||
var accumulated = 0f; // 累计带符号转角(deg)
|
||||
var start = DateTime.Now;
|
||||
var lastTime = start;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
|
||||
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
|
||||
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
|
||||
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
|
||||
|
||||
DLog.Log(
|
||||
$"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
|
||||
$"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
|
||||
$"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
|
||||
"FleetRotateDbg");
|
||||
|
||||
// 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
|
||||
// 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
|
||||
var stopReason = "stop()";
|
||||
while (true)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
|
||||
lastTime = now;
|
||||
var elapsed = (now - start).TotalSeconds;
|
||||
|
||||
float desiredMag;
|
||||
float curTh = 0f, remaining = 0f, actualRate = 0f;
|
||||
if (hasPos)
|
||||
{
|
||||
curTh = (float)DetourInterface.getCartLocation().th;
|
||||
var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
|
||||
accumulated += step;
|
||||
actualRate = dt > 1e-3 ? step / dt : 0f; // 实际角速率(deg/s)，用于对比指令
|
||||
prevTh = curTh;
|
||||
|
||||
remaining = targetMag - Math.Abs(accumulated);
|
||||
if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
|
||||
|
||||
// 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
|
||||
// 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
|
||||
desiredMag = remaining < slowDeg
|
||||
? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
|
||||
: maxOmega;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
|
||||
desiredMag = maxOmega;
|
||||
if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
|
||||
}
|
||||
|
||||
// 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
|
||||
// “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
|
||||
if (accel > 1e-3 && desiredMag > cmdMag)
|
||||
cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
|
||||
else
|
||||
cmdMag = desiredMag;
|
||||
|
||||
self.MultiVehicleScriptVth = dir * cmdMag;
|
||||
|
||||
// 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
|
||||
if ((now - lastLog).TotalMilliseconds >= 150)
|
||||
{
|
||||
lastLog = now;
|
||||
DLog.Log(
|
||||
hasPos
|
||||
? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
|
||||
: $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
|
||||
"FleetRotateDbg");
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
hasPos
|
||||
? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
|
||||
: $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
|
||||
"FleetRotate");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
// 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
|
||||
while (DateTime.Now < settleEnd)
|
||||
yield return true;
|
||||
|
||||
ClearScript();
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
|
||||
$"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
|
||||
"FleetRotateDbg");
|
||||
Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-原地旋转")]
|
||||
public class FleetRotateInPlaceTest : MovementTest
|
||||
{
|
||||
private FleetRotateInPlace _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetRotateInPlace
|
||||
{
|
||||
Omega = PilotDefinition.Conf.FleetRotateOmega,
|
||||
TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
|
||||
ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
|
||||
SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
|
||||
MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
|
||||
AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
|
||||
SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
|
||||
UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 车队联动-自动蟹行动作 =====
|
||||
// 以当前车队中心为起点，构造与车队朝向夹角 x、长度 y 的直线路径；执行侧复用
|
||||
// TickMultiVehicle 的脚本手动等价输入(mode=1)，也就是 FleetRemote 手动蟹行同一条下发链路。
|
||||
//
|
||||
// 手动蟹行已验证丝滑，自动动作只额外做两件事：
|
||||
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度和横向偏差；
|
||||
// 2) 用小幅、带斜率限制的方向修正写 MultiVehicleScriptVx/Vy，避免几何控制器 bias/dTh 阶跃造成抖动。
|
||||
//
|
||||
// 与 FleetRemote 手动蟹行(mode==1)对照：TickMultiVehicle 仍负责合成 frontTh==rearTh 的蟹行舵角并广播给从车。
|
||||
//
|
||||
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
|
||||
public class FleetCrabWalk : MovementDefinition
|
||||
{
|
||||
/// <summary>与当前车队朝向的夹角(deg，逆时针为正)。</summary>
|
||||
public float CrabAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径长度(mm)。</summary>
|
||||
public float CrabLengthMm = 2000f;
|
||||
|
||||
/// <summary>行驶速度(m/s)。</summary>
|
||||
public float CrabSpeed = 0.2f;
|
||||
|
||||
/// <summary>兼容旧配置；当前脚本手动等价实现不再直接使用几何控制器 gcp 上限。</summary>
|
||||
public float GcpThetaThreshold = 95f;
|
||||
|
||||
/// <summary>横向误差转向增益，沿用 Stanley 形式：atan(gain * lateral / speed)。</summary>
|
||||
public float CorrectionGain = 1f;
|
||||
|
||||
/// <summary>自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大收敛越快。</summary>
|
||||
public float CorrectionAngleThreshold = 8f;
|
||||
|
||||
/// <summary>脚本 Vx/Vy 命令斜率限制(m/s^2)，避免纠偏量变化造成舵角阶跃。</summary>
|
||||
public float CommandAccel = 0.4f;
|
||||
|
||||
private bool _stopping;
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
private static float Slew(float current, float target, float maxStep)
|
||||
{
|
||||
var diff = target - current;
|
||||
if (Math.Abs(diff) <= maxStep) return target;
|
||||
return current + Math.Sign(diff) * maxStep;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
_stopping = false;
|
||||
|
||||
DLog.Log(
|
||||
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
|
||||
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
|
||||
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
|
||||
$"manualLike=true corrGain={CorrectionGain:0.00} corrMax={CorrectionAngleThreshold:0.0} accel={CommandAccel:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
|
||||
// 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
|
||||
DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
|
||||
if (!self.TryGetFleetCenterFromSlam(out var x0, out var y0, out var theta))
|
||||
{
|
||||
DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
var dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0));
|
||||
var phiRad = phi / 180.0 * Math.PI;
|
||||
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) crabAngle={CrabAngleDeg:0.0} phi={phi:0.0} " +
|
||||
$"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
// 手动蟹行已验证丝滑：这里复用 TickMultiVehicle 的脚本手动等价输入(mode=1)，
|
||||
// 只在本动作内根据车队中心相对直线的横向误差缓慢调整 Vx/Vy 方向。
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
self.MultiVehicleScriptMode = 1;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
DLog.Log("WARMUP 已启用脚本蟹行(mode=1)，等待编队成员就位…", "FleetCrabDbg");
|
||||
|
||||
var warmEnd = DateTime.Now.AddSeconds(2.0);
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
self.MultiVehicleScriptMode = 1;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
if (warmIter % 5 == 0)
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
|
||||
$"scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
if (cnt >= conf.MultiVehicleFleetNum)
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log(
|
||||
$"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}",
|
||||
"FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (!warmReady)
|
||||
DLog.Log("WARMUP 超时：编队仍未就位，继续进入脚本蟹行（若不动请查看 FleetDiagClumsy ready/cnt）",
|
||||
"FleetCrabDbg");
|
||||
|
||||
Hedingben.ToastText($"车队蟹行 夹角{CrabAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
|
||||
|
||||
var iter = 0;
|
||||
var cmdVx = 0f;
|
||||
var cmdVy = 0f;
|
||||
var lastTime = DateTime.Now;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var finishDistance = Math.Max(20f, conf.FinishDistance);
|
||||
var stopReason = "done";
|
||||
|
||||
while (!_stopping)
|
||||
{
|
||||
iter++;
|
||||
var now = DateTime.Now;
|
||||
var dt = (float)Math.Min(0.2, Math.Max(0.001, (now - lastTime).TotalSeconds));
|
||||
lastTime = now;
|
||||
|
||||
self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth);
|
||||
var delta = new Vector2(cx - x0, cy - y0);
|
||||
var along = Vector2.Dot(delta, pathDir);
|
||||
var lateral = Vector2.Dot(delta, pathLeft);
|
||||
var remain = CrabLengthMm - along;
|
||||
if (remain <= finishDistance)
|
||||
break;
|
||||
|
||||
var speed = Math.Abs(CrabSpeed);
|
||||
if (remain < conf.SlowDistance && conf.SlowDistance > 1)
|
||||
{
|
||||
var ratio = (float)Math.Pow(Math.Max(0, remain) / conf.SlowDistance, conf.SlowingPow);
|
||||
speed = ratio * (speed - Math.Abs(conf.FinishSpeed)) + Math.Abs(conf.FinishSpeed);
|
||||
}
|
||||
|
||||
var correction = (float)(-Math.Atan(CorrectionGain * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
|
||||
correction = Math.Sign(correction) * Math.Min(Math.Abs(correction), Math.Abs(CorrectionAngleThreshold));
|
||||
var desiredWorldAngle = CommonMath.RoundTh(phi + correction);
|
||||
var localAngle = (float)CommonMath.ThDiff(desiredWorldAngle, cth);
|
||||
var localRad = localAngle / 180.0 * Math.PI;
|
||||
var targetVx = speed * (float)Math.Cos(localRad);
|
||||
var targetVy = speed * (float)Math.Sin(localRad);
|
||||
var maxStep = Math.Max(0.05f, CommandAccel) * dt;
|
||||
cmdVx = Slew(cmdVx, targetVx, maxStep);
|
||||
cmdVy = Slew(cmdVy, targetVy, maxStep);
|
||||
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
self.MultiVehicleScriptMode = 1;
|
||||
self.MultiVehicleScriptVx = cmdVx;
|
||||
self.MultiVehicleScriptVy = cmdVy;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
|
||||
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
|
||||
{
|
||||
lastLog = DateTime.Now;
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"along={along:0} lateral={lateral:0} remain={remain:0} corr={correction:0.0} " +
|
||||
$"localAngle={localAngle:0.0} cmd=({cmdVx:0.000},{cmdVy:0.000}) cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (_stopping)
|
||||
stopReason = "stop";
|
||||
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
self.MultiVehicleScriptMode = 1;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Cleanup();
|
||||
Hedingben.ToastText("车队蟹行完成", "FleetCrab");
|
||||
DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-自动蟹行")]
|
||||
public class FleetCrabWalkTest : MovementTest
|
||||
{
|
||||
private FleetCrabWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetCrabWalk
|
||||
{
|
||||
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
|
||||
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||||
CorrectionGain = PilotDefinition.Conf.FleetCrabCorrectionGain,
|
||||
CorrectionAngleThreshold = PilotDefinition.Conf.FleetCrabCorrectionAngleDeg,
|
||||
CommandAccel = PilotDefinition.Conf.FleetCrabCommandAccel
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
|
||||
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
|
||||
|
||||
|
||||
@@ -14,8 +14,14 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "[sync] 手动Vx系数")] public float ManualCarSyncVxFac = 1f;
|
||||
[FieldMember(desc = "[sync] 手动Vy系数(蟹行横向)")] public float ManualCarSyncVyFac = 1f;
|
||||
[FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
|
||||
[FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
||||
[FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
|
||||
[FieldMember(desc = "[sync] 是否启用Detour定位")] public bool PosAvailable = true;
|
||||
// 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
|
||||
// 默认 false：定位不参与车队内姿态纠正（各车按编队几何/互识别保持队形，不做 SLAM 逐车纠偏）。
|
||||
// 为 true：额外用 getCartLocation() 反推每台车相对编队中心的偏差并做 POS 补偿。
|
||||
// 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
|
||||
// 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
|
||||
[FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
|
||||
|
||||
[FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
|
||||
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
|
||||
@@ -34,6 +40,21 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
}
|
||||
|
||||
[FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
|
||||
|
||||
// E: 编队控制点半径(mm)。0 表示自动取 syncDistance/2（与 SetOriginBias 几何一致），>0 时按本值固定。
|
||||
// 取代历史硬编码 510，避免改间距后控制点半径不跟随导致转向/补偿几何错位。
|
||||
[FieldMember(desc = "多车联动：控制点半径(mm，0=syncDistance/2)")] public float MultiVehicleControlRadius = 0f;
|
||||
// B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)，
|
||||
// 即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
|
||||
[FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
|
||||
// C: fleet 成员存活 TTL(ms)。主车剔除超过此时长未 register/刷新的从车；编队就绪要求所有成员新鲜。
|
||||
// 0 表示自动取 max(500, interval*6)。
|
||||
[FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
|
||||
// D: 自动模式下用主车路径控制器的理想车队中心(idealPos/idealAngle)作为各车 layout 目标，
|
||||
// 弧线路径上做 per-car 前馈而非仅共用 frontTh/rearTh 事后纠偏。
|
||||
[FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
|
||||
// H: 自动模式必须有有效车队中心(SLAM 可反推)，全程定位丢失时停车，避免纯 SLAM 下盲跑。
|
||||
[FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
|
||||
[FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
|
||||
@@ -61,6 +82,10 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
// 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
|
||||
// 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
|
||||
[FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
|
||||
// 可选硬安全网：每轮纠偏速度幅值 ≤ 该比例×本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
|
||||
// 默认 <0 关闭——纠偏随转速缩放(代码 #1)已让"纠偏:切向"比例全程恒定，匀速段不应再被削弱。
|
||||
// 仅在极端启动偏差导致匀速段仍乱打方向时，可设为 ~1.0(偏角≤45°) 兜底。
|
||||
[FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限(默认-1关闭)")] public float MultiVehicleRotateCompTangentFrac = -1f;
|
||||
|
||||
[FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
|
||||
[FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
|
||||
@@ -92,6 +117,59 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
|
||||
public float InPlaceRotateWheelAlignDeg = 2f;
|
||||
|
||||
// ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
|
||||
// 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
|
||||
[FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
|
||||
public float FleetRotateOmega = 15f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
|
||||
public float FleetRotateTargetDeltaDeg = 90f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
|
||||
public float FleetRotateArriveDeg = 1.5f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
|
||||
public float FleetRotateSlowDeg = 25f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
|
||||
public float FleetRotateMinOmega = 3f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
|
||||
public float FleetRotateAccel = 20f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
|
||||
public float FleetRotateSettleSec = 0.5f;
|
||||
|
||||
// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。
|
||||
// false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。
|
||||
[FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
|
||||
public bool FleetRotateUseDetourHeading = true;
|
||||
|
||||
// ===== 车队联动-自动蟹行（FleetCrabWalk）=====
|
||||
// 以当前车队中心为起点，构造与车队朝向夹角 FleetCrabAngleDeg、长度 FleetCrabLengthMm 的直线路径，
|
||||
// 复用脚本手动等价输入(mode=1)斜向平移；动作侧只把横向误差转换成小幅、带斜率限制的蟹行方向修正。
|
||||
[FieldMember(desc = "车队蟹行：与车队朝向夹角(deg，逆时针为正)")]
|
||||
public float FleetCrabAngleDeg = 45f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：路径长度(mm)")]
|
||||
public float FleetCrabLengthMm = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
|
||||
public float FleetCrabSpeed = 0.2f;
|
||||
|
||||
// 兼容旧版几何控制器实现；当前自动蟹行走脚本手动等价输入，不再直接使用该上限。
|
||||
[FieldMember(desc = "车队蟹行：旧几何控制器gcp角度上限(deg)")]
|
||||
public float FleetCrabGcpThetaThreshold = 95f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：横向误差纠偏增益")]
|
||||
public float FleetCrabCorrectionGain = 1f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：自动纠偏最大改向角(deg)")]
|
||||
public float FleetCrabCorrectionAngleDeg = 8f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：脚本速度命令斜率(m/s^2)")]
|
||||
public float FleetCrabCommandAccel = 0.4f;
|
||||
|
||||
// ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
|
||||
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
|
||||
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
|
||||
|
||||
@@ -41,6 +41,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
[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;
|
||||
@@ -49,10 +59,45 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
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;
|
||||
|
||||
#region 夹臂变量
|
||||
@@ -97,6 +142,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
|
||||
// 原地旋转纠偏 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 读取）。
|
||||
@@ -211,8 +257,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
var infoJson = Uri.UnescapeDataString(query.Info ?? "");
|
||||
var info = JsonConvert.DeserializeObject<VehicleSyncInfo>(infoJson)
|
||||
?? throw new Exception("VehicleSyncInfo is null");
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
{
|
||||
MultiVehicleFleet[query.CarNum] = info;
|
||||
_multiVehicleFleetSeen[query.CarNum] = DateTime.Now; // C: 刷新存活时刻
|
||||
}
|
||||
return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
|
||||
}
|
||||
catch (Exception e)
|
||||
@@ -222,16 +271,30 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
}
|
||||
});
|
||||
|
||||
PicoHttpServer.AddGetHandler("/multi-vehicle-notify", new { Notification = "" }, query =>
|
||||
// F: notify 改用 POST + JSON body（取代 GET query 串），避免整队 Fleet 字典撑爆 URL 长度上限；
|
||||
// 用 Seq 丢弃乱序到达的旧包，避免从车短暂套用过期指令。
|
||||
PicoHttpServer.AddPostTextHandler("/multi-vehicle-notify", body =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var notifyJson = Uri.UnescapeDataString(query.Notification ?? "");
|
||||
var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(notifyJson)
|
||||
var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(body ?? "")
|
||||
?? throw new Exception("notification is null");
|
||||
// 乱序丢弃：仅应用序列号大于已应用值的包。Seq==0 视为旧版无序列号始终接受；
|
||||
// 若 Seq 明显回退(差值>100)，判定为主车重启的新会话，重新接受并对齐序列号。
|
||||
if (notification.Seq != 0 && notification.Seq <= _multiVehicleAppliedSeq &&
|
||||
notification.Seq > _multiVehicleAppliedSeq - 100)
|
||||
return JsonConvert.SerializeObject(new { code = 200, message = "stale" });
|
||||
_multiVehicleAppliedSeq = notification.Seq;
|
||||
|
||||
MultiVehicleAligned = notification.Aligned;
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
{
|
||||
MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
|
||||
// C: notify 内含整队成员，逐一刷新其本地存活时刻。
|
||||
var nowSeen = DateTime.Now;
|
||||
foreach (var key in MultiVehicleFleet.Keys)
|
||||
_multiVehicleFleetSeen[key] = nowSeen;
|
||||
}
|
||||
lock (_multiVehicleNotificationLock)
|
||||
{
|
||||
MultiVehicleNotification = notification;
|
||||
@@ -298,7 +361,14 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
{
|
||||
var isMaster = IsMultiVehicleMaster();
|
||||
var autoEnabled = false;
|
||||
var manualEnabled = MultiVehicleManualEnabled;
|
||||
// 手动等价输入 = 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;
|
||||
|
||||
if (isMaster)
|
||||
autoEnabled = MultiVehicleAutoEnabled;
|
||||
@@ -324,24 +394,30 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
{
|
||||
_mvDiskLastLog = DateTime.Now;
|
||||
int fleetCnt;
|
||||
lock (MultiVehicleFleet) fleetCnt = MultiVehicleFleet.Count;
|
||||
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} pos={Conf.PosAvailable}");
|
||||
$"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} useDetour={Conf.MultiVehicleSyncUseDetour}");
|
||||
}
|
||||
|
||||
if (!manualEnabled && !autoEnabled)
|
||||
{
|
||||
UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
|
||||
sendMotionPainter.Clear();
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
{
|
||||
MultiVehicleFleet.Clear();
|
||||
_multiVehicleFleetSeen.Clear();
|
||||
}
|
||||
MultiVehicleAutoEnabled = false;
|
||||
MultiVehicleAutoHasIdeal = false;
|
||||
_multiVehicleAppliedSeq = -1;
|
||||
_multiVehicleAccumulateTh = 0f;
|
||||
_multiVehicleLastThTime = DateTime.Now;
|
||||
|
||||
@@ -351,41 +427,53 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
return;
|
||||
}
|
||||
|
||||
if (isMaster && autoEnabled && !MultiVehicleManualEnabled && !FleetHasPosAvailable())
|
||||
// 自动模式整队姿态依赖 Detour 全局定位（与 MultiVehicleSyncUseDetour 无关）：要求编队至少一台车有定位。
|
||||
if (isMaster && autoEnabled && !manualEnabled && !FleetHasPosAvailable())
|
||||
{
|
||||
MultiVehicleAutoEnabled = false;
|
||||
Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
|
||||
return;
|
||||
}
|
||||
|
||||
var posAvailable = Conf.PosAvailable;
|
||||
// 两类用途解耦（关键语义）：
|
||||
// - useDetourCorrection (= MultiVehicleSyncUseDetour)：是否用 SLAM 位姿做"车队内姿态纠正"(POS 补偿)。
|
||||
// false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
|
||||
// - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
|
||||
// SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
|
||||
// 纠偏开启时本车也读。读取若因无有效定位阻塞，则联动线程随之阻塞、不下发速度（安全停车）。
|
||||
var autoMode = autoEnabled && !manualEnabled;
|
||||
var useDetourCorrection = Conf.MultiVehicleSyncUseDetour;
|
||||
var slamRead = useDetourCorrection || (isMaster && autoMode);
|
||||
float selfX = 0, selfY = 0, selfTh = 0;
|
||||
if (posAvailable)
|
||||
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=原地旋转
|
||||
|
||||
CenterX = CenterY = CenterTh = 0;
|
||||
float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
|
||||
float crabSteerLimit = 0;
|
||||
var crabReverseEquivalent = false;
|
||||
|
||||
if (isMaster)
|
||||
{
|
||||
if (MultiVehicleManualEnabled)
|
||||
if (manualEnabled)
|
||||
{
|
||||
syncTh = 0;
|
||||
fleetMode = MultiVehicleManualMode;
|
||||
fleetMode = manualMode;
|
||||
if (fleetMode == 2)
|
||||
{
|
||||
// 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
|
||||
fleetOmega = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
|
||||
fleetOmega = manualVth * Conf.ManualCarSyncVthFac;
|
||||
fleetVx = 0;
|
||||
fleetFrontTh = 0;
|
||||
fleetRearTh = 0;
|
||||
@@ -395,13 +483,19 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
else if (fleetMode == 1)
|
||||
{
|
||||
// 蟹行：把 (前后向Vx, 横向Vy) 合成速度矢量，四轮同向打到该方向（前后舵轮角相同）。
|
||||
// 舵轮角限制在 ±90°，超出则取反向并令速度取负，避免出现 180° 这类不可达转角。
|
||||
var vx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
|
||||
var vy = MultiVehicleManualVy * Conf.ManualCarSyncVyFac;
|
||||
// 舵轮物理/仿真限制约为 ±120°，不要把 ±90° 当边界；否则纯横移附近会在
|
||||
// -90° 与 +90°/反向速度两种等价表示之间跳变。只有超过蟹行舵角上限时才取等价反向。
|
||||
var vx = manualVx * Conf.ManualCarSyncVxFac;
|
||||
var vy = manualVy * 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; }
|
||||
crabInputVx = vx;
|
||||
crabInputVy = vy;
|
||||
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;
|
||||
@@ -410,8 +504,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
}
|
||||
else
|
||||
{
|
||||
fleetVx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
|
||||
var targetTh = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
|
||||
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;
|
||||
@@ -424,9 +518,26 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
}
|
||||
else
|
||||
{
|
||||
fleetVx = MultiVehicleAutoVx;
|
||||
fleetFrontTh = MultiVehicleAutoFrontTh;
|
||||
fleetRearTh = MultiVehicleAutoRearTh;
|
||||
// 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
|
||||
{
|
||||
fleetVx = fleetFrontTh = fleetRearTh = 0;
|
||||
MultiVehicleAutoVx = MultiVehicleAutoFrontTh = MultiVehicleAutoRearTh = 0;
|
||||
MultiVehicleAutoHasIdeal = false;
|
||||
MultiVehicleAutoEnabled = false;
|
||||
Hedingben.ToastText("自动速度命令超时，已停车（路径结束/控制器停发）", "MultiVehicle-auto-timeout");
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (MultiVehicleNotification != null)
|
||||
@@ -438,32 +549,42 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
syncTh = notification.SyncTh;
|
||||
syncDistance = notification.SyncDistance;
|
||||
deltaDetectCenter = notification.DeltaDetectCenter;
|
||||
CenterX = notification.CenterX;
|
||||
CenterY = notification.CenterY;
|
||||
CenterTh = notification.CenterTh;
|
||||
posAvailable = notification.PosAvailable;
|
||||
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;
|
||||
// D: 从车采用主车广播的理想车队中心（弧线时由 idealPos/idealAngle 而来）做前馈目标。
|
||||
MultiVehicleAutoHasIdeal = notification.HasIdeal;
|
||||
MultiVehicleAutoIdealX = notification.IdealX;
|
||||
MultiVehicleAutoIdealY = notification.IdealY;
|
||||
MultiVehicleAutoIdealTh = notification.IdealTh;
|
||||
}
|
||||
|
||||
var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
|
||||
|
||||
if (isMaster)
|
||||
{
|
||||
lock (MultiVehicleFleet)
|
||||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
|
||||
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))
|
||||
{
|
||||
CenterX = cx;
|
||||
CenterY = cy;
|
||||
CenterTh = cth;
|
||||
}
|
||||
PublishFleetCenter(cx, cy, cth);
|
||||
|
||||
// D: 自动模式（非手动）下，若控制器给出理想车队中心，则以理想位姿作为各车 layout 目标，
|
||||
// 使弧线路径上从车按各自相对曲率中心位置前馈，而非仅靠事后检测/SLAM 纠偏。
|
||||
if (!manualEnabled && MultiVehicleAutoEnabled &&
|
||||
Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal)
|
||||
PublishFleetCenter(MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh);
|
||||
}
|
||||
|
||||
var selfAligned = !Conf.MultiVehicleUseDetect;
|
||||
@@ -488,8 +609,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
else selfAligned = false;
|
||||
}
|
||||
|
||||
// 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（两台车都需有定位）
|
||||
if (float.IsNaN(currentSpacing) && posAvailable)
|
||||
// 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（需本车已读到全局定位）
|
||||
if (float.IsNaN(currentSpacing) && slamRead)
|
||||
currentSpacing = TryGetSlamSpacing(selfX, selfY);
|
||||
|
||||
if (float.IsNaN(currentSpacing))
|
||||
@@ -499,7 +620,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
$"间距 当前:{currentSpacing:F0}mm 目标:{syncDistance:F0}mm 差:{currentSpacing - syncDistance:F0}mm",
|
||||
$"MultiVehicle{CarNum}-spacing");
|
||||
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
{
|
||||
MultiVehicleAligned = MultiVehicleFleet.Count == Conf.MultiVehicleFleetNum &&
|
||||
MultiVehicleFleet.Values.All(v => v.Aligned);
|
||||
@@ -509,9 +630,24 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
// ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
|
||||
var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
|
||||
bool othersDetectOk;
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
|
||||
var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk);
|
||||
|
||||
// H: 自动模式（非手动）必须有有效车队中心——主车由 SLAM 反推、从车依赖主车广播 fleetPosValid。
|
||||
// 自动模式整队姿态始终依赖 Detour（与 MultiVehicleSyncUseDetour 无关）：定位全程丢失时强制停车。
|
||||
// （用 Detour 时若定位丢失，getCartLocation() 已先行阻塞，此处再兜底要求有效车队中心。）
|
||||
// 手动模式不受限（允许仅靠互识别/遥控行驶）。
|
||||
if (canMove && autoMode && Conf.MultiVehicleAutoRequireFleetCenter)
|
||||
{
|
||||
var fleetCenterValid = fleetPosValid && (!isMaster || TryInferFleetCenter(out _, out _, out _));
|
||||
if (!fleetCenterValid)
|
||||
{
|
||||
canMove = false;
|
||||
Hedingben.ToastText("自动模式无有效车队中心（定位丢失），已停车", $"MultiVehicle{CarNum}-autostop");
|
||||
}
|
||||
}
|
||||
|
||||
if (!canMove)
|
||||
{
|
||||
fleetVx = 0;
|
||||
@@ -531,7 +667,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
|
||||
var fleetReady = false;
|
||||
var fleetCount = 0;
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
{
|
||||
fleetCount = MultiVehicleFleet.Count;
|
||||
fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
|
||||
@@ -545,7 +681,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
if (fleetReady)
|
||||
{
|
||||
chassis.SetOriginBias(layoutX, layoutY, layoutTh);
|
||||
chassis.ControlPointRadius = syncDistance / 2f;
|
||||
// E: 统一控制点半径——配置 >0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。
|
||||
var controlRadius = Conf.MultiVehicleControlRadius > 0
|
||||
? Conf.MultiVehicleControlRadius
|
||||
: syncDistance / 2f;
|
||||
chassis.ControlPointRadius = controlRadius;
|
||||
// #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
|
||||
chassis.RotateCompTangentFrac = Conf.MultiVehicleRotateCompTangentFrac;
|
||||
|
||||
if (canMove && Conf.MultiVehicleUseDetect)
|
||||
{
|
||||
@@ -561,7 +703,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
thDetectCompensate = ClampBias(thDetectCompensate, Conf.MultiVehicleDetectBiasThThreshold);
|
||||
}
|
||||
|
||||
if (canMove && posAvailable && TryInferFleetCenter(out _, out _, out _))
|
||||
// 车队内姿态纠正(POS 补偿)：仅当 useDetourCorrection 开启且本车读到全局定位时施加。
|
||||
// 这是"定位参与车队内姿态纠正"的唯一开关点——关闭它不影响上面的整队姿态计算/安全门。
|
||||
if (canMove && useDetourCorrection && slamRead && TryInferFleetCenter(out _, out _, out _))
|
||||
{
|
||||
var supposedPos = LessMath.Transform2D(
|
||||
Tuple.Create(CenterX, CenterY, CenterTh),
|
||||
@@ -600,24 +744,41 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
var rotating = Math.Abs(fleetOmega) > Conf.MultiVehicleRotateActiveOmega;
|
||||
if (canMove && rotating)
|
||||
{
|
||||
// #3 抗饱和：上一拍舵轮未对齐(gate=0、车没真正转动)时冻结积分，避免卡死时积分越积越大。
|
||||
var allowInteg = chassis.LastRotateAligned;
|
||||
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);
|
||||
Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
|
||||
rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
|
||||
Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
|
||||
Conf.MultiVehicleRotateCompXyMax, dt);
|
||||
Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
|
||||
rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
|
||||
Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac,
|
||||
Conf.MultiVehicleRotateCompThMax, dt);
|
||||
Conf.MultiVehicleRotateCompThMax, 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;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 检测丢失或未指令旋转：清零补偿并复位积分/计时，停止时不再有残留驱动。
|
||||
// 检测丢失或未指令旋转：清零补偿并复位积分/计时/峰值，停止时不再有残留驱动。
|
||||
_rotIntegX = _rotIntegY = _rotIntegTh = 0;
|
||||
_rotPiLastTime = DateTime.MinValue;
|
||||
_rotOmegaPeak = 0;
|
||||
}
|
||||
|
||||
_mvRotCompVx = rotCompVx;
|
||||
@@ -642,7 +803,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
_rotPoseEpisode = false;
|
||||
_rotPosePrevTime = DateTime.MinValue;
|
||||
// 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
|
||||
chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510,
|
||||
chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
|
||||
localCompensateX: xDetectCompensate + xPosCompensate,
|
||||
localCompensateY: yDetectCompensate + yPosCompensate,
|
||||
localCompensateTh: thDetectCompensate + thPosCompensate);
|
||||
@@ -660,11 +821,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
var cx = xDetectCompensate + xPosCompensate;
|
||||
var cy = yDetectCompensate + yPosCompensate;
|
||||
var cth = thDetectCompensate + thPosCompensate;
|
||||
var crabDbg = isMaster && manualEnabled && fleetMode == 1
|
||||
? $"| CRAB in({crabInputVx:F3},{crabInputVy:F3}) raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
|
||||
: "";
|
||||
|
||||
var dbg =
|
||||
$"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} pos:{posAvailable} " +
|
||||
$"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} slam:{slamRead} corr:{useDetourCorrection} fleetPos:{fleetPosValid} " +
|
||||
$"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} 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} " +
|
||||
$"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
|
||||
@@ -690,20 +855,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
|
||||
if (isMaster)
|
||||
{
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
{
|
||||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
|
||||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
|
||||
layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
|
||||
}
|
||||
|
||||
if (fleetReady)
|
||||
{
|
||||
VehicleSyncNotification notification;
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
{
|
||||
notification = new VehicleSyncNotification
|
||||
{
|
||||
PosAvailable = posAvailable,
|
||||
// 广播"整队姿态有效"：主车读到全局定位即为真，从车据此放行自动模式安全门。
|
||||
PosAvailable = slamRead,
|
||||
CenterX = CenterX,
|
||||
CenterY = CenterY,
|
||||
CenterTh = CenterTh,
|
||||
@@ -715,10 +881,19 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
Mode = fleetMode,
|
||||
FleetOmega = fleetOmega,
|
||||
AutoEnabled = MultiVehicleAutoEnabled,
|
||||
ManualEnabled = MultiVehicleManualEnabled,
|
||||
// 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
|
||||
ManualEnabled = manualEnabled,
|
||||
SyncTh = syncTh,
|
||||
SyncDistance = syncDistance,
|
||||
DeltaDetectCenter = deltaDetectCenter
|
||||
DeltaDetectCenter = deltaDetectCenter,
|
||||
// F: 单调递增序列号（从 1 起），从车据此丢弃乱序旧包。
|
||||
Seq = ++_multiVehicleNotifySeq,
|
||||
// D: 透传理想车队中心（仅自动模式且控制器给出时有效）。
|
||||
HasIdeal = !manualEnabled && MultiVehicleAutoEnabled &&
|
||||
Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal,
|
||||
IdealX = MultiVehicleAutoIdealX,
|
||||
IdealY = MultiVehicleAutoIdealY,
|
||||
IdealTh = MultiVehicleAutoIdealTh
|
||||
};
|
||||
}
|
||||
|
||||
@@ -734,16 +909,40 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
}
|
||||
else
|
||||
{
|
||||
FireAndForgetRegister(hc, BuildSelfInfo(false, posAvailable, selfX, selfY, selfTh,
|
||||
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 (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
return MultiVehicleFleet.Values.Any(v => v.PosAvailable);
|
||||
}
|
||||
|
||||
@@ -751,7 +950,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
private float TryGetSlamSpacing(float selfX, float selfY)
|
||||
{
|
||||
List<VehicleSyncInfo> others;
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
others = MultiVehicleFleet
|
||||
.Where(kv => kv.Key != CarNum && kv.Value.PosAvailable)
|
||||
.Select(kv => kv.Value).ToList();
|
||||
@@ -764,7 +963,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
{
|
||||
centerX = centerY = centerTh = 0;
|
||||
List<VehicleSyncInfo> positioned;
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
positioned = MultiVehicleFleet.Values.Where(v => v.PosAvailable).ToList();
|
||||
|
||||
if (positioned.Count == 0) return false;
|
||||
@@ -791,6 +990,56 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
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();
|
||||
var (layoutX, layoutY, layoutTh) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
|
||||
var self = new VehicleSyncInfo
|
||||
{
|
||||
X = (float)carPos.x,
|
||||
Y = (float)carPos.y,
|
||||
Th = (float)carPos.th,
|
||||
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;
|
||||
}
|
||||
|
||||
private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
|
||||
{
|
||||
// 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
|
||||
@@ -829,7 +1078,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
// 这样手动模式无 SLAM 定位也能正确显示编队相对关系；避免之前用车队系 layout 坐标叠加本车位姿造成的整体平移。
|
||||
var fleetPainter = UI.GetPainter("MultiVehicleFleet-vis", false);
|
||||
Dictionary<int, VehicleSyncInfo> fleet;
|
||||
lock (MultiVehicleFleet)
|
||||
lock (FleetLock)
|
||||
fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet);
|
||||
|
||||
var egoLayout = Tuple.Create((double)egoLayoutX, (double)egoLayoutY, (double)egoLayoutTh);
|
||||
@@ -874,10 +1123,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
|
||||
private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
|
||||
{
|
||||
// F: POST + JSON body，payload 不再受 URL 长度限制；fire-and-forget 但记录失败。
|
||||
var notifyJson = JsonConvert.SerializeObject(notification);
|
||||
var url = $"http://{ip}:{port}/multi-vehicle-notify?Notification={Uri.EscapeDataString(notifyJson)}";
|
||||
_ = hc.GetStringAsync(url).ContinueWith(t =>
|
||||
var url = $"http://{ip}:{port}/multi-vehicle-notify";
|
||||
var content = new StringContent(notifyJson, System.Text.Encoding.UTF8, "application/json");
|
||||
_ = hc.PostAsync(url, content).ContinueWith(t =>
|
||||
{
|
||||
content.Dispose();
|
||||
if (t.IsFaulted)
|
||||
DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
|
||||
}, TaskScheduler.Default);
|
||||
@@ -904,18 +1156,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
/// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。
|
||||
/// </summary>
|
||||
private static float RotatePiTerm(float err, ref float integ, float deadband,
|
||||
float pFac, float iFac, float max, float dt)
|
||||
float pFac, float iFac, float max, float dt, bool allowIntegrate = true)
|
||||
{
|
||||
// 死区内：不再累加误差，仅输出已积累的积分项(维持对恒定扰动的稳态补偿)。
|
||||
if (Math.Abs(err) < deadband)
|
||||
return ClampBias(integ * iFac, max);
|
||||
|
||||
// 条件积分(抗 windup)：仅当总输出未在同向饱和时才累加误差。
|
||||
// 起步阶段大误差会让 P 项接近/超过 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 (!saturatedSameSign)
|
||||
if (allowIntegrate && !saturatedSameSign)
|
||||
integ += err * dt;
|
||||
|
||||
var iTerm = integ * iFac;
|
||||
|
||||
@@ -41,4 +41,12 @@ public class VehicleSyncNotification
|
||||
[JsonProperty("SyncTh")] public float SyncTh { get; set; }
|
||||
[JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
|
||||
[JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
|
||||
// F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
|
||||
[JsonProperty("Seq")] public long Seq { get; set; }
|
||||
// D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。
|
||||
// HasIdeal=true 时各从车按各自 layout 推算 per-car 目标位姿做前馈+补偿，弧线路径不再只靠事后纠偏。
|
||||
[JsonProperty("HasIdeal")] public bool HasIdeal { get; set; }
|
||||
[JsonProperty("IdealX")] public float IdealX { get; set; }
|
||||
[JsonProperty("IdealY")] public float IdealY { get; set; }
|
||||
[JsonProperty("IdealTh")] public float IdealTh { get; set; }
|
||||
}
|
||||
|
||||
@@ -6,13 +6,6 @@ public class MotorRoutine : LadderLogic<CartDefinition>
|
||||
{
|
||||
public override void Operation(int iteration)
|
||||
{
|
||||
cart.ActualSpeedLeftFront = cart.SpeedLeftFront;
|
||||
cart.ActualSpeedLeftRear = cart.SpeedLeftRear;
|
||||
cart.ActualSpeedRightFront = cart.SpeedRightFront;
|
||||
cart.ActualSpeedRightRear = cart.SpeedRightRear;
|
||||
cart.ActualThLeftFront = cart.ThLeftFront;
|
||||
cart.ActualThLeftRear = cart.ThLeftRear;
|
||||
cart.ActualThRightFront = cart.ThRightFront;
|
||||
cart.ActualThRightRear = cart.ThRightRear;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
"ManualCarSyncVxFac": 1.0,
|
||||
"ManualCarSyncVthFac": 1.0,
|
||||
"SyncThAccPerSec": 30,
|
||||
"PosAvailable": true,
|
||||
"MultiVehicleSyncUseDetour": true,
|
||||
"SimpleIp": "127.0.0.1",
|
||||
"CarNum": 1
|
||||
},
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
"ManualCarSyncVxFac": 1.0,
|
||||
"ManualCarSyncVthFac": 1.0,
|
||||
"SyncThAccPerSec": 30,
|
||||
"PosAvailable": true,
|
||||
"MultiVehicleSyncUseDetour": true,
|
||||
"SimpleIp": "127.0.0.1",
|
||||
"CarNum": 2
|
||||
},
|
||||
|
||||
@@ -0,0 +1,333 @@
|
||||
# 自动蟹行（FleetCrabWalk）工作上下文
|
||||
|
||||
> 本文档汇总 **当前仓库状态、外部依赖、运行/日志路径、代码地图与待解决问题**，便于后续继续调试「车队联动-自动蟹行」。
|
||||
>
|
||||
> 最后更新：2026-06-29
|
||||
|
||||
---
|
||||
|
||||
## 1. 功能现状
|
||||
|
||||
| 阶段 | 状态 | 说明 |
|
||||
|------|------|------|
|
||||
| 动作能启动、能下发运动 | ✅ 已解决 | 方案 1（预热）修复了启动期 `(0,0,0)` 快照导致 `Track()` 立即结束（`iter=0`）的问题 |
|
||||
| 路径跟踪质量 | 🔧 已改，待实测 | 2026-06-29 继续处理：自动蟹行改为复用手动蟹行同款 `mode=1` 下发链路，只叠加小幅平滑横向纠偏 |
|
||||
| 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）丝滑；因此自动抖动主要来自纠偏链路而非底盘执行能力 |
|
||||
|
||||
**触发方式**：主车 Clumsy → MovementTest 面板 → **「车队联动-自动蟹行」**（`FleetCrabWalkTest`）。
|
||||
|
||||
**前提**：
|
||||
- 主车 `MultiVehicleMasterEndpoint="/"`
|
||||
- 主车有 Detour 定位（反推车队中心起点）
|
||||
- 双车 Medulla + Clumsy + Detour 均已启动，编队成员数 = `MultiVehicleFleetNum`
|
||||
|
||||
---
|
||||
|
||||
## 2. 当前已知问题（待排查）
|
||||
|
||||
### 2.1 向 Y+ 方向漂移、偏离路径
|
||||
|
||||
**现象**：车队整体沿世界坐标 Y 正方向持续偏移，横向误差越来越大，未收敛到 `LineTrack`。
|
||||
|
||||
**可能相关机制**（按优先级，供下一轮对照日志）：
|
||||
|
||||
1. **控制器读到的「当前位姿」与真实 SLAM 中心不同步**
|
||||
- `AbstractGeometricController.Track()` 在 `MultiVehicleSync=true` 时通过 `MultiVehicleGetFleetPos()` 读 `PilotDefinition.GetFleetCenterSnapshot()`。
|
||||
- 已处理：`TickMultiVehicle` 每拍开头的 `PublishFleetCenter(0,0,0)` 已移除，避免动作/控制线程并发读到假中心。
|
||||
|
||||
2. **横向纠偏 `bias` 项在蟹行模式下的参考系**
|
||||
- `MultiWheelGeometricController.PerformGoing` 中 `bias = -bias` 后按 Stanley 形式修正 gcp（`BiasFac` / `BiasThreshold`）。
|
||||
- 蟹行时 `thDiff` 来自路径切线（≈夹角），`dTh` 参考固定 `CrabTargetHeading`；若 `bias` 符号或 fleet 中心更新滞后，会持续向一侧推。
|
||||
- 已绕开：`FleetCrabWalk` 当前不再用几何控制器直接下发 gcp；改为 Detour 计算 `along/lateral/remain`，再写脚本 `MultiVehicleScriptVx/Vy`。
|
||||
|
||||
3. **`MultiVehicleSyncUseDetour=true` 时的 POS 补偿与控制器抢方向盘**
|
||||
- 当前 `deploy/clumsy_agv1/clumsy.json` 中 `MultiVehicleSyncUseDetour: true`。
|
||||
- 各车 SLAM 偏差经 `PosBias*` 叠加到 `SendMotion`，可能与几何控制器横向纠偏形成耦合振荡。
|
||||
|
||||
4. **动作期间关闭了 `MultiVehicleAutoUseIdealCenter`**
|
||||
- 有意为之（避免 ideal 中心回灌快照、抹平真实 bias）。副作用是仅依赖「快照中心 + bias 闭环」，对快照质量更敏感。
|
||||
|
||||
5. **路径/起点几何**
|
||||
- 起点：`TryGetFleetCenterFromSlam()`；路径：`LineTrack(x0,y0 → dst)`，`phi = theta + CrabAngleDeg`。
|
||||
- 若 `theta` 与运行时 `CenterTh` 不一致，或 layout 反推中心与控制器使用的快照中心有系统偏差，会表现为沿某一轴漂移。
|
||||
|
||||
**建议下一轮日志对照**：
|
||||
- `FleetCrabDbg`：`lateral` 是否收敛、`corr/localAngle/cmd` 是否平滑、有无到达纠偏上限
|
||||
- `MultiVehicleDbg`：`frontTh/rearTh/speed` 是否接近手动蟹行、POS/Detect 补偿是否在持续驱动，`CRAB in/raw/limit/rev` 是否显示 `raw=-95°` 这类角度未被反向等价转换
|
||||
- 如需回退旧几何控制器路线，再看 `CrabDbg` 的 `bias/biasItem/gcp/fleetPos`
|
||||
|
||||
**2026-06-29 DLog 结论（自动蟹行仍抖动）**：
|
||||
- `FleetCrabDbg` 中 `along/lateral/remain/corr/localAngle/cmd` 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。
|
||||
- 主/从 `MultiVehicleDbg` 中 `BASE vx` 在正负之间跳，同时 `fTh/rTh` 在 `+90°/-90°` 附近翻转；这是同一横移矢量被错误地按 ±90° 边界转换成两种等价表示，底盘执行层会看到接近 180° 的转向跳变。
|
||||
- POS 补偿在该批日志中为关闭/零补偿（`corr:false`、`POS comp 0`），Detect 补偿有小幅值但不是主因。
|
||||
- 因此本轮判定为 **mode=1 蟹行矢量合成把 ±90° 误当舵角边界**，不是优先调 `FleetCrabCorrectionGain`。Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 默认约为 `-120/+120`，自动蟹行应允许 `-95°` 直接下发。
|
||||
|
||||
**2026-06-29 DLog 结论（±120 修复后仍 Y+ 漂移）**：
|
||||
- Clumsy 侧 `MultiVehicleDbg` 已显示 `CRAB raw=-9x`、`limit=120.0`、`rev:false`，`BASE vx` 不再正负翻转，说明上层 `mode=1` 表达已连续，剧烈抖动问题已消失。
|
||||
- 但 `FleetCrabDbg` 中 `lateral` 仍从 `0` 单调增长到约 `+171mm`，`corr` 到达 `-8°` 上限后无法拉回；主/从 `DETECT dy` 也增长到百毫米量级，`DETECT comp y` 达到 `20mm/s` 上限。
|
||||
- 进一步检查 Playground 发现：`D:\MDCS\Source\Core\Medulla\Playground\default_scene.json` 与运行目录 `bin\Debug\net8.0\default_scene.json` 中两台 `multi-steering` 仍为 `"maxSteeringAngle": 90`，而 `ActuatorModels.cs` 会把模块舵角 clamp 到 `[-MaxSteeringAngleRad,+MaxSteeringAngleRad]`。
|
||||
- 这意味着 Clumsy 发出的 `-98°` 路径纠偏，在 Playground 实际执行时会被夹回 `-90°`，纠偏分量被吞掉；这比继续调 `FleetCrabCorrectionGain` 更像 Y+ 漂移的直接原因。
|
||||
- 已把 Playground 源码场景和运行目录场景改为 `maxSteeringAngle: 120`，并在仿真器中加入 `multi-steering clamp` 节流日志；复测前必须重启 Playground 使场景重载。若复测时仍出现该日志，说明还有其他配置或场景副本在限制舵角。
|
||||
|
||||
### 2.2 两车抖动、不丝滑
|
||||
|
||||
**可能原因**：
|
||||
|
||||
1. 上节 **快照 `(0,0,0)` 窗口** + 50ms 联动周期 + 50ms `DriveTaskInterval` beat frequency
|
||||
2. **notify 经 GET fire-and-forget**（`MultiVehicleAutoSyncReview.md` §F），从车命令阶跃
|
||||
3. **`dTh` 差动 + `bias` 限幅** 在阈值边界来回切换（`DthLinearThreshold` / `BiasThreshold`）
|
||||
4. **`MultiVehicleSyncUseDetour` POS 补偿** 与主车控制器不同相位
|
||||
5. 预热结束后 **`PrimeMasterAutoFromSlam` 不再调用**（正常）；若 `WARMUP` 期间日志显示 `cnt` 反复变化，说明编队 TTL/register 不稳定
|
||||
|
||||
**建议对照实验**：
|
||||
- 手动 FleetRemote 蟹行（同速度、同角度）是否也抖
|
||||
- 临时 `MultiVehicleSyncUseDetour=false` 复测
|
||||
- 看 `FleetCrabDbg` 的 `corr/localAngle/cmd` 与 `MultiVehicleDbg` 的 `frontTh/rearTh` 是否周期跳变
|
||||
|
||||
---
|
||||
|
||||
## 3. Tutorial 仓库（本仓库）
|
||||
|
||||
**路径**：`D:\MDCS\Source\Tutorial`
|
||||
**分支**：`master`（截至文档编写时，自动蟹行相关改动**尚未单独 commit**，均为工作区修改）
|
||||
|
||||
### 3.1 已修改文件（git status）
|
||||
|
||||
| 路径 | 作用 |
|
||||
|------|------|
|
||||
| `MultiWheel/MultiWheelC/MovementTests.cs` | `FleetCrabWalk` / `FleetCrabWalkTest`；预热 WARMUP；诊断 `FleetCrabDbg` |
|
||||
| `MultiWheel/MultiWheelC/PilotDefinition.cs` | `TryGetFleetCenterFromSlam`、`PrimeMasterAutoFromSlam`；联动循环；fleet 快照 |
|
||||
| `MultiWheel/MultiWheelC/PilotConfig.cs` | `FleetCrab*` 配置字段 |
|
||||
| `MultiWheel/MultiWheelC/ChassisController.cs` | `MultiVehicleSendMotion` / `MultiVehicleGetFleetPos`；`SENDMOTION` 诊断 |
|
||||
| `MultiWheel/MultiWheelC/VehicleSyncModels.cs` | 同步模型（联动机制相关） |
|
||||
| `MultiWheel/MultiWheelM/MotorRoutine.cs` | Medulla 侧电机例程 |
|
||||
| `deploy/clumsy_agv1/clumsy.json` | 主车 Clumsy 配置模板 |
|
||||
| `deploy/clumsy_agv2/clumsy.json` | 从车 Clumsy 配置模板 |
|
||||
| `docs/MultiVehicleConfig.md` | §6 自动蟹行参数说明 |
|
||||
| `docs/MultiVehicleAutoSyncReview.md` | 自动联动机制问题清单 |
|
||||
| `docs/RunAndDeploy.md` | 运行部署说明 |
|
||||
|
||||
### 3.2 相关文档（本仓库）
|
||||
|
||||
| 文档 | 内容 |
|
||||
|------|------|
|
||||
| [RunAndDeploy.md](./RunAndDeploy.md) | 编译、双车启动、端口/tag 对照 |
|
||||
| [MultiVehicleConfig.md](./MultiVehicleConfig.md) | 全部联动参数；§6 自动蟹行 |
|
||||
| [MultiVehicleSync.md](./MultiVehicleSync.md) | 联动算法背景 |
|
||||
| [MultiVehicleAutoSyncReview.md](./MultiVehicleAutoSyncReview.md) | 自动联动已知缺陷（A–H） |
|
||||
| [BugFixes.md](./BugFixes.md) | 历史修复清单 |
|
||||
|
||||
---
|
||||
|
||||
## 4. 外部仓库 / 依赖（非 Tutorial git 管理）
|
||||
|
||||
Tutorial 插件通过 **`D:\MDCS\Release\`** 引用预编译二进制；改 MDCSToolbox **源码后须先编译再编 Tutorial**。
|
||||
|
||||
| 组件 | 源码 / 产物路径 | 说明 |
|
||||
|------|-----------------|------|
|
||||
| **MDCSToolBox** | 源码：`D:\MDCS\Source\Products\mdcstoolbox\` | 几何控制器、BasicGo、LineTrack |
|
||||
| | 编译：`dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release` | PostBuild → `D:\MDCS\Release\MDCSToolBox.dll` |
|
||||
| | 蟹行相关改动：`Clumsy/MotionControllers/MultiWheelGeometricController.cs` | `CrabHoldHeading` / `CrabTargetHeading`；`CrabDbg` |
|
||||
| | | `Clumsy/MotionControllers/AbstractGeometricController.cs` | `MultiVehicleSync` 时跳过 `firstTurnN`（TODO） |
|
||||
| | 参考：`Clumsy/AgvInterfaces/BasicInterface.cs` | `BasicGo` + `AddTrack` 模式 |
|
||||
| **Clumsy** | `D:\MDCS\Release\Clumsy\ClumsyLite.exe` | 运行时宿主 |
|
||||
| **Medulla** | `D:\MDCS\Release\Medulla\` | 车体插件宿主 |
|
||||
| **CommonUsage** | `D:\MDCS\Release\CommonUsage.dll` | `CommonMath`、坐标变换 |
|
||||
| **FundamentalLib** | `D:\MDCS\Release\deps\RefFundamentalLib.dll` | `DLog` 落盘 |
|
||||
| **Simple**（可选调度） | `D:\MDCS\Source\Core\Simple\` | `MultiWheelS` → `SimpleComposer.exe` |
|
||||
| **Detour / Playground** | 通常随仿真环境部署 | 非 Tutorial 子目录；见 §5 运行目录 |
|
||||
|
||||
### 4.1 编译顺序（改动了 MDCSToolBox 时）
|
||||
|
||||
```powershell
|
||||
# 1. 工具箱
|
||||
dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release
|
||||
|
||||
# 2. Tutorial 插件（MultiWheelC PostBuild 会把 Release 下 DLL 复制到 build/Clumsy*）
|
||||
cd D:\MDCS\Source\Tutorial
|
||||
dotnet build MultiWheel\MultiWheelC\MultiWheelC.csproj
|
||||
dotnet build MultiWheel\MultiWheelM\MultiWheelM.csproj
|
||||
```
|
||||
|
||||
仅改 Tutorial 侧 C# 时，只需第二步。
|
||||
|
||||
---
|
||||
|
||||
## 5. 测试执行：程序与工作目录
|
||||
|
||||
`build/` 为 **gitignore 运行目录**（首次编译后生成）。下列路径均相对于 `D:\MDCS\Source\Tutorial\`。
|
||||
|
||||
### 5.1 双车仿真典型启动顺序
|
||||
|
||||
1. **Playground**（仿真场景，含 `agv_multi_1` / `agv_multi_2`）
|
||||
2. **Detour ×2**（工作目录一般在 Clumsy build 树下）
|
||||
3. **Medulla ×2**
|
||||
4. **ClumsyLite ×2**
|
||||
|
||||
| 角色 | 工作目录 | 主程序 | 关键配置 |
|
||||
|------|----------|--------|----------|
|
||||
| AGV1 主车 | `build\Medulla\` | Medulla 控制台 | `startup.iocmd`：`SetShareObjectTag Multi1`，`CarNum 1` |
|
||||
| AGV1 Clumsy | `build\Clumsy\` | `ClumsyLite.exe` | `deploy\clumsy_agv1\` 模板；port **8008** |
|
||||
| AGV1 Detour | `build\Clumsy\`（或同树 `Detour\`） | DetourLite | HTTP **4321**，tag `Multi1` |
|
||||
| AGV2 从车 | `build\Medulla_AGV2\` | Medulla 控制台 | tag `Multi2`，`CarNum 2` |
|
||||
| AGV2 Clumsy | `build\Clumsy_AGV2\` | `ClumsyLite.exe` | port **8009**，master `127.0.0.1:8008` |
|
||||
| AGV2 Detour | `build\Clumsy_AGV2\Detour_AGV2\` 等 | DetourLite | HTTP **4421**，tag `Multi2` |
|
||||
|
||||
**快捷脚本**（在已配置好的 build 目录内）：
|
||||
- `deploy\start_clumsy_agv1.bat` → 复制配置后启动 `ClumsyLite.exe`（主车）
|
||||
- `deploy\start_clumsy_agv2.bat` → 从车
|
||||
- 一键 7 进程（若环境已装）：`DetourLite\bin\Debug\net8.0\start_all_sim.bat`（路径见 [RunAndDeploy.md](./RunAndDeploy.md)）
|
||||
|
||||
**编译产物落点**：
|
||||
|
||||
| 项目 | 输出 |
|
||||
|------|------|
|
||||
| `MultiWheelC.csproj` | `build\Clumsy\MultiWheelC.dll` + PostBuild 同步到 `build\Clumsy_AGV2\` |
|
||||
| `MultiWheelM.csproj` | `build\Medulla\plugins\MultiWheelM.dll`（AGV2 Medulla 需另行复制或 PostBuild） |
|
||||
|
||||
### 5.2 触发自动蟹行测试
|
||||
|
||||
1. 按上表启动双车栈
|
||||
2. 主车 Medulla 开启「车队联动」（手动联调时常按 **F**；纯自动蟹行 MovementTest 依赖 `MultiVehicleAutoEnabled`，动作内会自行置位 + 预热）
|
||||
3. 主车 `build\Clumsy\` 的 Clumsy UI → MovementTest → **车队联动-自动蟹行**
|
||||
|
||||
参数来源：`clumsy.json` → `msConf` → `PilotConfig`（未写入 json 的字段用代码默认值）。
|
||||
|
||||
---
|
||||
|
||||
## 6. DLog 日志目录与 Topic
|
||||
|
||||
### 6.1 落盘根目录
|
||||
|
||||
DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）。双车仿真时：
|
||||
|
||||
| 进程 | 日志根目录 |
|
||||
|------|------------|
|
||||
| 主车 Clumsy | `D:\MDCS\Source\Tutorial\build\Medulla\dlog\` |
|
||||
| 从车 Clumsy | `D:\MDCS\Source\Tutorial\build\Medulla_AGV2\dlog\` |
|
||||
|
||||
> 说明：用户实测路径为上述两处；topic 名对应子文件夹/文件。若 Clumsy 工作目录 strictly 为 `build\Clumsy*`，也可能在 `build\Clumsy\dlog\` —— **以实际进程 cwd 下是否生成 `dlog` 为准**。
|
||||
|
||||
目录结构（概念上）：`dlog\<TopicName>\` 下按 topic 滚动；同一 topic 的 `DLog.Log(msg, topic)` 归并到同一目录。
|
||||
|
||||
### 6.2 自动蟹行相关 Topic
|
||||
|
||||
| Topic | 来源 | 内容 |
|
||||
|-------|------|------|
|
||||
| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/corr/localAngle/cmd` |
|
||||
| **`CrabDbg`** | `MultiWheelGeometricController.cs` | 旧几何控制器路线诊断；当前脚本蟹行实现不再依赖 |
|
||||
| **`MultiVehicleDbg`** | `PilotDefinition.cs` | 联动循环：速度、舵角、补偿、ready 状态 |
|
||||
| **`FleetDiagClumsy`** | `PilotDefinition.cs` | 精简 fleet 诊断（带 `car{N}` 前缀） |
|
||||
| **`MultiVehicle`** | `PilotDefinition.cs` | 初始化、心跳、HTTP 错误 |
|
||||
| **`MotionControl`** | 控制器框架 | 通用运动控制（若启用） |
|
||||
|
||||
### 6.3 建议抓取顺序（排查漂移/抖动）
|
||||
|
||||
1. 主车 `FleetCrabDbg`：`WARMUP done` → `ITER#` 中 `lateral/remain/corr/localAngle/cmd`
|
||||
2. 主车 + 从车 `MultiVehicleDbg`：`frontTh/rearTh`、`PosBias*`、是否 `ready=false`
|
||||
3. 若回退旧几何控制器路线，再看主车 `CrabDbg`：`bias` 是否单调增大；`fleetPos` 是否偶发 `(0,0,0)`
|
||||
4. 从车 `FleetDiagClumsy`：是否频繁掉线 / register 超时
|
||||
|
||||
---
|
||||
|
||||
## 7. 代码地图（数据流）
|
||||
|
||||
```text
|
||||
MovementTest「车队联动-自动蟹行」
|
||||
FleetCrabWalk.Get()
|
||||
TryGetFleetCenterFromSlam() → 路径起点 (x0,y0,θ)
|
||||
phi = theta + FleetCrabAngleDeg
|
||||
MultiVehicleScriptEnabled = true
|
||||
MultiVehicleScriptMode = 1 → 复用 FleetRemote 手动蟹行下发链路
|
||||
WARMUP → 等编队成员就位
|
||||
loop:
|
||||
TryGetFleetCenterFromSlam() → 当前车队中心
|
||||
along/lateral/remain → 沿线进度、横向偏差、剩余距离
|
||||
corr = clamp(Stanley(lateral), ±FleetCrabCorrectionAngleDeg)
|
||||
localAngle = (phi + corr) - currentTheta
|
||||
Vx/Vy slew limit → FleetCrabCommandAccel 平滑
|
||||
MultiVehicleScriptVx/Vy = cmd
|
||||
PilotDefinition.TickMultiVehicle (50ms)
|
||||
manual/script mode==1
|
||||
Vx/Vy → speed + frontTh==rearTh
|
||||
notify → 从车 SendMotion + POS/Detect 补偿
|
||||
```
|
||||
|
||||
**对照 baseline**：`PilotDefinition.cs` 手动分支 `fleetMode == 1`（FleetRemote 蟹行）直接合成 `frontTh/rearTh`，不经几何控制器 `bias` 闭环。
|
||||
|
||||
---
|
||||
|
||||
## 8. 配置参数速查
|
||||
|
||||
### 8.1 自动蟹行专用（`PilotConfig` / `msConf`）
|
||||
|
||||
| 字段 | 默认 | 作用 |
|
||||
|------|------|------|
|
||||
| `FleetCrabAngleDeg` | 45 | 路径与车队朝向夹角 (deg) |
|
||||
| `FleetCrabLengthMm` | 2000 | 路径长度 (mm) |
|
||||
| `FleetCrabSpeed` | 0.2 | 速度 (m/s) |
|
||||
| `FleetCrabGcpThetaThreshold` | 95 | 兼容旧几何控制器实现；当前脚本蟹行不直接使用 |
|
||||
| `FleetCrabCorrectionGain` | 1.0 | 横向误差纠偏增益 |
|
||||
| `FleetCrabCorrectionAngleDeg` | 8 | 自动纠偏最大改向角，越小越接近手动蟹行 |
|
||||
| `FleetCrabCommandAccel` | 0.4 | 脚本 `Vx/Vy` 命令斜率限制(m/s²) |
|
||||
|
||||
动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleScript*` 和 `MultiVehicleAuto*`。
|
||||
|
||||
### 8.2 影响跟踪/手感的全局项（节选）
|
||||
|
||||
| 字段 | deploy 主车当前值 | 备注 |
|
||||
|------|-------------------|------|
|
||||
| `MultiVehicleSyncUseDetour` | **true** | 逐车 SLAM POS 补偿；怀疑与漂移/抖动相关 |
|
||||
| `MultiVehicleUseDetect` | false（默认） | true 时互识别安全门 |
|
||||
| `TestCarSyncDistance` | 2400 | 与 Playground 双车间距一致 |
|
||||
| `MultiVehicleSyncInterval` | 50 | 联动周期 ms |
|
||||
| `DriveTaskInterval` | 50 | `clumsy.json` 顶层 |
|
||||
| `BiasFac` / `DthLinearFac` 等 | 继承 `MultiWheelPilotConfig` | 几何控制器 PID 形态参数 |
|
||||
|
||||
详见 [MultiVehicleConfig.md](./MultiVehicleConfig.md) §2–§6。
|
||||
|
||||
---
|
||||
|
||||
## 9. 已实现的关键修复（便于回溯）
|
||||
|
||||
| 问题 | 处理 |
|
||||
|------|------|
|
||||
| 自动蟹行完全不动 (`iter=0`) | 方案1：`PrimeMasterAutoFromSlam` + WARMUP 后再 `Track()` |
|
||||
| 蟹行要求朝向不变但有纠偏 | `CrabHoldHeading` + `CrabTargetHeading`；保留 `dTh` |
|
||||
| 多车 firstTurn 破坏队形 | `MultiVehicleSync` 时跳过 `firstTurnN`（TODO 整队预旋转） |
|
||||
| gcp 被 45° 上限截断 | 动作侧 `GcpThetaThreshold=95` |
|
||||
| ideal 中心抹平横向误差 | 动作期间关 `MultiVehicleAutoUseIdealCenter` |
|
||||
| Tick 中间窗口发布 `(0,0,0)` 假中心 | 已移除 tick 开头 `PublishFleetCenter(0,0,0)` |
|
||||
| 自动蟹行纠偏导致抖动 | 已改为脚本手动蟹行链路 + 小幅平滑横向纠偏 |
|
||||
| 接近纯横移时速度符号/舵角表示翻转 | `fleetMode==1` 改为按 `MultiVehicleCrabSteerLimitDeg`（默认 120°）归一化；`-95°` 直接下发，超过上限才做速度取反的等价转换，并在 `MultiVehicleDbg` 输出 `CRAB in/raw/limit/rev` |
|
||||
|
||||
---
|
||||
|
||||
## 10. 后续工作建议（优先级）
|
||||
|
||||
1. **复测 -90° 自动蟹行**：重点看 `MultiVehicleDbg` 的 `CRAB raw=-9x`、`limit=120.0`、`rev:false`，以及 `BASE vx/fTh/rTh` 是否不再正负翻转。
|
||||
2. **A/B：`MultiVehicleCrabSteerLimitDeg`** 默认 120，应与 Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 匹配；若实际轮角限制不同，先同步该值。
|
||||
3. **A/B：`FleetCrabCorrectionAngleDeg`** 先试 4、8、12：4 最接近手动，12 收敛更快；当前不再因跨 ±90° 直接翻面。
|
||||
4. **A/B：`MultiVehicleSyncUseDetour=false`** 若仍抖，跑同一条蟹行，区分脚本纠偏 vs POS 补偿贡献。
|
||||
5. **路径误差**：若仍持续 Y+ 漂移，看 `lateral` 是否持续单向增长；若增长但 `corr` 已到上限，增大 `FleetCrabCorrectionAngleDeg` 或 `FleetCrabCorrectionGain`。
|
||||
6. **notify 平滑**（中长期）：见 `MultiVehicleAutoSyncReview.md` §F。
|
||||
|
||||
---
|
||||
|
||||
## 11. 快速命令备忘
|
||||
|
||||
```powershell
|
||||
# 编译
|
||||
dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release
|
||||
dotnet build D:\MDCS\Source\Tutorial\MultiWheel\MultiWheelC\MultiWheelC.csproj
|
||||
|
||||
# 查看工作区状态
|
||||
cd D:\MDCS\Source\Tutorial
|
||||
git status --short
|
||||
|
||||
# 查看最新 FleetCrab 日志（主车，PowerShell）
|
||||
Get-ChildItem D:\MDCS\Source\Tutorial\build\Medulla\dlog\FleetCrabDbg -ErrorAction SilentlyContinue |
|
||||
Sort-Object LastWriteTime -Descending | Select-Object -First 3
|
||||
Get-ChildItem D:\MDCS\Source\Tutorial\build\Medulla\dlog\CrabDbg -ErrorAction SilentlyContinue |
|
||||
Sort-Object LastWriteTime -Descending | Select-Object -First 3
|
||||
```
|
||||
@@ -194,3 +194,40 @@ lock (MultiVehicleFleet)
|
||||
| 日期 | 说明 |
|
||||
|------|------|
|
||||
| 2026-06-28 | 初版：基于 Tutorial MultiWheelC 自动联动实现与联调日志分析整理 |
|
||||
| 2026-06-28 | A~H 全部修复落地（PilotDefinition.cs / ChassisController.cs / VehicleSyncModels.cs / PilotConfig.cs），见下「修复实现」 |
|
||||
| 2026-06-29 | 修正 `MultiVehicleSyncUseDetour` 语义：仅控制"车队内姿态纠正"，整车队姿态计算始终用 Detour；并修复 `FleetRotateInPlace` 欠转（航向闭环判停），见下「语义修正」 |
|
||||
|
||||
## 修复实现（A~H）
|
||||
|
||||
- **A**：新增 `public readonly object FleetLock`，所有对 `MultiVehicleFleet` 的读写统一 `lock(FleetLock)`（含 ChassisController 回调），不再锁会被整体替换的字段引用。
|
||||
- **B**：`MultiVehicleSendMotion` 回调写入 `MultiVehicleAutoCmdTime`；主车自动分支按 `MultiVehicleAutoCmdTimeoutMs`(0=auto) 判定命令新鲜度，超时清零速度/idealPos 并关闭 `AutoEnabled`，避免末速度滑行。
|
||||
- **C**：新增本地 `_multiVehicleFleetSeen` 存活时刻表，register/notify 收到即刷新；主车 Tick `PruneStaleFleetMembers()` 按 `MultiVehicleMemberTtlMs`(0=auto) 剔除掉线成员，`fleetReady`(数量==总数) 因此蕴含全员新鲜。
|
||||
- **D**：回调不再丢弃 `idealPos/idealAngle`，写入 `MultiVehicleAutoIdeal*` 并经 notify(`HasIdeal/IdealX/Y/Th`) 广播；自动模式下以理想车队中心作为各车 layout 前馈目标（`MultiVehicleAutoUseIdealCenter`，默认开）。
|
||||
- **E**：新增 `MultiVehicleControlRadius`(0=syncDistance/2)，`ControlPointRadius` 与 `SendMotion(localControlRadius)` 统一取该值，删除硬编码 510。
|
||||
- **F**：notify 改为 POST + JSON body（取代 GET query 串）；新增单调递增 `Seq`，从车丢弃乱序旧包（含主车重启回退识别）。
|
||||
- **G**：新增 `FleetCenterSnapshot` 不可变快照 + `volatile` 引用，`PublishFleetCenter` 整体赋值，控制器线程 `GetFleetCenterSnapshot()` 只读完整快照，消除 torn read。
|
||||
- **H**：自动模式新增 `MultiVehicleAutoRequireFleetCenter`(默认开) 门控——无有效车队中心（定位丢失）时强制停车，补上纯 SLAM 模式安全网；手动模式不受限。
|
||||
|
||||
## 语义修正（2026-06-29）
|
||||
|
||||
### 1. `MultiVehicleSyncUseDetour` 重新定义：仅控制"车队内姿态纠正"
|
||||
|
||||
**问题**：原实现把"是否读 Detour 全局位姿"与"是否做车队内姿态纠正"绑在同一开关上。`false` 时 `posAvailable` 直接为假、根本不读 SLAM，导致 `TryInferFleetCenter` 失败、整车队姿态无法计算——这与该开关应有的含义不符。
|
||||
|
||||
**修正**（`PilotDefinition.TickMultiVehicle`）：将单一 `posAvailable` 拆为三个语义清晰的量：
|
||||
|
||||
| 变量 | 含义 | 取值 |
|
||||
|------|------|------|
|
||||
| `useDetourCorrection` | 是否做**车队内姿态纠正**（`PosBias*` 逐车 SLAM 补偿） | `= MultiVehicleSyncUseDetour` |
|
||||
| `slamRead` | 本车本轮是否读取 Detour 全局位姿 | `useDetourCorrection \|\| (isMaster && autoMode)` |
|
||||
| `fleetPosValid` | 整车队全局姿态是否已知（主车=自身读到，从车=主车广播） | 见代码 |
|
||||
|
||||
- **整车队姿态计算**（反推/广播车队中心、SLAM 间距、自动安全门 H、自动入口门）一律改用 `slamRead`/`fleetPosValid`，**始终依赖 Detour**，不再受开关限制；自动入口门与 H 门去掉 `&& MultiVehicleSyncUseDetour` 条件。
|
||||
- **车队内姿态纠正**（`PosBias*` 补偿块）是唯一受 `useDetourCorrection` 控制的开关点。
|
||||
- 即 `false` 时：自动模式主车仍 `getCartLocation()` 计算整车队姿态（无定位则阻塞停车），但不再逐车 SLAM 纠偏。
|
||||
|
||||
### 2. `FleetRotateInPlace` 欠转修复（航向闭环判停）
|
||||
|
||||
**问题**：原地旋转 MovementDefinition 的判停沿用 `MultiVehicleSyncUseDetour`，关闭时退化为"按估算时长开环停止"，实际转速 < 指令时（PI 纠偏吃速率 + 起步斜坡）会**没转到目标就停**（实测 180° 欠转）。
|
||||
|
||||
**修正**（`MovementTests.FleetRotateInPlace`）：新增 `FleetRotateUseDetourHeading`(默认 `true`)，**与 `MultiVehicleSyncUseDetour` 解耦**——转到指定角度属于"整车队姿态计算"，故默认读主车 SLAM 航向闭环累计实际转角，到 `|TargetDeltaDeg|` 才停。新增 `FleetRotateDbg` 落盘日志（实际航向/累计转角/实际vs指令角速率/判停原因）便于复现核对。
|
||||
|
||||
@@ -117,20 +117,61 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
|
||||
| `MultiVehicleUseDetect` | 启用互识别纠正 **+ 安全门**（任一车检测不到邻车→整队停车） | `true` |
|
||||
| `MultiVehicleDetectBiasXFac/YFac/ThFac` | 互识别补偿系数 | `0.5/0.5/0.5` |
|
||||
| `MultiVehicleDetectBiasXThreshold/YThreshold/ThThreshold` | 互识别补偿上限(mm/mm/deg) | `50/50/5` |
|
||||
| `MultiVehiclePosBiasXFac/YFac/ThFac` | SLAM 编队保持补偿系数 | `0.5/0.5/0.5` |
|
||||
| `MultiVehiclePosBiasXThreshold/YThreshold/ThThreshold` | SLAM 补偿上限(mm/mm/deg) | `50/50/5` |
|
||||
| `MultiVehiclePosBiasXFac/YFac/ThFac` | **车队内姿态纠正**（SLAM 逐车编队保持）补偿系数 | `0.5/0.5/0.5` |
|
||||
| `MultiVehiclePosBiasXThreshold/YThreshold/ThThreshold` | 车队内姿态纠正上限(mm/mm/deg) | `50/50/5` |
|
||||
| `SingleCarSyncPrecisionXy` / `SingleCarSyncPrecisionTh` | 对齐精度 / 补偿死区(mm/deg) | `10 / 0.2` |
|
||||
| `PosAvailable` | 是否启用 Detour 定位 | `true` |
|
||||
| `MultiVehicleSyncUseDetour` | **仅**控制"定位是否参与**车队内姿态纠正**"（即上面的 `PosBias*` 补偿）；**不影响**"整个车队姿态的计算" | `false` |
|
||||
|
||||
**`MultiVehicleSyncUseDetour` 语义（重要，勿混淆）**
|
||||
|
||||
该开关只切换 **"车队内姿态纠正"**（用 SLAM 逐车把每台车纠回其编队 slot，即 `PosBias*` 补偿），**不**切换 **"整个车队姿态的计算"**：
|
||||
|
||||
| 用途 | 是否受该开关控制 | 说明 |
|
||||
|------|------------------|------|
|
||||
| 车队内姿态纠正（`PosBias*` 逐车 SLAM 补偿） | **是**（false=关闭，仅靠编队几何/互识别保持队形） | 唯一开关点 |
|
||||
| 反推/广播车队中心、SLAM 间距、自动安全门、原地旋转判停航向 | **否，始终用 Detour** | 自动模式整队姿态恒依赖全局定位 |
|
||||
|
||||
- 即使 `MultiVehicleSyncUseDetour=false`，**自动模式主车仍调用 `getCartLocation()`** 反推车队中心；若无有效全局定位则该调用阻塞 → 联动线程阻塞不下发速度（安全停车），定位恢复后自动继续。
|
||||
- `false` 适用于：SLAM 两车相对精度不佳、希望只靠互识别/编队几何保持队形，但整车队的绝对位姿仍由 Detour 驱动（如自动循路径）。
|
||||
|
||||
**要点**
|
||||
|
||||
- `MultiVehicleUseDetect=true` 时若 2 腿检测没锁定，`fleetVx` 会被安全门置零（表现为摇杆"无效"——这是预期安全行为，不是 bug）。先确保检测稳定。
|
||||
- SLAM 补偿（`PosBias*`）要求两车**共享同一 SLAM 世界系**；否则编队中心反推会错。Playground 两车同图，满足。
|
||||
- 不需要绝对编队保持时，可把 `MultiVehiclePosBias*Fac` 设 0，仅靠互识别维持间距。
|
||||
- 车队内姿态纠正（`PosBias*`）要求两车**共享同一 SLAM 世界系**；否则编队中心反推会错。Playground 两车同图，满足。
|
||||
- 不需要绝对编队保持时，可把 `MultiVehiclePosBias*Fac` 设 0 或 `MultiVehicleSyncUseDetour=false`，仅靠互识别维持间距（整车队姿态仍由 Detour 计算）。
|
||||
|
||||
---
|
||||
|
||||
## 6. 常见坑位（排查清单）
|
||||
## 6. 自动蟹行动作（FleetCrabWalk / MovementTest「车队联动-自动蟹行」）
|
||||
|
||||
在 Clumsy 侧 MovementTest 面板触发，以**当前车队中心**为起点，构造一条与车队朝向夹角 `FleetCrabAngleDeg`、长度 `FleetCrabLengthMm` 的**直线路径**，执行侧复用 FleetRemote 已验证丝滑的脚本手动等价输入（`MultiVehicleScriptEnabled + mode=1`）让整队**斜向平移（蟹行）**：
|
||||
|
||||
- 动作每拍读取主车 Detour 反推车队中心，计算直线进度 `along`、横向偏差 `lateral` 和剩余距离 `remain`。
|
||||
- 横向偏差只转成一个**小幅、带斜率限制的蟹行方向修正**，再写入 `MultiVehicleScriptVx/Vy`；`TickMultiVehicle` 仍按手动蟹行逻辑合成 `frontTh==rearTh` 并广播从车。
|
||||
- 这样保留手动蟹行的平滑执行链路，同时让自动动作具备温和的路径纠偏；避免旧几何控制器 `bias/dTh` 直接叠到 gcp 时出现舵角阶跃。
|
||||
- **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点）。
|
||||
|
||||
| 字段（`clumsy.json` → `msConf`） | 含义 | 默认值 |
|
||||
|------|------|--------|
|
||||
| `FleetCrabAngleDeg` | 蟹行路径**与当前车队朝向的夹角**(deg，逆时针为正)。决定斜行方向：0=正前方，90=正左方平移，-90=正右方。稳态下即各舵轮的蟹行角 | `45` |
|
||||
| `FleetCrabLengthMm` | 蟹行路径**长度**(mm)，沿夹角方向行驶该距离后停车结束 | `2000` |
|
||||
| `FleetCrabSpeed` | 蟹行**行驶速度**(m/s) | `0.2` |
|
||||
| `FleetCrabGcpThetaThreshold` | 兼容旧几何控制器实现的 gcp 舵角上限；当前脚本手动等价实现不直接使用 | `95` |
|
||||
| `FleetCrabCorrectionGain` | 横向误差纠偏增益。增大后收敛更快，但更容易出现方向摆动 | `1` |
|
||||
| `FleetCrabCorrectionAngleDeg` | 自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大纠偏越强 | `8` |
|
||||
| `FleetCrabCommandAccel` | `Vx/Vy` 命令斜率限制(m/s²)，抑制纠偏方向突变 | `0.4` |
|
||||
| `MultiVehicleCrabSteerLimitDeg` | mode=1 蟹行舵角上限，应与 Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 匹配；`-95°` 在默认 120° 内会直接下发 | `120` |
|
||||
|
||||
**行为要点 / 注意**
|
||||
|
||||
- 当前实现不走 `MultiVehicleAuto*`/`MultiVehicleSendMotion`，也不再临时改 `MultiVehicleAutoUseIdealCenter`；结束/急停会清零脚本字段。
|
||||
- `FleetCrabDbg` 会记录 `along/lateral/remain/corr/localAngle/cmd(Vx,Vy)`；`MultiVehicleDbg` 可继续对照最终 `frontTh≈rearTh`、是否有 POS/Detect 补偿，以及 `CRAB in/raw/limit/rev` 是否在舵角上限内保持连续表达。
|
||||
- `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。
|
||||
- Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。
|
||||
|
||||
---
|
||||
|
||||
## 7. 常见坑位（排查清单）
|
||||
|
||||
- **组不成队 / `ready=false`**：`MultiVehicleFleetNum` 与实际车数不符；从车 `MultiVehicleMasterEndpoint` 没指向主车端口；`soTag` 不匹配。
|
||||
- **摇杆无反应**：`MultiVehicleUseDetect=true` 但检测没锁（安全门）；或误把系数设成 <1 / 滑条乘零；或在从车而非主车上操作。
|
||||
@@ -140,7 +181,7 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
|
||||
|
||||
---
|
||||
|
||||
## 7. 参考：Playground 双车验证值速查
|
||||
## 8. 参考：Playground 双车验证值速查
|
||||
|
||||
```jsonc
|
||||
// clumsy.json -> msConf （主车=agv_multi_1；从车把标注项改为车2值）
|
||||
@@ -155,7 +196,17 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
|
||||
"SyncThAccPerSec": 30,
|
||||
"MultiVehicleMasterEndpoint": "/", // 从车: "127.0.0.1:8008"
|
||||
"PlaygroundRobotName": "agv_multi_1", // 从车: "agv_multi_2"
|
||||
"TwoLegLidarName": "rear_left_lidar_1,rear_right_lidar_1" // 从车: *_2
|
||||
"TwoLegLidarName": "rear_left_lidar_1,rear_right_lidar_1", // 从车: *_2
|
||||
|
||||
// 自动蟹行动作（见 §6，仅主车触发）
|
||||
"FleetCrabAngleDeg": 45,
|
||||
"FleetCrabLengthMm": 2000,
|
||||
"FleetCrabSpeed": 0.2,
|
||||
"FleetCrabGcpThetaThreshold": 95,
|
||||
"FleetCrabCorrectionGain": 1.0,
|
||||
"FleetCrabCorrectionAngleDeg": 8,
|
||||
"FleetCrabCommandAccel": 0.4,
|
||||
"MultiVehicleCrabSteerLimitDeg": 120
|
||||
```
|
||||
|
||||
```text
|
||||
|
||||
@@ -77,7 +77,7 @@ Tutorial/
|
||||
## 6. 双车自动联动
|
||||
|
||||
1. 完成上一节双车启动
|
||||
2. 至少一台车 `PosAvailable=true`（Detour 定位有效）
|
||||
2. 可选：`MultiVehicleSyncUseDetour=true` 开启**车队内姿态纠正**（SLAM 逐车 `PosBias*` 补偿）。注意它**不影响**整车队姿态计算——自动模式无论该开关如何，主车都用 Detour 反推车队中心，无有效定位时会阻塞停车（详见 `MultiVehicleConfig.md` §5）
|
||||
3. 通过 Simple 调度触发自动联动（推荐）：
|
||||
- 编译 `MultiWheelS`（见第 2 节），确认 `build\Simple\plugins\MultiWheelS.dll` 存在
|
||||
- 在 `build\Simple\` 运行 `SimpleComposer.exe`（自动加载 `./plugins`）
|
||||
|
||||
Reference in New Issue
Block a user