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

Fix fleet in-place rotate sync

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-02 14:38:00 +08:00
parent d4d2b1a052
commit bf83bdb4b3
5 changed files with 367 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/MovementTests.cs
+65 -2
View File
@@ -206,7 +206,11 @@ public class FleetRotateInPlace : MovementDefinition
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);
@@ -239,20 +243,43 @@ public class FleetRotateInPlace : MovementDefinition
yield return true;
}
float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
if (hasPos)
{
prevTh = (float)DetourInterface.getCartLocation().th;
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)
@@ -266,7 +293,8 @@ public class FleetRotateInPlace : MovementDefinition
float curTh = 0f, remaining = 0f, actualRate = 0f;
if (hasPos)
{
curTh = (float)DetourInterface.getCartLocation().th;
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)，用于对比指令
@@ -280,6 +308,28 @@ public class FleetRotateInPlace : MovementDefinition
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
{
@@ -329,6 +379,19 @@ public class FleetRotateInPlace : MovementDefinition
$"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");
}
}