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

Update multi-vehicle sync and crab walk docs

Browse Source
This commit is contained in:
ruifeng.zhou
2026-06-29 16:00:34 +08:00
parent 8b19a15fbb
commit c093fa32e6
12 changed files with 1355 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/MovementTests.cs
+469
View File
@@ -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 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，&lt;=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度）。