From 381ae3de769c9c95a38f4daf90a31c504d9090e0 Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Tue, 7 Jul 2026 20:44:43 +0800 Subject: [PATCH] Simplify fleet curve configuration --- MultiWheel/MultiWheelC/AGV.cs | 9 ++++---- MultiWheel/MultiWheelC/FleetCrabWalk.cs | 4 +--- MultiWheel/MultiWheelC/FleetCurveWalk.cs | 4 +--- MultiWheel/MultiWheelC/MovementTests.cs | 25 ++++++++++++++--------- MultiWheel/MultiWheelC/PilotConfig.cs | 13 ++---------- MultiWheel/MultiWheelC/PilotDefinition.cs | 6 ++---- docs/MultiVehicleAutoSyncReview.md | 4 ++-- 7 files changed, 27 insertions(+), 38 deletions(-) diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 7af2057..b0c86c0 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -452,12 +452,12 @@ namespace MultiWheelC } track.Speed = speed; - track.CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias; + track.CarDirectionBias = 0f; 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})", + $"carDirectionBias=0.0)", "FleetCurveDbg"); if (dstId != -1) @@ -473,8 +473,7 @@ namespace MultiWheelC { Track = track, CurveSpeed = speed, - CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias, - BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution, + CarDirectionBias = 0f, SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance, FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance, FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed, @@ -492,7 +491,7 @@ namespace MultiWheelC if (srcId != -1) { Leave(srcId); - DLog.Log($"释放放车点{srcId}", "FleetCurveDbg"); + DLog.Log($"release srcId={srcId}", "FleetCurveDbg"); } } } diff --git a/MultiWheel/MultiWheelC/FleetCrabWalk.cs b/MultiWheel/MultiWheelC/FleetCrabWalk.cs index 8aeccf3..c75333d 100644 --- a/MultiWheel/MultiWheelC/FleetCrabWalk.cs +++ b/MultiWheel/MultiWheelC/FleetCrabWalk.cs @@ -287,9 +287,7 @@ public class FleetCrabWalk : MovementDefinition "FleetCrabDbg"); var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold)); - var controlRadius = Math.Max(1f, Math.Abs(conf.MultiVehicleControlRadius > 0 - ? conf.MultiVehicleControlRadius - : conf.TestCarSyncDistance / 2f)); + var controlRadius = Math.Max(1f, Math.Abs(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 _); diff --git a/MultiWheel/MultiWheelC/FleetCurveWalk.cs b/MultiWheel/MultiWheelC/FleetCurveWalk.cs index cb66bcd..c5b2e2e 100644 --- a/MultiWheel/MultiWheelC/FleetCurveWalk.cs +++ b/MultiWheel/MultiWheelC/FleetCurveWalk.cs @@ -289,9 +289,7 @@ public class FleetCurveWalk : MovementDefinition 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)); + var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f); ApplyFleetControlPointRadius(chassis, controlRadius); diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs index dc8e2c3..3d6322d 100644 --- a/MultiWheel/MultiWheelC/MovementTests.cs +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -9,6 +9,7 @@ using CommonUsage.Chassis; using CommonUsage.Mathematics; using MDCSToolBox.Clumsy.Movements; using MDCSToolBox.Clumsy.Pilot; +using MDCSToolBox.Clumsy.Tracks; namespace MultiWheelC; @@ -441,21 +442,25 @@ public class FleetCurveWalkTest : MovementTest return; } - if (!FleetCurveWalk.TryParsePointList(PilotDefinition.Conf.FleetCurveControlPoints, - out var relativePoints, out var error)) + var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount); + var controlPoints = new List(); + 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"); - Hedingben.ToastText(error, "FleetCurve"); - return; - } + Speed = PilotDefinition.Conf.FleetCurveSpeed, + CarDirectionBias = 0f + }; - var controlPoints = FleetCurveWalk.BuildRelativeControlPoints(new Vector2(x, y), th, relativePoints); _proc = new FleetCurveWalk { - ControlPoints = controlPoints, + Track = track, CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed, - CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias, - BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution, + CarDirectionBias = 0f, SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance, FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance, FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed, diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index 0887b5b..091b605 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -46,9 +46,6 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false; - // E: 编队控制点半径(mm)。0 表示自动取 syncDistance/2（与 SetOriginBias 几何一致），>0 时按本值固定。 - // 取代历史硬编码 510，避免改间距后控制点半径不跟随导致转向/补偿几何错位。 - [FieldMember(desc = "多车联动：控制点半径(mm，0=syncDistance/2)")] public float MultiVehicleControlRadius = 0f; // B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)， // 即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。 [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0; @@ -204,18 +201,12 @@ public class PilotConfig : MultiWheelPilotConfig 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 MovementTest Bezier control point count")] + public int FleetCurveTestControlPointCount = 4; [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; diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index 5c83c8e..26885ae 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -1098,10 +1098,8 @@ public class PilotDefinition : MultiWheelPilotDefinition0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。 - var controlRadius = Conf.MultiVehicleControlRadius > 0 - ? Conf.MultiVehicleControlRadius - : syncDistance / 2f; + // E: 统一控制点半径，取 syncDistance/2，与编队几何一致。 + var controlRadius = syncDistance / 2f; chassis.ControlPointRadius = controlRadius; // #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。 chassis.RotateCompTangentFrac = rotateParams.CompTangentFrac; diff --git a/docs/MultiVehicleAutoSyncReview.md b/docs/MultiVehicleAutoSyncReview.md index 5d6e622..c8e24e6 100644 --- a/docs/MultiVehicleAutoSyncReview.md +++ b/docs/MultiVehicleAutoSyncReview.md @@ -127,7 +127,7 @@ lock (MultiVehicleFleet) 编队间距配置变更时，实际控制点半径仍固定 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`，避免末速度滑行。 - **C**：新增本地 `_multiVehicleFleetSeen` 存活时刻表，register/notify 收到即刷新；主车 Tick `PruneStaleFleetMembers()` 按 `MultiVehicleMemberTtlMs`(0=auto) 剔除掉线成员，`fleetReady`(数量==总数) 因此蕴含全员新鲜。 - **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`，从车丢弃乱序旧包（含主车重启回退识别）。 - **G**：新增 `FleetCenterSnapshot` 不可变快照 + `volatile` 引用，`PublishFleetCenter` 整体赋值，控制器线程 `GetFleetCenterSnapshot()` 只读完整快照，消除 torn read。 - **H**：自动模式新增 `MultiVehicleAutoRequireFleetCenter`(默认开) 门控——无有效车队中心（定位丢失）时强制停车，补上纯 SLAM 模式安全网；手动模式不受限。