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

Merge branch 'master' of http://git.fairylandtech.com/ruifeng.zhou/Tutorial

Browse Source
This commit is contained in:
shuai.li
2026-07-01 22:58:36 +08:00
parent b1c50a0916 63ef3c8bb8
commit ca723fbdeb
8 changed files with 900 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitattributes
+2
View File
@@ -0,0 +1,2 @@
*.cs text eol=crlf
.gitattributes text eol=lf
MultiWheel/MultiWheelC/MovementTests.cs
+139 -71
View File
@@ -212,7 +212,7 @@ public class FleetRotateInPlace : MovementDefinition
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} " +
$"REQUEST 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");
@@ -224,6 +224,35 @@ public class FleetRotateInPlace : MovementDefinition
self.MultiVehicleScriptMode = 2;
self.MultiVehicleScriptVth = 0;
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleRotateWheelsReady = false;
self.MultiVehicleRotateFleetReady = false;
DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
while (!self.MultiVehicleRotateFleetReady)
{
self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptMode = 2;
self.MultiVehicleScriptVth = 0;
self.MultiVehicleScriptEnabled = true;
Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
yield return true;
}
if (hasPos)
{
prevTh = (float)DetourInterface.getCartLocation().th;
startTh = prevTh;
}
accumulated = 0f;
start = DateTime.Now;
lastTime = start;
lastLog = DateTime.MinValue;
cmdMag = 0f;
DLog.Log(
$"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
$"fleetAligned={self.MultiVehicleRotateFleetReady}",
"FleetRotateDbg");
var stopReason = "stop()";
while (true)
@@ -335,39 +364,32 @@ public class FleetRotateInPlaceTest : MovementTest
}
// ===== 车队联动-自动蟹行动作 =====
// 以当前车队中心为起点，构造与车队朝向夹角 x、长度 y 的直线路径；执行侧复用
// TickMultiVehicle 的脚本手动等价输入(mode=1)，也就是 FleetRemote 手动蟹行同一条下发链路。
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
//
// 手动蟹行已验证丝滑，自动动作只额外做两件事：
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度和横向偏差；
// 2) 用小幅、带斜率限制的方向修正写 MultiVehicleScriptVx/Vy，避免几何控制器 bias/dTh 阶跃造成抖动。
//
// 与 FleetRemote 手动蟹行(mode==1)对照：TickMultiVehicle 仍负责合成 frontTh==rearTh 的蟹行舵角并广播给从车。
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
//
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
public class FleetCrabWalk : MovementDefinition
{
/// <summary>与当前车队朝向的夹角(deg，逆时针为正)。</summary>
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
public float CrabAngleDeg = 45f;
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
public float BodyToPathAngleDeg = 45f;
/// <summary>路径长度(mm)。</summary>
public float CrabLengthMm = 2000f;
/// <summary>行驶速度(m/s)。</summary>
public float CrabSpeed = 0.2f;
/// <summary>兼容旧配置；当前脚本手动等价实现不再直接使用几何控制器 gcp 上限。</summary>
/// <summary>前后 GCP 舵角修正上限(deg)。</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()
@@ -391,11 +413,20 @@ public class FleetCrabWalk : MovementDefinition
Cleanup();
}
private static float Slew(float current, float target, float maxStep)
private static float Clamp(float value, float min, float max)
{
var diff = target - current;
if (Math.Abs(diff) <= maxStep) return target;
return current + Math.Sign(diff) * maxStep;
if (value < min) return min;
if (value > max) return max;
return value;
}
private static float ClampAbs(float value, float limit)
{
var absLimit = Math.Abs(limit);
if (absLimit <= 0) return value;
if (value > absLimit) return absLimit;
if (value < -absLimit) return -absLimit;
return value;
}
public override IEnumerable<bool> Get()
@@ -408,7 +439,8 @@ public class FleetCrabWalk : MovementDefinition
$"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}",
$"autoFields=true pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
$"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
"FleetCrabDbg");
if (conf.MultiVehicleMasterEndpoint != "/")
@@ -429,25 +461,34 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
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} " +
$"START center=({x0:0},{y0:0},{theta:0.0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
$"phi={phi:0.0} targetBody={targetBodyTh: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.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;
self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoIdealX = x0;
self.MultiVehicleAutoIdealY = y0;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
self.PrimeMasterAutoFromSlam();
DLog.Log("WARMUP 已启用脚本蟹行(mode=1)，等待编队成员就位…", "FleetCrabDbg");
DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg");
var warmEnd = DateTime.Now.AddSeconds(2.0);
var warmIter = 0;
@@ -455,8 +496,17 @@ public class FleetCrabWalk : MovementDefinition
while (!_stopping && DateTime.Now < warmEnd)
{
warmIter++;
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleScriptMode = 1;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoIdealX = x0;
self.MultiVehicleAutoIdealY = y0;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
self.PrimeMasterAutoFromSlam();
var snap = self.GetFleetCenterSnapshot();
int cnt;
@@ -464,7 +514,7 @@ public class FleetCrabWalk : MovementDefinition
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}",
$"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
"FleetCrabDbg");
if (cnt >= conf.MultiVehicleFleetNum)
{
@@ -477,27 +527,30 @@ public class FleetCrabWalk : MovementDefinition
yield return true;
}
if (!warmReady)
DLog.Log("WARMUP 超时：编队仍未就位，继续进入脚本蟹行（若不动请查看 FleetDiagClumsy ready/cnt）",
DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
"FleetCrabDbg");
Hedingben.ToastText($"车队蟹行 夹角{CrabAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
Hedingben.ToastText($"车队蟹行 路径{CrabAngleDeg:0.0}° 车身夹角{BodyToPathAngleDeg: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 slowDistance = Math.Max(finishDistance + 1f, conf.SlowDistance);
var baseSpeed = Math.Abs(CrabSpeed);
var finishSpeed = Math.Min(baseSpeed, Math.Abs(conf.FinishSpeed));
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
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);
if (!self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth))
{
stopReason = "fleet center invalid";
DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg");
break;
}
var delta = new Vector2(cx - x0, cy - y0);
var along = Vector2.Dot(delta, pathDir);
var lateral = Vector2.Dot(delta, pathLeft);
@@ -505,29 +558,37 @@ public class FleetCrabWalk : MovementDefinition
if (remain <= finishDistance)
break;
var speed = Math.Abs(CrabSpeed);
if (remain < conf.SlowDistance && conf.SlowDistance > 1)
var speed = baseSpeed;
if (remain < slowDistance)
{
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 ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow);
speed = ratio * (baseSpeed - finishSpeed) + 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);
var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
var dthItem = ClampAbs(conf.DthLinearFac * headingErr, conf.DthLinearThreshold);
var biasItem = (float)(-Math.Atan(conf.BiasFac * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
biasItem = ClampAbs(biasItem, conf.BiasThreshold);
var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
var idealAlong = Clamp(along, 0f, CrabLengthMm);
var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleScriptMode = 1;
self.MultiVehicleScriptVx = cmdVx;
self.MultiVehicleScriptVy = cmdVy;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;
self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = speed;
self.MultiVehicleAutoFrontTh = frontTh;
self.MultiVehicleAutoRearTh = rearTh;
self.MultiVehicleAutoIdealX = ideal.X;
self.MultiVehicleAutoIdealY = ideal.Y;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
{
@@ -537,8 +598,11 @@ public class FleetCrabWalk : MovementDefinition
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}",
$"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
$"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
$"auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
$"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
$"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
"FleetCrabDbg");
}
yield return true;
@@ -547,14 +611,20 @@ public class FleetCrabWalk : MovementDefinition
if (_stopping)
stopReason = "stop";
self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 0;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoCmdTime = DateTime.Now;
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
while (!_stopping && DateTime.Now < settleEnd)
{
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleScriptMode = 1;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoCmdTime = DateTime.Now;
yield return true;
}
@@ -575,12 +645,10 @@ public class FleetCrabWalkTest : MovementTest
_proc = new FleetCrabWalk
{
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
BodyToPathAngleDeg = 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
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
};
_task = new DriveTask(_proc.Get());
_task.Wait();
MultiWheel/MultiWheelC/PilotConfig.cs
+9 -14
View File
@@ -23,6 +23,8 @@ public class PilotConfig : MultiWheelPilotConfig
// 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
// 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
[FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
// 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
[FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
[FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
@@ -96,6 +98,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "Playground WebAPI 基地址")]
public string PlaygroundWebApiUrl = "http://localhost:18090";
[FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
[FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
public string PlaygroundRobotName = "agv_multi_1";
@@ -149,9 +154,9 @@ public class PilotConfig : MultiWheelPilotConfig
public bool FleetRotateUseDetourHeading = true;
// ===== 车队联动-自动蟹行（FleetCrabWalk）=====
// 以当前车队中心为起点，构造与车队朝向夹角 FleetCrabAngleDeg、长度 FleetCrabLengthMm 的直线路径，
// 复用脚本手动等价输入(mode=1)斜向平移；动作侧只把横向误差转换成小幅、带斜率限制的蟹行方向修正。
[FieldMember(desc = "车队蟹行：与车队朝向夹角(deg，逆时针为正)")]
// 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
// 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
[FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
public float FleetCrabAngleDeg = 45f;
[FieldMember(desc = "车队蟹行：路径长度(mm)")]
@@ -160,19 +165,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
public float FleetCrabSpeed = 0.2f;
// 兼容旧版几何控制器实现；当前自动蟹行走脚本手动等价输入，不再直接使用该上限。
[FieldMember(desc = "车队蟹行：旧几何控制器gcp角度上限(deg)")]
[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";
MultiWheel/MultiWheelC/PilotDefinition.cs
+393 -71
View File
@@ -13,10 +13,10 @@ using ClumsyCore.Pilot;
using ClumsyCore.Utilities;
using CommonUsage;
using CommonUsage.Chassis;
using CommonUsage.Mathematics;
using FundamentalLib;
using FundamentalLib.Utilities;
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
using Newtonsoft.Json;
using Vector = ClumsyCore.Utilities.Vector;
namespace MultiWheelC;
@@ -99,6 +99,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
private long _multiVehicleAppliedSeq = -1;
[AsLowerIO(desc = "车号")] public int CarNum = 1;
[FieldMember(desc = "多车联动：原地旋转本车舵轮已对齐")]
public bool MultiVehicleRotateWheelsReady = true;
[FieldMember(desc = "多车联动：原地旋转整队舵轮已对齐")]
public bool MultiVehicleRotateFleetReady = true;
#region 夹臂变量
[AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
@@ -138,6 +142,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
private bool _multiVehicleMotionFeasible = true;
private string _multiVehicleMotionInfeasibleReason = "";
private DateTime _multiVehicleStopLastLog = DateTime.MinValue;
private bool _multiVehicleRotateModeActive;
private float _multiVehicleRotateDirectionHint = 1f;
private string _multiVehicleRotateAlignDetail = "";
private DateTime _multiVehicleRotateAlignLastLog = DateTime.MinValue;
// 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
private DateTime _mvDbgLastLog = DateTime.MinValue;
@@ -167,6 +175,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
// 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
private DateTime _mvRemoteInputLastLog = DateTime.MinValue;
private DateTime _mvRemoteDecisionLastLog = DateTime.MinValue;
private DateTime _mvNotifyApplyLastLog = DateTime.MinValue;
private DateTime _mvRotateWheelOutputLastLog = DateTime.MinValue;
private void LogMultiVehicleRemoteInput(bool isMaster, bool scriptOn, bool manualEnabled, bool autoEnabled,
int manualMode, float manualVx, float manualVy, float manualVth)
@@ -198,12 +208,156 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
DLog.Log($"car{CarNum} {msg}", "MultiVehicleRemoteDbg");
}
private void LogRotateWheelOutputs(MultiWheelChassis chassis, string phase, float omega, bool force = false)
{
var now = DateTime.Now;
if (!force && (now - _mvRotateWheelOutputLastLog).TotalMilliseconds < 300) return;
_mvRotateWheelOutputLastLog = now;
#pragma warning disable CS0612, CS0618
var wheels = chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618
var parts = new List<string>();
for (var i = 0; i < wheels.Count; i++)
{
var wheel = wheels[i];
if (wheel is DiffSteerWheel diff)
{
parts.Add(
$"w{i}:tgt={diff.GetSendAngle():0.0} read={diff.ReadAngle():0.0} " +
$"L={diff.GetLeftSendSpeed():0.000} R={diff.GetRightSendSpeed():0.000}");
}
else
{
parts.Add(
$"w{i}:tgt={wheel.GetSendAngle():0.0} read={wheel.ReadAngle():0.0} " +
$"v={wheel.GetSendSpeed():0.000}");
}
}
DLog.Log(
$"car{CarNum} phase={phase} omega={omega:0.000} " + string.Join(" | ", parts),
"MultiVehicleRotateWheelOutput");
}
private void SetMultiVehicleMotionFeasible(bool feasible, string reason = "")
{
_multiVehicleMotionFeasible = feasible;
_multiVehicleMotionInfeasibleReason = feasible ? "" : (reason ?? "");
}
private static float ClampFloat(float value, float min, float max)
{
if (value < min) return min;
if (value > max) return max;
return value;
}
private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega)
{
var rotateActive = active && fleetMode == 2;
if (!rotateActive)
{
_multiVehicleRotateModeActive = false;
MultiVehicleRotateWheelsReady = true;
MultiVehicleRotateFleetReady = true;
_multiVehicleRotateDirectionHint = 1f;
_multiVehicleRotateAlignDetail = "";
return;
}
if (!_multiVehicleRotateModeActive)
{
MultiVehicleRotateWheelsReady = false;
MultiVehicleRotateFleetReady = false;
_multiVehicleRotateAlignDetail = "rotate mode just entered";
}
_multiVehicleRotateModeActive = true;
if (Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega)
_multiVehicleRotateDirectionHint = Math.Sign(requestedOmega);
}
private bool PrepareMultiVehicleRotateWheels(MultiWheelChassis chassis, float requestedOmega,
float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f,
bool rampStop = true)
{
var hint = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega
? Math.Sign(requestedOmega)
: Math.Sign(_multiVehicleRotateDirectionHint);
if (hint == 0) hint = 1;
_multiVehicleRotateDirectionHint = hint;
// Reuse SendRotateMotion's decomposition and angle-limit checks, but keep wheel speed at zero.
if (rampStop)
chassis.RampStop();
var alignOmega = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega
? requestedOmega
: 0.01f * hint;
var motionOk = chassis.SendRotateMotion(alignOmega, TimeSpan.Zero,
localCompensateX: localCompensateX, localCompensateY: localCompensateY,
localCompensateTh: localCompensateTh);
var wheelAligned = TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
out var alignDetail);
var aligned = motionOk && wheelAligned;
if (!motionOk)
alignDetail = string.IsNullOrWhiteSpace(chassis.LastMotionDecomposeFailureReason)
? alignDetail
: chassis.LastMotionDecomposeFailureReason;
MultiVehicleRotateWheelsReady = aligned;
_multiVehicleRotateAlignDetail = alignDetail;
var now = DateTime.Now;
if ((now - _multiVehicleRotateAlignLastLog).TotalMilliseconds >= 200)
{
_multiVehicleRotateAlignLastLog = now;
DLog.Log(
$"car{CarNum} ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} " +
$"hint={hint} comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
$"rampStop={rampStop} ok={motionOk} aligned={aligned} detail={alignDetail} " +
$"chassisReason={chassis.LastMotionDecomposeFailureReason}",
"MultiVehicleRemoteDbg");
FleetDiag(
$"ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} hint={hint} " +
$"comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
$"ok={motionOk} aligned={aligned} detail={alignDetail}");
}
Hedingben.ToastText(
aligned ? "车队原地旋转舵轮已对齐" : $"车队原地旋转舵轮预对齐中 {alignDetail}",
$"MultiVehicle{CarNum}-rotate-align");
return motionOk;
}
private static bool TryCheckRotateWheelAlignment(MultiWheelChassis chassis, float toleranceDeg, out string detail)
{
try
{
#pragma warning disable CS0612, CS0618
var wheels = chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618
var maxDth = 0f;
var parts = new List<string>();
for (var i = 0; i < wheels.Count; i++)
{
var sw = wheels[i];
var dth = Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle()));
maxDth = Math.Max(maxDth, dth);
parts.Add($"w{i}:{dth:0.0}");
}
detail = $"maxDth={maxDth:0.0} tol={toleranceDeg:0.0} {string.Join(",", parts)}";
return maxDth <= toleranceDeg;
}
catch (Exception e)
{
detail = $"read alignment failed: {e.Message}";
return chassis.LastRotateAligned;
}
}
private void LogMultiVehicleStop(string reason, bool force = false)
{
var now = DateTime.Now;
@@ -304,62 +458,32 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
var hc = new HttpClient { Timeout = TimeSpan.FromSeconds(2) };
PicoHttpServer.AddGetHandler("/multi-vehicle-register", new { CarNum = 0, Info = "" }, query =>
PicoHttpServer.AddPostByteHandler("/multi-vehicle-register-bin", body =>
{
try
{
var infoJson = Uri.UnescapeDataString(query.Info ?? "");
var info = JsonConvert.DeserializeObject<VehicleSyncInfo>(infoJson)
?? throw new Exception("VehicleSyncInfo is null");
lock (FleetLock)
{
MultiVehicleFleet[query.CarNum] = info;
_multiVehicleFleetSeen[query.CarNum] = DateTime.Now; // C: 刷新存活时刻
}
return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
var (carNum, info) = VehicleSyncBinaryCodec.DecodeRegister(body);
ApplyVehicleSyncRegister(carNum, info);
return "ok";
}
catch (Exception e)
{
DLog.Log($"/multi-vehicle-register error: {e.FormatEx()}", "MultiVehicle");
return JsonConvert.SerializeObject(new { code = 500, message = e.Message });
DLog.Log($"/multi-vehicle-register-bin error: {e.FormatEx()}", "MultiVehicle");
throw;
}
});
// F: notify 改用 POST + JSON body（取代 GET query 串），避免整队 Fleet 字典撑爆 URL 长度上限；
// 用 Seq 丢弃乱序到达的旧包，避免从车短暂套用过期指令。
PicoHttpServer.AddPostTextHandler("/multi-vehicle-notify", body =>
PicoHttpServer.AddPostByteHandler("/multi-vehicle-notify-bin", body =>
{
try
{
var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(body ?? "")
?? throw new Exception("notification is null");
// 乱序丢弃：仅应用序列号大于已应用值的包。Seq==0 视为旧版无序列号始终接受；
// 若 Seq 明显回退(差值>100)，判定为主车重启的新会话，重新接受并对齐序列号。
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 (FleetLock)
{
MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
// C: notify 内含整队成员，逐一刷新其本地存活时刻。
var nowSeen = DateTime.Now;
foreach (var key in MultiVehicleFleet.Keys)
_multiVehicleFleetSeen[key] = nowSeen;
}
lock (_multiVehicleNotificationLock)
{
MultiVehicleNotification = notification;
_multiVehicleLastNotifyTime = DateTime.Now;
}
return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
var notification = VehicleSyncBinaryCodec.DecodeNotification(body);
return ApplyVehicleSyncNotification(notification);
}
catch (Exception e)
{
DLog.Log($"/multi-vehicle-notify error: {e.FormatEx()}", "MultiVehicle");
return JsonConvert.SerializeObject(new { code = 500, message = e.Message });
DLog.Log($"/multi-vehicle-notify-bin error: {e.FormatEx()}", "MultiVehicle");
throw;
}
});
@@ -368,6 +492,53 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
new Thread(() => MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start();
}
private void ApplyVehicleSyncRegister(int carNum, VehicleSyncInfo info)
{
lock (FleetLock)
{
MultiVehicleFleet[carNum] = info;
_multiVehicleFleetSeen[carNum] = DateTime.Now;
}
}
private string ApplyVehicleSyncNotification(VehicleSyncNotification notification)
{
// Keep the same stale-packet semantics as the previous transport.
if (notification.Seq != 0 && notification.Seq <= _multiVehicleAppliedSeq &&
notification.Seq > _multiVehicleAppliedSeq - 100)
return "stale";
_multiVehicleAppliedSeq = notification.Seq;
MultiVehicleAligned = notification.Aligned;
lock (FleetLock)
{
MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
var nowSeen = DateTime.Now;
foreach (var key in MultiVehicleFleet.Keys)
_multiVehicleFleetSeen[key] = nowSeen;
}
lock (_multiVehicleNotificationLock)
{
MultiVehicleNotification = notification;
_multiVehicleLastNotifyTime = DateTime.Now;
}
var applyNow = DateTime.Now;
if ((applyNow - _mvNotifyApplyLastLog).TotalMilliseconds >= 200)
{
_mvNotifyApplyLastLog = applyNow;
DLog.Log(
$"car{CarNum} NOTIFY_APPLY seq={notification.Seq} mode={notification.Mode} " +
$"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " +
$"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " +
$"reason={notification.FleetStopReason}",
"MultiVehicleRemoteDbg");
}
return "ok";
}
private void MultiVehicleLoop(HttpClient hc)
{
var carLength = CarLength;
@@ -460,7 +631,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
$"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
$"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
$"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
$"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} useDetour={Conf.MultiVehicleSyncUseDetour}");
$"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} " +
$"useDetour={Conf.MultiVehicleSyncUseDetour} manualDetour={Conf.MultiVehicleManualUseDetourCorrection}");
}
if (!manualEnabled && !autoEnabled)
@@ -472,6 +644,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
LogMultiVehicleStop("fleet control disabled; ramp stop previous fleet command", true);
}
_multiVehicleWasActive = false;
UpdateMultiVehicleRotateModeState(0, false, 0);
LogMultiVehicleRemoteDecision(
$"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
$"rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
@@ -511,9 +684,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
// false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
// - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
// SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
// 纠偏开启时本车也读。读取若因无有效定位阻塞，则联动线程随之阻塞、不下发速度（安全停车）。
// 手动外部遥控默认不读 Detour，避免 getCartLocation 阻塞拖慢 2 腿检测；确需手动 POS 纠偏时再开
// MultiVehicleManualUseDetourCorrection。
var autoMode = autoEnabled && !manualEnabled;
var useDetourCorrection = Conf.MultiVehicleSyncUseDetour;
var manualDetourCorrection = manualEnabled && Conf.MultiVehicleManualUseDetourCorrection;
var useDetourCorrection = Conf.MultiVehicleSyncUseDetour && (!manualEnabled || manualDetourCorrection);
var slamRead = useDetourCorrection || (isMaster && autoMode);
float selfX = 0, selfY = 0, selfTh = 0;
if (slamRead)
@@ -535,6 +710,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
var notificationStopActive = false;
var notificationStopReason = "";
var notificationStopSourceCar = 0;
var notificationRequestedFleetOmega = 0f;
var notificationMotionReleased = true;
float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
float crabSteerLimit = 0;
var crabReverseEquivalent = false;
@@ -548,7 +725,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
if (fleetMode == 2)
{
// 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
fleetOmega = manualVth * Conf.ManualCarSyncVthFac;
fleetOmega = manualVth;
fleetVx = 0;
fleetFrontTh = 0;
fleetRearTh = 0;
@@ -559,7 +736,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
{
// 手动蟹行：Vx 只表示线速度，Vy 表示方向摇杆比例(-1..1)，由 VyFac 映射为舵角。
var speed = manualVx * Conf.ManualCarSyncVxFac;
var crabRatio = Math.Max(-60f, Math.Min(60f, manualVy));
var crabRatio = Math.Max(-90f, Math.Min(90f, manualVy));
var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
crabInputVx = speed;
crabInputVy = crabRatio;
@@ -631,6 +808,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
fleetRearTh = notification.FleetRearTh;
fleetMode = notification.Mode;
fleetOmega = notification.FleetOmega;
notificationRequestedFleetOmega = notification.RequestedFleetOmega;
notificationMotionReleased = notification.FleetMotionReleased;
notificationStopActive = notification.FleetStopActive;
notificationStopReason = notification.FleetStopReason ?? "";
notificationStopSourceCar = notification.FleetStopSourceCar;
@@ -641,6 +820,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
MultiVehicleAutoIdealTh = notification.IdealTh;
}
var requestedFleetOmega = !isMaster && fleetMode == 2
? ((Math.Abs(notificationRequestedFleetOmega) > 1e-6f || !notificationMotionReleased)
? notificationRequestedFleetOmega
: fleetOmega)
: fleetOmega;
UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega);
var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
if (isMaster)
@@ -716,6 +902,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
bool othersDetectOk;
string otherDetectLostCars;
List<KeyValuePair<int, VehicleSyncInfo>> motionInfeasibleMembers;
List<KeyValuePair<int, VehicleSyncInfo>> rotateUnalignedMembers;
lock (FleetLock)
{
othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
@@ -725,7 +912,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
motionInfeasibleMembers = MultiVehicleFleet
.Where(kv => kv.Key != CarNum && !kv.Value.MotionFeasible)
.ToList();
rotateUnalignedMembers = MultiVehicleFleet
.Where(kv => !kv.Value.RotateWheelsAligned)
.ToList();
}
if (!MultiVehicleRotateWheelsReady && rotateUnalignedMembers.All(kv => kv.Key != CarNum))
{
rotateUnalignedMembers.Add(new KeyValuePair<int, VehicleSyncInfo>(CarNum, new VehicleSyncInfo
{
RotateWheelsAligned = false,
RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
}));
}
var rotateUnalignedCars = string.Join(",", rotateUnalignedMembers.Select(kv => kv.Key.ToString()));
var rotateFleetWheelsAligned = fleetMode != 2 || (fleetReady && rotateUnalignedMembers.Count == 0);
MultiVehicleRotateFleetReady = rotateFleetWheelsAligned;
var fleetStopActive = false;
var fleetStopReason = "";
@@ -794,6 +995,26 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
}
var rotateHoldForAlignment = false;
if (fleetMode == 2 && fleetReady && !fleetStopActive && !notificationMotionReleased)
{
rotateHoldForAlignment = true;
fleetOmega = 0;
Hedingben.ToastText("车队原地旋转等待主车释放速度", $"MultiVehicle{CarNum}-rotate-align");
LogMultiVehicleRemoteDecision(
$"ROTATE_HOLD_RELEASE master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000}", true);
}
else if (fleetMode == 2 && fleetReady && !fleetStopActive && !rotateFleetWheelsAligned)
{
rotateHoldForAlignment = true;
fleetOmega = 0;
Hedingben.ToastText($"车队原地旋转舵轮预对齐中 car=[{rotateUnalignedCars}]", $"MultiVehicle{CarNum}-rotate-align");
LogMultiVehicleRemoteDecision(
$"ROTATE_HOLD_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} unalignedCars=[{rotateUnalignedCars}]", true);
}
if (fleetStopActive)
LogMultiVehicleStop(fleetStopReason);
@@ -814,6 +1035,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
{
chassis.RampStop();
SetMultiVehicleMotionFeasible(true);
if (fleetMode == 2)
{
MultiVehicleRotateWheelsReady = false;
MultiVehicleRotateFleetReady = false;
_multiVehicleRotateAlignDetail = $"fleet stop: {fleetStopReason}";
}
LogMultiVehicleRemoteDecision(
$"RAMP_STOP master={isMaster} manual={manualEnabled} auto={autoEnabled} ready={fleetReady} " +
$"reason={fleetStopReason}", true);
@@ -926,8 +1153,59 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
_mvRotCompVy = rotCompVy;
_mvRotCompOmega = rotCompOmega;
var motionOk = chassis.SendRotateMotion(fleetOmega,
localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);
bool motionOk;
if (rotateHoldForAlignment)
{
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
rotCompVx, rotCompVy, rotCompOmega);
}
else if (rotating)
{
// Preparation calls SendRotateMotion with zero delta; after release it would starve the speed ramp.
if (!MultiVehicleRotateWheelsReady)
{
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
rotateHoldForAlignment = true;
fleetOmega = 0;
chassis.RampStop();
if (!motionOk || !MultiVehicleRotateWheelsReady)
{
LogMultiVehicleRemoteDecision(
$"ROTATE_HOLD_LOCAL_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
}
else
{
LogMultiVehicleRemoteDecision(
$"ROTATE_HOLD_LOCAL_READY master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
}
}
else
{
motionOk = chassis.SendRotateMotion(fleetOmega,
localCompensateX: rotCompVx, localCompensateY: rotCompVy,
localCompensateTh: rotCompOmega);
var activeAligned = TryCheckRotateWheelAlignment(chassis,
Conf.InPlaceRotateWheelAlignDeg, out _multiVehicleRotateAlignDetail);
if (!motionOk)
MultiVehicleRotateWheelsReady = false;
if (!activeAligned)
LogMultiVehicleRemoteDecision(
$"ROTATE_ACTIVE_ALIGN_WAIT master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}");
}
}
else
{
motionOk = chassis.SendRotateMotion(fleetOmega,
localCompensateX: rotCompVx, localCompensateY: rotCompVy,
localCompensateTh: rotCompOmega);
MultiVehicleRotateWheelsReady = motionOk &&
TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
out _multiVehicleRotateAlignDetail);
}
SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
if (!motionOk)
{
@@ -936,13 +1214,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
canMove = false;
LogMultiVehicleStop(fleetStopReason, true);
}
LogRotateWheelOutputs(chassis, rotateHoldForAlignment ? "hold-align" : (rotating ? "rotate" : "idle"),
fleetOmega);
LogMultiVehicleRemoteDecision(
$"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
$"mode={fleetMode} omega={fleetOmega:0.000} comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) " +
$"holdAlign={rotateHoldForAlignment} wheelReady={MultiVehicleRotateWheelsReady} fleetReady={MultiVehicleRotateFleetReady} " +
$"mode={fleetMode} omegaReq={requestedFleetOmega:0.000} omega={fleetOmega:0.000} " +
$"comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) align={_multiVehicleRotateAlignDetail} " +
$"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
// 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
if (isMaster)
if (isMaster && Conf.MultiVehicleRotatePoseWebApiDiagEnabled)
LogRotatePoseSample();
}
else
@@ -950,6 +1232,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
_mvRotCompVx = 0;
_mvRotCompVy = 0;
_mvRotCompOmega = 0;
MultiVehicleRotateWheelsReady = true;
MultiVehicleRotateFleetReady = true;
_multiVehicleRotateAlignDetail = "";
// 退出原地旋转：清空 PI 积分与计时、位姿诊断片段，下次进入重新起算。
_rotIntegX = _rotIntegY = _rotIntegTh = 0;
_rotPiLastTime = DateTime.MinValue;
@@ -1003,7 +1288,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
$"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 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})";
$"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " +
$"| ROT_ALIGN hold:{rotateHoldForAlignment} wheelReady:{MultiVehicleRotateWheelsReady} fleetReady:{MultiVehicleRotateFleetReady} " +
$"unaligned:[{rotateUnalignedCars}] reqOmega:{requestedFleetOmega:F1} detail:{_multiVehicleRotateAlignDetail}";
DLog.Log(dbg, "MultiVehicleDbg");
FleetDiag(dbg);
@@ -1046,6 +1333,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
FleetRearTh = fleetStopActive ? 0 : fleetRearTh,
Mode = fleetMode,
FleetOmega = fleetStopActive ? 0 : fleetOmega,
RequestedFleetOmega = fleetMode == 2 && !fleetStopActive ? requestedFleetOmega : 0,
FleetMotionReleased = !fleetStopActive && !(fleetMode == 2 && rotateHoldForAlignment),
FleetStopActive = fleetStopActive,
FleetStopReason = fleetStopReason,
FleetStopSourceCar = fleetStopSourceCar,
@@ -1066,6 +1355,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
};
}
LogMultiVehicleRemoteDecision(
$"NOTIFY_SEND seq={notification.Seq} mode={notification.Mode} " +
$"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " +
$"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " +
$"reason={notification.FleetStopReason} fleetCnt={notification.Fleet.Count}/{Conf.MultiVehicleFleetNum}");
foreach (var kv in notification.Fleet)
{
var ip = kv.Value.Ip;
@@ -1240,7 +1535,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
Aligned = aligned,
DetectOk = detectOk,
MotionFeasible = _multiVehicleMotionFeasible,
MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason
MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason,
RotateWheelsAligned = MultiVehicleRotateWheelsReady,
RotateWheelAlignDetail = _multiVehicleRotateAlignDetail
};
}
@@ -1283,29 +1580,52 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
private void FireAndForgetRegister(HttpClient hc, VehicleSyncInfo info)
{
ParseMasterEndpoint(out var masterIp, out var masterPort);
var infoJson = JsonConvert.SerializeObject(info);
var url =
$"http://{masterIp}:{masterPort}/multi-vehicle-register?CarNum={CarNum}&Info={Uri.EscapeDataString(infoJson)}";
_ = hc.GetStringAsync(url).ContinueWith(t =>
try
{
if (t.IsFaulted)
DLog.Log($"register failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
}, TaskScheduler.Default);
ParseMasterEndpoint(out var masterIp, out var masterPort);
var payload = VehicleSyncBinaryCodec.EncodeRegister(CarNum, info);
var url = $"http://{masterIp}:{masterPort}/multi-vehicle-register-bin";
_ = PostBinaryAsync(hc, url, payload, "register");
}
catch (Exception e)
{
DLog.Log($"register binary encode failed: {e.GetBaseException().Message}", "MultiVehicle");
}
}
private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
private void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
{
// F: POST + JSON body，payload 不再受 URL 长度限制；fire-and-forget 但记录失败。
var notifyJson = JsonConvert.SerializeObject(notification);
var url = $"http://{ip}:{port}/multi-vehicle-notify";
var content = new StringContent(notifyJson, System.Text.Encoding.UTF8, "application/json");
_ = hc.PostAsync(url, content).ContinueWith(t =>
try
{
content.Dispose();
if (t.IsFaulted)
DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
}, TaskScheduler.Default);
var payload = VehicleSyncBinaryCodec.EncodeNotification(notification);
var url = $"http://{ip}:{port}/multi-vehicle-notify-bin";
_ = PostBinaryAsync(hc, url, payload, $"notify {ip}:{port}");
}
catch (Exception e)
{
DLog.Log($"notify {ip}:{port} binary encode failed: {e.GetBaseException().Message}", "MultiVehicle");
}
}
private static async Task PostBinaryAsync(HttpClient hc, string url, byte[] payload, string description)
{
try
{
using var content = new ByteArrayContent(payload);
content.Headers.ContentType =
new System.Net.Http.Headers.MediaTypeHeaderValue("application/octet-stream");
using var response = await hc.PostAsync(url, content).ConfigureAwait(false);
if (response.IsSuccessStatusCode)
return;
DLog.Log(
$"{description} binary failed: HTTP {(int)response.StatusCode} {response.ReasonPhrase}",
"MultiVehicle");
}
catch (Exception e)
{
DLog.Log($"{description} binary failed: {e.GetBaseException().Message}", "MultiVehicle");
}
}
private void ResolveMultiVehicleSelfEndpoint(out string ip, out int port)
@@ -1384,6 +1704,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
/// </summary>
private void LogRotatePoseSample()
{
if (!Conf.MultiVehicleRotatePoseWebApiDiagEnabled) return;
var now = DateTime.Now;
if ((now - _rotPoseLastLog).TotalMilliseconds < 200) return;
_rotPoseLastLog = now;
MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs
+249
View File
@@ -0,0 +1,249 @@
using System;
using System.Collections.Generic;
using System.IO;
using System.Text;
namespace MultiWheelC;
internal static class VehicleSyncBinaryCodec
{
private const byte Version = 1;
private const byte RegisterType = 1;
private const byte NotificationType = 2;
private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info)
{
using var stream = new MemoryStream();
using var writer = new BinaryWriter(stream, Encoding.UTF8);
WriteHeader(writer, RegisterType);
writer.Write(carNum);
WriteInfo(writer, info);
writer.Flush();
return stream.ToArray();
}
public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload)
{
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
using var reader = new BinaryReader(stream, Encoding.UTF8);
ReadHeader(reader, RegisterType);
var carNum = reader.ReadInt32();
var info = ReadInfo(reader);
EnsureFullyRead(stream);
return (carNum, info);
}
public static byte[] EncodeNotification(VehicleSyncNotification notification)
{
using var stream = new MemoryStream();
using var writer = new BinaryWriter(stream, Encoding.UTF8);
WriteHeader(writer, NotificationType);
writer.Write(notification.Seq);
writer.Write(BuildNotificationFlags(notification));
writer.Write(notification.Mode);
writer.Write(notification.FleetStopSourceCar);
writer.Write(notification.CenterX);
writer.Write(notification.CenterY);
writer.Write(notification.CenterTh);
writer.Write(notification.FleetVx);
writer.Write(notification.FleetFrontTh);
writer.Write(notification.FleetRearTh);
writer.Write(notification.FleetOmega);
writer.Write(notification.RequestedFleetOmega);
writer.Write(notification.SyncTh);
writer.Write(notification.SyncDistance);
writer.Write(notification.DeltaDetectCenter);
writer.Write(notification.IdealX);
writer.Write(notification.IdealY);
writer.Write(notification.IdealTh);
WriteString(writer, notification.FleetStopReason);
var fleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
if (fleet.Count > ushort.MaxValue)
throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit.");
writer.Write((ushort)fleet.Count);
foreach (var kv in fleet)
{
writer.Write(kv.Key);
WriteInfo(writer, kv.Value);
}
writer.Flush();
return stream.ToArray();
}
public static VehicleSyncNotification DecodeNotification(byte[] payload)
{
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
using var reader = new BinaryReader(stream, Encoding.UTF8);
ReadHeader(reader, NotificationType);
var notification = new VehicleSyncNotification
{
Seq = reader.ReadInt64()
};
ApplyNotificationFlags(notification, reader.ReadUInt16());
notification.Mode = reader.ReadInt32();
notification.FleetStopSourceCar = reader.ReadInt32();
notification.CenterX = reader.ReadSingle();
notification.CenterY = reader.ReadSingle();
notification.CenterTh = reader.ReadSingle();
notification.FleetVx = reader.ReadSingle();
notification.FleetFrontTh = reader.ReadSingle();
notification.FleetRearTh = reader.ReadSingle();
notification.FleetOmega = reader.ReadSingle();
notification.RequestedFleetOmega = reader.ReadSingle();
notification.SyncTh = reader.ReadSingle();
notification.SyncDistance = reader.ReadSingle();
notification.DeltaDetectCenter = reader.ReadSingle();
notification.IdealX = reader.ReadSingle();
notification.IdealY = reader.ReadSingle();
notification.IdealTh = reader.ReadSingle();
notification.FleetStopReason = ReadString(reader);
var fleetCount = reader.ReadUInt16();
notification.Fleet = new Dictionary<int, VehicleSyncInfo>(fleetCount);
for (var i = 0; i < fleetCount; ++i)
{
var carNum = reader.ReadInt32();
notification.Fleet[carNum] = ReadInfo(reader);
}
EnsureFullyRead(stream);
return notification;
}
private static void WriteHeader(BinaryWriter writer, byte type)
{
writer.Write(Magic);
writer.Write(Version);
writer.Write(type);
writer.Write((ushort)0);
}
private static void ReadHeader(BinaryReader reader, byte expectedType)
{
for (var i = 0; i < Magic.Length; ++i)
{
if (reader.ReadByte() != Magic[i])
throw new InvalidDataException("Invalid multi-vehicle sync binary magic.");
}
var version = reader.ReadByte();
if (version != Version)
throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
var type = reader.ReadByte();
if (type != expectedType)
throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}.");
var reserved = reader.ReadUInt16();
if (reserved != 0)
throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
}
private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
{
writer.Write(BuildInfoFlags(info));
WriteString(writer, info.Ip);
writer.Write(info.Port);
writer.Write(info.X);
writer.Write(info.Y);
writer.Write(info.Th);
writer.Write(info.LayoutX);
writer.Write(info.LayoutY);
writer.Write(info.LayoutTh);
WriteString(writer, info.MotionInfeasibleReason);
WriteString(writer, info.RotateWheelAlignDetail);
}
private static VehicleSyncInfo ReadInfo(BinaryReader reader)
{
var info = new VehicleSyncInfo();
ApplyInfoFlags(info, reader.ReadUInt16());
info.Ip = ReadString(reader);
info.Port = reader.ReadInt32();
info.X = reader.ReadSingle();
info.Y = reader.ReadSingle();
info.Th = reader.ReadSingle();
info.LayoutX = reader.ReadSingle();
info.LayoutY = reader.ReadSingle();
info.LayoutTh = reader.ReadSingle();
info.MotionInfeasibleReason = ReadString(reader);
info.RotateWheelAlignDetail = ReadString(reader);
return info;
}
private static ushort BuildInfoFlags(VehicleSyncInfo info)
{
ushort flags = 0;
if (info.Master) flags |= 1 << 0;
if (info.PosAvailable) flags |= 1 << 1;
if (info.Aligned) flags |= 1 << 2;
if (info.DetectOk) flags |= 1 << 3;
if (info.MotionFeasible) flags |= 1 << 4;
if (info.RotateWheelsAligned) flags |= 1 << 5;
return flags;
}
private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags)
{
info.Master = (flags & (1 << 0)) != 0;
info.PosAvailable = (flags & (1 << 1)) != 0;
info.Aligned = (flags & (1 << 2)) != 0;
info.DetectOk = (flags & (1 << 3)) != 0;
info.MotionFeasible = (flags & (1 << 4)) != 0;
info.RotateWheelsAligned = (flags & (1 << 5)) != 0;
}
private static ushort BuildNotificationFlags(VehicleSyncNotification notification)
{
ushort flags = 0;
if (notification.PosAvailable) flags |= 1 << 0;
if (notification.Aligned) flags |= 1 << 1;
if (notification.FleetMotionReleased) flags |= 1 << 2;
if (notification.FleetStopActive) flags |= 1 << 3;
if (notification.AutoEnabled) flags |= 1 << 4;
if (notification.ManualEnabled) flags |= 1 << 5;
if (notification.HasIdeal) flags |= 1 << 6;
return flags;
}
private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags)
{
notification.PosAvailable = (flags & (1 << 0)) != 0;
notification.Aligned = (flags & (1 << 1)) != 0;
notification.FleetMotionReleased = (flags & (1 << 2)) != 0;
notification.FleetStopActive = (flags & (1 << 3)) != 0;
notification.AutoEnabled = (flags & (1 << 4)) != 0;
notification.ManualEnabled = (flags & (1 << 5)) != 0;
notification.HasIdeal = (flags & (1 << 6)) != 0;
}
private static void WriteString(BinaryWriter writer, string value)
{
var bytes = Encoding.UTF8.GetBytes(value ?? "");
if (bytes.Length > ushort.MaxValue)
throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit.");
writer.Write((ushort)bytes.Length);
writer.Write(bytes);
}
private static string ReadString(BinaryReader reader)
{
var length = reader.ReadUInt16();
var bytes = reader.ReadBytes(length);
if (bytes.Length != length)
throw new EndOfStreamException("Truncated multi-vehicle sync string payload.");
return Encoding.UTF8.GetString(bytes);
}
private static void EnsureFullyRead(MemoryStream stream)
{
if (stream.Position != stream.Length)
throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet.");
}
}
MultiWheel/MultiWheelC/VehicleSyncModels.cs
+4
View File
@@ -21,6 +21,8 @@ public class VehicleSyncInfo
[JsonProperty("DetectOk")] public bool DetectOk { get; set; }
[JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
[JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
[JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
[JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
}
public class VehicleSyncNotification
@@ -38,6 +40,8 @@ public class VehicleSyncNotification
[JsonProperty("Mode")] public int Mode { get; set; }
// 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
[JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
[JsonProperty("RequestedFleetOmega")] public float RequestedFleetOmega { get; set; }
[JsonProperty("FleetMotionReleased")] public bool FleetMotionReleased { get; set; } = true;
[JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
[JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
[JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
docs/FleetCrabWalkWorkContext.md
+72 -63
View File
@@ -2,7 +2,7 @@
> 本文档汇总 **当前仓库状态、外部依赖、运行/日志路径、代码地图与待解决问题**，便于后续继续调试「车队联动-自动蟹行」。
>
> 最后更新：2026-06-29
> 最后更新：2026-07-01
---
@@ -11,8 +11,8 @@
| 阶段 | 状态 | 说明 |
|------|------|------|
| 动作能启动、能下发运动 | ✅ 已解决 | 方案 1（预热）修复了启动期 `(0,0,0)` 快照导致 `Track()` 立即结束（`iter=0`）的问题 |
| 路径跟踪质量 | 🔧 已改，待实测 | 2026-06-29 继续处理：自动蟹行改为复用手动蟹行同款 `mode=1` 下发链路，只叠加小幅平滑横向纠偏 |
| 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）丝滑；因此自动抖动主要来自纠偏链路而非底盘执行能力 |
| 路径跟踪质量 | 🔧 已改，待实测 | 2026-07-01 改为自动字段链路；MovementTest 中 `FleetCrabAngleDeg=-x` 表示车身保持当前角度，以 x 度夹角追踪路径 |
| 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）仍保留；自动蟹行不再复用脚本手动链路 |
**触发方式**：主车 Clumsy → MovementTest 面板 → **「车队联动-自动蟹行」**（`FleetCrabWalkTest`）。
@@ -31,64 +31,66 @@
**可能相关机制**（按优先级，供下一轮对照日志）：
1. **控制器读到的「当前位姿」与真实 SLAM 中心不同步**
- `AbstractGeometricController.Track()` 在 `MultiVehicleSync=true` 时通过 `MultiVehicleGetFleetPos()` 读 `PilotDefinition.GetFleetCenterSnapshot()`。
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`。
2. **横向纠偏 `bias` 项在蟹行模式下的参考系**
- 当前实现不改 MDCSToolbox，只参考几何控制器思路在 `MultiWheelC` 内计算。
- `lateral` 通过 `BiasFac/BiasThreshold` 转为前后 GCP 同向修正；`headingErr` 通过 `DthLinearFac/DthLinearThreshold` 转为前后 GCP 反向修正。
- 输出直接写 `MultiVehicleAutoVx/FrontTh/RearTh/IdealX/Y/Th`，由 `TickMultiVehicle` 自动分支统一下发。
- MovementTest 会令 `BodyToPathAngleDeg = FleetCrabAngleDeg`，因此 `targetBodyTh = pathTh - BodyToPathAngleDeg = 启动时车队朝向`。
3. **`MultiVehicleSyncUseDetour=true` 时的 POS 补偿与控制器抢方向盘**
- 当前 `deploy/clumsy_agv1/clumsy.json` 中 `MultiVehicleSyncUseDetour: true`。
3. **`MultiVehicleSyncUseDetour=true` 时的 POS 补偿与控制器抢方向盘**
- 当前 `deploy/clumsy_agv1/clumsy.json` 中 `MultiVehicleSyncUseDetour: true`。
- 各车 SLAM 偏差经 `PosBias*` 叠加到 `SendMotion`，可能与几何控制器横向纠偏形成耦合振荡。
4. **动作期间关闭了 `MultiVehicleAutoUseIdealCenter`**
- 有意为之（避免 ideal 中心回灌快照、抹平真实 bias）。副作用是仅依赖「快照中心 + bias 闭环」，对快照质量更敏感。
4. **理想车队中心前馈**
- 当前动作会发布 `MultiVehicleAutoIdealX/Y/Th`。
- `MultiVehicleAutoUseIdealCenter=true` 时，从车使用该理想中心做 layout 前馈；关闭后只用当前广播中心和补偿项。
5. **路径/起点几何**
- 起点：`TryGetFleetCenterFromSlam()`；路径：`LineTrack(x0,y0 → dst)`，`phi = theta + CrabAngleDeg`。
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`
- `FleetCrabDbg`：`lateral` 是否收敛、`headingErr` 是否收敛、`bias/dth` 是否到达阈值、`auto(vx,fTh,rTh)` 是否稳定
- `MultiVehicleDbg`：自动分支是否为 `auto:true/manual:false/script:false`，`BASE/SEND` 是否接近 `FleetCrabDbg` 输出，POS/Detect 补偿是否持续驱动
- 重点看 `FleetCrabGcpThetaThreshold`、`BiasThreshold`、`DthLinearThreshold` 三个限幅是否过早截断纠偏
**2026-06-29 DLog 结论（自动蟹行仍抖动）**：
- `FleetCrabDbg` 中 `along/lateral/remain/corr/localAngle/cmd` 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。
**2026-06-29 DLog 结论（旧脚本链路下自动蟹行仍抖动）**：
- 旧 `FleetCrabDbg` 中 `along/lateral/remain/旧方向修正/旧命令` 基本平滑，横向误差多在几十 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°` 直接下发。
- 因此本轮判定为 **mode=1 蟹行矢量合成把 ±90° 误当舵角边界**，不是优先调横向纠偏增益。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+ 漂移的直接原因。
- 这意味着 Clumsy 发出的 `-98°` 路径纠偏，在 Playground 实际执行时会被夹回 `-90°`，纠偏分量被吞掉；这比继续调横向纠偏增益更像 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 补偿** 与主车控制器不同相位
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` 是否周期跳变
- 手动 FleetRemote 蟹行（同速度、同角度）是否也抖
- 临时 `MultiVehicleSyncUseDetour=false` 复测
- 看 `FleetCrabDbg` 的 `auto(vx,fTh,rTh)` 与 `MultiVehicleDbg` 的 `BASE/SEND` 是否周期跳变
---
## 3. Tutorial 仓库（本仓库）
**路径**：`D:\MDCS\Source\Tutorial`
**路径**：`D:\MDCS\Source\Tutorial`
**分支**：`master`（截至文档编写时，自动蟹行相关改动**尚未单独 commit**，均为工作区修改）
### 3.1 已修改文件（git status）
@@ -187,8 +189,8 @@ dotnet build MultiWheel\MultiWheelM\MultiWheelM.csproj
### 5.2 触发自动蟹行测试
1. 按上表启动双车栈
2. 主车 Medulla 开启「车队联动」（手动联调时常按 **F**；纯自动蟹行 MovementTest 依赖 `MultiVehicleAutoEnabled`，动作内会自行置位 + 预热）
1. 按上表启动双车栈
2. 主车 Medulla 开启「车队联动」（手动联调时常按 **F**；纯自动蟹行 MovementTest 依赖 `MultiVehicleAutoEnabled`，动作内会自行置位 + 预热）
3. 主车 `build\Clumsy\` 的 Clumsy UI → MovementTest → **车队联动-自动蟹行**
参数来源：`clumsy.json` → `msConf` → `PilotConfig`（未写入 json 的字段用代码默认值）。
@@ -214,8 +216,8 @@ DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）
| Topic | 来源 | 内容 |
|-------|------|------|
| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/corr/localAngle/cmd` |
| **`CrabDbg`** | `MultiWheelGeometricController.cs` | 旧几何控制器路线诊断；当前脚本蟹行实现不再依赖 |
| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/headingErr/baseTh/bias/dth/auto/ideal` |
| **`CrabDbg`** | `MultiWheelGeometricController.cs` | MDCSToolbox 几何控制器诊断；当前自动蟹行只参考其思路，不修改也不依赖该源码 |
| **`MultiVehicleDbg`** | `PilotDefinition.cs` | 联动循环：速度、舵角、补偿、ready 状态 |
| **`FleetDiagClumsy`** | `PilotDefinition.cs` | 精简 fleet 诊断（带 `car{N}` 前缀） |
| **`MultiVehicle`** | `PilotDefinition.cs` | 初始化、心跳、HTTP 错误 |
@@ -223,10 +225,10 @@ DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）
### 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 超时
1. 主车 `FleetCrabDbg`：`WARMUP done` → `ITER#` 中 `lateral/remain/headingErr/bias/dth/auto(vx,fTh,rTh)`
2. 主车 + 从车 `MultiVehicleDbg`：`auto:true/manual:false`、`BASE/SEND`、`PosBias*`、是否 `ready=false`
3. 若怀疑 MDCSToolbox 自动路径，再看主车 `CrabDbg`；当前 `FleetCrabWalk` 不直接调用该控制器
4. 从车 `FleetDiagClumsy`：是否频繁掉线 / register 超时
---
@@ -237,23 +239,26 @@ MovementTest「车队联动-自动蟹行」
FleetCrabWalk.Get()
TryGetFleetCenterFromSlam() → 路径起点 (x0,y0,θ)
phi = theta + FleetCrabAngleDeg
MultiVehicleScriptEnabled = true
MultiVehicleScriptMode = 1 → 复用 FleetRemote 手动蟹行下发链路
BodyToPathAngleDeg = FleetCrabAngleDeg
targetBodyTh = phi - BodyToPathAngleDeg = theta
MultiVehicleScriptEnabled = false
MultiVehicleAutoEnabled = true → 进入 TickMultiVehicle 自动分支
WARMUP → 等编队成员就位
loop:
TryGetFleetCenterFromSlam() → 当前车队中心
along/lateral/remain → 沿线进度、横向偏差、剩余距离
corr = clamp(Stanley(lateral), ±FleetCrabCorrectionAngleDeg)
localAngle = (phi + corr) - currentTheta
Vx/Vy slew limit → FleetCrabCommandAccel 平滑
MultiVehicleScriptVx/Vy = cmd
along/lateral/remain/headingErr → 沿线进度、横向偏差、剩余距离、车身目标朝向偏差
bias = clamp(Stanley(lateral), ±BiasThreshold)
dth = clamp(DthLinearFac * (targetBodyTh-currentTheta), ±DthLinearThreshold)
frontTh/rearTh = clamp(phi-currentTheta + bias ± dth, ±FleetCrabGcpThetaThreshold)
ideal = pathStart + pathDir * clamp(along, 0, FleetCrabLengthMm)
MultiVehicleAutoVx/FrontTh/RearTh/Ideal* = cmd
PilotDefinition.TickMultiVehicle (50ms)
manual/script mode==1
Vx/Vy → speed + frontTh==rearTh
auto branch
MultiVehicleAuto* → speed + frontTh/rearTh + ideal center
notify → 从车 SendMotion + POS/Detect 补偿
```
**对照 baseline**：`PilotDefinition.cs` 手动分支 `fleetMode == 1`（FleetRemote 蟹行）直接合成 `frontTh/rearTh`，不经几何控制器 `bias` 闭环。
**对照 baseline**：`PilotDefinition.cs` 手动分支 `fleetMode == 1`（FleetRemote 蟹行）仍直接合成 `frontTh/rearTh`；自动蟹行当前不走该分支。
---
@@ -263,15 +268,14 @@ MovementTest「车队联动-自动蟹行」
| 字段 | 默认 | 作用 |
|------|------|------|
| `FleetCrabAngleDeg` | 45 | 路径与车队朝向夹角 (deg) |
| `FleetCrabAngleDeg` | 45 | 路径方向相对启动时车队朝向的夹角 (deg)。MovementTest 同时把车身-路径夹角设为该值；若输入“路径与小车夹角 x 度”，应填 `-x` 以保持当前车身角度 |
| `FleetCrabLengthMm` | 2000 | 路径长度 (mm) |
| `FleetCrabSpeed` | 0.2 | 速度 (m/s) |
| `FleetCrabGcpThetaThreshold` | 95 | 兼容旧几何控制器实现；当前脚本蟹行不直接使用 |
| `FleetCrabCorrectionGain` | 1.0 | 横向误差纠偏增益 |
| `FleetCrabCorrectionAngleDeg` | 8 | 自动纠偏最大改向角，越小越接近手动蟹行 |
| `FleetCrabCommandAccel` | 0.4 | 脚本 `Vx/Vy` 命令斜率限制(m/s²) |
| `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由通用减速参数下调 |
| `FleetCrabGcpThetaThreshold` | 95 | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限，应给实际舵角限位与 `AngleLimitMarginDeg` 留余量 |
动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleScript*` 和 `MultiVehicleAuto*`。
已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略，不能再作为有效调参项。
动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleAuto*`，并保持 `MultiVehicleScriptEnabled=false`。
### 8.2 影响跟踪/手感的全局项（节选）
@@ -282,7 +286,12 @@ MovementTest「车队联动-自动蟹行」
| `TestCarSyncDistance` | 2400 | 与 Playground 双车间距一致 |
| `MultiVehicleSyncInterval` | 50 | 联动周期 ms |
| `DriveTaskInterval` | 50 | `clumsy.json` 顶层 |
| `BiasFac` / `DthLinearFac` 等 | 继承 `MultiWheelPilotConfig` | 几何控制器 PID 形态参数 |
| `BiasFac` / `BiasThreshold` | 继承 `MultiWheelPilotConfig` | 横向偏差 `lateral` → 前后 GCP 同向修正 |
| `DthLinearFac` / `DthLinearThreshold` | 继承 `MultiWheelPilotConfig` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正 |
| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 继承 `BasicPilotConfig` | 自动蟹行终点减速和结束判定 |
| `MultiVehicleAutoUseIdealCenter` | true（默认） | 使用自动蟹行发布的 ideal center 给从车做前馈 |
| `MultiVehicleAutoRequireFleetCenter` | true（默认） | 自动模式无有效车队中心时整队停车 |
| `MultiVehicleAutoCmdTimeoutMs` | 0（auto） | 自动命令新鲜度超时，避免控制器停发后沿末速度滑行 |
详见 [MultiVehicleConfig.md](./MultiVehicleConfig.md) §2–§6。
@@ -296,20 +305,20 @@ MovementTest「车队联动-自动蟹行」
| 蟹行要求朝向不变但有纠偏 | `CrabHoldHeading` + `CrabTargetHeading`；保留 `dTh` |
| 多车 firstTurn 破坏队形 | `MultiVehicleSync` 时跳过 `firstTurnN`（TODO 整队预旋转） |
| gcp 被 45° 上限截断 | 动作侧 `GcpThetaThreshold=95` |
| ideal 中心抹平横向误差 | 动作期间关 `MultiVehicleAutoUseIdealCenter` |
| ideal 中心抹平横向误差 | 已改为显式发布 `MultiVehicleAutoIdealX/Y/Th`，由 `MultiVehicleAutoUseIdealCenter` 控制是否前馈 |
| Tick 中间窗口发布 `(0,0,0)` 假中心 | 已移除 tick 开头 `PublishFleetCenter(0,0,0)` |
| 自动蟹行纠偏导致抖动 | 已改为脚本手动蟹行链路 + 小幅平滑横向纠偏 |
| 自动蟹行纠偏导致抖动 | 已改为自动字段链路，按 `lateral/headingErr/remain` 计算 `MultiVehicleAuto*` |
| 接近纯横移时速度符号/舵角表示翻转 | `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`。
1. **复测自动蟹行**：重点看 `FleetCrabDbg` 的 `auto=(vx,fTh,rTh)` 与 `MultiVehicleDbg` 的 `auto:true/manual:false` 是否一致。
2. **A/B：`FleetCrabGcpThetaThreshold`** 默认 95，应与实车舵角限制和 `AngleLimitMarginDeg` 匹配；若输出很快被限幅，先核对该值。
3. **A/B：`BiasFac/BiasThreshold`** 若 `lateral` 单向增长，先看 `bias` 是否到上限；需要更强横向纠偏时调这组参数。
4. **A/B：`DthLinearFac/DthLinearThreshold`** 若车队朝向偏差收敛慢或前后 GCP 差动过大，调这组参数。
5. **A/B：`MultiVehicleSyncUseDetour=false`** 若仍抖，跑同一条蟹行，区分自动路径纠偏 vs POS 补偿贡献。
6. **notify 平滑**（中长期）：见 `MultiVehicleAutoSyncReview.md` §F。
---
docs/MultiVehicleConfig.md
+32 -19
View File
@@ -144,28 +144,45 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
## 6. 自动蟹行动作（FleetCrabWalk / MovementTest「车队联动-自动蟹行」）
在 Clumsy 侧 MovementTest 面板触发，以**当前车队中心**为起点，构造一条与车队朝向夹角 `FleetCrabAngleDeg`、长度 `FleetCrabLengthMm` 的**直线路径**，执行侧复用 FleetRemote 已验证丝滑的脚本手动等价输入（`MultiVehicleScriptEnabled + mode=1`）让整队**斜向平移（蟹行）**：
在 Clumsy 侧 MovementTest 面板触发，以**当前车队中心**为起点，构造一条与启动时车队朝向夹角 `FleetCrabAngleDeg`、长度 `FleetCrabLengthMm` 的**直线路径**。MovementTest 会保持启动时车身朝向追踪路径；因此如果“路径相对小车”的夹角为 `x` 度（路径在车体右侧为正），应配置 `FleetCrabAngleDeg = -x`。当前实现不再复用脚本手动链路，而是参考几何控制器思路，在 `MultiWheelC` 内计算并写入 `MultiVehicleAuto...` 字段：
- 动作每拍读取主车 Detour 反推车队中心，计算直线进度 `along`、横向偏差 `lateral` 和剩余距离 `remain`。
- 横向偏差只转成一个**小幅、带斜率限制的蟹行方向修正**，再写入 `MultiVehicleScriptVx/Vy`；`TickMultiVehicle` 仍按手动蟹行逻辑合成 `frontTh==rearTh` 并广播从车。
- 这样保留手动蟹行的平滑执行链路，同时让自动动作具备温和的路径纠偏；避免旧几何控制器 `bias/dTh` 直接叠到 gcp 时出现舵角阶跃。
- **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点）。
- 动作每拍读取主车 Detour 反推车队中心，计算直线进度 `along`、横向偏差 `lateral`、剩余距离 `remain` 和车身目标朝向偏差 `headingErr`。
- `lateral` 通过 `BiasFac/BiasThreshold` 转为前后 GCP 同向舵角修正；`headingErr` 通过 `DthLinearFac/DthLinearThreshold` 转为前后 GCP 反向舵角修正。
- 动作直接输出 `MultiVehicleAutoVx/FrontTh/RearTh/IdealX/IdealY/IdealTh`，由 `TickMultiVehicle` 的自动分支统一广播、下发 `SendMotion`，并继续受识别丢失、成员超时、舵角余量不足等整队缓停联锁保护。
- **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点与运行中闭环）。
| 字段（`clumsy.json` → `msConf`） | 含义 | 默认值 |
|------|------|--------|
| `FleetCrabAngleDeg` | 蟹行路径**与当前车队朝向的夹角**(deg，逆时针为正)。决定斜行方向：0=正前方，90=正左方平移，-90=正右方。稳态下即各舵轮的蟹行角 | `45` |
| `FleetCrabAngleDeg` | 蟹行路径方向相对**启动时车队朝向**的夹角(deg，逆时针为正)。MovementTest 同时把车身-路径夹角设为该值，因此 `FleetCrabAngleDeg=-x` 会让车身保持启动朝向，并以 `x` 度夹角追踪路径 | `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` |
| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被 `SlowDistance/SlowingPow/FinishSpeed` 降速 | `0.2` |
| `FleetCrabGcpThetaThreshold` | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限(deg)。应小于实际舵角可行范围，并给 `AngleLimitMarginDeg` 留余量 | `95` |
自动蟹行还会使用下列通用控制参数：
| 字段 | 影响 | 默认来源 |
|------|------|----------|
| `BiasFac` / `BiasThreshold` | 横向偏差 `lateral` → 前后 GCP 同向修正。增大后收敛更快，但过大可能摆动 | `MultiWheelPilotConfig` |
| `DthLinearFac` / `DthLinearThreshold` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正，用于保持车身与路径夹角 | `MultiWheelPilotConfig` |
| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 终点减速和结束判定 | `BasicPilotConfig` |
| `MultiVehicleAutoUseIdealCenter` | 是否把自动蟹行计算出的 `IdealX/Y/Th` 广播给从车做前馈 | `true` |
| `MultiVehicleAutoRequireFleetCenter` | 自动模式是否要求有效车队中心；定位/车队中心失效时整队停车 | `true` |
| `MultiVehicleAutoCmdTimeoutMs` | 自动命令新鲜度超时，0 表示按联动周期自动计算 | `0` |
| `MultiVehicleUseDetect` / `MultiVehicleDetectBias*` | 互识别纠正与安全门；检测丢失时整队停车 | 见 §5 |
| `MultiVehicleSyncUseDetour` / `MultiVehiclePosBias*` | 车队内姿态纠正（POS 补偿）；不影响整车队中心计算 | 见 §5 |
已删除的旧自动蟹行参数：
| 已删除字段 | 原用途 | 当前替代 |
|------------|--------|----------|
| `FleetCrabCorrectionGain` | 旧脚本链路的横向误差纠偏增益 | `BiasFac` |
| `FleetCrabCorrectionAngleDeg` | 旧脚本链路的最大改向角 | `BiasThreshold` / `FleetCrabGcpThetaThreshold` |
| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 由自动联动周期、底盘速度斜坡和终点减速共同约束 |
**行为要点 / 注意**
- 当前实现不走 `MultiVehicleAuto*`/`MultiVehicleSendMotion`，也不再临时改 `MultiVehicleAutoUseIdealCenter`；结束/急停会清零脚本字段。
- `FleetCrabDbg` 会记录 `along/lateral/remain/corr/localAngle/cmd(Vx,Vy)`；`MultiVehicleDbg` 可继续对照最终 `frontTh≈rearTh`、是否有 POS/Detect 补偿，以及 `CRAB in/raw/limit/rev` 是否在舵角上限内保持连续表达。
- 当前实现走 `MultiVehicleAuto*` 自动字段链路，不再开启 `MultiVehicleScriptEnabled`，也不受 `MultiVehicleCrabSteerLimitDeg` 影响（该字段只影响手动 `mode=1` 蟹行）。
- `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。
- `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。
- Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。
@@ -202,11 +219,7 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
"FleetCrabAngleDeg": 45,
"FleetCrabLengthMm": 2000,
"FleetCrabSpeed": 0.2,
"FleetCrabGcpThetaThreshold": 95,
"FleetCrabCorrectionGain": 1.0,
"FleetCrabCorrectionAngleDeg": 8,
"FleetCrabCommandAccel": 0.4,
"MultiVehicleCrabSteerLimitDeg": 120
"FleetCrabGcpThetaThreshold": 95
```
```text