diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs
index cf35434..7af2057 100644
--- a/MultiWheel/MultiWheelC/AGV.cs
+++ b/MultiWheel/MultiWheelC/AGV.cs
@@ -400,6 +400,102 @@ namespace MultiWheelC
}
}
}
+
+ public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+ float speed, params float[] trackTypeInfo)
+ {
+ if (trackTypeInfo == null || trackTypeInfo.Length < 2)
+ {
+ DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
+ return;
+ }
+
+ var trackType = (int)trackTypeInfo[0];
+ if (trackType != 2)
+ {
+ DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
+ "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
+ return;
+ }
+
+ var controlPointNum = (int)trackTypeInfo[1];
+ var expectedLength = 2 + controlPointNum * 2;
+ if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
+ {
+ DLog.Log(
+ $"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
+ $"length={trackTypeInfo.Length}, expected>={expectedLength}.",
+ "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
+ return;
+ }
+
+ BezierTrack track;
+ try
+ {
+ track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
+ }
+ catch (Exception ex)
+ {
+ DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
+ return;
+ }
+
+ if (track == null)
+ {
+ DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
+ return;
+ }
+
+ track.Speed = speed;
+ track.CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias;
+
+ DLog.Log(
+ $"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+ $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
+ $"carDirectionBias={PilotDefinition.Conf.FleetCurveCarDirectionBias:0.0})",
+ "FleetCurveDbg");
+
+ if (dstId != -1)
+ {
+ while (!TryLock(dstId))
+ {
+ Thread.Sleep(50);
+ }
+ DLog.Log($"锁点{dstId}完成", "FleetCurveDbg");
+ }
+
+ var action = new MultiWheelC.FleetCurveWalk
+ {
+ Track = track,
+ CurveSpeed = speed,
+ CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias,
+ BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution,
+ SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+ FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+ FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+ SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+ GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+ StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+ };
+
+ try
+ {
+ new DriveTask(action.Get()).Wait();
+ }
+ finally
+ {
+ if (srcId != -1)
+ {
+ Leave(srcId);
+ DLog.Log($"释放放车点{srcId}", "FleetCurveDbg");
+ }
+ }
+ }
public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
{
diff --git a/MultiWheel/MultiWheelC/FleetCrabWalk.cs b/MultiWheel/MultiWheelC/FleetCrabWalk.cs
new file mode 100644
index 0000000..8aeccf3
--- /dev/null
+++ b/MultiWheel/MultiWheelC/FleetCrabWalk.cs
@@ -0,0 +1,528 @@
+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.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC;
+
+// ===== 车队联动-自动蟹行动作 =====
+// 以当前车队中心为起点，构造指定方向和长度的直线路径；
+// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
+//
+// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
+// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
+// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
+// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
+//
+// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
+public class FleetCrabWalk : MovementDefinition
+{
+ /// 路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。
+ public float CrabAngleDeg = 45f;
+
+ /// 路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。
+ public float BodyToPathAngleDeg = 45f;
+
+ /// 路径长度(mm)。
+ public float CrabLengthMm = 2000f;
+
+ /// 行驶速度(m/s)。
+ public float CrabSpeed = 0.2f;
+
+ /// 速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。
+ public float FleetCrabAccel = 0.2f;
+
+ /// 预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。
+ public float FleetCrabStartAccel = 0.01f;
+
+ /// 末端开始减速距离(mm)。
+ public float FleetCrabSlowDistance = 2000f;
+
+ /// 完成距离(mm)，低于该剩余距离结束动作。
+ public float FleetCrabFinishDistance = 20f;
+
+ /// 末端最低速度(m/s)。
+ public float FleetCrabFinishSpeed = 0.02f;
+
+ /// 末端减速曲线指数。
+ public float FleetCrabSlowingPow = 0.8f;
+
+ /// 前后 GCP 舵角修正上限(deg)。
+ 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;
+ }
+
+ private static float AverageAngle(float frontTh, float rearTh)
+ {
+ var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
+ return (float)CommonMath.RoundTh(rearTh + diff / 2f);
+ }
+
+ private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
+ out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
+ {
+ var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
+ rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
+ driveSpeed = speed;
+ frontTh = rawFrontTh;
+ rearTh = rawRearTh;
+ reverseEquivalent = false;
+
+ if (rawBaseTh > limit)
+ {
+ frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
+ rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
+ driveSpeed = -driveSpeed;
+ reverseEquivalent = true;
+ }
+ else if (rawBaseTh < -limit)
+ {
+ frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
+ rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
+ driveSpeed = -driveSpeed;
+ reverseEquivalent = true;
+ }
+
+ frontTh = ClampAbs(frontTh, limit);
+ rearTh = ClampAbs(rearTh, limit);
+ }
+
+ private static float ProbeSpeed(float speed)
+ {
+ return Math.Abs(speed) > 1e-4f ? speed : 1f;
+ }
+
+ private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
+ out float yawSign)
+ {
+ yawSign = 0f;
+ if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
+ return false;
+
+ var radius = Math.Max(1f, Math.Abs(controlRadius));
+ Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
+ normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
+ normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
+ var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
+ if (!intersect)
+ return false;
+
+ // Match MultiWheelChassis.SendMotion: the tangent side is selected by
+ // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
+ var tangentSign = center.Y > 1f ? 1f : -1f;
+ var speedSign = driveSpeed >= 0f ? 1f : -1f;
+ yawSign = speedSign * tangentSign;
+ return true;
+ }
+
+ private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
+ {
+ const float probeDth = 1f;
+ ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
+ out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+ return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
+ ? yawSign
+ : 1f;
+ }
+
+ private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
+ Vector2 pathLeft)
+ {
+ var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
+ var rad = motionTh / 180f * Math.PI;
+ var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
+ if (driveSpeed < 0f)
+ dir = -dir;
+ return Vector2.Dot(dir, pathLeft);
+ }
+
+ private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+ float lateral, float biasProbe)
+ {
+ ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
+ out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+ var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
+ return -Math.Sign(lateral) * lateralVelocity;
+ }
+
+ private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+ float lateral)
+ {
+ if (Math.Abs(lateral) <= 1e-3f)
+ return 1f;
+
+ const float probeBias = 1f;
+ var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
+ var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
+ return positiveScore >= negativeScore ? 1f : -1f;
+ }
+
+ private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+ out float centerTh, out string source)
+ {
+ if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+ {
+ source = "fleet";
+ return true;
+ }
+
+ if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+ {
+ source = "slam";
+ return true;
+ }
+
+ source = "none";
+ return false;
+ }
+
+ public override IEnumerable Get()
+ {
+ var self = PilotDefinition.Self;
+ var conf = PilotDefinition.Conf;
+ var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+ if (chassis == null)
+ {
+ DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
+ yield break;
+ }
+ _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=relative pathAngle={CrabAngleDeg:0.0} " +
+ $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
+ $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac: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 (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+ {
+ DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
+ Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
+ yield break;
+ }
+ DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+ DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+ var pathStart = new Vector2(x0, y0);
+ var pathLengthMm = CrabLengthMm;
+ var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+ var 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=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} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
+ $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
+ $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
+ "FleetCrabDbg");
+
+ var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+ var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
+ ? conf.MultiVehicleControlRadius
+ : conf.TestCarSyncDistance / 2f));
+ ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
+ (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
+ out _, out _);
+ var warmStart = DateTime.Now;
+ var warmSeqBaseline = self.BeginFleetMotionWarmup();
+
+ self.MultiVehicleScriptEnabled = false;
+ self.MultiVehicleScriptMode = 0;
+ self.MultiVehicleScriptVx = 0;
+ self.MultiVehicleScriptVy = 0;
+ self.MultiVehicleScriptVth = 0;
+ self.MultiVehicleAutoEnabled = true;
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoFrontTh = holdFrontTh;
+ self.MultiVehicleAutoRearTh = holdRearTh;
+ 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 startup sync seqBase={warmSeqBaseline} " +
+ $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
+ "FleetCrabDbg");
+
+ var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
+ var warmIter = 0;
+ var warmReady = false;
+ var warmDetail = "";
+ while (!_stopping && DateTime.Now < warmEnd)
+ {
+ warmIter++;
+ self.MultiVehicleScriptEnabled = false;
+ self.MultiVehicleScriptMode = 0;
+ self.MultiVehicleAutoEnabled = true;
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoFrontTh = holdFrontTh;
+ self.MultiVehicleAutoRearTh = holdRearTh;
+ 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} " +
+ $"detail={warmDetail}",
+ "FleetCrabDbg");
+ if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+ conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+ {
+ warmReady = true;
+ DLog.Log(
+ $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
+ "FleetCrabDbg");
+ break;
+ }
+ yield return true;
+ }
+ if (!warmReady)
+ {
+ DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
+ "FleetCrabDbg");
+ Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
+ Cleanup();
+ yield break;
+ }
+
+ Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
+
+ if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
+ {
+ x0 = warmX;
+ y0 = warmY;
+ theta = warmTh;
+ pathStart = new Vector2(x0, y0);
+ phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+ dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+ targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+ phiRad = phi / 180.0 * Math.PI;
+ pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
+ pathLeft = new Vector2(-pathDir.Y, pathDir.X);
+ self.MultiVehicleAutoIdealX = pathStart.X;
+ self.MultiVehicleAutoIdealY = pathStart.Y;
+ self.MultiVehicleAutoIdealTh = targetBodyTh;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
+ DLog.Log(
+ $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
+ "FleetCrabDbg");
+ }
+
+ 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 startAccel = Math.Abs(FleetCrabStartAccel);
+ var cmdSpeed = 0f;
+ var lastTick = DateTime.Now;
+ var speedRampStart = DateTime.Now;
+ var stopReason = "done";
+
+ while (!_stopping)
+ {
+ iter++;
+
+ if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
+ {
+ stopReason = "fleet center invalid";
+ DLog.Log("ABORT: TryGetControlFleetCenter 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 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);
+ var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
+ var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
+ var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
+ var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
+ Math.Max(speed, 0.3f)) / Math.PI * 180.0);
+ var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
+ var rawBiasItem = rawBiasMagnitude * biasSign;
+ var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
+ var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
+ var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
+ var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
+ var rawFrontTh = baseCrabTh + biasItem + dthItem;
+ var rawRearTh = baseCrabTh + biasItem - dthItem;
+ ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
+ out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
+ 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 = driveSpeed;
+ 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} 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} 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");
+ DLog.Log(
+ $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
+ $"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} " +
+ $"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} 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;
+ }
+
+ 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");
+ }
+}
diff --git a/MultiWheel/MultiWheelC/FleetCurveWalk.cs b/MultiWheel/MultiWheelC/FleetCurveWalk.cs
new file mode 100644
index 0000000..cb66bcd
--- /dev/null
+++ b/MultiWheel/MultiWheelC/FleetCurveWalk.cs
@@ -0,0 +1,413 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+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 FleetCurveWalk : MovementDefinition
+{
+ public BezierTrack Track;
+ public List ControlPoints = new();
+ public float CurveSpeed = 0.2f;
+ public float CarDirectionBias = 0f;
+ public int BezierResolution = 100;
+ public float SlowDistance = 2000f;
+ public float FinishDistance = 20f;
+ public float FinishSpeed = 0.02f;
+ public float SlowingPow = 0.8f;
+ public float GcpThetaThreshold = 95f;
+ public float StartSyncTimeoutSec = 8f;
+
+ private bool _stopping;
+ private MultiWheelGeometricController _controller;
+ private MultiWheelChassis _chassis;
+ private bool _savedControlPoints;
+ private float _savedControlRadius;
+ private Vector2 _savedGcp0;
+ private Vector2 _savedGcp1;
+
+ public void Stop()
+ {
+ _stopping = true;
+ if (_controller != null)
+ _controller.BreakAndHold = true;
+ Cleanup();
+ }
+
+ public static bool TryParsePointList(string text, out List points, out string error)
+ {
+ points = new List();
+ error = "";
+ if (string.IsNullOrWhiteSpace(text))
+ {
+ error = "empty control point list";
+ return false;
+ }
+
+ var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
+ for (var i = 0; i < segments.Length; i++)
+ {
+ var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
+ if (pair.Length != 2)
+ {
+ error = $"invalid point #{i + 1}: {segments[i]}";
+ return false;
+ }
+
+ if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
+ {
+ error = $"invalid number in point #{i + 1}: {segments[i]}";
+ return false;
+ }
+ points.Add(new Vector2(x, y));
+ }
+
+ if (points.Count < 3)
+ {
+ error = "Bezier curve requires at least 3 control points";
+ return false;
+ }
+
+ return true;
+ }
+
+ public static List BuildRelativeControlPoints(Vector2 start, float startTh, List relativePoints)
+ {
+ var source = relativePoints ?? new List();
+ var normalized = new List();
+ if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
+ normalized.Add(Vector2.Zero);
+ for (var i = 0; i < source.Count; i++)
+ normalized.Add(source[i]);
+ if (normalized.Count < 2)
+ normalized.Add(new Vector2(1000f, 0f));
+ if (normalized.Count < 3)
+ normalized.Add(new Vector2(2000f, 0f));
+
+ var result = new List();
+ for (var i = 0; i < normalized.Count; i++)
+ result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
+ return result;
+ }
+
+ public static List BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
+ params float[] controlPointCoords)
+ {
+ var src = new Vector2(srcX, srcY);
+ var dst = new Vector2(dstX, dstY);
+ var result = new List();
+ if (controlPointCoords == null || controlPointCoords.Length == 0)
+ {
+ result.Add(src);
+ result.Add((src + dst) / 2f);
+ result.Add(dst);
+ return result;
+ }
+
+ if (controlPointCoords.Length % 2 != 0)
+ throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
+
+ var supplied = new List();
+ for (var i = 0; i < controlPointCoords.Length; i += 2)
+ supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
+
+ if (supplied.Count >= 3 &&
+ Vector2.Distance(supplied[0], src) <= 10f &&
+ Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
+ return supplied;
+
+ result.Add(src);
+ for (var i = 0; i < supplied.Count; i++)
+ result.Add(supplied[i]);
+ result.Add(dst);
+ if (result.Count < 3)
+ result.Insert(1, (src + dst) / 2f);
+ return result;
+ }
+
+ private static bool TryParseFloat(string text, out float value)
+ {
+ return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
+ float.TryParse(text, out 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 bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+ out float centerTh, out string source)
+ {
+ if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+ {
+ source = "fleet";
+ return true;
+ }
+
+ if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+ {
+ source = "slam";
+ return true;
+ }
+
+ source = "none";
+ return false;
+ }
+
+ 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;
+ RestoreControlPointRadius();
+ }
+
+ private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
+ {
+ if (!_savedControlPoints)
+ {
+ _chassis = chassis;
+ _savedControlRadius = chassis.ControlPointRadius;
+ var gcps = chassis.GetGeometricControlPoints();
+ if (gcps.Count >= 2)
+ {
+ _savedGcp0 = gcps[0].Position;
+ _savedGcp1 = gcps[1].Position;
+ }
+ _savedControlPoints = true;
+ }
+
+ chassis.ControlPointRadius = radius;
+ var points = chassis.GetGeometricControlPoints();
+ if (points.Count >= 2)
+ {
+ points[0].Position = new Vector2(radius, 0);
+ points[1].Position = new Vector2(-radius, 0);
+ }
+ }
+
+ private void RestoreControlPointRadius()
+ {
+ if (!_savedControlPoints || _chassis == null)
+ return;
+
+ _chassis.ControlPointRadius = _savedControlRadius;
+ var points = _chassis.GetGeometricControlPoints();
+ if (points.Count >= 2)
+ {
+ points[0].Position = _savedGcp0;
+ points[1].Position = _savedGcp1;
+ }
+ _savedControlPoints = false;
+ }
+
+ private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
+ float frontTh, float rearTh)
+ {
+ self.MultiVehicleScriptEnabled = false;
+ self.MultiVehicleScriptMode = 0;
+ self.MultiVehicleScriptVx = 0;
+ self.MultiVehicleScriptVy = 0;
+ self.MultiVehicleScriptVth = 0;
+ self.MultiVehicleAutoEnabled = true;
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoFrontTh = frontTh;
+ self.MultiVehicleAutoRearTh = rearTh;
+ self.MultiVehicleAutoIdealX = idealPos.X;
+ self.MultiVehicleAutoIdealY = idealPos.Y;
+ self.MultiVehicleAutoIdealTh = idealTh;
+ self.MultiVehicleAutoHasIdeal = true;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
+ }
+
+ public override IEnumerable Get()
+ {
+ var self = PilotDefinition.Self;
+ var conf = PilotDefinition.Conf;
+ var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+ _stopping = false;
+
+ if (chassis == null)
+ {
+ DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
+ yield break;
+ }
+
+ if (conf.MultiVehicleMasterEndpoint != "/")
+ {
+ DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
+ "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
+ yield break;
+ }
+
+ if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
+ {
+ DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
+ yield break;
+ }
+
+ if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+ {
+ DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
+ yield break;
+ }
+
+ var baseSpeed = Math.Abs(CurveSpeed);
+ if (baseSpeed <= 1e-4f)
+ {
+ DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
+ yield break;
+ }
+
+ var resolution = Math.Max(2, BezierResolution);
+ var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
+ var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+ var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
+ ? conf.MultiVehicleControlRadius
+ : conf.TestCarSyncDistance / 2f));
+
+ ApplyFleetControlPointRadius(chassis, controlRadius);
+
+ try
+ {
+ var track = Track;
+ var trackSource = "external";
+ if (track == null)
+ {
+ var points = new List(ControlPoints);
+ track = new BezierTrack(points, resolution);
+ trackSource = "controlPoints";
+ }
+ track.CarDirectionBias = CarDirectionBias;
+ track.Speed = baseSpeed;
+
+ var center = new Vector2(x0, y0);
+ var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
+ var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
+ var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
+ var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
+
+ DLog.Log(
+ $"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
+ $"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
+ $"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
+ $"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
+ $"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
+ "FleetCurveDbg");
+
+ var warmStart = DateTime.Now;
+ var warmSeqBaseline = self.BeginFleetMotionWarmup();
+ WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+ self.PrimeMasterAutoFromSlam();
+
+ var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
+ var warmIter = 0;
+ var warmReady = false;
+ var warmDetail = "";
+ while (!_stopping && DateTime.Now < warmEnd)
+ {
+ warmIter++;
+ WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+ self.PrimeMasterAutoFromSlam();
+
+ if (warmIter % 5 == 0)
+ {
+ var snap = self.GetFleetCenterSnapshot();
+ int cnt;
+ lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
+ DLog.Log(
+ $"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
+ $"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
+ "FleetCurveDbg");
+ }
+
+ if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+ conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+ {
+ warmReady = true;
+ DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
+ break;
+ }
+ yield return true;
+ }
+
+ if (!warmReady)
+ {
+ DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
+ "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
+ Cleanup();
+ yield break;
+ }
+
+ _controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
+ _controller.MultiVehicleSync = true;
+ _controller.BaseSpeed = baseSpeed;
+ _controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
+ _controller.FinishDistance = Math.Max(0f, FinishDistance);
+ _controller.FinishSpeed = speedFinish;
+ _controller.SlowingPow = Math.Max(0.01f, SlowingPow);
+ _controller.GcpThetaThreshold = gcpLimit;
+ _controller.AddTrack(track, "FleetCurve");
+
+ Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
+
+ foreach (var running in _controller.Track())
+ {
+ if (_stopping)
+ break;
+ if (!running)
+ break;
+ yield return true;
+ }
+
+ if (!_stopping)
+ {
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
+ var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+ while (!_stopping && DateTime.Now < settleEnd)
+ {
+ self.MultiVehicleAutoEnabled = true;
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
+ yield return true;
+ }
+ }
+
+ DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
+ }
+ finally
+ {
+ Cleanup();
+ _controller = null;
+ }
+ }
+}
diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs
index a10ce79..dc8e2c3 100644
--- a/MultiWheel/MultiWheelC/MovementTests.cs
+++ b/MultiWheel/MultiWheelC/MovementTests.cs
@@ -7,10 +7,8 @@ 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;
@@ -426,518 +424,53 @@ public class FleetRotateInPlaceTest : MovementTest
}
}
-// ===== 车队联动-自动蟹行动作 =====
-// 以当前车队中心为起点，构造指定方向和长度的直线路径；
-// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
-//
-// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
-// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
-// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
-// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
-//
-// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
-public class FleetCrabWalk : MovementDefinition
+[MovementTest(name = "车队联动-曲线行走")]
+public class FleetCurveWalkTest : MovementTest
{
- /// 路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。
- public float CrabAngleDeg = 45f;
+ private FleetCurveWalk _proc;
+ private DriveTask _task;
- /// 路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。
- public float BodyToPathAngleDeg = 45f;
-
- /// 路径长度(mm)。
- public float CrabLengthMm = 2000f;
-
- /// 行驶速度(m/s)。
- public float CrabSpeed = 0.2f;
-
- /// 速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。
- public float FleetCrabAccel = 0.2f;
-
- /// 预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。
- public float FleetCrabStartAccel = 0.01f;
-
- /// 末端开始减速距离(mm)。
- public float FleetCrabSlowDistance = 2000f;
-
- /// 完成距离(mm)，低于该剩余距离结束动作。
- public float FleetCrabFinishDistance = 20f;
-
- /// 末端最低速度(m/s)。
- public float FleetCrabFinishSpeed = 0.02f;
-
- /// 末端减速曲线指数。
- public float FleetCrabSlowingPow = 0.8f;
-
- /// 前后 GCP 舵角修正上限(deg)。
- public float GcpThetaThreshold = 95f;
-
- private bool _stopping;
-
- private void Cleanup()
+ public override void Test()
{
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;
- }
-
- private static float AverageAngle(float frontTh, float rearTh)
- {
- var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
- return (float)CommonMath.RoundTh(rearTh + diff / 2f);
- }
-
- private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
- out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
- {
- var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
- rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
- driveSpeed = speed;
- frontTh = rawFrontTh;
- rearTh = rawRearTh;
- reverseEquivalent = false;
-
- if (rawBaseTh > limit)
+ if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
+ !self.TryGetFleetCenterFromSlam(out x, out y, out th))
{
- frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
- rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
- driveSpeed = -driveSpeed;
- reverseEquivalent = true;
- }
- else if (rawBaseTh < -limit)
- {
- frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
- rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
- driveSpeed = -driveSpeed;
- reverseEquivalent = true;
+ DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
+ Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
+ return;
}
- frontTh = ClampAbs(frontTh, limit);
- rearTh = ClampAbs(rearTh, limit);
+ if (!FleetCurveWalk.TryParsePointList(PilotDefinition.Conf.FleetCurveControlPoints,
+ out var relativePoints, out var error))
+ {
+ DLog.Log($"FleetCurveWalkTest abort: {error}", "FleetCurveDbg");
+ Hedingben.ToastText(error, "FleetCurve");
+ return;
+ }
+
+ var controlPoints = FleetCurveWalk.BuildRelativeControlPoints(new Vector2(x, y), th, relativePoints);
+ _proc = new FleetCurveWalk
+ {
+ ControlPoints = controlPoints,
+ CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
+ CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias,
+ BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution,
+ SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+ FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+ FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+ SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+ GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+ StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+ };
+ _task = new DriveTask(_proc.Get());
+ _task.Wait();
}
- private static float ProbeSpeed(float speed)
+ public override void TestStop()
{
- return Math.Abs(speed) > 1e-4f ? speed : 1f;
- }
-
- private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
- out float yawSign)
- {
- yawSign = 0f;
- if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
- return false;
-
- var radius = Math.Max(1f, Math.Abs(controlRadius));
- Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
- normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
- normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
- var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
- if (!intersect)
- return false;
-
- // Match MultiWheelChassis.SendMotion: the tangent side is selected by
- // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
- var tangentSign = center.Y > 1f ? 1f : -1f;
- var speedSign = driveSpeed >= 0f ? 1f : -1f;
- yawSign = speedSign * tangentSign;
- return true;
- }
-
- private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
- {
- const float probeDth = 1f;
- ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
- out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
- return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
- ? yawSign
- : 1f;
- }
-
- private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
- Vector2 pathLeft)
- {
- var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
- var rad = motionTh / 180f * Math.PI;
- var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
- if (driveSpeed < 0f)
- dir = -dir;
- return Vector2.Dot(dir, pathLeft);
- }
-
- private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
- float lateral, float biasProbe)
- {
- ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
- out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
- var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
- return -Math.Sign(lateral) * lateralVelocity;
- }
-
- private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
- float lateral)
- {
- if (Math.Abs(lateral) <= 1e-3f)
- return 1f;
-
- const float probeBias = 1f;
- var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
- var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
- return positiveScore >= negativeScore ? 1f : -1f;
- }
-
- private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
- out float centerTh, out string source)
- {
- if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
- {
- source = "fleet";
- return true;
- }
-
- if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
- {
- source = "slam";
- return true;
- }
-
- source = "none";
- return false;
- }
-
- public override IEnumerable Get()
- {
- var self = PilotDefinition.Self;
- var conf = PilotDefinition.Conf;
- var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
- if (chassis == null)
- {
- DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
- yield break;
- }
- _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=relative pathAngle={CrabAngleDeg:0.0} " +
- $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
- $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac: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 (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
- {
- DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
- Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
- yield break;
- }
- DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
-
- DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
-
- var pathStart = new Vector2(x0, y0);
- var pathLengthMm = CrabLengthMm;
- var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
- var 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=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} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
- $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
- $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
- "FleetCrabDbg");
-
- var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
- var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0
- ? conf.MultiVehicleControlRadius
- : conf.TestCarSyncDistance / 2f));
- ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
- (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
- out _, out _);
- var warmStart = DateTime.Now;
- var warmSeqBaseline = self.BeginFleetMotionWarmup();
-
- self.MultiVehicleScriptEnabled = false;
- self.MultiVehicleScriptMode = 0;
- self.MultiVehicleScriptVx = 0;
- self.MultiVehicleScriptVy = 0;
- self.MultiVehicleScriptVth = 0;
- self.MultiVehicleAutoEnabled = true;
- self.MultiVehicleAutoVx = 0;
- self.MultiVehicleAutoFrontTh = holdFrontTh;
- self.MultiVehicleAutoRearTh = holdRearTh;
- 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 startup sync seqBase={warmSeqBaseline} " +
- $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
- "FleetCrabDbg");
-
- var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
- var warmIter = 0;
- var warmReady = false;
- var warmDetail = "";
- while (!_stopping && DateTime.Now < warmEnd)
- {
- warmIter++;
- self.MultiVehicleScriptEnabled = false;
- self.MultiVehicleScriptMode = 0;
- self.MultiVehicleAutoEnabled = true;
- self.MultiVehicleAutoVx = 0;
- self.MultiVehicleAutoFrontTh = holdFrontTh;
- self.MultiVehicleAutoRearTh = holdRearTh;
- 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} " +
- $"detail={warmDetail}",
- "FleetCrabDbg");
- if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
- conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
- {
- warmReady = true;
- DLog.Log(
- $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
- "FleetCrabDbg");
- break;
- }
- yield return true;
- }
- if (!warmReady)
- {
- DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
- "FleetCrabDbg");
- Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
- Cleanup();
- yield break;
- }
-
- Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
-
- if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
- {
- x0 = warmX;
- y0 = warmY;
- theta = warmTh;
- pathStart = new Vector2(x0, y0);
- phi = CommonMath.RoundTh(theta + CrabAngleDeg);
- dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
- targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
- phiRad = phi / 180.0 * Math.PI;
- pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
- pathLeft = new Vector2(-pathDir.Y, pathDir.X);
- self.MultiVehicleAutoIdealX = pathStart.X;
- self.MultiVehicleAutoIdealY = pathStart.Y;
- self.MultiVehicleAutoIdealTh = targetBodyTh;
- self.MultiVehicleAutoCmdTime = DateTime.Now;
- DLog.Log(
- $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
- "FleetCrabDbg");
- }
-
- 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 startAccel = Math.Abs(FleetCrabStartAccel);
- var cmdSpeed = 0f;
- var lastTick = DateTime.Now;
- var speedRampStart = DateTime.Now;
- var stopReason = "done";
-
- while (!_stopping)
- {
- iter++;
-
- if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
- {
- stopReason = "fleet center invalid";
- DLog.Log("ABORT: TryGetControlFleetCenter 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 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);
- var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
- var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
- var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
- var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
- Math.Max(speed, 0.3f)) / Math.PI * 180.0);
- var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
- var rawBiasItem = rawBiasMagnitude * biasSign;
- var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
- var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
- var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
- var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
- var rawFrontTh = baseCrabTh + biasItem + dthItem;
- var rawRearTh = baseCrabTh + biasItem - dthItem;
- ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
- out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
- 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 = driveSpeed;
- 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} 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} 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");
- DLog.Log(
- $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
- $"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} " +
- $"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} 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;
- }
-
- 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");
+ _proc?.Stop();
+ _task?.Stop();
}
}
diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs
index 2a82bdd..0887b5b 100644
--- a/MultiWheel/MultiWheelC/PilotConfig.cs
+++ b/MultiWheel/MultiWheelC/PilotConfig.cs
@@ -203,6 +203,31 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
public float FleetCrabStartWheelAlignDeg = 2f;
+ // ===== Fleet linked Bezier curve walk =====
+ [FieldMember(desc = "FleetCurve MovementTest control points relative to fleet start; format x,y;x,y;...")]
+ public string FleetCurveControlPoints = "0,0;1000,800;2000,800;3000,0";
+
+ [FieldMember(desc = "FleetCurve Bezier resolution")]
+ public int FleetCurveBezierResolution = 100;
+
+ [FieldMember(desc = "FleetCurve speed(m/s)")]
+ public float FleetCurveSpeed = 0.2f;
+
+ [FieldMember(desc = "FleetCurve car direction bias(deg); body target follows tangent+bias")]
+ public float FleetCurveCarDirectionBias = 0f;
+
+ [FieldMember(desc = "FleetCurve slow distance(mm)")]
+ public float FleetCurveSlowDistance = 2000f;
+
+ [FieldMember(desc = "FleetCurve finish distance(mm)")]
+ public float FleetCurveFinishDistance = 20f;
+
+ [FieldMember(desc = "FleetCurve finish speed(m/s)")]
+ public float FleetCurveFinishSpeed = 0.02f;
+
+ [FieldMember(desc = "FleetCurve slowing curve exponent")]
+ public float FleetCurveSlowingPow = 0.8f;
+
// ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";