Simplify fleet curve configuration

This commit is contained in:
2026-07-07 20:44:43 +08:00
parent 0d44fb8fde
commit 381ae3de76
7 changed files with 27 additions and 38 deletions
+4 -5
View File
@@ -452,12 +452,12 @@ namespace MultiWheelC
} }
track.Speed = speed; track.Speed = speed;
track.CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias; track.CarDirectionBias = 0f;
DLog.Log( DLog.Log(
$"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " + $"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}, " + $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
$"carDirectionBias={PilotDefinition.Conf.FleetCurveCarDirectionBias:0.0})", $"carDirectionBias=0.0)",
"FleetCurveDbg"); "FleetCurveDbg");
if (dstId != -1) if (dstId != -1)
@@ -473,8 +473,7 @@ namespace MultiWheelC
{ {
Track = track, Track = track,
CurveSpeed = speed, CurveSpeed = speed,
CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias, CarDirectionBias = 0f,
BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution,
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance, SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance, FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed, FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
@@ -492,7 +491,7 @@ namespace MultiWheelC
if (srcId != -1) if (srcId != -1)
{ {
Leave(srcId); Leave(srcId);
DLog.Log($"閲婃斁鏀捐溅鐐箋srcId}", "FleetCurveDbg"); DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
} }
} }
} }
+1 -3
View File
@@ -287,9 +287,7 @@ public class FleetCrabWalk : MovementDefinition
"FleetCrabDbg"); "FleetCrabDbg");
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold)); var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0 var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
? conf.MultiVehicleControlRadius
: conf.TestCarSyncDistance / 2f));
ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta), ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
(float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh, (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
out _, out _); out _, out _);
+1 -3
View File
@@ -289,9 +289,7 @@ public class FleetCurveWalk : MovementDefinition
var resolution = Math.Max(2, BezierResolution); var resolution = Math.Max(2, BezierResolution);
var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed)); var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold)); var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0 var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
? conf.MultiVehicleControlRadius
: conf.TestCarSyncDistance / 2f));
ApplyFleetControlPointRadius(chassis, controlRadius); ApplyFleetControlPointRadius(chassis, controlRadius);
+15 -10
View File
@@ -9,6 +9,7 @@ using CommonUsage.Chassis;
using CommonUsage.Mathematics; using CommonUsage.Mathematics;
using MDCSToolBox.Clumsy.Movements; using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot; using MDCSToolBox.Clumsy.Pilot;
using MDCSToolBox.Clumsy.Tracks;
namespace MultiWheelC; namespace MultiWheelC;
@@ -441,21 +442,25 @@ public class FleetCurveWalkTest : MovementTest
return; return;
} }
if (!FleetCurveWalk.TryParsePointList(PilotDefinition.Conf.FleetCurveControlPoints, var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
out var relativePoints, out var error)) var controlPoints = new List<Vector2>();
for (var i = 0; i < pointCount; i++)
controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
var fleetCenter = new Vector2(x, y);
if (Vector2.Distance(fleetCenter, controlPoints[0]) >
Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
controlPoints.Reverse();
var track = new BezierTrack(controlPoints)
{ {
DLog.Log($"FleetCurveWalkTest abort: {error}", "FleetCurveDbg"); Speed = PilotDefinition.Conf.FleetCurveSpeed,
Hedingben.ToastText(error, "FleetCurve"); CarDirectionBias = 0f
return; };
}
var controlPoints = FleetCurveWalk.BuildRelativeControlPoints(new Vector2(x, y), th, relativePoints);
_proc = new FleetCurveWalk _proc = new FleetCurveWalk
{ {
ControlPoints = controlPoints, Track = track,
CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed, CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias, CarDirectionBias = 0f,
BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution,
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance, SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance, FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed, FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+2 -11
View File
@@ -46,9 +46,6 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "多车联动:启用互识别纠正")] public bool MultiVehicleUseDetect = false; [FieldMember(desc = "多车联动:启用互识别纠正")] public bool MultiVehicleUseDetect = false;
// E: 编队控制点半径(mm)。0 表示自动取 syncDistance/2(与 SetOriginBias 几何一致),>0 时按本值固定。
// 取代历史硬编码 510,避免改间距后控制点半径不跟随导致转向/补偿几何错位。
[FieldMember(desc = "多车联动:控制点半径(mm0=syncDistance/2)")] public float MultiVehicleControlRadius = 0f;
// B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿), // B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿),
// 即视为失效并清零下发速度,避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。 // 即视为失效并清零下发速度,避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
[FieldMember(desc = "多车联动:自动速度命令超时(ms0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0; [FieldMember(desc = "多车联动:自动速度命令超时(ms0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
@@ -204,18 +201,12 @@ public class PilotConfig : MultiWheelPilotConfig
public float FleetCrabStartWheelAlignDeg = 2f; public float FleetCrabStartWheelAlignDeg = 2f;
// ===== Fleet linked Bezier curve walk ===== // ===== Fleet linked Bezier curve walk =====
[FieldMember(desc = "FleetCurve MovementTest control points relative to fleet start; format x,y;x,y;...")] [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
public string FleetCurveControlPoints = "0,0;1000,800;2000,800;3000,0"; public int FleetCurveTestControlPointCount = 4;
[FieldMember(desc = "FleetCurve Bezier resolution")]
public int FleetCurveBezierResolution = 100;
[FieldMember(desc = "FleetCurve speed(m/s)")] [FieldMember(desc = "FleetCurve speed(m/s)")]
public float FleetCurveSpeed = 0.2f; 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)")] [FieldMember(desc = "FleetCurve slow distance(mm)")]
public float FleetCurveSlowDistance = 2000f; public float FleetCurveSlowDistance = 2000f;
+2 -4
View File
@@ -1098,10 +1098,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
if (fleetReady && !fleetStopActive) if (fleetReady && !fleetStopActive)
{ {
chassis.SetOriginBias(layoutX, layoutY, layoutTh); chassis.SetOriginBias(layoutX, layoutY, layoutTh);
// E: 统一控制点半径——配置 >0 用配置值,否则取 syncDistance/2与编队几何一致),不再硬编码 510 // E: 统一控制点半径取 syncDistance/2与编队几何一致。
var controlRadius = Conf.MultiVehicleControlRadius > 0 var controlRadius = syncDistance / 2f;
? Conf.MultiVehicleControlRadius
: syncDistance / 2f;
chassis.ControlPointRadius = controlRadius; chassis.ControlPointRadius = controlRadius;
// #1 纠偏随旋转缩放:把每轮纠偏钳到旋转切向的比例,减速末段切向变小时纠偏同步缩小,杜绝轮向乱摆。 // #1 纠偏随旋转缩放:把每轮纠偏钳到旋转切向的比例,减速末段切向变小时纠偏同步缩小,杜绝轮向乱摆。
chassis.RotateCompTangentFrac = rotateParams.CompTangentFrac; chassis.RotateCompTangentFrac = rotateParams.CompTangentFrac;
+2 -2
View File
@@ -127,7 +127,7 @@ lock (MultiVehicleFleet)
编队间距配置变更时,实际控制点半径仍固定 510mm,补偿/转向几何与 `TestCarSyncDistance` 不一致,调参困难。 编队间距配置变更时,实际控制点半径仍固定 510mm,补偿/转向几何与 `TestCarSyncDistance` 不一致,调参困难。
**建议修复** **建议修复**
统一使用 `syncDistance / 2f` 或配置项 `MultiVehicleControlRadius`,删除 magic number 510。 统一使用 `syncDistance / 2f`,删除 magic number 510。
--- ---
@@ -203,7 +203,7 @@ lock (MultiVehicleFleet)
- **B**`MultiVehicleSendMotion` 回调写入 `MultiVehicleAutoCmdTime`;主车自动分支按 `MultiVehicleAutoCmdTimeoutMs`(0=auto) 判定命令新鲜度,超时清零速度/idealPos 并关闭 `AutoEnabled`,避免末速度滑行。 - **B**`MultiVehicleSendMotion` 回调写入 `MultiVehicleAutoCmdTime`;主车自动分支按 `MultiVehicleAutoCmdTimeoutMs`(0=auto) 判定命令新鲜度,超时清零速度/idealPos 并关闭 `AutoEnabled`,避免末速度滑行。
- **C**:新增本地 `_multiVehicleFleetSeen` 存活时刻表,register/notify 收到即刷新;主车 Tick `PruneStaleFleetMembers()``MultiVehicleMemberTtlMs`(0=auto) 剔除掉线成员,`fleetReady`(数量==总数) 因此蕴含全员新鲜。 - **C**:新增本地 `_multiVehicleFleetSeen` 存活时刻表,register/notify 收到即刷新;主车 Tick `PruneStaleFleetMembers()``MultiVehicleMemberTtlMs`(0=auto) 剔除掉线成员,`fleetReady`(数量==总数) 因此蕴含全员新鲜。
- **D**:回调不再丢弃 `idealPos/idealAngle`,写入 `MultiVehicleAutoIdeal*` 并经 notify(`HasIdeal/IdealX/Y/Th`) 广播;自动模式下以理想车队中心作为各车 layout 前馈目标(`MultiVehicleAutoUseIdealCenter`,默认开)。 - **D**:回调不再丢弃 `idealPos/idealAngle`,写入 `MultiVehicleAutoIdeal*` 并经 notify(`HasIdeal/IdealX/Y/Th`) 广播;自动模式下以理想车队中心作为各车 layout 前馈目标(`MultiVehicleAutoUseIdealCenter`,默认开)。
- **E**新增 `MultiVehicleControlRadius`(0=syncDistance/2)`ControlPointRadius``SendMotion(localControlRadius)` 统一取该值,删除硬编码 510。 - **E**`ControlPointRadius``SendMotion(localControlRadius)` 统一取 `syncDistance / 2f`,删除硬编码 510。
- **F**notify 改为 POST + JSON body(取代 GET query 串);新增单调递增 `Seq`,从车丢弃乱序旧包(含主车重启回退识别)。 - **F**notify 改为 POST + JSON body(取代 GET query 串);新增单调递增 `Seq`,从车丢弃乱序旧包(含主车重启回退识别)。
- **G**:新增 `FleetCenterSnapshot` 不可变快照 + `volatile` 引用,`PublishFleetCenter` 整体赋值,控制器线程 `GetFleetCenterSnapshot()` 只读完整快照,消除 torn read。 - **G**:新增 `FleetCenterSnapshot` 不可变快照 + `volatile` 引用,`PublishFleetCenter` 整体赋值,控制器线程 `GetFleetCenterSnapshot()` 只读完整快照,消除 torn read。
- **H**:自动模式新增 `MultiVehicleAutoRequireFleetCenter`(默认开) 门控——无有效车队中心(定位丢失)时强制停车,补上纯 SLAM 模式安全网;手动模式不受限。 - **H**:自动模式新增 `MultiVehicleAutoRequireFleetCenter`(默认开) 门控——无有效车队中心(定位丢失)时强制停车,补上纯 SLAM 模式安全网;手动模式不受限。