Add FleetCrabWalk startup motion config

MultiWheel/MultiWheelC/AGV.cs

@@ -322,10 +322,13 @@ namespace MultiWheelC
 
             var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
             var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
+            var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
+            var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
             DLog.Log(
                 $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
                 $"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
-                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0})",
+                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
+                $"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
                 "FleetCrabDbg");
 
             if (dstId != -1)
@@ -340,10 +343,11 @@ namespace MultiWheelC
             var action = new MultiWheelC.FleetCrabWalk
             {
                 CrabAngleDeg = crabAngle,
-                BodyToPathAngleDeg = crabAngle,
+                BodyToPathAngleDeg = bodyToPathAngle,
                 CrabLengthMm = pathLength,
                 CrabSpeed = speed,
                 FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+                FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
                 FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
                 FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
                 FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,

MultiWheel/MultiWheelC/MovementTests.cs

@@ -453,6 +453,9 @@ public class FleetCrabWalk : MovementDefinition
     /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
     public float FleetCrabAccel = 0.2f;
 
+    /// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabStartAccel = 0.01f;
+
     /// <summary>末端开始减速距离(mm)。</summary>
     public float FleetCrabSlowDistance = 2000f;
 
@@ -657,7 +660,7 @@ public class FleetCrabWalk : MovementDefinition
             $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
             $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
             $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
-            $"biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
+            $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
             "FleetCrabDbg");
 
         if (conf.MultiVehicleMasterEndpoint != "/")
@@ -692,7 +695,7 @@ public class FleetCrabWalk : MovementDefinition
             $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=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} " +
+            $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
             $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
             $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
             "FleetCrabDbg");
@@ -806,8 +809,10 @@ public class FleetCrabWalk : MovementDefinition
         var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
         var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
         var accel = Math.Abs(FleetCrabAccel);
+        var startAccel = Math.Abs(FleetCrabStartAccel);
         var cmdSpeed = 0f;
         var lastTick = DateTime.Now;
+        var speedRampStart = DateTime.Now;
         var stopReason = "done";
 
         while (!_stopping)
@@ -837,7 +842,9 @@ public class FleetCrabWalk : MovementDefinition
             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;
+            var rampElapsed = (now - speedRampStart).TotalSeconds;
+            var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
+            var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
             cmdSpeed = speed;
 
             var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
@@ -850,8 +857,8 @@ public class FleetCrabWalk : MovementDefinition
             var rawBiasItem = rawBiasMagnitude * biasSign;
             var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
             var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
-            var rawDthItem = conf.DthLinearFac * headingErr * yawSplitSign;
+            var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
-            var dthItem = ClampAbs(rawDthItem, conf.DthLinearThreshold);
+            var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
             var rawFrontTh = baseCrabTh + biasItem + dthItem;
             var rawRearTh = baseCrabTh + biasItem - dthItem;
             ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
@@ -886,7 +893,7 @@ public class FleetCrabWalk : MovementDefinition
                     $"ITER#{iter} centerSrc={centerSource} 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:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
+                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed: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");
@@ -895,11 +902,11 @@ public class FleetCrabWalk : MovementDefinition
                     $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
                     $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
                     $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
-                    $"dthFac:{conf.DthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.DthLinearThreshold:0.00} " +
+                    $"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
                     $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
                     $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
                     $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
-                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
+                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
                     "FleetCrabHeadingDbg");
             }
             yield return true;
@@ -949,6 +956,7 @@ public class FleetCrabWalkTest : MovementTest
             CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
             CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
             FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+            FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
             FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
             FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
             FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -161,6 +161,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
     public float FleetCrabAngleDeg = 45f;
 
+    [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
+    public float FleetCrabBodyWorldHeadingDeg = 0f;
+
     [FieldMember(desc = "车队蟹行：路径长度(mm)")]
     public float FleetCrabLengthMm = 2000f;
 
@@ -170,6 +173,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
     public float FleetCrabAccel = 0.2f;
 
+    [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
+    public float FleetCrabStartAccel = 0.01f;
+
     [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
     public float FleetCrabSlowDistance = 2000f;
 
@@ -185,6 +191,12 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
     public float FleetCrabGcpThetaThreshold = 95f;
 
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
+    public float FleetCrabDthLinearFac = 1f;
+
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
+    public float FleetCrabDthLinearThreshold = 10f;
+
     [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
     public float FleetCrabStartSyncTimeoutSec = 8f;