903 lines
38 KiB
C#
903 lines
38 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Numerics;
|
||
using ClumsyCore;
|
||
using ClumsyCore.Interfaces;
|
||
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;
|
||
|
||
public class MultiForwardTest : MovementDefinition
|
||
{
|
||
public float Speed = 0.2f;
|
||
public float DurationSeconds = 2f;
|
||
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||
chassis.SetOriginBias(0, 0, 0);
|
||
var end = DateTime.Now.AddSeconds(DurationSeconds);
|
||
while (DateTime.Now < end)
|
||
{
|
||
chassis.SendMotion(Speed, 0, 0);
|
||
yield return true;
|
||
}
|
||
|
||
chassis.SendMotion(0, 0, 0);
|
||
}
|
||
}
|
||
|
||
// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
|
||
public class MultiRotateToWorldAngle : MovementDefinition
|
||
{
|
||
/// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
|
||
public float TargetWorldDeg;
|
||
|
||
/// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
|
||
public float RotSpeed = 30f;
|
||
|
||
/// <summary>到位角度精度（deg）。</summary>
|
||
public float ArriveDeg = 1f;
|
||
|
||
/// <summary>起转前舵轮对齐精度（deg）。</summary>
|
||
public float WheelAlignDeg = 2f;
|
||
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||
chassis.SetOriginBias(0, 0, 0);
|
||
|
||
// 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
|
||
while (true)
|
||
{
|
||
chassis.SendRotateMotion(0);
|
||
if (WheelsAligned(chassis, WheelAlignDeg)) break;
|
||
yield return true;
|
||
}
|
||
|
||
// 阶段二：旋转到目标世界朝向，到位即停。
|
||
var target = CommonMath.RoundTh(TargetWorldDeg);
|
||
while (true)
|
||
{
|
||
var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
|
||
var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
|
||
if (Math.Abs(diff) <= ArriveDeg) break;
|
||
chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
|
||
yield return true;
|
||
}
|
||
|
||
chassis.PredefinedDriveStop();
|
||
}
|
||
|
||
private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
|
||
{
|
||
#pragma warning disable CS0612, CS0618
|
||
var wheels = chassis.GetSteerWheels();
|
||
#pragma warning restore CS0612, CS0618
|
||
foreach (var sw in wheels)
|
||
if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
|
||
return false;
|
||
return true;
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-前进2秒")]
|
||
public class MultiForwardMovementTest : MovementTest
|
||
{
|
||
private DriveTask _task;
|
||
|
||
public override void Test()
|
||
{
|
||
_task = new DriveTask(new MultiForwardTest().Get());
|
||
_task.Wait();
|
||
}
|
||
|
||
public override void TestStop() => _task?.Stop();
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-原地旋转到目标角度")]
|
||
public class MultiRotateMovementTest : MovementTest
|
||
{
|
||
private DriveTask _task;
|
||
|
||
public override void Test()
|
||
{
|
||
_task = new DriveTask(new MultiRotateToWorldAngle
|
||
{
|
||
TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
|
||
RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
|
||
ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||
WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
|
||
}.Get());
|
||
_task.Wait();
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
_task?.Stop();
|
||
((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
|
||
}
|
||
}
|
||
|
||
// ===== 车队联动-原地旋转动作 =====
|
||
// 等价于 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 lastCenterLog = DateTime.MinValue;
|
||
var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
|
||
var centerTracking = false;
|
||
float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
|
||
float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
|
||
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
|
||
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
|
||
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
|
||
|
||
DLog.Log(
|
||
$"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");
|
||
|
||
// 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
|
||
// 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
|
||
self.MultiVehicleScriptVx = 0;
|
||
self.MultiVehicleScriptVy = 0;
|
||
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;
|
||
}
|
||
|
||
float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
|
||
if (hasPos)
|
||
{
|
||
var startPos = DetourInterface.getCartLocation();
|
||
centerStartCarX = (float)startPos.x;
|
||
centerStartCarY = (float)startPos.y;
|
||
centerStartCarTh = (float)startPos.th;
|
||
prevTh = centerStartCarTh;
|
||
startTh = prevTh;
|
||
if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
|
||
out centerStartX, out centerStartY, out centerStartTh))
|
||
{
|
||
centerLastX = centerStartX;
|
||
centerLastY = centerStartY;
|
||
centerLastTh = centerStartTh;
|
||
centerMaxDrift = 0;
|
||
centerTracking = true;
|
||
}
|
||
}
|
||
accumulated = 0f;
|
||
start = DateTime.Now;
|
||
lastTime = start;
|
||
lastLog = DateTime.MinValue;
|
||
lastCenterLog = 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");
|
||
if (centerTracking)
|
||
{
|
||
DLog.Log(
|
||
$"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||
$"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
|
||
$"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
|
||
"FleetRotateCenterDbg");
|
||
}
|
||
|
||
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)
|
||
{
|
||
var carPos = DetourInterface.getCartLocation();
|
||
curTh = (float)carPos.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;
|
||
|
||
if (centerTracking &&
|
||
self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||
out centerLastX, out centerLastY, out centerLastTh))
|
||
{
|
||
var centerDx = centerLastX - centerStartX;
|
||
var centerDy = centerLastY - centerStartY;
|
||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||
centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
|
||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||
if ((now - lastCenterLog).TotalMilliseconds >= 250)
|
||
{
|
||
lastCenterLog = now;
|
||
DLog.Log(
|
||
$"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||
$"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||
$"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
|
||
"FleetRotateCenterDbg");
|
||
}
|
||
}
|
||
}
|
||
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");
|
||
if (centerTracking)
|
||
{
|
||
var centerDx = centerLastX - centerStartX;
|
||
var centerDy = centerLastY - centerStartY;
|
||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||
DLog.Log(
|
||
$"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||
$"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
|
||
"FleetRotateCenterDbg");
|
||
}
|
||
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();
|
||
}
|
||
}
|
||
|
||
// ===== 车队联动-自动蟹行动作 =====
|
||
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
|
||
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
|
||
//
|
||
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
|
||
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
|
||
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
|
||
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
|
||
//
|
||
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
|
||
public class FleetCrabWalk : MovementDefinition
|
||
{
|
||
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
|
||
public float CrabAngleDeg = 45f;
|
||
|
||
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
|
||
public float BodyToPathAngleDeg = 45f;
|
||
|
||
/// <summary>路径长度(mm)。</summary>
|
||
public float CrabLengthMm = 2000f;
|
||
|
||
/// <summary>使用调度给定的绝对起终点路径，而不是用当前车队中心和 CrabAngleDeg 生成路径。</summary>
|
||
public bool UseAbsolutePath = false;
|
||
|
||
public float PathStartX;
|
||
public float PathStartY;
|
||
public float PathEndX;
|
||
public float PathEndY;
|
||
|
||
/// <summary>行驶速度(m/s)。</summary>
|
||
public float CrabSpeed = 0.2f;
|
||
|
||
/// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||
public float FleetCrabAccel = 0.2f;
|
||
|
||
/// <summary>末端开始减速距离(mm)。</summary>
|
||
public float FleetCrabSlowDistance = 2000f;
|
||
|
||
/// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
|
||
public float FleetCrabFinishDistance = 20f;
|
||
|
||
/// <summary>末端最低速度(m/s)。</summary>
|
||
public float FleetCrabFinishSpeed = 0.02f;
|
||
|
||
/// <summary>末端减速曲线指数。</summary>
|
||
public float FleetCrabSlowingPow = 0.8f;
|
||
|
||
/// <summary>前后 GCP 舵角修正上限(deg)。</summary>
|
||
public float GcpThetaThreshold = 95f;
|
||
|
||
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 Clamp(float value, float min, float max)
|
||
{
|
||
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;
|
||
}
|
||
|
||
private static float Slew(float current, float target, float maxDelta)
|
||
{
|
||
if (maxDelta <= 0) return target;
|
||
if (target > current + maxDelta) return current + maxDelta;
|
||
if (target < current - maxDelta) return current - maxDelta;
|
||
return target;
|
||
}
|
||
|
||
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} " +
|
||
$"autoFields=true pathMode={(UseAbsolutePath ? "absolute" : "relative")} 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 != "/")
|
||
{
|
||
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");
|
||
|
||
Vector2 pathStart;
|
||
Vector2 dst;
|
||
float pathLengthMm;
|
||
float phi;
|
||
if (UseAbsolutePath)
|
||
{
|
||
pathStart = new Vector2(PathStartX, PathStartY);
|
||
dst = new Vector2(PathEndX, PathEndY);
|
||
var pathVec = dst - pathStart;
|
||
pathLengthMm = pathVec.Length();
|
||
if (pathLengthMm <= 1f)
|
||
{
|
||
DLog.Log(
|
||
$"ABORT: absolute path is too short src=({PathStartX:0},{PathStartY:0}) dst=({PathEndX:0},{PathEndY:0}) len={pathLengthMm:0.0}",
|
||
"FleetCrabDbg");
|
||
Hedingben.ToastText("车队蟹行路径长度过短", "FleetCrab");
|
||
yield break;
|
||
}
|
||
phi = CommonMath.RoundTh((float)(Math.Atan2(pathVec.Y, pathVec.X) / Math.PI * 180.0));
|
||
}
|
||
else
|
||
{
|
||
pathStart = new Vector2(x0, y0);
|
||
pathLengthMm = CrabLengthMm;
|
||
phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||
dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||
}
|
||
var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||
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}) pathMode={(UseAbsolutePath ? "absolute" : "relative")} " +
|
||
$"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
|
||
$"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
|
||
$"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " +
|
||
$"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
|
||
$"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
|
||
"FleetCrabDbg");
|
||
|
||
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 = pathStart.X;
|
||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||
self.MultiVehicleAutoHasIdeal = true;
|
||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||
self.PrimeMasterAutoFromSlam();
|
||
DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg");
|
||
|
||
var warmEnd = DateTime.Now.AddSeconds(2.0);
|
||
var warmIter = 0;
|
||
var warmReady = false;
|
||
while (!_stopping && DateTime.Now < warmEnd)
|
||
{
|
||
warmIter++;
|
||
self.MultiVehicleScriptEnabled = false;
|
||
self.MultiVehicleScriptMode = 0;
|
||
self.MultiVehicleAutoEnabled = true;
|
||
self.MultiVehicleAutoVx = 0;
|
||
self.MultiVehicleAutoFrontTh = 0;
|
||
self.MultiVehicleAutoRearTh = 0;
|
||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||
self.MultiVehicleAutoHasIdeal = true;
|
||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||
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} " +
|
||
$"autoEn={self.MultiVehicleAutoEnabled} 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 timeout: fleet members are not ready; continue with auto fields and safety interlock.",
|
||
"FleetCrabDbg");
|
||
|
||
Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
|
||
|
||
var iter = 0;
|
||
var lastLog = DateTime.MinValue;
|
||
var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
|
||
var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
|
||
var baseSpeed = Math.Abs(CrabSpeed);
|
||
var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
|
||
var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
|
||
var accel = Math.Abs(FleetCrabAccel);
|
||
var cmdSpeed = 0f;
|
||
var lastTick = DateTime.Now;
|
||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||
var holdFrontTh = ClampAbs((float)CommonMath.ThDiff(phi, theta), gcpLimit);
|
||
var holdRearTh = holdFrontTh;
|
||
var stopReason = "done";
|
||
|
||
while (!_stopping)
|
||
{
|
||
iter++;
|
||
|
||
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 - pathStart.X, cy - pathStart.Y);
|
||
var along = Vector2.Dot(delta, pathDir);
|
||
var lateral = Vector2.Dot(delta, pathLeft);
|
||
var remain = pathLengthMm - along;
|
||
if (remain <= finishDistance)
|
||
break;
|
||
|
||
var targetSpeed = baseSpeed;
|
||
var slowRatio = 1f;
|
||
if (remain < slowDistance)
|
||
{
|
||
slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
|
||
targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
|
||
}
|
||
var now = DateTime.Now;
|
||
var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
|
||
lastTick = now;
|
||
var speed = accel > 0 ? Slew(cmdSpeed, targetSpeed, accel * dt) : targetSpeed;
|
||
cmdSpeed = speed;
|
||
|
||
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);
|
||
holdFrontTh = frontTh;
|
||
holdRearTh = rearTh;
|
||
var idealAlong = Clamp(along, 0f, pathLengthMm);
|
||
var ideal = pathStart + pathDir * idealAlong;
|
||
|
||
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)
|
||
{
|
||
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} headingErr={headingErr:0.0} " +
|
||
$"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
|
||
$"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} 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;
|
||
}
|
||
|
||
if (_stopping)
|
||
stopReason = "stop";
|
||
|
||
self.MultiVehicleAutoVx = 0;
|
||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||
DLog.Log(
|
||
$"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
|
||
"FleetCrabDbg");
|
||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||
while (!_stopping && DateTime.Now < settleEnd)
|
||
{
|
||
self.MultiVehicleScriptEnabled = false;
|
||
self.MultiVehicleScriptMode = 0;
|
||
self.MultiVehicleAutoEnabled = true;
|
||
self.MultiVehicleAutoVx = 0;
|
||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||
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,
|
||
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
|
||
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
|
||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||
};
|
||
_task = new DriveTask(_proc.Get());
|
||
_task.Wait();
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
_proc?.Stop();
|
||
_task?.Stop();
|
||
}
|
||
}
|
||
|
||
// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
|
||
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI前移")]
|
||
public class WebApiForwardMoveTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||
|
||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||
// 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
|
||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
|
||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||
Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI左移")]
|
||
public class WebApiLeftMoveTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||
|
||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||
// 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
|
||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
|
||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||
Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI旋转")]
|
||
public class WebApiRotateTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||
var deg = PilotDefinition.Conf.WebApiRotateDeg;
|
||
|
||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||
var ny = pose.YawDeg + deg; // 逆时针为正
|
||
PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
|
||
Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI恢复运动")]
|
||
public class WebApiMotionResumeTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
PlaygroundWebApi.ResumeMotion(url);
|
||
Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "多舵轮-WebAPI暂停运动")]
|
||
public class WebApiMotionPauseTest : MovementTest
|
||
{
|
||
public override void Test()
|
||
{
|
||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||
PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
|
||
Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
}
|
||
}
|