Add fleet curve walk support
This commit is contained in:
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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
|
||||
{
|
||||
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
|
||||
public float CrabAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
|
||||
public float BodyToPathAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径长度(mm)。</summary>
|
||||
public float CrabLengthMm = 2000f;
|
||||
|
||||
/// <summary>行驶速度(m/s)。</summary>
|
||||
public float CrabSpeed = 0.2f;
|
||||
|
||||
/// <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;
|
||||
|
||||
/// <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;
|
||||
}
|
||||
|
||||
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<bool> 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");
|
||||
}
|
||||
}
|
||||
@@ -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<Vector2> 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<Vector2> points, out string error)
|
||||
{
|
||||
points = new List<Vector2>();
|
||||
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<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
|
||||
{
|
||||
var source = relativePoints ?? new List<Vector2>();
|
||||
var normalized = new List<Vector2>();
|
||||
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<Vector2>();
|
||||
for (var i = 0; i < normalized.Count; i++)
|
||||
result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
|
||||
return result;
|
||||
}
|
||||
|
||||
public static List<Vector2> 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<Vector2>();
|
||||
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<Vector2>();
|
||||
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<bool> 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<Vector2>(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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
{
|
||||
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
|
||||
public float CrabAngleDeg = 45f;
|
||||
private FleetCurveWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
|
||||
public float BodyToPathAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径长度(mm)。</summary>
|
||||
public float CrabLengthMm = 2000f;
|
||||
|
||||
/// <summary>行驶速度(m/s)。</summary>
|
||||
public float CrabSpeed = 0.2f;
|
||||
|
||||
/// <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;
|
||||
|
||||
/// <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()
|
||||
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<bool> 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();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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";
|
||||
|
||||
Reference in New Issue
Block a user