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

Add absolute fleet crab walk path support

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-02 17:17:32 +08:00
parent 71444b362d
commit cb104b13cc
2 changed files with 109 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/MovementTests.cs
+50 -15
View File
@@ -447,6 +447,14 @@ public class FleetCrabWalk : MovementDefinition
/// <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;
@@ -525,8 +533,9 @@ public class FleetCrabWalk : MovementDefinition
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
$"autoFields=true pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
$"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
$"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 != "/")
@@ -546,17 +555,43 @@ public class FleetCrabWalk : MovementDefinition
}
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
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 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}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
$"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={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " +
$"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");
@@ -570,8 +605,8 @@ public class FleetCrabWalk : MovementDefinition
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoIdealX = x0;
self.MultiVehicleAutoIdealY = y0;
self.MultiVehicleAutoIdealX = pathStart.X;
self.MultiVehicleAutoIdealY = pathStart.Y;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
@@ -590,8 +625,8 @@ public class FleetCrabWalk : MovementDefinition
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoIdealX = x0;
self.MultiVehicleAutoIdealY = y0;
self.MultiVehicleAutoIdealX = pathStart.X;
self.MultiVehicleAutoIdealY = pathStart.Y;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
@@ -618,7 +653,7 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
"FleetCrabDbg");
Hedingben.ToastText($"车队蟹行 路径{CrabAngleDeg:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
var iter = 0;
var lastLog = DateTime.MinValue;
@@ -645,10 +680,10 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg");
break;
}
var delta = new Vector2(cx - x0, cy - y0);
var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
var along = Vector2.Dot(delta, pathDir);
var lateral = Vector2.Dot(delta, pathLeft);
var remain = CrabLengthMm - along;
var remain = pathLengthMm - along;
if (remain <= finishDistance)
break;
@@ -674,8 +709,8 @@ public class FleetCrabWalk : MovementDefinition
var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
holdFrontTh = frontTh;
holdRearTh = rearTh;
var idealAlong = Clamp(along, 0f, CrabLengthMm);
var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
var idealAlong = Clamp(along, 0f, pathLengthMm);
var ideal = pathStart + pathDir * idealAlong;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;